盧金燕，戚肖克

（1.河南工程學院電氣信息工程學院，鄭州 451191；2.中國政法大學法治信息管理學院，北京 102249）

0 引言

視覺伺服是近年來機器人和自動化領(lǐng)域的研究熱點之一。它利用視覺傳感器捕捉豐富的環(huán)境信息，在視覺定位、目標跟蹤、拾取和定位、裝配等領(lǐng)域得到了廣泛的應用［1-4］。

根據(jù)視覺誤差信號的選擇，視覺伺服可分為基于位置的視覺伺服（Position Based Visual Servoing，PBVS）、基于圖像的視覺伺服（Image Based Visual Servoing，IBVS）和混合視覺伺服（Hybrid Visual Servoing，HVS）。其中，IBVS 對相機的標定誤差和空間模型誤差不敏感，控制器設(shè)計靈活，計算過程相對簡單，是最常用的視覺伺服方法。

由于點特征的圖像坐標變化與機器人運動速度之間的映射關(guān)系非常容易獲取，因此點特征成為大多數(shù)視覺伺服系統(tǒng)的首選?；邳c特征，文獻［5］提出了一種應用在非完整移動機器人上的主從編隊跟蹤控制方案。它避免了測量位置和速度，但需要在圖像空間中建立主從運動學模型。為了將軟體操作手應用于受限環(huán)境當中，在機械臂長度和目標位置未知的情況下，文獻［6］使用人工標記的點特征設(shè)計自適應視覺伺服控制器。然而，該方法假設(shè)曲率是分段常量，在實際情況中可能很難滿足。利用至少三個非共線點特征，文獻［7］提出了一種基于圖像的四旋翼無人機視覺伺服方法。該方法不需要每個點特征的實時深度值，但需要將圖像平面的所有點特征投影到虛擬平面，并推導出點特征在虛擬平面中的變化特性。為了解決三個圖像點的視覺伺服問題，文獻［8］提出了一種基于Fisher 信息矩陣最優(yōu)度量的主動視覺任務(wù)函數(shù)，并將該函數(shù)和視覺伺服共同用于多目標優(yōu)化框架，以提高姿態(tài)估計精度；然而，該方法只在視覺伺服仿真平臺（VIsual Servoing Platform，VISP）上進行了驗證。文獻［9］使用共面的四個點表示移動機器人當前和期望的位姿，但該方法只構(gòu)建了沿x軸和z軸的平移，以及沿y軸的旋轉(zhuǎn)。文獻［10］利用點特征計算無人機的三維空間位置，使用基于虛擬相機圖像的視覺伺服方法實現(xiàn)欠驅(qū)動無人機的三維空間軌跡控制。

作為一種高維特征，直線可以提供多種環(huán)境信息，也經(jīng)常被用于視覺伺服當中。文獻［11］對點、直線和圓特征分別推導了交互矩陣，其中直線的交互矩陣需要用到含有直線的平面在攝像機坐標系中的方程參數(shù)。為擺脫平面參數(shù)的約束，文獻［12］利用兩點的極坐標推導出直線的交互矩陣，并給出直線交互矩陣的求取方法。文獻［13］通過直線的二維/三維對應關(guān)系集合處理相機的姿態(tài)估計問題，即PnL（Perspective-n-Line）問題，為需要處理直線對應關(guān)系的應用提供了一種解決方案，例如基于直線特征的目標跟蹤或增強現(xiàn)實中的相機定位。文獻［14］利用三維直線構(gòu)建基于圖的可視化的即時定位與地圖構(gòu)建（Simultaneous Localization And Mapping，SLAM）系統(tǒng)，從而實現(xiàn)機器人的定位和映射。文獻［15］利用建筑物的結(jié)構(gòu)線條作為圖像特征，實現(xiàn)了機器人在有多個人造物體的室內(nèi)場景中的定位。文獻［16］提出一種基于單目像機的三維直線主動估計方法，該方法用球坐標表示直線的矩矢量，并在系統(tǒng)動力學中引入代數(shù)約束，計算較為復雜。文獻［17］采用基于四元數(shù)的視覺伺服方法，利用跟蹤的標記直線表示目標位姿，實現(xiàn)機器人對空間任意移動物體的抓取。

