采用內點法和圖像反饋的機器人視覺伺服預測控制

滕 游，劉安東，俞 立

(浙江工業大學 信息工程學院，杭州 310023)

1 引 言

視覺傳感器提供了豐富的外部環境測量信息.視覺伺服是一種利用視覺測量數據作為反饋信號的機器人運動控制器的設計技術，在工業機器人、無人飛行器、智能機器人等領域得到了廣泛的應用[1-4].

根據選取的誤差信號，視覺伺服的控制策略可以分為3類：基于位置的視覺伺服(PBVS)、基于圖像的視覺伺服(IBVS)和混合視覺伺服[4].PBVS利用視覺處理算法獲得的相機和物體之間的相對位姿定義系統誤差，使得機器人的運動具有較好的三維軌跡，但容易受到系統標定誤差、視覺測量噪聲的影響.IBVS直接采用目標物體特征分別在期望圖像平面和當前圖像平面上的投影來定義系統誤差，對相機參數和測量誤差具有較好的魯棒性，且算法運算較少，吸引了很多機器人領域的研究者的關注.混合視覺伺服同時利用相對位姿和特征的圖像投影定義誤差，同樣容易受到模型和測量誤差的影響.

傳統的IBVS方法利用固定增益和雅可比矩陣的偽逆設計控制律，只能保證局部穩定性，且增益的選取會對系統性能產生重要影響，比如增益太小會影響系統收斂速度，增益過大會增加系統不穩定以及特征從圖像有效區域丟失的可能性[5].特征丟失是視覺伺服的普遍問題，對于直接處理圖像特征誤差的IBVS，將特征點始終維持在視野內至關重要[6].文獻[7]和文獻[8]利用導航勢函數避免特征被障礙物遮擋.文獻[9]為攝像機生成了類似圓弧的軌跡，以避免大位移的視覺伺服過程中的特征丟失.但是這些方法的通用性不強，難以應用到各種復雜的不同應用中.從最優化的角度來說，維持特征不丟失是一種系統約束.模型預測控制是一種非線性最優控制策略，特別適合處理控制中存在的各種約束問題.近年來，采用模型預測控制的處理方法解決IBVS控制問題得到了廣泛的關注[10,11]，但是大部分文獻直接利用MATLAB中的內置優化函數解決模型預測控制中的優化問題，不利于在實際工程中實現.針對移動機器人平臺，文獻[11]和文獻[12]采用神經網絡解決QP問題，但是存在著神經網絡的訓練等問題.針對上述問題，本文采用模型預測控制處理方法，將控制量的飽和、特征在圖像空間的坐標限制作為約束，把IBVS轉化成一個有限時域內帶約束的優化問題，通過障礙函數法把不等式約束轉化成等式約束，給出了采用LOQO內點法[14]計算最優控制量的算法.最后通過仿真和Panda機器人上的實驗驗證了算法的有效性.

2 問題描述

本文采用一個六自由度的協作機器人Franka Panda和一個RGB-D相機SR300，將RGB-D相機安裝在協作機器人末端夾具上，通過RGB-D相機獲得當前待抓取物體的圖像，驅動機器人運動，使得物體的圖像最終和期望的圖像一致.通過將協作機器人拖動到期望位姿進行測量，或者利用物體的CAD三維模型計算得到期望圖像.

考慮上述系統在eye-in-hand框架下的IBVS控制問題如圖1所示.其中，相機安裝在協作機器人末端，目標物體局部坐標系為{L}，當前攝像機坐標系為{B}，期望的攝像機坐標系為{D}.

圖1 基于圖像的機器人控制Fig.1 Image-based robot control

為了實現通過像素控制機器人運動，首先假設攝像機已經完成標定且滿足針孔投影模型，從而可以獲得攝像機的內部參數矩陣.根據得到的特征點的像素坐標，利用攝像機的內部參數矩陣可以計算特征點的歸一化圖像平面坐標x=(x，y)T.其中，x和對應的特征點p在攝像機坐標系中的坐標X=(X，Y，Z)T的關系如下：

x=X/Z，y=Y/Z

(1)

令p在期望圖像平面上的坐標為x*=(x*，y*)T，則可以定義圖像平面上的系統偏差為e=x-x*.

