基于點線特征的解耦視覺伺服控制方法

盧金燕，戚肖克

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

0 引言

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

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

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

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

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

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

1 問題描述

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

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

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

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

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

其中：λ是調節系數。

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

其中：是Ls的偽逆。

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

2 控制律

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

2.1 姿態控制律

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

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

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

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

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

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

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

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

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

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

2.2 位置控制律

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

其中：（u，v）是圖像坐標；kx、ky、u0和v0是相機的內參數。

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

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

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

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

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

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

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

2.3 在線深度估計

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

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

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

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

3 控制系統設計

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

圖1 本文控制系統框圖Fig.1 Block diagram of the proposed control system

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

4 實驗與結果分析

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

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

4.1 實驗設置

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

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

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

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

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

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

4.2 實驗結果分析

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結語

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