為了充分利用點和直線的特性，研究者提出將點與直線相結(jié)合的視覺伺服方法。文獻［18］提出了一種針對低紋理的室內(nèi)環(huán)境的魯棒SLAM 方法，該方法使用位姿求解器處理點和直線的對應關(guān)系，并構(gòu)建統(tǒng)一的優(yōu)化模型以同時最小化點和直線的重投影誤差；文獻［19］針對紋理較差的場景或運動模糊圖像，提出了一種基于點和直線的實時單目視覺SLAM，該方法可以根據(jù)3 張連續(xù)圖像中的5 條直線的對應關(guān)系估計相機姿態(tài)和構(gòu)建三維地圖。為了估計相機的狀態(tài)，文獻［20］提出了一種利用點和直線的緊耦合單目視覺慣性測程系統(tǒng)。在建立點特征和直線特征的投影模型后，文獻［21］構(gòu)造一種新的重組Hessian 矩陣和梯度向量來迭代估計位姿，該方法可以自適應相機的運動狀態(tài)；文獻［22］提出一種雷達-單目視覺測程方法，該方法將點特征和直線特征融合為地標，并利用點和直線的地標投影誤差實現(xiàn)目標跟蹤。此類方法能夠利用點和直線使視覺測量更加準確；但大多需要建立點特征和直線特征的對應關(guān)系，計算過程繁瑣，且容易導致后續(xù)的控制不夠靈活。文獻［23］提出一種基于虛擬目標引導的快速搜索隨機樹的視覺伺服方法，實現(xiàn)視野受限的移動機器人的位姿控制，該方法利用直線特征檢測相機視野是否受限，利用點特征使機器人沿規(guī)劃軌跡到達期望位姿，能夠同時滿足相機的視野限制和速度限制；但是計算量較大，實時性不高。

針對上述問題，本文聯(lián)合使用點特征和直線特征，提出一種分階段的視覺伺服方法。分析并利用文獻［12］推導的直線交互矩陣，解耦了IBVS 的姿態(tài)控制和位置控制，并將IBVS 分解為姿態(tài)控制和位姿控制兩個階段，最終實現(xiàn)了機器人的六自由度對準。在姿態(tài)控制階段，利用直線特征及其交互矩陣消除旋轉(zhuǎn)偏差；在位置控制階段，利用點特征及其交互矩陣消除平移偏差。同時，根據(jù)相機運動量以及相機運動前后特征的變化量，可對深度進行在線估計。此外，通過監(jiān)督器調(diào)節(jié)相機的運動速度，從而保證特征不會在相機視野中丟失。

1 問題描述

基于單目視覺機器人系統(tǒng)，本文方法的目標是將機器人移向目標，實現(xiàn)末端與目標的位姿對準。當機器人完成對準后，可以進行后續(xù)的裝配、焊接等操作。這是一個典型的視覺控制問題，而IBVS 可以提供一個很好的解決方案。具體來說，可以通過將實際的視覺特征與期望的視覺特征相匹配以實現(xiàn)該任務(wù)。

定義圖像平面中視覺特征的實際值和期望值之間的誤差e為：

其中：m是圖像特征的集合，如點、直線等。a是一組視覺伺服系統(tǒng)參數(shù)，可用來獲取潛在的系統(tǒng)知識，如相機參數(shù)、目標模型等。利用m和a，可得到視覺特征向量s（m，a）。向量s*是視覺特征向量s（m，a）的期望值。

在IBVS 中，通過設(shè)計合適的控制律，將提取的特征m映射到目標特征s的空間中，并與期望特征s*進行比較。當兩者之差為零，即誤差e為零時，系統(tǒng)達到穩(wěn)定的收斂狀態(tài)。

