本發(fā)明涉及基于機(jī)器視覺的位姿檢測(cè)系統(tǒng)，尤其涉及基于雙目視覺、針對(duì)并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿的檢測(cè)方法。

背景技術(shù)：

在并聯(lián)機(jī)構(gòu)控制中，末端運(yùn)動(dòng)位姿是反映機(jī)構(gòu)運(yùn)動(dòng)狀態(tài)的重要參數(shù)，精確測(cè)得機(jī)構(gòu)的末端運(yùn)動(dòng)位姿可有效避免通過運(yùn)動(dòng)學(xué)模型解算所帶來的誤差，這將利于實(shí)現(xiàn)并聯(lián)機(jī)構(gòu)的高性能控制。相對(duì)于其它檢測(cè)手段，機(jī)器視覺具有非接觸、適用性強(qiáng)、高性價(jià)比等優(yōu)點(diǎn)，尤其適用于具有運(yùn)動(dòng)多自由度、運(yùn)動(dòng)軌跡復(fù)雜、難以直接檢測(cè)的并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿檢測(cè)。視覺位姿檢測(cè)系統(tǒng)通?？煞譃閱文俊㈦p目以及多目檢測(cè)系統(tǒng)。由于雙目視覺算法的魯棒性和精確性優(yōu)于單目算法，而其算法的實(shí)時(shí)性相對(duì)于多目視覺又具有較大的優(yōu)勢(shì)，因此，雙目視覺得到了較為廣泛的應(yīng)用。對(duì)于并聯(lián)機(jī)構(gòu)的末端運(yùn)動(dòng)位姿檢測(cè)問題，采用基于雙目視覺檢測(cè)方法的難點(diǎn)在于如何將視覺系統(tǒng)采集到的并聯(lián)機(jī)構(gòu)圖像經(jīng)過圖像處理后快速而精確地獲取該機(jī)構(gòu)的末端位姿信息。在雙目視覺位姿檢測(cè)過程中，立體匹配過程最為重要，匹配的速度與精度直接決定整個(gè)位姿檢測(cè)系統(tǒng)的速度與精度，但由于雙目視覺算法復(fù)雜，光照變化、背景反光、噪聲干擾等外界因素會(huì)引起雙目視覺系統(tǒng)的圖像匹配難以精確實(shí)現(xiàn)，導(dǎo)致視覺檢測(cè)系統(tǒng)的速度和精度受到影響。

文獻(xiàn)《一種新型的并聯(lián)機(jī)器人位姿立體視覺檢測(cè)系統(tǒng)》(吳迪飛，丁永生等.計(jì)算機(jī)工程與應(yīng)用，2007，43(33):190-192)建立了一種基于尺度不變特征變換(SIFT)的雙目視覺檢測(cè)系統(tǒng)對(duì)并聯(lián)機(jī)器人進(jìn)行位姿檢測(cè)，并用仿真實(shí)驗(yàn)證明了該檢測(cè)算法的魯棒性和可行性，但由于該檢測(cè)系統(tǒng)匹配過程所采用的SIFT算法會(huì)產(chǎn)生大量維數(shù)較大的特征向量，增大了算法的復(fù)雜度，導(dǎo)致位姿檢測(cè)的實(shí)時(shí)性受到影響；文獻(xiàn)《Harris-SIFT算法及其在雙目立體視覺中的應(yīng)用》(趙欽君，趙東標(biāo)等.電子科技大學(xué)學(xué)報(bào)，2010，39(4):546-550)結(jié)合Harris顯著性和SIFT描述子，提出一種Harris-SIFT算法運(yùn)用于雙目視覺匹配中，并實(shí)驗(yàn)驗(yàn)證了方法的有效性，該方法有效提高了匹配算法的實(shí)時(shí)性，但由于Harris檢測(cè)出的角點(diǎn)所含信息少于SIFT檢測(cè)特征點(diǎn)所含信息，Harris-SIFT算法存在錯(cuò)誤匹配和誤差匹配的問題。

針對(duì)Harris-SIFT算法存在的誤匹配問題，可考慮采用提純算法對(duì)匹配結(jié)果進(jìn)行提純。常規(guī)提純算法通常根據(jù)一組包含異常點(diǎn)的數(shù)據(jù)集，計(jì)算出匹配點(diǎn)對(duì)的目標(biāo)模型，再利用剩余點(diǎn)來檢驗(yàn)?zāi)Ｐ?，從而得到有效?shù)據(jù)樣本，因此，提純算法的關(guān)鍵在于快速找到正確的目標(biāo)模型。然而，目標(biāo)模型的獲取通常存在一些問題：比如選取隨機(jī)樣本時(shí)，存在兩個(gè)候選點(diǎn)距離過近而被認(rèn)為是一個(gè)點(diǎn)，從而導(dǎo)致求取的目標(biāo)模型不準(zhǔn)確的問題；而且每次挑選隨機(jī)樣本集，都要尋找其對(duì)應(yīng)候選模型的支撐集，這將導(dǎo)致花費(fèi)過多時(shí)間尋找存在較多誤差觀測(cè)數(shù)據(jù)集的對(duì)應(yīng)支撐點(diǎn)集上。

技術(shù)實(shí)現(xiàn)要素：