由于攝像機安裝在運動的機器人末端上，攝像機坐標系中的特征點坐標與機器人末端的速度緊密相關.根據機器人末端的運動關系，特征點在攝像機坐標系中的坐標和攝像機的運動速度之間的關系如下所示：

(2)

其中，v和w分別為攝像機自身的平動速度和轉動速度.對式(1)取微分，并代入式(2)，可以得到歸一化圖像特征點與攝像機運動速度之間的運動學方程：

(3)

其中，u=(vT，wT)T，

由式(3)可知不同的控制量u可以產生不同的系統狀態軌跡.控制律設計的目標即利用特征點的圖像誤差產生合適的控制序列，使得攝像機到達合適的位姿.由于攝像機安裝在機器人末端上，攝像機的運動是通過機器人的關節電機驅動的，電機具有最大轉速的限制，從而使得控制量中的線速度和角速度需要滿足電機轉速約束，即式(3)中控制輸入u的約束問題.另一方面，在運動過程中必須保持特征點始終位于攝像機的視野內，因此在設計視覺伺服控制器的過程中有必要考慮攝像機視野約束問題，即式(3)中的狀態x的約束問題.

本文的目的是針對具有攝像機視野約束和控制輸入約束的IBVS控制問題，利用目標在當前圖像平面和期望圖像平面上的特征偏差，采用MPC方法設計視覺伺服控制器，驅動攝像機到達期望的位姿.

3 基于預測控制的機器人視覺伺服控制器設計

MPC是一種處理約束控制問題的有效方法.在MPC框架中，控制問題通常被轉化為一個帶約束的最小化問題.因此，本節針對具有視野約束和控制量約束的視覺伺服問題，設計基于MPC的IBVS控制器，并采用LOQO內點法求解控制輸入，實現點鎮定控制.

3.1 模型預測控制

為了獲得預測模型，首先以采樣周期T將目標特征點的式(3)離散化，表示成如下的離散時間狀態空間模型：

xk+1=xk+LkukT

(4)

為了驅動機器人到達期望目標位姿，采用圖像平面上的偏差量e作為狀態量，從而可進一步寫出有限時域內的預測誤差和輸入之間的關系為：

(5)

另外，在實際物理系統中，機器人運動都是通過電機驅動的，電機具有最大轉速的限制，從而使得攝像機的線速度和角速度需要滿足最大轉速的限制.由式(3)可知，線速度和角速度約束即為控制量約束，因此控制量需要滿足約束條件：

umin

(6)

其中umin和umax分別為相機速度的最小值和最大值，通常具有關系umin=-umax.更重要的是，為了保證可視性約束，系統狀態必須滿足約束條件：

xmin

(7)

其中xmin和xmax分別為歸一化圖像平面坐標的最小值和最大值.本文考慮鎮定問題，使用有限時域內的狀態偏差和控制量以及終端懲罰函數構造代價函數，從而將鎮定問題轉化成如下的最小化問題：

(8)

其中，Qj和Rj表示權重矩陣，F(ek+N)為終端懲罰項.因此，IBVS系統的控制器設計問題轉化為一個帶約束的二次優化問題.

為了計算出N個時域的最優控制序列，重新定義以下向量：

從而，可以將公式(7)表示成：

(9)

其中，

進一步，系統性能指標函數可改寫為：

(10)

其中，Q和R分別為由Qj和Rj組成的權重矩陣.式(10)中的最后一項是終端懲罰項，將其展開后可得：

(11)

(12)

(13)

3.2 內點法

公式(12)中的不等式約束的數量和特征點的數量與系統的預測時域有關.針對單個時域，關于控制輸入的約束不等式有12個，關于可視性的不等式約束數量為特征點數量的4倍.令N個時域的所有約束數量為M，采用障礙函數法將所有約束轉化成等式約束，優化問題轉化成：

(14)

(15)

其中y為引入的拉格朗日乘子.由最優性原理可知，上述拉格朗日方程的最優值滿足一階KKT條件：

-μe+WYe=0

(16)

其中，W=diag(ωi)，Y=diag(yi).

進一步令式(15)的Hessian矩陣和式(13)的Jacobian矩陣為：