其中：Ls是特征s的交互矩陣。

其中：λ是調(diào)節(jié)系數(shù)。

將vc作為速度控制的輸入，根據(jù)式（2）～（3），可得到如下的IBVS 控制律：

其中：是Ls的偽逆。

由式（4）可知，根據(jù)特征誤差和交互矩陣可以得到相機的運動速度控制量，從而使誤差穩(wěn)定收斂，實現(xiàn)實際圖像特征和期望圖像特征的匹配。

2 控制律

為了實現(xiàn)機器人的位姿對準，本文選擇點和直線作為圖像特征，將IBVS 分解為姿態(tài)控制和位置控制兩個階段。在姿態(tài)控制階段利用直線特征消除姿態(tài)偏差，在位置控制階段利用點特征消除位置偏差，實現(xiàn)機器人的六自由度控制。

2.1 姿態(tài)控制律

焦距歸一化成像平面上的直線方程可以用極坐標表示為：

其中：θ為直線的正交線與x1c軸之間的夾角；ρ為焦距歸一化成像平面上原點到直線的距離。

將相機速度記為vc=[vca，ωca]，其中vca=［vcax，vcay，vcaz］T和ωca=［ωcax，ωcay，ωcaz］T分別是相機的平移和旋轉(zhuǎn)速度。由文獻［9］可知，當相機運動時，直線角度參數(shù)θ在焦距歸一化成像平面上的變化情況如下：

其中：Ll1是一條直線的角度參數(shù)θ的交互矩陣。

從式（6）可以看出，只有旋轉(zhuǎn)運動對焦距歸一化平面上的直線角度變化有貢獻。將Ll1寫為Ll1=［0，Ll1w］，Ll1w是一條直線的角度參數(shù)θ與相機旋轉(zhuǎn)之間的交互矩陣。

根據(jù)式（6）～（7），焦距歸一化成像平面上直線角度參數(shù)θ的變化與相機旋轉(zhuǎn)的關(guān)系可表示為：

由式（8）可知，直線角度θ的變化僅由相機旋轉(zhuǎn)引起，此時旋轉(zhuǎn)運動可以獨立于平移運動。進一步，如果選擇合適的直線作為圖像特征，可以將旋轉(zhuǎn)運動和平移運動分離，從而實現(xiàn)IBVS 中姿態(tài)控制和位置控制的解耦。

根據(jù)式（8），選擇直線的角度變化作為圖像特征表示相機的旋轉(zhuǎn)運動，則相機的旋轉(zhuǎn)可以用至少三條直線的期望角度和當前角度之間的誤差控制，因此可以得到：

其中：Lwm是m條直線的角度變化與相機旋轉(zhuǎn)之間的交互矩陣。

進一步，根據(jù)式（4）、（9），通過直線的角度偏差可以得到相機的姿態(tài)控制律：

2.2 位置控制律

假設(shè)鏡頭畸變可以被忽略或預先校正，那么相機的內(nèi)參數(shù)可以用針孔模型描述。對于相機坐標系中的點（xc，yc，zc），記其在焦距歸一化平面的坐標為（x1c，y1c，1）。對于單目視覺系統(tǒng)，一般情況下（xc，yc，zc）是未知的，而x1c和y1c可以根據(jù)圖像坐標和相機的內(nèi)參數(shù)計算。

其中：（u，v）是圖像坐標；kx、ky、u0和v0是相機的內(nèi)參數(shù)。

當相機運動時，一個點特征在焦距歸一化平面上的變化可以表示為：

其中：Lp1是一個點的交互矩陣。

其中：(x1c，y1c)是點特征在焦距歸一化平面的坐標；zc是點特征在相機坐標系的深度。

由于在姿態(tài)控制階段已經(jīng)消除了旋轉(zhuǎn)偏差，此階段只進行平移控制。在這種情況下，相機的旋轉(zhuǎn)速度ωca為0，點特征在焦距歸一化平面上的坐標變化僅由相機的平移運動引起。