鑒于上述雙目視覺檢測(cè)存在的問題，本發(fā)明提出一種基于雙目視覺的并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿檢測(cè)方法，該方法提出了一種新的匹配算法，算法通過快速Harris-SIFT算法實(shí)現(xiàn)圖像特征點(diǎn)匹配，并提出一種新的提純算法對(duì)匹配結(jié)果進(jìn)行提純，從而解決傳統(tǒng)雙目視覺算法復(fù)雜，空間匹配難以精確實(shí)現(xiàn)，所導(dǎo)致的檢測(cè)實(shí)時(shí)性差、檢測(cè)精度不高的問題。

本發(fā)明采用的技術(shù)方案是采用如下步驟：

一種基于雙目視覺的并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿檢測(cè)方法，包括如下步驟：

步驟1)利用雙目相機(jī)采集并聯(lián)機(jī)構(gòu)的原始圖像，并對(duì)采集到的機(jī)構(gòu)圖像進(jìn)行基于小波變換的圖像去噪預(yù)處理；

步驟2)采用基于Harris算子的角點(diǎn)提取法對(duì)機(jī)構(gòu)圖像特征進(jìn)行提??；

步驟3)采用Harris-SIFT算法對(duì)并聯(lián)機(jī)構(gòu)圖像進(jìn)行特征匹配；該匹配算法首先通過Harris算子提取圖像特征點(diǎn)，再利用SIFT特征描述子對(duì)Harris算子提取出的特征點(diǎn)進(jìn)行匹配；

步驟4)針對(duì)Harris-SIFT算法存在錯(cuò)誤匹配和誤差匹配的問題，采用新的提純算法對(duì)匹配結(jié)果做進(jìn)一步提純處理：通過分塊取點(diǎn)和提前取點(diǎn)驗(yàn)算臨時(shí)模型改進(jìn)提純算法，其中，分塊取點(diǎn)：采取圖像分塊且每個(gè)塊中隨機(jī)取一個(gè)匹配點(diǎn)的方式；提前取點(diǎn)驗(yàn)算臨時(shí)模型：隨機(jī)選取9個(gè)匹配對(duì)，用8個(gè)匹配求出臨時(shí)模型參數(shù)，而第9個(gè)匹配對(duì)用來驗(yàn)證臨時(shí)模型的正確性，以此快速確定圖像模型；

步驟5)根據(jù)雙目視覺原理，通過前期圖像處理提取到的末端特征點(diǎn)計(jì)算出該末端特征點(diǎn)的三維坐標(biāo)，并計(jì)算出并聯(lián)機(jī)構(gòu)末端姿態(tài)角，最終獲得該并聯(lián)機(jī)構(gòu)末端位姿信息。

進(jìn)一步，所述步驟2)中，Harris算子的角點(diǎn)提取法可直接調(diào)用OpenCV庫函數(shù)實(shí)現(xiàn)，在Harris特征點(diǎn)檢測(cè)中，像素點(diǎn)的二階矩可表示為:

式中I_x、I_y表示像素點(diǎn)的灰度在x和y方向的梯度；I_xy為二階混合偏導(dǎo)；像素點(diǎn)的Harris響應(yīng)函數(shù)為：

R＝det M-k(traceM)²

其中det M為矩陣M的行列式，traceM為矩陣M的真跡；k為經(jīng)驗(yàn)常數(shù)，通常取值范圍0.04～0.06；設(shè)定閾值t，根據(jù)上述Harris算子計(jì)算響應(yīng)值R，若R<t，則該點(diǎn)為特征點(diǎn)。

進(jìn)一步，所述步驟4)中，所述提純算法通過一組包含異常點(diǎn)的數(shù)據(jù)集，計(jì)算出匹配點(diǎn)對(duì)的目標(biāo)模型，再利用剩余點(diǎn)來檢驗(yàn)?zāi)Ｐ?，從而得到有效?shù)據(jù)樣本；具體步驟如下：

4.1)根據(jù)第一幅圖中匹配點(diǎn)的坐標(biāo)邊界將圖片平均分成b×b塊，其中，b>3，隨機(jī)選取9個(gè)互不相同的塊且每個(gè)塊中隨機(jī)選取一個(gè)匹配點(diǎn)，構(gòu)成9個(gè)匹配點(diǎn)對(duì)的隨機(jī)樣本集S；

4.2)由隨機(jī)樣本集S隨機(jī)選取8個(gè)匹配點(diǎn)對(duì)求出臨時(shí)候選模型F；

4.3)檢測(cè)第9個(gè)點(diǎn)是否為臨時(shí)候選模型F的支撐集：如果是，則此臨時(shí)候選模型F為候選模型F；否則，重新選擇9對(duì)匹配點(diǎn)，重復(fù)4.1)、4.2)進(jìn)程；

4.4)通過候選模型F和閾值k檢測(cè)所有匹配點(diǎn)對(duì)，得到候選模型D的支撐集對(duì)數(shù)m；

4.5)若m≥閾值t，則得到目標(biāo)模型F；否則，重新選擇9對(duì)匹配點(diǎn)，重復(fù)4.1)、4.2)、4.3)、4.4)進(jìn)程；

4.6)對(duì)目標(biāo)模型F進(jìn)行優(yōu)化，最終確定目標(biāo)模型H；

4.7)每次選擇隨機(jī)樣本集，計(jì)數(shù)器都加1，若重復(fù)k次沒有找到模型參數(shù)，則終止程序。

