基于眼到手視覺伺服的移動機器人模型預測控制

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

0 引言

視覺傳感器作為移動機器人的眼睛，被應用于視覺伺服、視覺導航及視覺定位等場合，是移動機器人領域運用最為廣泛的傳感器。因此，移動機器人的視覺伺服控制一直是移動機器人研究領域的熱點。按照建模方法的不同，視覺伺服可分為基于圖像的視覺伺服(Image-Based Visual Servoing，IBVS)[1]、基于位置的視覺伺服(Position-Based Visual Servoing，PBVS)[2]以及混合視覺伺服(Hybrid Visual Servoing，HVS)[3]。PBVS主要思想是利用傳感器去估計機器人的位姿信息，并與給定的位姿信號進行比較得到誤差量來設計反饋控制律以實現機器人的控制，其缺點在于PBVS的伺服效率取決于估計姿態的準確性。IBVS主要思想是直接在圖像層對機器人進行控制，將視覺傳感器獲得的圖像與參考圖像進行對比得到圖像誤差來設計反饋控制率。HVS誤差模型同時取決于圖像特征以及相機的姿態，其通過模板圖像得到期望的圖像特征信息，并將圖像的特征信息做差來計算單應性矩陣。但在實際中，混合視覺伺服無法保證圖像始終在視野范圍內，并且仍受到相機標定精度和圖像質量的影響。

IBVS相比于PBVS和HVS，對相機的標定及模型誤差具有魯棒性，且不需要三維坐標，計算速度更快。因此，IBVS受到了更多學者的關注。根據相機安裝的位置不同，IBVS有兩種不同的模型，分別為眼到手模型和眼在手模型。根據眼在手和眼到手模型的控制方法不同，可分為自適應控制，PID控制，智能控制及預測控制等。在眼在手模型中，文獻[4]提出了一種基于圖像的自適應視覺伺服算法，并引入李雅普諾夫函數保證相機參數未知的情況下，系統是穩定的，移動機器人軌跡跟蹤誤差能夠收斂到零。文獻[5]設計了基于圖像的視覺伺服PD控制器，并對移動機器人的動態運動采用前饋神經網絡補償，利用圖像誤差信息結合運動控制器，使圖像平面上的軌跡跟蹤誤差趨向于零。但是對于眼在手模型，當移動機器人運動時，相機和變化的工作空間的幾何關系不容易確定，且不容易獲得全局的視覺信息?；谘鄣绞挚蚣?，文獻[6]利用固定在天花板上的針孔相機，建立了相機透視投影模型，并采用模型預測控制方法提出了一種新的動態反饋跟蹤控制器，解決視覺軌跡跟蹤問題。文獻[7]在相機參數及移動機器人動力學參數未知的情況下，設計了一個自適應的軌跡跟蹤控制器，實現了視覺全局漸近跟蹤。上述視覺伺服控制方法中，PID控制方法是利用控制輸入的給定值和輸出值構成偏差，來對系統進行反饋控制，但是其PID參數不易調到最優，精度不高。自適應控制對于移動機器人模型和參數的不確定性具有很好的魯棒性，其根據實際獲得的數據與期望的數據進行對比，自動調整控制器參數和控制率使系統到達最優狀態，并通過李亞普諾夫函數保證系統的穩定性。但是自適應控制的參數選擇沒有一個具體規則，在實際運用中可能無法獲得一個最優的參數。智能控制是人工智能和自動控制的結合，為了處理視覺伺服中存在的各種不確定性及提高視覺伺服系統的穩定性，許多學者將神經網絡、遺傳算法引入到視覺伺服系統中，其主要形成了模糊控制、神經網絡控制等方法。神經網絡的視覺伺服通過網絡的大量訓練和神經網絡對于非線性映射的逼近能力，來建立機器人空間和圖像空間的對應關系，其無需分析視覺模型。但是對于不同的工作環境，神經網絡都需要重新訓練模型，其適應普及能力較弱。

模預測控制(Model Predictive Control，MPC)能夠根據系統的當前輸入以及過程的歷史信息，預測下一時刻輸出的值[8]，并很好的用于非線性系統處理移動機器人的約束及多變量問題，且魯棒性較好。文獻[9]基于模型預測控制方法使用了視覺追蹤算法提取特征點完成視覺跟蹤任務，并設計了上位機實時繪制移動機器人的軌跡。文獻[10]提出了一種模型預測控制方法解決了移動機器人視覺軌跡跟蹤問題。因此，本文采用了模型預測控制方法設計控制器。在實際中，由于受電機轉速的影響，移動機器人存在速度約束，而文獻[9]和[10]所提方法無法直接處理速度約束問題。為解決帶有速度約束的視覺軌跡跟蹤問題，文獻[11]將原始對偶神經網絡和模型預測控制結合，解決了帶有速度約束的視覺軌跡跟蹤問題。文獻[12]采用基于管道的模型預測控制及3個神經網絡分別設計了線速度控制器和角速度控制器，解決了視覺點鎮定問題。但模型預測控制與神經網絡結合求解優化問題往往比較復雜，計算量大。

