楊馬英，鄭亞飛

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

基于視覺反饋的機械臂預測控制

楊馬英，鄭亞飛

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

摘要：針對基于視覺的機器人目標抓取問題，構(gòu)建一個基于圖像的單目視覺伺服系統(tǒng).預先建立基于圖像雅克比矩陣的模型，在明確考慮目標可見性約束及機器人執(zhí)行器約束的前提下，將預測控制算法引入到機械臂的視覺伺服控制中，基于視覺特征的運動預測設(shè)計一個簡化的多變量機械臂視覺伺服控制器，用以實現(xiàn)對機械臂末端位姿的控制.借助NAO機器人平臺，利用機器人自帶的Naomark標簽進行物體的快速識別，并應用上述基于視覺反饋的機械臂預測控制方法實現(xiàn)NAO機器人手臂控制.實驗結(jié)果驗證了方法的可行性，算法簡單易于實現(xiàn)，且控制精度令人滿意.

關(guān)鍵詞：視覺伺服；圖像雅克比矩陣；預測控制；NAO機器人

機械臂視覺伺服[1-5]是將視覺傳感器作為感知機構(gòu)，實時采集目標物與機械臂末端效應器的圖像信息，通過圖像預處理、特征提取等環(huán)節(jié)獲得機械臂工作環(huán)境的信息反饋，構(gòu)建閉環(huán)控制，驅(qū)動機械臂運動到指定位姿.視覺伺服使機器人更智能化，增強對外界環(huán)境的適應性，在眾多應用領(lǐng)域得到了廣泛的關(guān)注，如農(nóng)業(yè)采摘機器人[6].在伺服控制算法上有經(jīng)典控制方法，如反饋比例控制器，該方法易于實現(xiàn)，但忽略了機器人運動學及動力學限制，這類約束對控制性能卻有顯著影響，當系統(tǒng)初始位姿與期望位姿之間存有較大位移時，由于系統(tǒng)可見性約束或者執(zhí)行機構(gòu)的機械約束，造成視覺伺服任務失敗[7-9].智能控制方法也被應用于機器人視覺伺服研究，如神經(jīng)網(wǎng)絡[10]、模糊控制[11-12]以及預測控制[13-15]等.神經(jīng)網(wǎng)絡需預先進行離線訓練，環(huán)境變化時則重新訓練, 限制了其應用.模糊控制利用模糊規(guī)則描述視覺伺服系統(tǒng)中各變量之間的關(guān)系，不需要精確的系統(tǒng)模型, 但需一定的先驗知識或離線學習.對于多自由度視覺伺服系統(tǒng), 變量之間關(guān)系復雜且耦合嚴重, 模糊規(guī)則的設(shè)計比較困難.預測控制的模型不要求嚴謹?shù)慕Y(jié)構(gòu)形式，只強調(diào)其預測功能,預測控制策略雖發(fā)端于工業(yè)過程控制，也適用于機械臂控制[16-17].Lazar等用非線性模型預測控制設(shè)計視覺控制器，考慮可見性約束，通過非線性約束最優(yōu)問題實現(xiàn)了機器人運動的收斂[18].

因預測控制對機械臂模型要求不高,有通過DH運動學參數(shù)確定機械臂運動學模型，運用運動學逆解求解得相對應的關(guān)節(jié)角變量，但運動學逆解運算量大且復雜.也有利用拉格朗日方程建立機械臂動力學方程，但機械臂動力學方程是時變的強非線性方程，計算量大.在一定的操作空間內(nèi)，圖像雅可比矩陣可直接將機器人在工作空間的速度變化映射為目標在圖像特征參數(shù)空間的變化，使機器人視覺伺服不再受復雜的機器人和攝像機系統(tǒng)建模限制，只要求出這種映射關(guān)系就可以通過視覺特征誤差計算機器人控制量，實現(xiàn)機器人控制.因此，在明確考慮系統(tǒng)約束問題前提下，提出簡化的基于視覺反饋的機械臂預測控制方法.采用基于圖像雅克比矩陣的預測模型，設(shè)計預測控制器，其中控制器的設(shè)定和測量反饋均是視覺特征位置量，考慮目標可見性約束及機器人執(zhí)行器約束，通過在線滾動優(yōu)化計算機械臂的關(guān)節(jié)角運動控制量，驅(qū)使機械臂末端向指定位姿運動.最后在NAO機器人實驗平臺進行驗證，實驗結(jié)果表明了該方法的控制可行性，且計算相對簡單，易于實現(xiàn).

1預測控制算法的實現(xiàn)