進(jìn)一步，所述步驟5)中，根據(jù)投影定理求出并聯(lián)機(jī)構(gòu)末端特征點(diǎn)三維坐標(biāo)(X,Y,Z)，再通過攝像機(jī)旋轉(zhuǎn)矩陣R，求出并聯(lián)機(jī)構(gòu)末端姿態(tài)角；其中并聯(lián)機(jī)構(gòu)末端位置信息如下：

P_w＝(A^TA)^-1A^TB

其中，(u₁,v₁)、(u₂,v₂)分別表示末端特征點(diǎn)在左右相機(jī)投影點(diǎn)的圖像坐標(biāo)；表示左右投影矩陣的第i行，第j列；

同時(shí)，并聯(lián)機(jī)構(gòu)末端姿態(tài)角為：

其中，R_ij(i＝1,2,3；j＝1,2,3)表示旋轉(zhuǎn)矩陣R的第i行，第j列，α為繞X軸旋轉(zhuǎn)的俯仰角，單位為：rad，β為繞Y軸旋轉(zhuǎn)的翻滾角，單位為：rad，γ為繞Z軸旋轉(zhuǎn)的航向角，單位為：rad。

進(jìn)一步，所述步驟5)中，還包括，三維重建時(shí)，首先需要得到所建雙目視覺模型的攝像機(jī)參數(shù)，該攝像機(jī)參數(shù)可通過攝像機(jī)標(biāo)定獲得；采用張正友平面標(biāo)定法對(duì)雙目視覺系統(tǒng)進(jìn)行攝像機(jī)標(biāo)定。

進(jìn)一步，所述張正友平面標(biāo)定法過程為：首先固定相機(jī)，然后對(duì)標(biāo)定板進(jìn)行拍照，再將標(biāo)定板圖像輸入計(jì)算機(jī)并獲取標(biāo)定板上每個(gè)格點(diǎn)的像素坐標(biāo)，接著把標(biāo)定板的已知三維坐標(biāo)載入計(jì)算機(jī)，通過將標(biāo)定板的像素坐標(biāo)和三維坐標(biāo)代入標(biāo)定模型，求解出攝像機(jī)的內(nèi)外參數(shù)，經(jīng)過左右攝像機(jī)的分別標(biāo)定。

本發(fā)明提出一種基于雙目視覺的并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿檢測(cè)方法，通過采用上述技術(shù)方案后，其具有以下有益效果：

1、本發(fā)明在立體匹配階段采用Harris-SIFT算法實(shí)現(xiàn)立體匹配，通過算法簡(jiǎn)單的Harris算子提取圖像特征點(diǎn)，再利用SIFT特征描述子對(duì)圖像進(jìn)行匹配，使得匹配結(jié)果兼具實(shí)時(shí)性和穩(wěn)定性；

2、本發(fā)明針對(duì)匹配算法存在的誤差匹配和錯(cuò)誤匹配問題，提出一種新的提純算法，即通過分塊取點(diǎn)獲得更為準(zhǔn)確的圖像模型參數(shù)和提前取點(diǎn)驗(yàn)算臨時(shí)模型以快速確定圖像模型的方式來改進(jìn)提純算法，用以提純Harris-SIFT匹配結(jié)果，剔除Harris-SIFT算法存在的誤匹配點(diǎn)，在保證匹配算法實(shí)時(shí)性的同時(shí)，提高匹配的正確率，從而提高機(jī)構(gòu)末端三維位姿檢測(cè)的精度。

附圖說明

以下結(jié)合附圖和具體實(shí)施方式對(duì)本發(fā)明作進(jìn)一步詳細(xì)說明。

圖1是本發(fā)明所提出的一種基于雙目視覺的并聯(lián)機(jī)構(gòu)末端位姿檢測(cè)系統(tǒng)流程圖。

圖2是采用基于小波變換方法的并聯(lián)機(jī)構(gòu)圖像去噪效果圖。其中：a為有噪聲的圖像，b為去噪后的圖像。

圖3是Harris特征點(diǎn)提取圖。

圖4是基于Harris-SIFT算法的特征點(diǎn)立體匹配效果圖。

圖5是對(duì)匹配結(jié)果提純后的效果圖。

圖6是本發(fā)明方法的雙目視覺成像原理圖。

圖7是建立在機(jī)構(gòu)平臺(tái)的定坐標(biāo)系與動(dòng)坐標(biāo)系。

圖8是本發(fā)明方法實(shí)驗(yàn)結(jié)果與激光測(cè)距儀和電子羅盤測(cè)量結(jié)果對(duì)比得到的機(jī)構(gòu)末端位姿參數(shù)的跟蹤誤差圖。

具體實(shí)施方式

本發(fā)明提供了一種基于雙目視覺的并聯(lián)機(jī)構(gòu)末端位姿檢測(cè)方法，為解決雙目視覺匹配過程實(shí)時(shí)性差的問題，本發(fā)明采用Harris-SIFT算法實(shí)現(xiàn)立體匹配；而針對(duì)Harris-SIFT算法存在錯(cuò)誤匹配和誤差匹配的問題，本發(fā)明提出一種提純算法，以剔除Harris-SIFT算法存在的誤匹配點(diǎn)，在保證匹配算法實(shí)時(shí)性的同時(shí)，提高匹配的正確率，進(jìn)而使得位姿檢測(cè)的實(shí)時(shí)性和精度都得以提高。具體步驟如下：