根據(jù)式（12）～（13），可以得到點特征變化與相機平移運動之間的關(guān)系：

其中：Lpnv是n個點特征在焦距歸一化平面上的坐標變化與相機平移之間的交互矩陣。

根據(jù)式（4）、（14），可以得相機的位置控制律：

2.3 在線深度估計

從式（14）可以看出，為了計算交互矩陣Lpnv，必須估計點特征在相機坐標系的深度。點特征的深度可以通過相機的運動在線估計。將式（13）代入（12）中，可以得到：

因此，根據(jù)攝像機的運動速度和特征點的運動速度估計特征點的深度，整理式（16）可得：

根據(jù)式（17），在相機每次運動后，利用相機的運動量、相機運動前后點特征在焦距歸一化平面上的坐標變化以及點特征在焦距歸一化平面上的坐標，可以重新計算點特征的深度zc，從而實現(xiàn)在線深度估計。

需要指出的是，采集第一幀圖像進行運動控制時，由于沒有相機運動量初值，無法進行深度估計。因此，在第一幀圖像時，只提取圖像特征，不計算運動調(diào)整量，第一步的運動量人為設(shè)定。機器人主動運動后，根據(jù)主動運動前后的特征參數(shù)和攝像機主動運動量，進行深度估計。

3 控制系統(tǒng)設(shè)計

根據(jù)上述控制律，設(shè)計的控制系統(tǒng)如圖1 所示，控制系統(tǒng)由視覺檢測、姿態(tài)控制、位置控制、深度估計、監(jiān)督器和機器人6 部分構(gòu)成。在IBVS 過程中，先根據(jù)視覺檢測得到的直線特征進行姿態(tài)控制，當直線特征的實際值與期望值小于設(shè)定閾值時，根據(jù)視覺檢測得到的點特征進行位置控制。當點特征的實際值與期望值小于設(shè)定閾值時，IBVS 過程結(jié)束。

圖1 本文控制系統(tǒng)框圖Fig.1 Block diagram of the proposed control system

視覺檢測部分用于采集目標圖像，提取點和直線特征。姿態(tài)控制部分根據(jù)直線特征的期望值與當前值Fθc=(θ1，θ2，…，θm)之間的偏差，以及Lwm，計算出旋轉(zhuǎn)速度ωca，控制機器人的姿態(tài)，控制律見式（10）。根據(jù)點特征的期望值與當前值Fpc=((x1c1，y1c1)，(x1c2，y1c2)，…，(x1cn，y1cn))之間的偏差，以及Lpnv，計算出平移速度vca，控制機器人的位置，控制律見式（15）。深度估計部分根據(jù)相機運動前后特征的變化，利用式（16），計算點特征在相機坐標系的深度值。監(jiān)督器用于控制相機的旋轉(zhuǎn)和平移調(diào)節(jié)速度，如果大于設(shè)定的閾值，則減小本次的控制系數(shù)。機器人為被控對象，執(zhí)行姿態(tài)控制和位置控制模塊的運動命令。控制器的輸出為機器人末端在相機坐標系下的速度，機器人接收到控制輸出后，將其轉(zhuǎn)換為機器人末端坐標系或基坐標系下的速度進行運動。

4 實驗與結(jié)果分析

采用Eye-in-Hand 機器人平臺驗證本文算法的性能，如圖2 所示。該平臺采用安川公司的六自由度機器人Motoman UP6，大恒公司的MER-200-14GX 相機，相機固定在機器人的末端上，構(gòu)成Eye-in-Hand 系統(tǒng)。圖像大小為1 626×1 238 像素。相機的內(nèi)參數(shù)模型為針孔模型，鏡頭畸變采用四階布朗畸變模型。

圖2 Eye-in-Hand機器人平臺Fig.2 Eye-in-Hand robot platform

4.1 實驗設(shè)置