1.1預測模型的建立

對于任意一個實際控制系統(tǒng)，好的模型可以在一定程度上簡化控制器設(shè)計的復雜度，從而提高系統(tǒng)控制性能.因此，為了描述機器人手眼映射關(guān)系，需要選擇一種比較合適的模型.圖像處理與定位是視覺伺服控制系統(tǒng)的重要組成部分，近年已有很多研究成果[19].因而不再詳細討論圖像特征提取技術(shù)，而是在此基礎(chǔ)上選擇經(jīng)典視覺伺服控制內(nèi)部交互矩陣作為預測模型.通過模型，可以在有限的預測域內(nèi)預測得到視覺特征相對于攝像機移動速度的變化率.

經(jīng)典內(nèi)部模型特征點速度與輸入的關(guān)系表達式為

(1)

假設(shè)預測控制中每次迭代的視覺特征取樣周期為Te,離散后得

Np-1)Tev(k+Np-1|k)

(2)

其中：Jp為雅克比矩陣；sm(k)為圖像在k時刻的視覺特征點；v(k)為機械臂的運動矢量；Np為模型預測的時域范圍.式(2)為圖像特征迭代的一步預測模型.

解析法、在線估計法和離線學習法是雅克比矩陣最常用的求解方法.利用解析法求解圖像雅克比矩陣,需要對攝像機進行標定,而且需要求取機器人的模型.在線估計法與初始值的選取密切相關(guān).離線學習不要求對攝像機內(nèi)部參數(shù)和機器人參數(shù)進行標定,但要求進行離線學習.

通過預先求取圖像雅克比矩陣，在實現(xiàn)視覺伺服任務過程中保持圖像雅克比矩陣不變.該方法只需計算一次圖像雅克比矩陣，較為簡單.該方法適用于攝像機和目標物體不同時運動的場合，即眼固定型視覺定位或跟蹤問題.

1.2視覺伺服預測控制

有賴于其滾動優(yōu)化機制,模型預測控制在降低建模難度、抗干擾性和減小控制誤差方面具有明顯的優(yōu)勢.預測控制與其他的控制方法一個很大的區(qū)別也在于控制信號的實施方法上.一般情況下,模型預測控制通過當前采樣時刻來解決一個優(yōu)化問題并且獲得當前采樣時刻和未來時刻的控制量,但是只有當前這個采樣時刻的控制量被施加到控制系統(tǒng)中.在接下來的采樣時刻,重復進行以上的優(yōu)化問題[20].

視覺伺服預測控制系統(tǒng)選擇內(nèi)部模型控制(IMC)結(jié)構(gòu)，如圖1所示.

(1)托輥間距合理加大。托輥間距應同時滿足兩個條件：一是托輥承載能力及使用壽命要求；二是保證膠帶的承載和回程段的最大撓度符合規(guī)定要求。當運輸物料的松散密度小于1 600 kg/m3時，我國目前采用的DTII型帶式輸送機的托輥間距取1.2 m。通過動態(tài)分析計算，本膠帶機確定選用承載托輥間距1.5 m，承載托輥數(shù)量減少了20%，直接降低了膠帶機成本。

圖1　控制系統(tǒng)結(jié)構(gòu)圖Fig.1　Structure chart of control system

圖1中，被控過程包括機械臂和視覺系統(tǒng).過程輸入U為機械臂控制變量，即機械臂關(guān)節(jié)角度；輸出s(k)為機械臂當前的位置信息；參考值s*(k)為期望的位置信息；sm(k)為系統(tǒng)模型預測得到的位置信息；ε(k)為當前位置信息和系統(tǒng)模型預測的位置信息之間所有的模型誤差和干擾.由圖1可得

ε(k)=s(k)-sm(k)

(3)

sd(k)=s*(k)-ε(k)=s*(k)-(s(k)-sm(k))

(4)

e(k)=sd(k)-sm(k)=s*(k)-s(k)

(5)

從圖1中可以看出：使被控制過程輸出s(k)跟蹤參考值s*(k)，與使系統(tǒng)模型輸出sm(k)跟蹤期望值 sd(k)是等價的.假設(shè)參考值s*(k)已知，通過在有限預測域NP內(nèi)的模型可以預測下一時刻位置信息sm(k)的變化.

需要考慮的約束[13-15]有以下3種：

1) 機械約束，即機械臂關(guān)節(jié)的角度限制，關(guān)節(jié)角度q，qmin≤q≤qmax.

2) 可視性約束，用來確保目標圖像一直處在像素平面，可視區(qū)V，Vmin≤V≤Vmax.

