專利名稱：基于非線性模型預測的智能小車2.5維視覺伺服控制方法
技術領域：
本發(fā)明涉及智能機器人技術領域，具體地說是指一種基于非線性模型預測的智能小車2. 5維視覺伺服控制方法。
背景技術：
智能小車視覺伺服是指利用視覺傳感器獲取的視覺信息對小車位置和方向進行精確控制。視覺伺服對于提高小車的智能化水平與工作能力具有非常重要的意義，可以提高小車對外界環(huán)境的學習和適應能力。按照反饋信息類型的差別，視覺伺服可以分為三維視覺伺服、二維視覺伺服以及2. 5維視覺伺服，其中2. 5維視覺伺服是一種將二維信息與三維信息有機結合的混合伺服方法，可以在一定程度上解決三維和二維視覺伺服存在的魯棒性、奇異性、局部極小等問題。因此，智能小車2. 5維視覺伺服越來越受到廣泛關注。
近年來，國內外許多研究學者對2. 5維視覺伺服控制方法做了大量的研究。FangYongchun 等人在 2005 年 IEEE Transactions on Systems, Man, and Cybernetics, PartB: Cybernetics, 35(5)期刊上設計了一種基于單應矩陣的2. 5維視覺伺服控制策略,這種方法在深度信息未知的情況下仍然能夠使移動機器人漸近運動到由一副參考圖像定義的期望位姿° Wang Chaoli 在 2011 年 IEEE International Conference on Robotics andAutomation會議上采用兩階段技術設計了一種魯棒視覺控制器，在缺乏深度信息和精確的視覺參數(shù)情況下實現(xiàn)了對移動機器人圖像位置和三維姿態(tài)的控制。針對系統(tǒng)模型參數(shù)的不確定性及外界干擾，Yang Fang等人在2011年Acta Automatica Sinica, 37 (7)期刊上采用自適應控制和滑?？刂萍夹g來設計視覺控制器。以上方法對于實現(xiàn)智能小車2. 5維視覺伺服控制具有重要的借鑒作用，但并未考慮運動執(zhí)行系統(tǒng)存在的速度和力矩約束及攝像機的可見性約束，在伺服過程中參考目標可能偏離于攝像機的視野之外，從而致使伺服失敗。