1、參見圖1，通過雙目視覺左右相機(jī)采集并聯(lián)機(jī)構(gòu)原始圖像。通過雙目相機(jī)實(shí)時(shí)獲取并聯(lián)機(jī)構(gòu)圖像信息，相機(jī)應(yīng)調(diào)節(jié)到適宜的位置以保證在并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)過程中，并聯(lián)機(jī)構(gòu)末端始終在相機(jī)視野范圍之內(nèi)。

2、參見圖1和圖2，將左右攝像機(jī)采集到的原始圖像進(jìn)行基于小波變換的圖像去噪預(yù)處理操作。通過采用Visual Studio集成開發(fā)工具進(jìn)行檢測(cè)系統(tǒng)人機(jī)界面開發(fā)，并使用C/C++語言編程，同時(shí)結(jié)合OpenCV(開源跨平臺(tái)計(jì)算機(jī)視覺庫)，實(shí)現(xiàn)各模塊相關(guān)功能的編程。去噪過程采用圖像小波閾值去噪(WaveDec())的方法對(duì)原始機(jī)構(gòu)圖像進(jìn)行預(yù)處理，通過設(shè)置閾值(g_HighPassFilterValue)，再對(duì)高頻小波系數(shù)進(jìn)行閾值處理，最后對(duì)圖像進(jìn)行小波重構(gòu)(WaveRec())，實(shí)現(xiàn)圖像去噪。