3) 控制量約束，即執(zhí)行機構(gòu)速度的限制，執(zhí)行機構(gòu)速度U，Umin≤U≤Umax.

以上約束可以寫成非線性方程，即

(6)

并且定義約束域K.

通過優(yōu)化算法計算得到期望軌跡sd(k)與系統(tǒng)預測模型輸出sm(k)偏差的最小值為

(7)

其中Q(i)為一個對角陣.由期望位置與預測模型輸出的位置之差定義性能指標方程J,通過J可以在預測時域Np內(nèi)得到最優(yōu)控制序列U,但只有控制序列中的第一個U(k)被輸入到被控過程中.總結(jié)預測控制用于視覺伺服的步驟如圖2所示.

圖2　控制系統(tǒng)流程圖Fig.2　Flow chart of the control system

2實驗設(shè)計

2.1NAO機器人視覺系統(tǒng)

將基于視覺反饋的機械臂預測控制用于實驗室NAO移動機器人的手臂運動控制.NAO機器人[21-22]的視覺系統(tǒng)是由兩個位于機器人的面部且處于同一鉛垂面上的攝像機組成，如圖3所示.NAO機器人眉心位置的攝像機(CameraTop)的位置過高，執(zhí)行伺服任務過程中容易發(fā)生機器人手末端執(zhí)行器和目標物體沒有同時出現(xiàn)在攝像機視野范圍內(nèi)的情況，從而導致不能全面地獲取兩者之間的相對信息，繼而影響到對機器人手的動作控制.另一個位于嘴部位置的攝像機(CameraButton)，位置更為合適，可以較全面地掃描周邊環(huán)境，實現(xiàn)輔助機器人手的運動控制.由于NAO機器人視覺系統(tǒng)具有以上所述的局限性，而且兩個攝像機不能同時開啟，所以很難借助NAO機器人視覺系統(tǒng)構(gòu)造基于雙目立體視覺系統(tǒng).另外，NAO機器人攝像機具有一定的實驗角度范圍，查NAO攝像頭參數(shù)表，得知NAO機器人垂直視覺范圍為47.64°，水平視覺范圍為60.97°，可見性約束是為了確保目標物和機械手在NAO的視野范圍內(nèi).

圖3　NAO視覺系統(tǒng)Fig.3　NAO vision system

如圖4所示，Naomark是由AldebaranRobotics公司設(shè)計，以一個白色圓圈為圓心，周圍有若干圓圈為中心打開的白色扇形，白色圓圈里的數(shù)字叫做MarkID，是對不同Naomark的標記.視覺特征的正確選取對于基于圖像的視覺伺服系統(tǒng)具有舉足輕重的作用,但是對于NAO機器人而言，只要將Naomark放在目標物體上，然后將目標物體放在NAO機器人行動范圍內(nèi)的不同位置，NAO機器人便可以把直接尋找目標物體的任務轉(zhuǎn)換為通過檢測Naomark，進而實現(xiàn)目標物體的間接定位.

圖4　不同的NaomarksFig.4　Different Naomarks

2.2NAO機器人手臂的雅克比矩陣

NAO機器人手臂具有5個自由度，其左手臂各關(guān)節(jié)名稱如表1所示.通過實驗測試發(fā)現(xiàn)，其中LWristYaw和LElbowYaw對軌跡規(guī)劃的影響較小可以忽略不計，因此預先設(shè)定為固定的角度.從圖像雅克比矩陣的物理定義出發(fā)，通過觀察已知的機器人運動所引起的特征變化，來估計相應的圖像雅克比矩陣.具體操作方法是：根據(jù)機器人手臂的自由度n，選取m(m≥n)個充分有效的圖像特征，在具體實施每一步控制前，讓手臂完成n次試探運動，并觀察相應的圖像變化，然后用最小二乘法估計得到當前的圖像雅克比矩陣.

對于NAO機器人，因為控制對象是NAO機器人手臂，機械約束和控制量約束可以合并為一類，它們的約束范圍如表1所示.因LWristYaw和LElbowYaw預先設(shè)定為固定值，所以只要考慮LShoulderPitch, LShoulderRoll及 LElbowRoll三個關(guān)節(jié)的范圍.預測控制輸出的是下一步三個關(guān)節(jié)的角度，因此只需把三個關(guān)節(jié)的范圍作為狀態(tài)變量約束.

表1　NAO機械臂關(guān)節(jié)參數(shù)表

3實驗場景及結(jié)果

