專利名稱：基于視覺的移動小車位姿鎮(zhèn)定控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及智能機器人技術(shù)領(lǐng)域，具體地說是指一種基于視覺的移動小車位姿鎮(zhèn)定控制方法。
背景技術(shù)：
近年來移動小車在工業(yè)、民用、科學探索等多種行業(yè)和領(lǐng)域得到廣泛應用。隨著計算機技術(shù)、微電子技術(shù)及人工智能等技術(shù)的發(fā)展，移動小車在未知或時變環(huán)境下自主工作的可靠性、安全性和智能化水平不斷提高。視覺傳感器由于具有成本低、信息豐富、可靠性高等特點被廣泛應用于移動小車控制系統(tǒng)，而位姿鎮(zhèn)定是移動小車運動控制最基本問題，研究移動小車視覺伺服鎮(zhèn)定問題具有重要理論與應用價值。移動小車是一種受非完整約束限制的系統(tǒng)，任何連續(xù)時不變的狀態(tài)反饋控制方法都很難有效地解決其鎮(zhèn)定問題；另外視覺信息中包含大量的數(shù)據(jù)，要從中獲得有用的信息，需要復雜的算法、耗費大量的運算時間。因此，基于視覺的移動小車位姿鎮(zhèn)定是一個十分具有挑戰(zhàn)性的課題，吸引眾多學者的研 究興趣。Lopez-Nicolas 等人在 2OlO 年 IEEE Transactions on Systems, Man, andCybernetics, Part B:Cybernetics, 40(4)期刊上利用當前圖像和目標圖像間的單應性關(guān)系設(shè)計一種切換控制律，使小車沿著一些最佳的路線到達期望的位姿。Becerra等人在2011 年 IEEEInternational Conference on Robotics and Automation 會議上利用極線幾何對移動小車的位姿進行動態(tài)估計，獲得其位置和方向。雖然這些方法能夠有效地解決基于視覺的位姿鎮(zhèn)定問題，但是還存在以下主要不足(I)設(shè)計視覺控制器時都只考慮小車的運動學模型，把小車當作一個理想的點來看待，未考慮最能反映小車本質(zhì)特性的動力學模型；(2)伺服速度慢，視覺控制算法在設(shè)計時沒有和控制理論相結(jié)合，在實際應用中很難獲得滿意的性能。

發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種基于視覺的移動小車位姿鎮(zhèn)定控制方法，以解決上述方法在基于視覺的位姿鎮(zhèn)定控制時未考慮小車動力學模型和伺服速度慢問題。本發(fā)明方法利用Epipolar geometry, ID trifocal tensor及反演方法設(shè)計一種動態(tài)切換控制律,使小車沿著最短路徑快速鎮(zhèn)定到期望位姿。一種基于視覺的移動小車位姿鎮(zhèn)定控制方法，包含以下步驟I. I.構(gòu)建基于視覺的移動小車運動學模型和動力學模型，攝像機模型；I. 2.根據(jù)步驟I. I中的攝像機模型，建立攝像機在起始位姿、運動過程中所處的位姿和期望位姿處拍攝到的圖像間的幾何約束關(guān)系；圖像間的幾何約束關(guān)系包括兩幅圖像間對極幾何關(guān)系和二幅圖像間二線性約束關(guān)系，分別用Epipolar geometry模型和ID trifocal tensor模型表示，其中C1= (x1； Z1, 0 j)、C2=(x2, z2, 0 2)和Cd= (0，0, 0)分別為攝像機的初始位姿、當前位姿和期望位姿，eld, e2d，edl和e21為相應圖像上的極點在圖像平面x軸方向上的坐標，T1和T2為I階張量;I. 3.根據(jù)步驟I. I中的移動小車運動學模型和步驟I. 2中的圖像間的幾何約束關(guān)系，采用切換控制策略設(shè)計數(shù)個獨立有序的運動學控制器；I. 4.根據(jù)步驟I. I中的移動小車動力學模型和步驟I. 3中設(shè)計的運動學控制器，利用反演方法設(shè)計三個獨立有序的動力學控制器，完成整個視覺控制器的設(shè)計。
步驟I. 3中根據(jù)步驟I. 2中圖像間的幾何約束關(guān)系采用三步控制策略設(shè)計切換的運動學控制器，為動力學控制器提供期望速度小車在原地旋轉(zhuǎn),期望線速度u dl為零，利用Epipolar geometry設(shè)計角速度控制器，其輸出作為動力學控制器的輸入，為控制小車旋轉(zhuǎn)直至攝像機光軸指向期望位置提供所需的期望角速度《dl;當攝像機光軸指向期望位置時，利用ID trifocal tensor和Epipolar geometry分別設(shè)計線速度控制器和角速度控制器，其輸出作為動力學控制器的輸入，為控制小車做直線運動到達期望位置提供所需的期望線速度Ud2和期望角速度COd2;當小車到達期望位置時,期望線速度u d3為零，利用Epipolar geometry設(shè)計角速度控制器，其輸出作為動力學控制器的輸入，為控制小車旋轉(zhuǎn)到期望方向提供所需的期望角速度《d3。步驟I. 4中利用反演方法設(shè)計的動力學控制器將步驟I. 3中設(shè)計的運動學控制器所提供的期望速度作為輸入，用其輸出力矩來控制小車的速度，使其趨向于期望速度，從而控制小車沿著最短路徑鎮(zhèn)定到期望位姿首先，驅(qū)動力矩為零，設(shè)計轉(zhuǎn)動力矩控制器控制小車的角速度，使其趨向于期望的角速度《dl，從而控制小車旋轉(zhuǎn)使攝像機光軸方向與基線C1Cd重合；其次，當攝像機光軸方向與基線C1Cd重合時，設(shè)計驅(qū)動力矩控制器和轉(zhuǎn)動力矩控制器分別控制小車的線速度和角速度，使其趨向于期望的線速度Ud2和角速度COd2，從而控制小車沿著基線C1Cd到達期望位置；最后，當小車到達期望位置時，驅(qū)動力矩為零，設(shè)計轉(zhuǎn)動力矩控制器控制小車的角速度，使其趨向于期望的角速度《 d3，從而控制小車旋轉(zhuǎn)使其方向與期望的方向一致。本發(fā)明一種基于視覺的移動小車位姿鎮(zhèn)定控制方法，與現(xiàn)有技術(shù)相比，具有以下明顯的優(yōu)勢和有益效果本發(fā)明設(shè)計過程明確簡單，魯棒性好，伺服速度快。本發(fā)明解決了現(xiàn)有視覺伺服方法在位姿鎮(zhèn)定時沒有考慮機器人本質(zhì)特性以及鎮(zhèn)定過程時間長的問題，大大提高視覺位姿鎮(zhèn)定的速度和精度。