3、參見圖1和圖3，采用基于Harris算子的角點(diǎn)提取法對(duì)機(jī)構(gòu)圖像特性進(jìn)行提取。為了保證特征提取的快速性和穩(wěn)定性，考慮到并聯(lián)機(jī)構(gòu)圖像是剛性圖像且存在多個(gè)角點(diǎn)，而Harris角點(diǎn)檢測(cè)不僅計(jì)算簡(jiǎn)單、穩(wěn)定，而且不易受光照、剛體幾何形變的影響，因此，采用Harris角點(diǎn)檢測(cè)算法對(duì)機(jī)構(gòu)圖像進(jìn)行特征點(diǎn)提取。Harris特征點(diǎn)提取可直接調(diào)用OpenCV庫函數(shù)((conerHarris())實(shí)現(xiàn)，在Harris特征點(diǎn)檢測(cè)中，像素點(diǎn)的二階矩可表示為:

式中I_x、I_y表示像素點(diǎn)的灰度在x和y方向的梯度；I_xy為二階混合偏導(dǎo)。因此，像素點(diǎn)的Harris響應(yīng)函數(shù)為:

R＝det M-k(traceM)² (2)

其中det M為矩陣M的行列式，traceM為矩陣M的真跡；k為經(jīng)驗(yàn)常數(shù)，通常取值范圍0.04～0.06。設(shè)定閾值t，根據(jù)上述Harris算子計(jì)算響應(yīng)值R，若R<t，則該點(diǎn)為特征點(diǎn)。

4、參見圖1和圖4，采用Harris-SIFT算法實(shí)現(xiàn)特征點(diǎn)立體匹配。通過使用Harris特征點(diǎn)取代SIFT中的極值點(diǎn)，并為每個(gè)特征點(diǎn)定義主方向，根據(jù)特征點(diǎn)生成特征向量描述子，再根據(jù)SIFT匹配算法的歐氏距離進(jìn)行特征點(diǎn)匹配，將Harris角點(diǎn)檢測(cè)算法與SIFT尺度不變特征變換有機(jī)結(jié)合，使得到的特征點(diǎn)具有兩種算法的共同特征，不僅可提高SIFT算子的實(shí)時(shí)性，而且能提高特征點(diǎn)的穩(wěn)定性?；贖arris-SIFT算法的特征匹配需要編程實(shí)現(xiàn)，程序編寫時(shí)采用了如下公式：

其中：公式(3)為特征點(diǎn)x的DoG算子在尺度空間的泰勒展開式，D(x)表示空間尺度函數(shù)，△x為特征點(diǎn)x的偏移量；公式(4)為特征點(diǎn)精確位置的偏移量；公式(5)表示非邊沿點(diǎn)的判斷條件，r為設(shè)定的閾值，H為2×2的Hessian矩陣，Tr(H)為H矩陣的真跡，Det(H)為H矩陣的行列式；公式(6)為2×2的Hessian矩陣，D_xx表示特征點(diǎn)的DOG算子沿x方向的二階偏導(dǎo)數(shù)，D_yy表示特征點(diǎn)的DOG算子沿y方向的二階偏導(dǎo)數(shù)，D_xy表示特征點(diǎn)的DOG算子沿xy方向的二階偏導(dǎo)數(shù)；公式(7)表示特征點(diǎn)處高斯梯度的模值m(x,y)和方向θ(x,y)，L(x,y)表示特征點(diǎn)所在的尺度空間值。

5、參見圖1和圖5，采用新的提純算法對(duì)匹配結(jié)果進(jìn)行提純，剔除誤匹配對(duì)。針對(duì)Harris-SIFT算法存在錯(cuò)誤匹配和誤差匹配的問題，本發(fā)明采用新的提純算法對(duì)匹配結(jié)果做進(jìn)一步提純處理，以剔除Harris-SIFT算法存在的誤匹配點(diǎn)，在保證匹配算法實(shí)時(shí)性的同時(shí)，提高匹配的正確率，從而提高機(jī)構(gòu)末端三維位姿檢測(cè)的精度。進(jìn)行提純處理時(shí)，由于提純算法的關(guān)鍵在于快速找到正確的目標(biāo)模型，而目標(biāo)模型是由包含內(nèi)點(diǎn)數(shù)最多的候選模型優(yōu)化而得到，為了快速求出候選模型參數(shù)，本發(fā)明隨機(jī)選取9個(gè)匹配對(duì)，其中8個(gè)匹配對(duì)用來求出臨時(shí)模型參數(shù)，而第9個(gè)匹配對(duì)用來驗(yàn)證臨時(shí)模型的正確性，從而快速確定目標(biāo)模型；同時(shí)，本發(fā)明采取圖像分塊且每個(gè)塊中隨機(jī)取一個(gè)匹配點(diǎn)的方式避免存在兩個(gè)匹配點(diǎn)距離過近而被認(rèn)為是一個(gè)點(diǎn)，而導(dǎo)致的基本矩陣不準(zhǔn)確的問題。獲取目標(biāo)模型的具體步驟如下：

[1]根據(jù)第一幅圖中匹配點(diǎn)的坐標(biāo)邊界將圖片平均分成b×b(b>3)塊，隨機(jī)選取9個(gè)互不相同的塊且每個(gè)塊中隨機(jī)選取一個(gè)匹配點(diǎn)，構(gòu)成9個(gè)匹配點(diǎn)對(duì)的隨機(jī)樣本集S。

[2]由隨機(jī)樣本集S隨機(jī)選取8個(gè)匹配點(diǎn)對(duì)求出臨時(shí)候選模型F。

[3]檢測(cè)第9個(gè)點(diǎn)是否為臨時(shí)候選模型F的支撐集：是，則此臨時(shí)候選模型F為候選模型F；否則，重新選擇9對(duì)匹配點(diǎn)，重復(fù)[1]、[2]進(jìn)程。

[4]通過候選模型F和閾值k檢測(cè)所有匹配點(diǎn)對(duì)，得到候選模型D的支撐集對(duì)數(shù)m。

[5]若m≥閾值t，則得到目標(biāo)模型F；否則，重新選擇9對(duì)匹配點(diǎn)，重復(fù)[1]、[2]、[3]、[4]進(jìn)程。

[6]對(duì)目標(biāo)模型F進(jìn)行優(yōu)化，最終確定目標(biāo)模型H。

[7]每次選擇隨機(jī)樣本集，計(jì)數(shù)器都加1，若重復(fù)k次沒有找到模型參數(shù)，則終止程序。

6、參見圖1和圖6，根據(jù)雙目視覺原理實(shí)現(xiàn)三維重建，將前期圖像處理提取到的末端匹配點(diǎn)對(duì)帶入雙目視覺模型，最終獲得并聯(lián)機(jī)構(gòu)末端位姿信息。三維重建時(shí)，首先需要得到所建雙目視覺模型的攝像機(jī)參數(shù)，該攝像機(jī)參數(shù)可通過攝像機(jī)標(biāo)定獲得，本發(fā)明采用張正友標(biāo)定法對(duì)雙目視覺系統(tǒng)進(jìn)行攝像機(jī)標(biāo)定，根據(jù)張正友平面標(biāo)定流程：首先固定相機(jī)，然后對(duì)標(biāo)定板進(jìn)行拍照，再將標(biāo)定板圖像輸入計(jì)算機(jī)并獲取標(biāo)定板上每個(gè)格點(diǎn)的像素坐標(biāo)，接著把標(biāo)定板的已知三維坐標(biāo)載入計(jì)算機(jī)，通過將標(biāo)定板的像素坐標(biāo)和三維坐標(biāo)代入標(biāo)定模型，求解出攝像機(jī)的內(nèi)外參數(shù)，經(jīng)過左右攝像機(jī)的分別標(biāo)定，再根據(jù)式(8)便可獲得雙目視覺系統(tǒng)的攝像機(jī)參數(shù):

式中，R_l、T_l和R_r、T_r分別表示左右攝像機(jī)的旋轉(zhuǎn)矩陣和平移矩陣。

攝像機(jī)標(biāo)定完成后便可得到雙目視覺投影矩陣。假定并聯(lián)機(jī)構(gòu)末端特征點(diǎn)P在左右攝像機(jī)的像點(diǎn)分別為Pl和Pr，左右攝像機(jī)已完成標(biāo)定且它們的投影矩陣分別為Ml、Mr，則根據(jù)投影定理有：

其中s_l，s_r表示比例因子，將上式展開來則有：

式中，(u₁,v₁,1)，(u₂,v₂,1)分別表示投影點(diǎn)P_l和P_r的齊次坐標(biāo)；(X,Y,Z,1)為末端特征點(diǎn)在世界坐標(biāo)系下的齊次坐標(biāo)；表示左右投影矩陣的第i行，第j列。

式(10)和式(11)聯(lián)立后可得如下方程：

令：則可采用最小二乘法求出并聯(lián)機(jī)構(gòu)末端位置三維坐標(biāo)P_W，其表示如下：

P_w＝(A^TA)^-1A^TB (13)

再將旋轉(zhuǎn)矩陣表示為如式(14)所示：

則并聯(lián)機(jī)構(gòu)末端姿態(tài)角為式(15)所示：

式中，α為繞X軸旋轉(zhuǎn)的俯仰角(單位為：rad)，β為繞Y軸旋轉(zhuǎn)的翻滾角(單位為：rad)，γ為繞Z軸旋轉(zhuǎn)的航向角(單位為：rad)。

至此，并聯(lián)機(jī)構(gòu)的末端位姿參數(shù)求解已完成。

實(shí)施例

本發(fā)明著重提出了一種新的基于雙目視覺的并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿檢測(cè)方法，以解決傳統(tǒng)雙目視覺算法在并聯(lián)機(jī)構(gòu)末端位姿檢測(cè)中存在的實(shí)時(shí)性差、檢測(cè)精度不高的問題。下面以一種新型3-DOF并聯(lián)機(jī)構(gòu)末端為檢測(cè)對(duì)象，該檢測(cè)方法的具體實(shí)施方式如下：

1、采集并聯(lián)機(jī)構(gòu)原始圖像。通過維視雙目相機(jī)采集新型3-DOF并聯(lián)機(jī)構(gòu)圖像，其中，相機(jī)型號(hào)為MV-1300FM，鏡頭型號(hào)為AFT-0814MP，相機(jī)安裝并聯(lián)機(jī)構(gòu)的前方，且距地高度和角度均可靈活調(diào)節(jié)。此外，本檢測(cè)系統(tǒng)采用計(jì)算機(jī)操作系統(tǒng)為Windows7，處理器型號(hào)為Intel(R)Core(TM)2Duo，主頻為2.66GHz，內(nèi)存為2GB。

2、對(duì)原始圖像進(jìn)行基于小波變換的去噪處理。為了降低噪聲對(duì)并聯(lián)機(jī)構(gòu)末端位姿檢測(cè)所帶來的不良影響，采用基于小波變換的去噪方法對(duì)原始圖像進(jìn)行預(yù)處理，通過去噪處理有效抑制圖像噪聲，以便下一步特征提取操作，去噪效果如圖2所示。

3、采用基于Harris算子的角點(diǎn)提取法對(duì)機(jī)構(gòu)圖像特性進(jìn)行提取。考慮到新型3-DOF并聯(lián)機(jī)構(gòu)具有明顯的剛體特征，而Harris特征點(diǎn)檢測(cè)不僅在剛體幾何變形和亮度變化方面有著高復(fù)檢率，而且算法簡(jiǎn)單、穩(wěn)定，因此選取Harris角點(diǎn)檢測(cè)算法對(duì)機(jī)構(gòu)圖像進(jìn)行特征點(diǎn)提取。像素點(diǎn)的Harris響應(yīng)函數(shù)為：

R＝det M-k(traceM)² (16)

其中det M為矩陣M的行列式，traceM為矩陣M的真跡；k為經(jīng)驗(yàn)常數(shù)，通常取值范圍0.04～0.06。設(shè)定閾值t，根據(jù)上述Harris算子計(jì)算響應(yīng)值R，若R<t，則該點(diǎn)為特征點(diǎn)，本實(shí)施例取經(jīng)驗(yàn)常數(shù)k＝0.04，并直接通過調(diào)用OpenCV庫函數(shù)((conerHarris())實(shí)現(xiàn)新型3-DOF并聯(lián)機(jī)構(gòu)特征點(diǎn)提取，新型3-DOF并聯(lián)機(jī)構(gòu)特征點(diǎn)提取圖如圖3所示。

4、采用Harris-SIFT算法實(shí)現(xiàn)特征點(diǎn)立體匹配。本實(shí)施例采用Harris-SIFT算法進(jìn)行新型3-DOF并聯(lián)機(jī)構(gòu)圖像特征點(diǎn)匹配。該匹配算法使用Harris特征點(diǎn)取代SIFT中的極值點(diǎn)，采用擬合三維二次函數(shù)的方法實(shí)現(xiàn)對(duì)特征點(diǎn)的精確定位，并剔除對(duì)噪聲敏感的低對(duì)比度點(diǎn)和難以定位的處于圖像邊沿的點(diǎn)，從而篩選出穩(wěn)定的點(diǎn)作為圖像的SIFT特征點(diǎn)?；贖arris-SIFT算法的立體匹配分為以下步驟：

(1)對(duì)特征點(diǎn)x的DoG算子進(jìn)行泰勒展開：

其中，△x為特征點(diǎn)x的偏移量，當(dāng)x為DoG算子的極值點(diǎn)時(shí)，D(x)的一階偏導(dǎo)為0，則特征點(diǎn)精確位置的偏移量△x可求得：

經(jīng)過多次迭代后得到被測(cè)點(diǎn)的精確位置和尺度，將其代入公式(16)可求得該點(diǎn)的DoG值并取其絕對(duì)值，設(shè)定對(duì)比度閾值Tc，從而剔除對(duì)比度絕對(duì)值小于閾值Tc的低對(duì)比度點(diǎn)。

而對(duì)于邊沿點(diǎn)的剔除，可通過DoG函數(shù)極值點(diǎn)的主曲率與閾值比較來實(shí)現(xiàn)，通常，邊沿的DoG函數(shù)極值點(diǎn)比非邊沿點(diǎn)的主曲率大，因此，可將主曲率比值大于閾值的邊沿點(diǎn)剔除。非邊沿點(diǎn)滿足關(guān)系如式(19)所示：

其中，r為設(shè)定的閾值，本實(shí)施例取r＝10；H為2×2的Hessian矩陣，H矩陣表示如式(20)所示：

(2)確定特征點(diǎn)方向。設(shè)點(diǎn)L(x,y)的梯度模為m(x,y)，方向?yàn)棣?x,y)，則該點(diǎn)高斯梯度模和方向表示如式(21)所示：