基于以上的討論，受文獻[13]和[14]的啟發，本文提出了一種基于LOQO內點法的模型預測控制方法解決具有速度約束的移動機器人視覺軌跡跟蹤問題。首先，結合眼到手視覺伺服模型建立移動機器人的軌跡跟蹤誤差模型，并對移動機器人的誤差模型進行離散化，給出基于模型預測控制的跟蹤代價函數。考慮到移動機器人存在的速度約束問題，采用障礙函數法[15]將不等式約束轉化為等式約束并結合模型預測控制方法建立并引入優化問題。最后，使用LOQO內點法求解建立的預測控制最小化問題，得到基于視覺的軌跡跟蹤控制器。通過數值仿真驗證所提算法的有效性。

1 問題描述

1)基于眼到手的相機模型：

眼到手視覺伺服框架如圖1所示，其中相機固定在天花板上，且相機與移動機器人平面平行。世界坐標系為X-Y-Z，相機坐標系為XC-YC-ZC，圖像坐標系為XP-YP-ZP，像素坐標系為U-O-V。

圖1 相機模型

在圖1中，C點是相機光軸和X-Y平面的交點，它在X-Y平面上的坐標為(Ocx,Ocy)。相機光軸與圖像坐標平面的交點坐標為(Opx,Opy)。機器人質心P在X-Y平面上的坐標為(x,y)，由針孔成像原理可得P在像素坐標系下的坐標P2(xr,yr)為：

(1)

2)移動機器人運動學模型:

考慮兩輪差分式移動機器人，其模型如圖2所示。

圖2 移動機器人模型

根據圖2，移動機器人的運動學模型表示為如下微分方程的形式：

(2)

式中，是移動機器人的行進方向速度，為移動機器人的角速度。對式(1)進行求導并結合式(2)可得眼到手的移動機器人視覺伺服運動學模型為：

(3)

3)軌跡跟蹤誤差模型：

在視覺軌跡跟蹤任務中，期望的軌跡定義為：

qr(t)=(xr(t),yr(t),θr(t))T

(4)

其中:(xr,yr)為機器人質心(x,y)在像素坐標系下的參考坐標，θr為機器人在世界坐標系下的參考前進方向的角度。不失一般性，假設在像素坐標系下U軸上縮放的倍數與V軸上縮放的倍數相等，即α=β，對式(4)進行微分可以得到下式：

(5)

其中:ωr為參考機器人的角速度，vr為參考機器人的進行方向速度?；谝陨戏治?，可得跟蹤誤差向量為：

(6)

對式(6)進行求導并結合式(3)和(5)可得眼到手的視覺伺服移動機器人軌跡跟蹤的誤差系統為：

(7)

對式(7)進行離散化，可得跟蹤誤差系統的離散化形式為：

x(k+1)=f(x(k))+g(x(k))u(k)

(8)

本文的目的是在eye-to-hand的框架下，針對具有速度約束的移動機器人視覺軌跡跟蹤問題，采用基于LOQO內點法的模型預測控制方法設計基于視覺的軌跡跟蹤控制器，實現移動機器人的軌跡跟蹤控制。

2 控制器設計

模型預測控制方法被稱為滾動時域控制方法(RHC)，是一種最優控制策略，其突出優點是對系統模型要求不高，可以處理控制量和狀態量的約束，以及多目標優化。考慮如下的模型預測控制代價函數：

(9)

其中:m和N分別為預測時域和控制時域，x(k+j|k)和u(k+j|k)分別是x(k)和u(k)在k時刻的預測值，Q與R是權值矩陣。另一方面，考慮移動機器人的速度約束，可以將模型預測控制最小化問題描述為如下帶有輸入約束的性能指標優化問題：

(10)

(11)

針對具有輸入約束的優化問題，首先，采用障礙函數法[15]將不等式約束轉換為等式約束，并將其引入到代價函數中，則優化問題(10)可重寫為：

(12)

(13)

▽uι=▽uΓ-λ1+λ2=0

(14)

▽ξι=-ηD-1e+λ1=0

(15)

▽γι=-ηE-1e+λ2=0

(16)

(17)

(18)

其中:D和E為二維的對角矩陣，其對角線上的第i個元素分別為ξi和γi，對式(15)和(16)左乘矩陣D和E，可得原始對偶方程：

▽uΓ-λ1+λ2=0

(19)

-ηe+DΛ1e=0

(20)

-ηe+EΛ2e=0

(21)

(22)

(23)