實驗中，在白板上粘貼一個矩形圖案作為目標對象。選取矩形的四條邊進行姿態(tài)控制，選取矩形的四個角點進行位置控制。為了保證視覺特征的準確性，在圖像處理過程中首先提取矩形的四條邊，利用Canny 算子進行檢測，然后利用概率霍夫變換進行提取。四個角點通過邊的交點得到。

根據(jù)點特征的圖像坐標和相機的內(nèi)參數(shù)，可以得到點特征在焦距歸一化平面上的坐標。從左上角開始，沿逆時針方向，將矩形的四條邊分別記為直線1～4，將矩形的四個角點分別記為點A、B、C、D。它們的期望值分別如表1～2 所示。

表1 直線的期望值Tab.1 Expectedd values for lines

表2 點的期望值Tab.2 Expected values for points

由于本平臺的機器人只接收姿態(tài)命令，所以速度用增量除以采樣周期時間代替。另外，為了控制機器人，通過式（18）將相機的運動轉(zhuǎn)換為末端的運動，表示為：

其中：Te為末端的位姿矩陣；Tca為相機的位姿矩陣；Mex是從末端坐標系到相機坐標系的變換矩陣，即手眼關(guān)系。

4.2 實驗結(jié)果分析

首先，根據(jù)上述控制律和相關(guān)設(shè)置，對本文方法進行實驗驗證。整個實驗過程分為姿態(tài)控制和位置控制兩個階段。首先控制姿態(tài)，當姿態(tài)誤差小于設(shè)定閾值后，進行位置控制。實驗過程中，分別記錄姿態(tài)控制階段的直線角度誤差和位置控制階段的點特征誤差進行實驗分析。在姿態(tài)控制階段，為了接近目標，每一步讓機器人沿深度方向移動100 mm。

在姿態(tài)控制階段，提取矩形的四條邊作為特征，計算直線角度當前值與期望值之間的偏差。首先用式（9）計算交互矩陣Lwm，然后用式（10）計算相機的姿態(tài)調(diào)整量。實驗過程中的四條邊的角度誤差如圖3 所示。從圖3 可以看出，直線角度誤差經(jīng)過7 步收斂到設(shè)定閾值，即姿態(tài)控制階段在第7步結(jié)束。

圖3 四條邊的角度誤差Fig.3 Angle errors of four edges

在位置控制階段，提取矩形的四個角點作為特征，計算點特征當前值與期望值之間的偏差。首先用式（17）估計點特征的深度，然后用式（14）計算交互矩陣Lpnv，最后用式（15）計算相機的位置調(diào)整量。實驗過程中四個角點在焦距歸一化平面的誤差如圖4 所示。從圖4 可以看出，四個角點經(jīng)過9 步都收斂到設(shè)定值，即位置控制階段在第9 步結(jié)束。

圖4 四個角點在焦距歸一化平面的誤差Fig.4 Errors of four points on focal length normalization plane

然后，將傳統(tǒng)IBVS 作為對比方法開展對比實驗。選擇矩形的四個角點作為圖像特征，傳統(tǒng)IBVS 方法與本文方法的條件設(shè)置完全相同。在傳統(tǒng)IBVS 方法中，用式（13）計算點特征的交互矩陣，用式（4）計算相機的位姿調(diào)整量，傳統(tǒng)IBVS 方法的四個角點在焦距歸一化平面誤差如圖5 所示。

在相同的期望位姿、期望特征參數(shù)、圖像處理過程的情況下，從圖5 可以看出，經(jīng)過19 步調(diào)整后，傳統(tǒng)IBVS 方法的控制過程結(jié)束。由圖3～4 可知，本文方法的控制過程在第16步結(jié)束。因此，本文方法的控制誤差收斂速度略高于傳統(tǒng)IBVS 方法。由于本文方法聯(lián)合使用了點和直線兩種圖像特征，而對比方法僅使用了點特征，所以本文方法能從圖像特征得到更豐富的信息，視覺測量結(jié)果更加準確。另外，相較于傳統(tǒng)IBVS 方法，本文方法對姿態(tài)和位置進行了解耦和分階段控制，使位置調(diào)整過程中的點特征變化不受姿態(tài)控制的影響，因此控制過程更加高效。