統(tǒng)計(jì)關(guān)鍵點(diǎn)鄰域像素的梯度分布，并選取直方圖的主峰值作為關(guān)鍵點(diǎn)的方向。至此，每個(gè)特征點(diǎn)都具備了3個(gè)重要的信息：位置、尺度和方向。

(3)生成特征描述子。生成特征描述子需要將坐標(biāo)軸旋轉(zhuǎn)為特征點(diǎn)方向，并均勻劃分該特征點(diǎn)相鄰的16×16像素區(qū)域?yàn)?個(gè)4×4的子區(qū)域，接著在每個(gè)4×4的子區(qū)域上計(jì)算8個(gè)方向的梯度直方圖，這樣每個(gè)特征點(diǎn)就可產(chǎn)生128個(gè)數(shù)據(jù)以形成128維的SIFT特征向量，將生成的特征向量進(jìn)行長(zhǎng)度歸一化，可進(jìn)一步減少光照變化的影響。

(4)特征描述子生成后，便可根據(jù)SIFT匹配算法的歐氏距離進(jìn)行特征點(diǎn)匹配。具體實(shí)現(xiàn)如下：首先，將左圖像中的某一特征點(diǎn)p與待匹配的右圖像的所有特征點(diǎn)分別進(jìn)行歐氏距離計(jì)算；然后，取出右圖像中與p點(diǎn)歐氏距離最近和次近的兩個(gè)點(diǎn)p1和p2；最后，計(jì)算最近歐式距離與次近歐式距離的比值并與一個(gè)閾值相比較，若該比值小于閾值，則匹配成功，此時(shí)，(p1,p2)為圖像序列的一個(gè)匹配點(diǎn)對(duì)，反之，則匹配失敗。本實(shí)施例設(shè)定閾值為0.5，基于Harris-SIFT算法的新型3-DOF并聯(lián)機(jī)構(gòu)匹配效果如圖4所示。