實驗場景如圖5所示，分別在墻壁和NAO機器人手部貼上Naomark標簽，它們在NAO機器人視覺系統(tǒng)中圖像如圖6所示,在機器人的視野范圍內(nèi)選擇不同的位置作為期望位置，利用預測控制求取手臂關(guān)節(jié)角度，控制NAO手末端向著期望位置靠近.

圖5　實驗場景Fig.5　Experiment scene

圖6　Nao視野場景Fig.6　Experiment scene in NAO vision

實驗1為定值控制實驗，即期望位置固定在NAO視野范圍內(nèi)，NAO手臂的初始位置是在NAO視野范圍內(nèi)任意給定，共進行了4次實驗，結(jié)果如表2所示，其中y為循環(huán)結(jié)束時機械臂末端與期望位置之間的空間距離.從表2中可以看出：機械臂最后停在離期望位置距離0.2 cm左右的位置，控制誤差相對來說比較小.實驗1的手臂末端控制誤差曲線如圖7所示，從圖7中也可以看出:機械臂末端與期望位置之間的空間距離y不斷減少，整個過程動態(tài)響應令人滿意.

圖7　定值控制實驗結(jié)果圖Fig.7　Experimental result of fixed value control

實驗坐標初始位置/cm期望位置/cm最后位置/cm循環(huán)數(shù)/次y/cm1X4.604.494.29Y2.162.132.18Z48.8647.5347.51100.212X4.304.774.63Y2.311.932.18Z47.4846.9646.9550.283X4.304.784.70Y2.312.062.08Z47.4847.0146.8470.194X4.573.733.84Y2.011.872.12Z46.7347.4047.4560.28

實驗2為跟蹤控制實驗，期望位置在實驗過程中有發(fā)生變化，但一定處于NAO視野范圍內(nèi).NAO手臂的初始位置是在NAO視野范圍內(nèi)任意給定.期望位置在第5次循環(huán)時發(fā)生變化，實驗結(jié)果如圖8所示.從圖8中可以看出：機械手不斷朝著期望位置靠攏，當期望位置發(fā)生變化后，機械手又朝著更新后的期望位置靠攏.

作為對比，實驗3使用同一組初始位置和期望位置進行了兩組實驗，一組運用了基于視覺反饋的機械臂預測控制方法，一組運用傳統(tǒng)的基于圖像的視覺伺服方法，該方法也采用基于圖像雅克比矩陣的模型，但是控制器選用比例反饋控制器.兩組實驗的實驗結(jié)果如圖9所示.從圖9中可以看出：比例控制結(jié)果曲線在第4次就不再更新，因為傳統(tǒng)的基于圖像的視覺伺服方法沒有考慮約束問題，在第3次控制量傳給機器人后，機械臂已經(jīng)超出了機器人的視野范圍，獲取不到最新的位置信息，導致伺服任務失敗.對比上述實驗效果，驗證了帶有約束的視覺伺服預測控制算法的可行性.

圖8　跟蹤控制實驗結(jié)果圖Fig.8　Experimental result of tracking control

圖9　實驗結(jié)果Fig.9　Experiment result

4結(jié)論

針對機械臂的視覺伺服控制，在明確考慮系統(tǒng)約束問題的前提下，提出簡化的基于視覺反饋的機械臂預測控制方法.通過實驗，測試了這一方法的可行性.實驗表明：以固定的圖像雅克比矩陣模型為基礎(chǔ)的視覺預測控制方法，實現(xiàn)機械手向目標逼近是可行的，建模簡單，算法計算量小容易實現(xiàn)，并且控制誤差在一定的允許范圍內(nèi).而要進一步保證復雜場景下的視覺伺服控制精度，則需考慮作為預測模型的圖像雅克比矩陣的在線估計問題.

參考文獻：

[1]CHAUMETTE F, HUTCHINSON S. Visual servo control I basic approaches[J]. IEEE robotics & automation magazine,2006,13(4):82-90.

[2]林靖，陳輝堂，王月娟，等．機器人視覺伺服系統(tǒng)的研究[J]．控制理論與應用，2000，17(4):476-481．

[3]薛定宇，項龍江，司秉玉，等．視覺伺服分類及其動態(tài)過程[J]．東北大學學報(自然科學版)，2003，24(6):543-547．

[4]方勇純．機器人視覺伺服研究綜述[J]．智能系統(tǒng)學報，2008，3(2):109-114．

[5]賈丙西，劉山，張凱祥，等．機器人視覺伺服研究進展：視覺系統(tǒng)與控制策略[J]．自動化學報，2015，41(5):861-873．