圖I是本發(fā)明中的一個帶有單目攝像機的四輪差動驅(qū)動移動小車模型示意圖；圖2是本發(fā)明中的三視角間的對極幾何關(guān)系示意圖；圖3是本發(fā)明中的攝像機相對位置關(guān)系示意圖；圖4是本發(fā)明中的移動小車二步運動不意圖；圖5是本發(fā)明中的基于視覺的移動小車位姿鎮(zhèn)定控制方法流程圖6 13是本發(fā)明中的基于視覺的移動小車位姿鎮(zhèn)定仿真結(jié)果圖。
具體實施例方式為了更好地理解本發(fā)明的技術(shù)方案，以一個帶有單目攝像機的四輪差動驅(qū)動移動小車為研究對象對本發(fā)明的實施方式做進一步的介紹。I. I.構(gòu)建基于視覺的移動小車運動學模型和動力學模型，攝像機模型如圖I所示為四輪移動小車模型，前兩輪從動，后兩輪差動驅(qū)動，單目攝像機固定安裝在小車中心處且其光軸指向小車運動方向。O-XYZ為世界坐標系，C-Xcjc^為攝像機坐標系，原點C與小車質(zhì)心O重合。該小車的位姿由向量q=[x, z, 0]T表示，(x，z)為小車質(zhì)心O的坐標，0為車身方向與Z軸夾角，L為后兩輪中心的距離為輪子半徑，u和o分別為小車的線速度和角速度。根據(jù)圖I所定義的坐標系，在不考慮輪子側(cè)滑等因素時，移動小車的運動學模型可表示為X = -U sin 6^ Z -V cos 0(I)
9 — co移動小車非完整約束條件為i cos 0 + z sin (9 = 0(2)米用拉格朗日建模方法，移動小車的動力學模型可表不為M{q)q + Vm (q,i[)q + C,Uj) = Nunr + AT (q)A(3)其中M是正定慣性矩陣,1(94)是向心力和哥式力矩陣，G(q)是重力向量矩陣，N(q)是輸入轉(zhuǎn)換矩陣，T是小車轉(zhuǎn)矩，\是Lagrange乘子。在不考慮阻力矩時，式(3)中的變量定義如下
~m 0 Ol「0 0 0]
, x =[ x 1； t2]t
r
L -L其中m為小車質(zhì)量，I為小車相對于質(zhì)心O的轉(zhuǎn)動慣量，T^t2分別為后兩輪左右輪的輸出力矩。式⑶變?yōu)镸{q)q = N{q)r + ,4,' (q)A(4)對式(I)進行微分后代入式(4)，并考慮式(2)，則有
m Oir{>1 I Tl I ![r, I…=-(5)
0I o) r L -L tr _定義Td =-(1, +T2),Tt =-(h -A)分別為小車后兩輪的驅(qū)動力矩和轉(zhuǎn)動力矩，式
rr
(5)變?yōu)?br> 權(quán)利要求
1. 一種基于視覺的移動小車位姿鎮(zhèn)定控制方法，其特征在于包含以下步驟 1.1.構(gòu)建基于視覺的移動小車運動學模型和動力學模型，攝像機模型； I. 2.根據(jù)步驟I. I中的攝像機模型，建立攝像機在起始位姿、運動過程中所處的位姿和期望位姿處拍攝到的圖像間的幾何約束關(guān)系； 所述的圖像間的幾何約束關(guān)系包括兩幅圖像間對極幾何關(guān)系和三幅圖像間三線性約束關(guān)系，分別用Epipolar geometry模型和ID trifocal tensor模型表示，其中C1= (x1； Z1, Θ j)、C2=(x2, ζ2, Θ 2)和Cd= (O, O, O)分別為攝像機的初始位姿、當前位姿和期望位姿，eld, e2d，edl和e21為相應圖像上的極點在圖像平面x軸方向上的坐標，T1和T2為I階張量; I. 3.根據(jù)步驟I. I中的移動小車運動學模型和步驟I. 2中的圖像間的幾何約束關(guān)系，采用切換控制策略設(shè)計數(shù)個獨立有序的運動學控制器； 1.4.根據(jù)步驟I. I中的移動小車動力學模型和步驟I. 3中設(shè)計的運動學控制器，利用反演方法設(shè)計三個獨立有序的動力學控制器，完成整個視覺控制器的設(shè)計。
2.根據(jù)權(quán)利要求I所述的基于視覺的移動小車位姿鎮(zhèn)定控制方法，其特征在于步驟I. 3中根據(jù)步驟I. 2中圖像間的幾何約束關(guān)系采用三步控制策略設(shè)計切換的運動學控制器，為動力學控制器提供期望速度 小車在原地旋轉(zhuǎn)，期望線速度U dl為零，利用Epipolar geometry設(shè)計角速度控制器，其輸出作為動力學控制器的輸入，為控制小車旋轉(zhuǎn)直至攝像機光軸指向期望位置提供所需的期望角速度ω 1 當攝像機光軸指向期望位置時，利用ID trifocal tensor和Epipolar geometry分別設(shè)計線速度控制器和角速度控制器，其輸出作為動力學控制器的輸入，為控制小車做直線運動到達期望位置提供所需的期望線速度ud2和期望角速度cod2; 當小車到達期望位置時，期望線速度U d3為零，利用Epipolar geometry設(shè)計角速度控制器，其輸出作為動力學控制器的輸入，為控制小車旋轉(zhuǎn)到期望方向提供所需的期望角速度 ω<13。
3.根據(jù)權(quán)利要求I所述的基于視覺的移動小車位姿鎮(zhèn)定控制方法，其特征在于步驟I.4中利用反演方法設(shè)計的動力學控制器將步驟I. 3中設(shè)計的運動學控制器所提供的期望速度作為輸入，用其輸出力矩來控制小車的速度，使其趨向于期望速度，從而控制小車沿著最短路徑鎮(zhèn)定到期望位姿 首先，驅(qū)動力矩為零，設(shè)計轉(zhuǎn)動力矩控制器控制小車的角速度，使其趨向于期望的角速度《dl，從而控制小車旋轉(zhuǎn)使攝像機光軸方向與基線C1Cd重合； 其次，當攝像機光軸方向與基線C1Cd重合時，設(shè)計驅(qū)動力矩控制器和轉(zhuǎn)動力矩控制器分別控制小車的線速度和角速度，使其趨向于期望的線速度Ud2和角速度COd2，從而控制小車沿著基線C1Cd到達期望位置； 最后，當小車到達期望位置時，驅(qū)動力矩為零，設(shè)計轉(zhuǎn)動力矩控制器控制小車的角速度，使其趨向于期望的角速度《d3，從而控制小車旋轉(zhuǎn)使其方向與期望的方向一致。
全文摘要
本發(fā)明公開了一種基于視覺的移動小車位姿鎮(zhèn)定控制方法，充分考慮小車的運動學模型和動力學模型以及攝像機模型，在起始位姿和期望位姿處通過攝像機分別獲取初始圖像和期望圖像，并在運動過程中實時獲取當前圖像；然后利用拍攝到的圖像間的對極幾何關(guān)系和三線性約束關(guān)系，基于Epipolar geometry和1D trifocal tensor采用三步切換控制策略設(shè)計三個獨立有序的運動學控制器；最后將運動學控制器的輸出作為動力學控制器的輸入利用反演方法設(shè)計一種動態(tài)切換控制律，使小車沿著最短路徑快速鎮(zhèn)定到期望位姿。本發(fā)明解決傳統(tǒng)視覺伺服方法在位姿鎮(zhèn)定控制時未考慮小車動力學特性以及伺服速度慢問題，更切合實際，能夠使小車更快地鎮(zhèn)定到期望位姿。
文檔編號G05D1/02GK102736626SQ20121014696
公開日2012年10月17日 申請日期2012年5月11日 優(yōu)先權(quán)日2012年5月11日
發(fā)明者付宜利, 曹政才, 殷龍杰, 靳保 申請人:北京化工大學