5、采用本發(fā)明提出的提純算法剔除匹配結(jié)果中存在的誤匹配點(diǎn)。進(jìn)行提純處理時(shí)，可根據(jù)一組包含異常點(diǎn)的數(shù)據(jù)集，計(jì)算出匹配點(diǎn)對(duì)的目標(biāo)模型，再利用剩余點(diǎn)來檢驗(yàn)?zāi)Ｐ?，從而得到有效?shù)據(jù)樣本。為了快速求出候選模型參數(shù)，根據(jù)左圖中匹配點(diǎn)的坐標(biāo)邊界將圖片平均分成16塊，隨機(jī)選取9個(gè)互不相同的塊且每個(gè)塊中隨機(jī)選取一個(gè)匹配點(diǎn)，從9個(gè)匹配對(duì)中隨機(jī)選擇8個(gè)匹配對(duì)用來求出臨時(shí)模型參數(shù)，而第9個(gè)匹配對(duì)用來驗(yàn)證臨時(shí)模型的正確性，從而快速確定目標(biāo)模型。提純匹配結(jié)果效果如圖5所示，目標(biāo)模型H的選取和Sampson距離表示如下：

(1)兩圖像間目標(biāo)模型H的選取：按照以上分塊取點(diǎn)的方式隨機(jī)選擇8個(gè)匹配點(diǎn)對(duì)計(jì)算臨時(shí)候選模型F，并求出模型F所對(duì)應(yīng)匹配點(diǎn)對(duì)的Sampson距離d，設(shè)定閾值k，將符合d<k條件的點(diǎn)作為內(nèi)點(diǎn)，并將內(nèi)點(diǎn)數(shù)最多的候選模型F做進(jìn)一步優(yōu)化得到目標(biāo)模型H，最后通過目標(biāo)模型H重新估計(jì)匹配點(diǎn)，剔除誤匹配對(duì)。投影變換模型表示如式(22)所示：

式中，(x,y,1)和(x’,y’,1)分別表示匹配點(diǎn)對(duì)m(x,y)和m(x’,y’)的齊次坐標(biāo)。

(2)Sampson距離d的表示：對(duì)于任一匹配點(diǎn)對(duì)m(x,y)和m(x’,y’)來說，其在候選模型F中的Sampson距離d可表示為：

6、求解新型3-DOF并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿。根據(jù)新型3-DOF并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)特點(diǎn)，建立機(jī)構(gòu)平臺(tái)的定坐標(biāo)系與動(dòng)坐標(biāo)系如圖7所示，其中，{B}＝{O-XYZ}、{T}＝{O'-X'Y'Z'}分別表示定坐標(biāo)系和動(dòng)坐標(biāo)系，則并聯(lián)機(jī)構(gòu)末端位姿的變化即可表示為動(dòng)平臺(tái)中心點(diǎn)O'點(diǎn)的位姿變化。本實(shí)施例采用張正友標(biāo)定法實(shí)現(xiàn)雙目視覺系統(tǒng)的攝像機(jī)標(biāo)定，并通過前期圖像處理和后期坐標(biāo)變換求出新型3-DOF并聯(lián)機(jī)構(gòu)末端位置信息如下：

P_w＝(A^TA)^-1A^TB (24)

其中，(u₁,v₁)、(u₂,v₂)分別表示末端特征點(diǎn)在左右相機(jī)投影點(diǎn)的圖像坐標(biāo)，表示左右投影矩陣的第i行，第j列。