(17)

從而得到式(16)對應的牛頓方程為：

(18)

(19)

(20)

(21)

進而采用消元法可得式(18)的解為：

(22)

其中，

ω(j+1)=ω(j)+α(j)Δω(j)

y(j+1)=y(j)+α(j)Δy(j)

其中，α(j)為迭代步長.

綜上所述，將LOQO內點法解決IBVS的優化計算過程總結如下：

3)根據公式(18)計算σ，-γ，ρ；

7)如果滿足要求，則得到最優值，否則跳轉到步驟 2).

4 實 驗

本節通過數值仿真和機器人實驗對提出的方法進行驗證.仿真周期T設定為40 ms，采用一個邊長為50cm的正方形的4個頂點作為特征點，實驗中，把物體坐標系的原點設置在正方形中心，XY平面設置在正方形所在平面.攝像機相對于物體坐標系的真實期望位姿為(0，0，-0.5，0，0，0)，其中前3個參數為攝像機坐標系相對于目標物體坐標系的位置，后3個參數為攝像機坐標系相對于目標物體坐標系的方位角.在期望狀態下，4個特征點的歸一化參考圖像坐標為x*=(-0.5，0.5，0.5，0.5，0.5，-0.5，-0.5，-0.5).

本文比較了兩種不同的初始位姿情況下，傳統IBVS和本文提出的方法的實驗結果.第1種情況下，初始位姿和期望位姿之間的偏差為繞z軸旋轉180°，傳統的IBVS會導致攝像機沿著z軸后退，最終停留在一個導致雅可比矩陣失效的狀態，無法到達期望狀態.采用本文的方法雖然也會產生后退情況，但在運行過程中會產生旋轉運動，且會改變攝像機平動方向，使得攝像機沿著z軸前進，最終可以到達期望狀態.傳統IBVS控制器和本文提出的控制器的控制效果分別如圖2(a)和圖2(b)所示.

圖2 第1種初始位姿下控制器運行結果Fig.2 Results of controllers under the first initial pose

第2種初始位姿為(1，1，-3，0，0，0.6)，傳統IBVS控制器產生的輸出和本文提出的控制器的輸出如圖3(a)和圖3(b)所示.從圖中可以看出，為傳統IBVS控制器設置合適的控制器增益系數后，控制器的輸出能較長時間維持在最大值，但是當接近目標狀態時，控制量的輸出會緩慢下降，收斂速度將變得非常緩慢，在22.1s時到達期望狀態.而本文提出的控制器的輸出，在接近目標狀態時，也可以維持在最大值，在13.9s時到達期望狀態.

圖3 第2種初始位姿下控制器的輸出Fig.3 Outputs of controllers under the second initial pose

使用六自由度的協作機器人Franka Panda和RGB-D相機SR300對本文提出的方法進行了驗證.將目標物體隨意放置在相機的視野內，IBVS控制器和本文提出的控制器都能夠驅動相機到達期望姿態和位置，兩種控制器作用下，特征點在圖像平面上的軌跡分別如圖4(a)和圖4(b)所示，控制器的輸出速度分別如圖4(c)和圖4(d)所示，從圖中可以看出，本文提出的控制器的收斂速度更快.在實際應用中，本文提出的方法通過求解一個帶約束的非線性優化過程獲得當前時刻的控制量，避免了傳統IBVS方法中存在的特征丟失問題、局部最小問題，并極大地改善了臨近期望狀態時的收斂速度，但是算法的實現更加復雜，對計算能力的要求更高.

圖4 在Panda機器人上運行的實驗結果Fig.4 Results running on a Panda robot

5 結束語

本文將IBVS問題表示成一個有限時域模型預測控制問題，采用非線性優化方法計算所需的控制量.顯著優點是在計算控制量的過程中可以考慮2D圖像平面的視野約束和物理執行部件輸出的控制量約束.實驗表明，和傳統的IBVS方法相比，本文提出的方法有效地考慮了可視性約束，避免特征從圖像平面的有效視野中消失，同時考慮了控制量約束，可以避免產生過大的控制量，同時使得控制量盡可能長時間地維持在最大值，解決了傳統IBVS中存在的特征丟失和增益無法調節的問題.