[6]鮑官軍,荀一,戚利勇,等.機器視覺在黃瓜采摘機器人中的應用研究[J]．浙江工業(yè)大學學報,2010,38(1):114-118.

[7]王婷婷.帶有約束的機器人視覺伺服控制方法的研究[D].無錫：江南大學，2012.

[8]CHESI G, HUNG Y S. Global path-planning for constrained

and optimal visual servoing[J]. IEEE transactions on robotics,2007,23(5):1050-1060.

[9]董輝,仲曉帆,黃勝．一種六關(guān)節(jié)機器人關(guān)節(jié)空間軌跡規(guī)劃方法[J]．浙江工業(yè)大學學報,2015,43(3):336-339．

[10]李優(yōu)新,毛宗源,田聯(lián)房.基于圖像矩與神經(jīng)網(wǎng)絡的機器人四自由度視覺伺服[J].控制理論與應用,2009,26(10):1162-1166.

[11]SIRADJUDDIN I, BEHERA L, MCGINNITY T M, et al. Image-based visual servoing of a 7-DOF robot manipulator using an adaptive distributed fuzzy PD controller[J].IEEE/ASME transactions on mechatronics,2014,19(2):512-523.

[12]LóPEZ M B. Fuzzy vs nonfuzzy in 2D visual servoing for robot manipulators[J]. International journal of advanced robotic systems,2013,10(2):1-11.

[13]ALLIBERT G, COURTIAL E, TOURE Y. Real-time visual predictive controller for image-based trajectory tracking of mobile robot[C]//International Fedration of Automatic Control World Congress. Seoul, Korea: International Federation of Automatic Control,2008:11244-11249.

[14]ALLIBERT G, COURTIAL E, CHAUMETTE F．Predictive control for con-strained image-based visual servoing[J]. IEEE transactions on robotics,2010,26(5):933-939.

[15]ALLIBERT G, COURTIAL E, CHAUMETTE F. Visual servoing via nonlinear predictive control[J]. Lecture notes in control & information sciences,2010,401:375-393.

[16]張曉穎，李德偉，席裕庚，等．六自由度機械臂約束預測控制系統(tǒng)的設(shè)計[J]．控制理論與應用，2014,31(11):1464-1472.

[17]FERREIRA P, PINTO J R C. Visual based predictive control for a six degrees of freedom robot[C]//Emerging Technologies and Factory Automation. Prague: IEEE,2006:846-853.

[18]LAZAR C, BURTACU A．Visual servoing of robot manipulators using model-based predictive control[C]//Proceedings of IEEE International Conference on Industrial Informatics. Cardiff, Wales: IEEE，2009:690-695.

[19]俞立,唐俊淮,陳軍統(tǒng)．一種基于觀測范圍約束的改進EKF-SLAM 算法[J].浙江工業(yè)大學學報,2014,42(5):566-571.

[20]邢滿榮，紀春明.淺析預測控制的特點及應用[J].中國科技信息，2006(9)：188-188.

[21]張生.NAO機器人的目標識別與定位研究[D].合肥：安徽大學,2013.

[22]孫漫漫.數(shù)據(jù)驅(qū)動的NAO機器人關(guān)節(jié)運動控制[D].武漢：武漢理工大學，2014.

(責任編輯：劉巖)

Predictive control of manipulators based on visual feedback

YANG Maying, ZHENG Yafei

(College of Information Engineering, Zhejiang University of Technology, Hangzhou 310023, China)

Abstract:In regarding to the vision-based target capturing of robots, an image-based monocular vision servo system is constructed. The model based on image-based jacobian matrix is obtained in advance. And the predictive control algorithm is introduced into the visual servo control of manipulators under the premise of target visibility constraints and actuator constraints of robots. In order to realize the control of end position of manipulator, a simplified multivariable mechanical arm visual servo controller is designed using motion prediction based on visual features. Then predictive control method based on visual feedback of the robot manipulator is applied to NAO robot, and the rapid identification of object is achieved by using Naomark label with its own. Experimental results demonstrate that the proposed method is feasible. Moreover, the algorithm is simple and easy to be implemented, and the control precision is satisfied.

Keywords:visual servo control; image-based jacobian matrix; model predictive control; NAO robot

收稿日期：2015-11-18

作者簡介：楊馬英(1966—)，女，浙江海寧人，教授，研究方向為過程控制與機器人控制技術(shù)，E-mail：myyang@zjut.edu.cn.

中圖分類號：TP273

文獻標志碼：A

文章編號：1006-4303(2016)03-0260-06