同時(shí)，新型3-DOF并聯(lián)機(jī)構(gòu)末端姿態(tài)角為式(25)所示：

由于本并聯(lián)機(jī)構(gòu)做三自由度運(yùn)動(dòng)，因此可求出其在Y軸、Z軸的平動(dòng)位移，以及繞Y軸的翻滾角β，則機(jī)構(gòu)位姿參數(shù)即為(Y,Z,β)。

本實(shí)施例定時(shí)采集12張圖像，然后求出每個(gè)時(shí)刻機(jī)構(gòu)三維位姿參數(shù)，并將結(jié)果與激光測(cè)距儀INR-Laser Scanner H和電子羅盤Honeywell-HMR3100測(cè)得的機(jī)構(gòu)末端實(shí)際位姿參數(shù)進(jìn)行對(duì)比，跟蹤誤差如圖8所示。分析圖8可見，測(cè)量點(diǎn)P1和P7在Y方向的跟蹤誤差較大，而P4和P10在Z方向的跟蹤誤差較大，這與機(jī)構(gòu)的對(duì)稱性和運(yùn)行軌跡有關(guān)，由于機(jī)構(gòu)在上述測(cè)量點(diǎn)方向發(fā)生改變，使得速度和加速度變化較大，外加機(jī)構(gòu)慣性的影響，導(dǎo)致誤差較大，這表明實(shí)驗(yàn)測(cè)量得到的并聯(lián)機(jī)構(gòu)末端位姿運(yùn)動(dòng)狀態(tài)與實(shí)際運(yùn)動(dòng)狀態(tài)相一致；同時(shí)，根據(jù)跟蹤誤差可計(jì)算出各個(gè)測(cè)量點(diǎn)的最大偏差分別為Y方向：0.622mm，Z方向：0.782mm，β旋轉(zhuǎn)角：0.677°，說明檢測(cè)具有較高精度。實(shí)驗(yàn)結(jié)果表明，本發(fā)明所提出的基于雙目視覺的位姿檢測(cè)方法，能較好地實(shí)現(xiàn)并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿的實(shí)時(shí)檢測(cè)。

綜上，本發(fā)明的一種基于雙目視覺的并聯(lián)機(jī)構(gòu)末端運(yùn)動(dòng)位姿檢測(cè)方法，首先，將采集到的并聯(lián)機(jī)構(gòu)圖像進(jìn)行基于小波變換的圖像去噪預(yù)處理，以消除圖像噪聲對(duì)隨后特征提取造成的影響；然后，采用Harris-SIFT算法對(duì)并聯(lián)機(jī)構(gòu)圖像進(jìn)行特征匹配。不同于一般的匹配算法，該匹配算法首先通過Harris算子提取圖像特征點(diǎn)，再利用SIFT特征描述子對(duì)Harris算子提取出的特征點(diǎn)進(jìn)行匹配，使得匹配結(jié)果兼具實(shí)時(shí)性和穩(wěn)定性；接著，為進(jìn)一步提高匹配正確率，提出一種新的提純算法對(duì)匹配結(jié)果進(jìn)行提純，該提純算法通過分塊取點(diǎn)和提前取點(diǎn)驗(yàn)算臨時(shí)模型的方法，解決了提純算法中目標(biāo)模型獲取費(fèi)時(shí)、模型求解不準(zhǔn)確的問題；最后，將匹配提純后的并聯(lián)機(jī)構(gòu)末端特征點(diǎn)對(duì)帶入雙目視覺模型，通過坐標(biāo)變換求出機(jī)構(gòu)末端三維位姿。本發(fā)明所提出的基于雙目視覺的并聯(lián)機(jī)構(gòu)末端位姿檢測(cè)方法，由于在匹配階段首先通過Harris算子提取圖像特征點(diǎn)，再利用SIFT特征描述子對(duì)Harris算子提取出的特征點(diǎn)進(jìn)行匹配，可大幅度降低圖像處理時(shí)間，另通過所提出的新的提純算法對(duì)匹配結(jié)果進(jìn)行提純，可進(jìn)一步提高匹配正確率，進(jìn)而使得并聯(lián)機(jī)構(gòu)末端位姿檢測(cè)的實(shí)時(shí)性和精度都得以提高。

在本說明書的描述中，參考術(shù)語“一個(gè)實(shí)施例”、“一些實(shí)施例”、“示意性實(shí)施例”、“示例”、“具體示例”、或“一些示例”等的描述意指結(jié)合該實(shí)施例或示例描述的具體特征、結(jié)構(gòu)、材料或者特點(diǎn)包含于本發(fā)明的至少一個(gè)實(shí)施例或示例中。在本說明書中，對(duì)上述術(shù)語的示意性表述不一定指的是相同的實(shí)施例或示例。而且，描述的具體特征、結(jié)構(gòu)、材料或者特點(diǎn)可以在任何的一個(gè)或多個(gè)實(shí)施例或示例中以合適的方式結(jié)合。

盡管已經(jīng)示出和描述了本發(fā)明的實(shí)施例，本領(lǐng)域的普通技術(shù)人員可以理解：在不脫離本發(fā)明的原理和宗旨的情況下可以對(duì)這些實(shí)施例進(jìn)行多種變化、修改、替換和變型，本發(fā)明的范圍由權(quán)利要求及其等同物限定。