圖5 傳統(tǒng)IBVS方法的四個角點在焦距歸一化平面的誤差Fig.5 Errors of four points on focal length normalization plane for traditional IBVS method

本文方法和傳統(tǒng)IBVS 方法的特征點軌跡如圖6～7 所示，實心標記表示期望位姿下特征點的圖像位置。從圖6～7可以看出，點特征和直線特征的聯(lián)合使用有助于提高視覺測量的準確性，因此本文方法的特征點能夠有序到達期望位姿下的圖像位置附近，而傳統(tǒng)IBVS 方法的特征點在圖像平面的坐標變化比較無序，容易導致目標丟失，造成伺服失敗。

圖6 本文方法的特征點軌跡Fig.6 Feature point trajectories by using the proposed method

實驗過程中，為了控制機器人，利用式（18）將相機的運動轉(zhuǎn)換為末端的運動，并記錄機器人末端的位姿變化。本文方法的機器人末端位置和姿態(tài)變化如圖8 所示；傳統(tǒng)IBVS方法的機器人末端位置和姿態(tài)變化如圖9 所示。從機器人末端位姿曲線可以看出，本文方法和傳統(tǒng)IBVS 方法的末端位姿變化與特征點圖像坐標變化相吻合。

圖7 傳統(tǒng)IBVS方法的特征點軌跡Fig.7 Feature point trajectories by using traditional IBVS method

圖8 本文方法的機器人末端位置和姿態(tài)變化Fig.8 Position and pose changes of end-effector of robot by using the proposed method

圖9 傳統(tǒng)IBVS方法的機器人末端位置和姿態(tài)變化Fig.9 Position and pose changes of end-effector of robot by using traditional IBVS method

本文方法和傳統(tǒng)IBVS 方法的機器人末端軌跡如圖10～11 所示?？梢钥闯?，本文方法的機器人末端位姿變化較平穩(wěn)，而傳統(tǒng)IBVS 方法在控制過程的后半段收斂較慢。表3給出了對準結(jié)束時，兩種方法的機器人末端位姿偏差，可以看出，本文方法平移方向最大誤差3.26 mm，旋轉(zhuǎn)方向最大誤差0.72°，而傳統(tǒng)IBVS 方法平移方向最大誤差7.35 mm，旋轉(zhuǎn)方向最大誤差1.26°。因此，本文方法的對準誤差更小。

圖10 本文方法的機器人末端軌跡Fig.10 End-effector trajectory of robot by using the proposed method

由以上實驗結(jié)果及分析可知，點特征和直線特征的聯(lián)合使用使本文方法的視覺測量結(jié)果更加可靠，姿態(tài)控制和位置控制的解耦降低了本文方法對視覺測量誤差的敏感性。因此，相較于傳統(tǒng)IBVS 方法，本文方法的控制過程更加高效，控制誤差收斂更快，對準誤差更小。

圖11 傳統(tǒng)IBVS方法的機器人末端軌跡Fig.11 End-effector trajectory of robot by using traditional IBVS method

5 結(jié)語

本文以點和直線作為特征，提出一種新的基于圖像的視覺伺服方法。利用圖像特征的交互矩陣解耦了IBVS 的姿態(tài)控制和位置控制，進而分別針對姿態(tài)和位置設(shè)計了控制定律，通過兩個階段的控制最終實現(xiàn)機器人的六自由度對準。同時，本文方法可以根據(jù)相機的運動對特征的深度進行在線估計，并通過監(jiān)督器使特征始終保持在相機視野當中。實驗結(jié)果表明，本文方法能夠?qū)崿F(xiàn)快速、高精度的對準。但是，本文方法需要離線標定相機參數(shù)，未來將研究在線標定的視覺控制，并應用到機器人裝配系統(tǒng)中。