專利名稱：基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法
技術(shù)領(lǐng)域：
本發(fā)明涉及智能機(jī)器人技術(shù)領(lǐng)域，具體地說(shuō)是指一種基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法。
背景技術(shù)：
近年來(lái)移動(dòng)小車在工業(yè)、民用、科學(xué)探索等多種行業(yè)和領(lǐng)域得到廣泛應(yīng)用。隨著計(jì)算機(jī)技術(shù)、微電子技術(shù)及人工智能等技術(shù)的發(fā)展，移動(dòng)小車在未知或時(shí)變環(huán)境下自主工作的可靠性、安全性和智能化水平不斷提高。視覺(jué)傳感器由于具有成本低、信息豐富、可靠性高等特點(diǎn)被廣泛應(yīng)用于移動(dòng)小車控制系統(tǒng)，而位姿鎮(zhèn)定是移動(dòng)小車運(yùn)動(dòng)控制最基本問(wèn)題，研究移動(dòng)小車視覺(jué)伺服鎮(zhèn)定問(wèn)題具有重要理論與應(yīng)用價(jià)值。移動(dòng)小車是一種受非完整約束限制的系統(tǒng)，任何連續(xù)時(shí)不變的狀態(tài)反饋控制方法都很難有效地解決其鎮(zhèn)定問(wèn)題；另外視覺(jué)信息中包含大量的數(shù)據(jù)，要從中獲得有用的信息，需要復(fù)雜的算法、耗費(fèi)大量的運(yùn)算時(shí)間。因此，基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定是一個(gè)十分具有挑戰(zhàn)性的課題，吸引眾多學(xué)者的研 究興趣。Lopez-Nicolas 等人在 2OlO 年 IEEE Transactions on Systems, Man, andCybernetics, Part B:Cybernetics, 40(4)期刊上利用當(dāng)前圖像和目標(biāo)圖像間的單應(yīng)性關(guān)系設(shè)計(jì)一種切換控制律，使小車沿著一些最佳的路線到達(dá)期望的位姿。Becerra等人在2011 年 IEEEInternational Conference on Robotics and Automation 會(huì)議上利用極線幾何對(duì)移動(dòng)小車的位姿進(jìn)行動(dòng)態(tài)估計(jì)，獲得其位置和方向。雖然這些方法能夠有效地解決基于視覺(jué)的位姿鎮(zhèn)定問(wèn)題，但是還存在以下主要不足(I)設(shè)計(jì)視覺(jué)控制器時(shí)都只考慮小車的運(yùn)動(dòng)學(xué)模型，把小車當(dāng)作一個(gè)理想的點(diǎn)來(lái)看待，未考慮最能反映小車本質(zhì)特性的動(dòng)力學(xué)模型；(2)伺服速度慢，視覺(jué)控制算法在設(shè)計(jì)時(shí)沒(méi)有和控制理論相結(jié)合，在實(shí)際應(yīng)用中很難獲得滿意的性能。
發(fā)明內(nèi)容
本發(fā)明的目的在于提供一種基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法，以解決上述方法在基于視覺(jué)的位姿鎮(zhèn)定控制時(shí)未考慮小車動(dòng)力學(xué)模型和伺服速度慢問(wèn)題。本發(fā)明方法利用Epipolar geometry, ID trifocal tensor及反演方法設(shè)計(jì)一種動(dòng)態(tài)切換控制律,使小車沿著最短路徑快速鎮(zhèn)定到期望位姿。一種基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法，包含以下步驟I. I.構(gòu)建基于視覺(jué)的移動(dòng)小車運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，攝像機(jī)模型；I. 2.根據(jù)步驟I. I中的攝像機(jī)模型，建立攝像機(jī)在起始位姿、運(yùn)動(dòng)過(guò)程中所處的位姿和期望位姿處拍攝到的圖像間的幾何約束關(guān)系；圖像間的幾何約束關(guān)系包括兩幅圖像間對(duì)極幾何關(guān)系和二幅圖像間二線性約束關(guān)系，分別用Epipolar geometry模型和ID trifocal tensor模型表示，其中C1= (x1； Z1, 0 j)、C2=(x2, z2, 0 2)和Cd= (0，0, 0)分別為攝像機(jī)的初始位姿、當(dāng)前位姿和期望位姿，eld, e2d，edl和e21為相應(yīng)圖像上的極點(diǎn)在圖像平面x軸方向上的坐標(biāo)，T1和T2為I階張量;I. 3.根據(jù)步驟I. I中的移動(dòng)小車運(yùn)動(dòng)學(xué)模型和步驟I. 2中的圖像間的幾何約束關(guān)系，采用切換控制策略設(shè)計(jì)數(shù)個(gè)獨(dú)立有序的運(yùn)動(dòng)學(xué)控制器；I. 4.根據(jù)步驟I. I中的移動(dòng)小車動(dòng)力學(xué)模型和步驟I. 3中設(shè)計(jì)的運(yùn)動(dòng)學(xué)控制器，利用反演方法設(shè)計(jì)三個(gè)獨(dú)立有序的動(dòng)力學(xué)控制器，完成整個(gè)視覺(jué)控制器的設(shè)計(jì)。
步驟I. 3中根據(jù)步驟I. 2中圖像間的幾何約束關(guān)系采用三步控制策略設(shè)計(jì)切換的運(yùn)動(dòng)學(xué)控制器，為動(dòng)力學(xué)控制器提供期望速度小車在原地旋轉(zhuǎn),期望線速度u dl為零，利用Epipolar geometry設(shè)計(jì)角速度控制器，其輸出作為動(dòng)力學(xué)控制器的輸入，為控制小車旋轉(zhuǎn)直至攝像機(jī)光軸指向期望位置提供所需的期望角速度《dl;當(dāng)攝像機(jī)光軸指向期望位置時(shí)，利用ID trifocal tensor和Epipolar geometry分別設(shè)計(jì)線速度控制器和角速度控制器，其輸出作為動(dòng)力學(xué)控制器的輸入，為控制小車做直線運(yùn)動(dòng)到達(dá)期望位置提供所需的期望線速度Ud2和期望角速度COd2;當(dāng)小車到達(dá)期望位置時(shí),期望線速度u d3為零，利用Epipolar geometry設(shè)計(jì)角速度控制器，其輸出作為動(dòng)力學(xué)控制器的輸入，為控制小車旋轉(zhuǎn)到期望方向提供所需的期望角速度《d3。步驟I. 4中利用反演方法設(shè)計(jì)的動(dòng)力學(xué)控制器將步驟I. 3中設(shè)計(jì)的運(yùn)動(dòng)學(xué)控制器所提供的期望速度作為輸入，用其輸出力矩來(lái)控制小車的速度，使其趨向于期望速度，從而控制小車沿著最短路徑鎮(zhèn)定到期望位姿首先，驅(qū)動(dòng)力矩為零，設(shè)計(jì)轉(zhuǎn)動(dòng)力矩控制器控制小車的角速度，使其趨向于期望的角速度《dl，從而控制小車旋轉(zhuǎn)使攝像機(jī)光軸方向與基線C1Cd重合；其次，當(dāng)攝像機(jī)光軸方向與基線C1Cd重合時(shí)，設(shè)計(jì)驅(qū)動(dòng)力矩控制器和轉(zhuǎn)動(dòng)力矩控制器分別控制小車的線速度和角速度，使其趨向于期望的線速度Ud2和角速度COd2，從而控制小車沿著基線C1Cd到達(dá)期望位置；最后，當(dāng)小車到達(dá)期望位置時(shí)，驅(qū)動(dòng)力矩為零，設(shè)計(jì)轉(zhuǎn)動(dòng)力矩控制器控制小車的角速度，使其趨向于期望的角速度《 d3，從而控制小車旋轉(zhuǎn)使其方向與期望的方向一致。本發(fā)明一種基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法，與現(xiàn)有技術(shù)相比，具有以下明顯的優(yōu)勢(shì)和有益效果本發(fā)明設(shè)計(jì)過(guò)程明確簡(jiǎn)單，魯棒性好，伺服速度快。本發(fā)明解決了現(xiàn)有視覺(jué)伺服方法在位姿鎮(zhèn)定時(shí)沒(méi)有考慮機(jī)器人本質(zhì)特性以及鎮(zhèn)定過(guò)程時(shí)間長(zhǎng)的問(wèn)題，大大提高視覺(jué)位姿鎮(zhèn)定的速度和精度。
圖I是本發(fā)明中的一個(gè)帶有單目攝像機(jī)的四輪差動(dòng)驅(qū)動(dòng)移動(dòng)小車模型示意圖；圖2是本發(fā)明中的三視角間的對(duì)極幾何關(guān)系示意圖；圖3是本發(fā)明中的攝像機(jī)相對(duì)位置關(guān)系示意圖；圖4是本發(fā)明中的移動(dòng)小車二步運(yùn)動(dòng)不意圖；圖5是本發(fā)明中的基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法流程圖6 13是本發(fā)明中的基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定仿真結(jié)果圖。
具體實(shí)施例方式為了更好地理解本發(fā)明的技術(shù)方案，以一個(gè)帶有單目攝像機(jī)的四輪差動(dòng)驅(qū)動(dòng)移動(dòng)小車為研究對(duì)象對(duì)本發(fā)明的實(shí)施方式做進(jìn)一步的介紹。I. I.構(gòu)建基于視覺(jué)的移動(dòng)小車運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，攝像機(jī)模型如圖I所示為四輪移動(dòng)小車模型，前兩輪從動(dòng)，后兩輪差動(dòng)驅(qū)動(dòng)，單目攝像機(jī)固定安裝在小車中心處且其光軸指向小車運(yùn)動(dòng)方向。O-XYZ為世界坐標(biāo)系，C-Xcjc^為攝像機(jī)坐標(biāo)系，原點(diǎn)C與小車質(zhì)心O重合。該小車的位姿由向量q=[x, z, 0]T表示，(x，z)為小車質(zhì)心O的坐標(biāo)，0為車身方向與Z軸夾角，L為后兩輪中心的距離為輪子半徑，u和o分別為小車的線速度和角速度。根據(jù)圖I所定義的坐標(biāo)系，在不考慮輪子側(cè)滑等因素時(shí)，移動(dòng)小車的運(yùn)動(dòng)學(xué)模型可表示為X = -U sin 6^ Z -V cos 0(I)
9 — co移動(dòng)小車非完整約束條件為i cos 0 + z sin (9 = 0(2)米用拉格朗日建模方法，移動(dòng)小車的動(dòng)力學(xué)模型可表不為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乘子。在不考慮阻力矩時(shí)，式(3)中的變量定義如下
~m 0 Ol「0 0 0]
, x =[ x 1； t2]t
r
L -L其中m為小車質(zhì)量，I為小車相對(duì)于質(zhì)心O的轉(zhuǎn)動(dòng)慣量，T^t2分別為后兩輪左右輪的輸出力矩。式⑶變?yōu)镸{q)q = N{q)r + ,4,' (q)A(4)對(duì)式(I)進(jìn)行微分后代入式(4)，并考慮式(2)，則有
m Oir{>1 I Tl I ![r, I…=-(5)
0I o) r L -L tr _定義Td =-(1, +T2),Tt =-(h -A)分別為小車后兩輪的驅(qū)動(dòng)力矩和轉(zhuǎn)動(dòng)力矩，式
rr
(5)變?yōu)?br>
權(quán)利要求
1. 一種基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法，其特征在于包含以下步驟 1.1.構(gòu)建基于視覺(jué)的移動(dòng)小車運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型，攝像機(jī)模型； I. 2.根據(jù)步驟I. I中的攝像機(jī)模型，建立攝像機(jī)在起始位姿、運(yùn)動(dòng)過(guò)程中所處的位姿和期望位姿處拍攝到的圖像間的幾何約束關(guān)系； 所述的圖像間的幾何約束關(guān)系包括兩幅圖像間對(duì)極幾何關(guān)系和三幅圖像間三線性約束關(guān)系，分別用Epipolar geometry模型和ID trifocal tensor模型表示，其中C1= (x1； Z1, Θ j)、C2=(x2, ζ2, Θ 2)和Cd= (O, O, O)分別為攝像機(jī)的初始位姿、當(dāng)前位姿和期望位姿，eld, e2d，edl和e21為相應(yīng)圖像上的極點(diǎn)在圖像平面x軸方向上的坐標(biāo)，T1和T2為I階張量; I. 3.根據(jù)步驟I. I中的移動(dòng)小車運(yùn)動(dòng)學(xué)模型和步驟I. 2中的圖像間的幾何約束關(guān)系，采用切換控制策略設(shè)計(jì)數(shù)個(gè)獨(dú)立有序的運(yùn)動(dòng)學(xué)控制器； 1.4.根據(jù)步驟I. I中的移動(dòng)小車動(dòng)力學(xué)模型和步驟I. 3中設(shè)計(jì)的運(yùn)動(dòng)學(xué)控制器，利用反演方法設(shè)計(jì)三個(gè)獨(dú)立有序的動(dòng)力學(xué)控制器，完成整個(gè)視覺(jué)控制器的設(shè)計(jì)。
2.根據(jù)權(quán)利要求I所述的基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法，其特征在于步驟I. 3中根據(jù)步驟I. 2中圖像間的幾何約束關(guān)系采用三步控制策略設(shè)計(jì)切換的運(yùn)動(dòng)學(xué)控制器，為動(dòng)力學(xué)控制器提供期望速度 小車在原地旋轉(zhuǎn)，期望線速度U dl為零，利用Epipolar geometry設(shè)計(jì)角速度控制器，其輸出作為動(dòng)力學(xué)控制器的輸入，為控制小車旋轉(zhuǎn)直至攝像機(jī)光軸指向期望位置提供所需的期望角速度ω 1 當(dāng)攝像機(jī)光軸指向期望位置時(shí)，利用ID trifocal tensor和Epipolar geometry分別設(shè)計(jì)線速度控制器和角速度控制器，其輸出作為動(dòng)力學(xué)控制器的輸入，為控制小車做直線運(yùn)動(dòng)到達(dá)期望位置提供所需的期望線速度ud2和期望角速度cod2; 當(dāng)小車到達(dá)期望位置時(shí)，期望線速度U d3為零，利用Epipolar geometry設(shè)計(jì)角速度控制器，其輸出作為動(dòng)力學(xué)控制器的輸入，為控制小車旋轉(zhuǎn)到期望方向提供所需的期望角速度 ω<13。
3.根據(jù)權(quán)利要求I所述的基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法，其特征在于步驟I.4中利用反演方法設(shè)計(jì)的動(dòng)力學(xué)控制器將步驟I. 3中設(shè)計(jì)的運(yùn)動(dòng)學(xué)控制器所提供的期望速度作為輸入，用其輸出力矩來(lái)控制小車的速度，使其趨向于期望速度，從而控制小車沿著最短路徑鎮(zhèn)定到期望位姿 首先，驅(qū)動(dòng)力矩為零，設(shè)計(jì)轉(zhuǎn)動(dòng)力矩控制器控制小車的角速度，使其趨向于期望的角速度《dl，從而控制小車旋轉(zhuǎn)使攝像機(jī)光軸方向與基線C1Cd重合； 其次，當(dāng)攝像機(jī)光軸方向與基線C1Cd重合時(shí)，設(shè)計(jì)驅(qū)動(dòng)力矩控制器和轉(zhuǎn)動(dòng)力矩控制器分別控制小車的線速度和角速度，使其趨向于期望的線速度Ud2和角速度COd2，從而控制小車沿著基線C1Cd到達(dá)期望位置； 最后，當(dāng)小車到達(dá)期望位置時(shí)，驅(qū)動(dòng)力矩為零，設(shè)計(jì)轉(zhuǎn)動(dòng)力矩控制器控制小車的角速度，使其趨向于期望的角速度《d3，從而控制小車旋轉(zhuǎn)使其方向與期望的方向一致。
全文摘要
本發(fā)明公開了一種基于視覺(jué)的移動(dòng)小車位姿鎮(zhèn)定控制方法，充分考慮小車的運(yùn)動(dòng)學(xué)模型和動(dòng)力學(xué)模型以及攝像機(jī)模型，在起始位姿和期望位姿處通過(guò)攝像機(jī)分別獲取初始圖像和期望圖像，并在運(yùn)動(dòng)過(guò)程中實(shí)時(shí)獲取當(dāng)前圖像；然后利用拍攝到的圖像間的對(duì)極幾何關(guān)系和三線性約束關(guān)系，基于Epipolar geometry和1D trifocal tensor采用三步切換控制策略設(shè)計(jì)三個(gè)獨(dú)立有序的運(yùn)動(dòng)學(xué)控制器；最后將運(yùn)動(dòng)學(xué)控制器的輸出作為動(dòng)力學(xué)控制器的輸入利用反演方法設(shè)計(jì)一種動(dòng)態(tài)切換控制律，使小車沿著最短路徑快速鎮(zhèn)定到期望位姿。本發(fā)明解決傳統(tǒng)視覺(jué)伺服方法在位姿鎮(zhèn)定控制時(shí)未考慮小車動(dòng)力學(xué)特性以及伺服速度慢問(wèn)題，更切合實(shí)際，能夠使小車更快地鎮(zhèn)定到期望位姿。
文檔編號(hào)G05D1/02GK102736626SQ20121014696
公開日2012年10月17日 申請(qǐng)日期2012年5月11日 優(yōu)先權(quán)日2012年5月11日
發(fā)明者付宜利, 曹政才, 殷龍杰, 靳保 申請(qǐng)人:北京化工大學(xué)