發(fā)明內容
本發(fā)明的目的在于提供一種基于非線性模型預測的智能小車2. 5維視覺伺服控制方法，以解決現(xiàn)有2. 5維視覺伺服控制方法在設計視覺控制器時未考慮運動執(zhí)行系統(tǒng)存在的控制約束和攝像機的可見性約束問題。本發(fā)明方法利用非線性模型預測控制設計一種多級視覺預測控制器，控制小車從初始位姿運動到期望位姿同時使參考目標始終處于攝像機視野之內。本發(fā)明方法采用的技術方案如下I)在當前位姿和期望位姿處通過攝像機分別獲取參考目標的當前圖像和期望圖像；2)從獲取的當前圖像和期望圖像中提取參考目標的特征點及小車的姿態(tài)信號，建立2. 5維視覺誤差模型；設目標特征點在當前圖像中的位置為p=[px，py]T，在期望圖像中的位置為pd=[pxd，Pyd]T，Θ和別為智能小車的當前方向角和期望方向角，通過坐標變換將二維圖像信號與三維姿態(tài)信號進行有機結合，構造一種2. 5維視覺誤差信號，其中Sl，Sld，S2，S2d為經過坐標變換的二維圖像信號，Θ ed為三維姿態(tài)誤差信號；3)根據(jù)步驟2)中的2. 5維視覺誤差模型，利用非線性模型預測控制方法設計一種多級視覺預測控制器，使2. 5維視覺誤差信號漸近收斂，進而完成小車視覺伺服任務。步驟3)中設計的多級視覺預測控制器至少由兩個子控制器組成，一個為運動學控制器，另一個為動力學控制器，其中運動學控制器的輸出作為動力學控制器的輸入。運動學控制器的控制包含以下步驟首先，采用歐拉近似法對2. 5維視覺誤差模型進行離散化處理，獲得視覺誤差預測模型e(k+j|k)，其中，T為采樣周期，v(k)和ω (k)分別為小車在k時刻的線速度和角速度，符號(k+j |k)表示在k時刻向前預測j步得到的預測值；
其次，將小車速度ν=[ν，ω]τ作為控制輸入，構造一個由2. 5維視覺誤差預測信號和控制輸入表示的二次目標函數(shù)J (k)，其中Np為預測時域，N。為控制時域，Q和R為正定的斜對稱加權矩陣；
_5]最后，加入可見性約束(iw，u_)，( Uniin,，u_)和速度約束(Vniin，，ν_)，其中(U，υ)為目標特征點在圖像平面上的像素坐標，通過求解帶有約束的二次目標函數(shù)J(k)的優(yōu)化問題來獲得最優(yōu)控制序列/，取其第一項v(k|k)作為運動學控制器的輸出，使得視覺誤差信號漸近收斂，即Iimpooe=O ；動力學控制器的控制包含以下步驟首先，利用歐拉近似法對小車動力學模型進行離散化處理，獲得小車實際速度預測模型V (k+j Ik)，其中τ (k) = [Tl(k)，τ2(10]Τ分別為小車在k時刻的驅動力矩和轉動力矩；其次，將運動學控制器的輸出作為動力學控制器的參考輸入，定義δ為小車實際速度與參考輸入速度之間的速度誤差，構造一個由預測的速度誤差信號和力矩表示的二次目標函數(shù)舛幻其中義為預測時域，F(xiàn)c為控制時域，泛和瓦為正定的斜對稱加權矩陣；最后，加入力矩約束(τ_，，τ_)，最小化二次目標函數(shù)火幻獲得最優(yōu)控制序列 取其第一項τ (k|k)作為動力學控制器的輸出，控制小車速度，使得速度誤差漸近收
斂，即 Iimt^00 δ =0,從而使得 Iimt^00 e=0。與現(xiàn)有技術相比，本發(fā)明具有以下優(yōu)點及有益效果本發(fā)明利用非線性模型預測控制技術將2. 5維視覺伺服任務轉化為非線性優(yōu)化問題，在設計視覺控制器時能夠方便處理系統(tǒng)中存在的約束。本發(fā)明解決現(xiàn)有2. 5維視覺伺服控制方法不能處理運動執(zhí)行系統(tǒng)存在的速度和力矩約束以及攝像機的可見性約束問題，能夠確保參考目標在伺服過程中始終保持可見，大大提高視覺伺服系統(tǒng)的可靠性和安全性。本發(fā)明實現(xiàn)簡單，魯棒性強，不需要精確的過程模型。


圖I是本發(fā)明中的一個兩輪智能小車系統(tǒng)模型示意圖；圖2是本發(fā)明中的智能小車2. 5維視覺伺服控制方法結構示意圖；圖3 8是本發(fā)明中的未考慮可見性約束時智能小車2. 5維視覺伺服控制仿真結果圖；其中圖3為智能小車在XY平面上的運動軌跡仿真圖；圖4為特征點在圖像平面上的運動軌跡仿真圖；圖5為2. 5維視覺誤差曲線圖；圖6為智能小車實際線速度和期望線速度曲線圖；圖7為智能小車實際角速度和期望角速度曲線圖；圖8為智能小車驅動力矩和轉動力矩曲線圖；圖9 14是本發(fā)明中的考慮可見性約束時智能小車2. 5維視覺伺服控制仿真結果圖；其中 圖9為智能小車在XY平面上的運動軌跡仿真圖；圖10為特征點在圖像平面上的運動軌跡仿真圖；圖11為2. 5維視覺誤差曲線圖；圖12為智能小車實際線速度和期望線速度曲線圖；圖13為智能小車實際角速度和期望角速度曲線圖；圖14為智能小車驅動力矩和轉動力矩曲線圖。
具體實施例方式為了更好地理解本發(fā)明的技術方案，下面結合實施例和附圖對本發(fā)明的實施方式做進一步的介紹。實施例如圖I所示為兩輪智能小車系統(tǒng)，左右輪獨立驅動，攝像機固定安裝在小車上。0-XYZ,o-xryr,C-xcyczc分別為世界坐標系、機器人坐標系和攝像機坐標系，小車質心ο位于兩驅動輪的中心，攝像機原點C與質心ο的距離為1，r為輪子半徑，L為輪子到質心的距離。視覺伺服的任務是利用攝像機獲取的視覺信息控制小車從初始位姿運動到期望位姿，并保證目標在運動過程中始終可見。實施步驟如下I)在當前位姿和期望位姿處通過攝像機分別獲取參考目標的當前圖像和期望圖像；2)從獲取的當前圖像和期望圖像中提取參考目標的特征點及小車的姿態(tài)信號，建立2. 5維視覺誤差模型；設所提取的目標特征點在當前圖像中的位置為p=[px，py]T，在期望圖像中的位置為Pd=[Pxd，Pyd]T，θ和別為移動小車的當前方向角和期望方向角，為了便于后續(xù)控制器的設計，對二維圖像坐標進行如下坐標變換
Γ ^ IΓ Pxa ~S = F5lI= P>f，sd=pld]=.(I)
AJ _丄 KdJ
—Py JL PyA _其中f為攝像機的焦距。將二維圖像信號與三維姿態(tài)信號有機結合，構造如下2. 5維視覺誤差信號
權利要求
1. 一種基于非線性模型預測的智能小車2. 5維視覺伺服控制方法，其特征在于包含以下步驟 1.1.在當前位姿和期望位姿處通過攝像機分別獲取參考目標的當前圖像和期望圖像； I. 2.從獲取的當前圖像和期望圖像中提取參考目標的特征點及小車的姿態(tài)信號，建立.2.5維視覺誤差模型； 設目標特征點在當前圖像中的位置為P=[PX，Py]T，在期望圖像中的位置為pd=[pxd，PyJτ，Θ和別為移動小車的當前方向角和期望方向角，通過坐標變換將二維圖像信號與三維姿態(tài)信號進行有機結合，構造一種2. 5維視覺誤差信號，其中sl，sld, s2，s2d為經過坐標變換的二維圖像信號，Θ ed為三維姿態(tài)誤差信號； .1.3.根據(jù)步驟I. 2中的2. 5維視覺誤差模型，利用非線性模型預測控制方法設計一種多級視覺預測控制器，使2. 5維視覺誤差信號漸近收斂。
2.一種基于非線性模型預測的智能小車2. 5維視覺伺服控制方法，其特征在于所述的多級視覺預測控制器由至少兩個子控制器組成，一個為運動學控制器，另一個為動力學控制器，其中運動學控制器的輸出作為動力學控制器的輸入。
3.根據(jù)權利要求2所述的一種基于非線性模型預測的智能小車2.5維視覺伺服控制方法，其特征在于所述的運動學控制器的控制包含以下步驟 首先，采用歐拉近似法對視覺誤差模型進行離散化處理，獲得視覺誤差預測模型6&+」|10，其中，1'為采樣周期^(10和ω (k)分別為小車在k時刻的線速度和角速度，符號(k+j I k)表示在k時刻向前預測j步得到的預測值； 其次，將小車速度V= [V，ω ]τ作為控制輸入，構造一個由2. 5維視覺誤差預測信號和控制輸入表示的二次目標函數(shù)J (k)，其中Np為預測時域，N。為控制時域，Q和R為正定的斜對稱加權矩陣； 最后，加入可見性約束(1η，，ιι_)，(Uniin,, Umax)和速度約束(Vniin,，v_)，其中(U，υ)為目標特征點在圖像平面上的像素坐標，通過求解帶有約束的二次目標函數(shù)J(k)的優(yōu)化問題來獲得最優(yōu)控制序列取其第一項v(k|k)作為運動學控制器的輸出，使得視覺誤差信號漸近收斂，即Iimt^0Oe=Ot5
4.根據(jù)權利要求2所述的一種基于非線性模型預測的智能小車2.5維視覺伺服控制方法，其特征在于動力學控制器的控制包含以下步驟 首先，利用歐拉近似法對小車動力學模型進行離散化處理，獲得小車實際速度預測模型V (k+j I k)，其中τ (k) = [ τ i (k)，τ 2 (k) ]T分別為小車在k時刻的驅動力矩和轉動力矩； 其次，將運動學控制器的輸出作為動力學預測控制器的參考輸入，定義δ為小車實際速度與參考輸入速度之間的速度誤差，構造一個由預測的速度誤差信號和力矩表示的二次目標函數(shù)，其中Ip為預測時域，Arti為控制時域，泛和瓦為正定的斜對稱加權矩陣； 最后，加入力矩約束(Tmin,，τ_)，最小化二次目標函數(shù)火句獲得最優(yōu)控制序列取其第一項τ (k|k)作為動力學控制器的輸出，控制小車速度，使得速度誤差漸近收斂，即Iimt^00 δ =0,從而使得 Iim^0Oe=Ot全文摘要
本發(fā)明公開一種基于非線性模型預測的智能小車2.5維視覺伺服控制方法，首先在當前位姿和期望位姿處通過攝像機分別獲取參考目標的當前圖像和期望圖像；然后從獲取的圖像中提取參考目標的特征點以及小車的姿態(tài)信息，通過坐標變換將二維圖像信號與三維姿態(tài)信號進行有機結合，建立2.5維視覺誤差模型；最后針對2.5維視覺誤差模型，利用非線性模型預測控制方法設計一種多級視覺預測控制器。本發(fā)明解決現(xiàn)有2.5維視覺伺服控制方法不能處理運動執(zhí)行系統(tǒng)存在的速度和力矩約束以及攝像機的可見性約束問題，能夠確保參考目標在伺服過程中始終保持可見，大大提高視覺伺服系統(tǒng)的可靠性和安全性。
文檔編號G05B17/00GK102880062SQ20121032463
公開日2013年1月16日 申請日期2012年9月4日 優(yōu)先權日2012年9月4日
發(fā)明者曹政才, 殷龍杰, 付宜利, 王永吉 申請人:北京化工大學