(24)

對式(24)兩邊同時左乘得：

(25)

圖3 系統狀態誤差 圖4 系統輸入誤差

其中:ru=▽uι，rξ=▽ξι，rγ=▽γι，rλ=▽λι。

圖5 速度跟蹤曲線 圖6 實際軌跡與真實軌跡

通過計算可得Δξ，Δγ：

(26)

(27)

將式(26)～(27)代入式(25)，可得如下降維系統：

(28)

ui+1=ui+ρ(i)Δu(i)

(29)

ξ(i+1)=ξ(i)+ρ(i)Δξ(i)

(30)

γ(i+1)=γ(i)+ρ(i)Δγ(i)

(31)

λ1(i + 1)=λ1(i)+ρ(i)Δλ1(i)

(32)

λ2(i + 1)=λ2(i)+ρ(i)Δλ2(i)

(33)

其中:ρ(i)為搜索方向，每次迭代都要求解式(28)得到Δu，Δλ1，Δλ2；Δξ和Δγ可求解式(26)～(27)得到。

定理2.1：當Ζ=-Hu-D-1Λ1-E-1Λ2，如果矩陣Ζ是非奇異的，則式(28)具有唯一的解。

Δu=Ζ-1(▽uΓ+D-1Λ1rλ1-E-1Λ2rλ2-ηD-1e+ηE-1e)

(34)

Δλ1=-D-1Λ1(rλ 1+Δu)-rξ

(35)

Δλ2=-E-1Λ2(rλ2-Δu)-rγ

(36)

證明：對式(28)采用消元法可得到式(34)，(35)，(36)。證畢。

注：在式(10)中，權重矩陣Q和R為正定矩陣，因此，可知Hessian矩陣Hu也是正定矩陣。松弛變量ξ，γ和拉格朗日乘子λ1，λ2都為正的，即對角矩陣D，E，Λ1和Λ2也都是正定的。所以保證了矩陣Z是非奇異的。

3 實驗仿真分析

在仿真中，搜索方向ρ(i)選取為：

ρ(i)=0.95*

期望跟蹤的軌跡描述為：

仿真結果如圖3～6所示。

圖3表示系統的狀態誤差變化圖，從中可以看到，系統狀態在4秒后趨于零，表明系統的狀態誤差具有良好的收斂性，軌跡跟蹤的橫坐標、縱坐標及角度誤差能夠收斂為零。圖4表示系統的輸入誤差，在圖中可以看到短時間內輸入誤差能夠快速收斂到零。圖5表示移動機器人的實際線速度和實際角速度，在圖5中可以看到，移動機器人的實際線速度和角速度能夠在短時間內跟蹤上期望的線速度和角速度，并能夠保持穩定。圖6是像素坐標系中移動機器人的實際軌跡與真實軌跡圖，從圖6中可以看到，對于一個給定的期望軌跡，當初始位置偏離實際軌跡時，移動機器人仍可快速跟蹤期望的軌跡，完成對一個圓的軌跡跟蹤任務。

從上述仿真結果可以看出，當跟蹤問題中含有不等式約束問題時，基于LOQO內點法的預測控制方法設計的控制器能夠很好地保證視覺軌跡跟蹤系統的穩定性，系統的狀態誤差及輸入誤差都能夠較快的收斂到零，移動機器人的線速度和角速度也能很快的保持穩定，并且仿真中移動機器人能夠快速的跟上期望的參考軌跡，完成視覺軌跡跟蹤任務。從表1中可以看出，在仿真中，移動機器人第10秒的最終線速度為0.249 99 m/s，最終角速度為0.312 50 rad/s，都能夠跟上期望的線速度和角速度。對于第10秒的系統輸入誤差，誤差u1和u2都能夠收斂于零。從表1中可以看到，移動機器人的系統誤差e1，e2，e3都能夠收斂于零，表明了移動機器人的橫軸誤差、縱軸誤差及角速度誤差都能夠無限趨向于零。根據上述分析，基于LOQO內點法的模型預測控制方法能夠精確的完成帶速度約束的移動機器人軌跡跟蹤任務。

表1 第10秒跟蹤誤差仿真數據表

4 結束語

為了解決具有速度約束的移動機器人視覺軌跡跟蹤問題，本文提出了一種基于LOQO內點法的模型預測控制方法。首先在眼到手框架下建立了移動機器人系統誤差模型并給出基于模型預測控制的移動機器人視覺伺服跟蹤的代價函數。同時考慮移動機器人速度約束，采用障礙函數法將不等式約束轉化為等式約束，并引入到代價函數中。進而，采用LOQO內點法對代價函數進行求解，得到基于視覺的軌跡跟蹤控制器。最后，仿真結果表明，本文所提出的算法能夠很好地完成視覺軌跡跟蹤任務。