一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法
【專利摘要】本發(fā)明公開(kāi)了一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法。首先，建立輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型，通過(guò)外部傳感器獲取目標(biāo)的相對(duì)位置，并建立虛擬跟蹤目標(biāo)。然后，設(shè)計(jì)線速度及角速度控制器，通過(guò)李雅普諾夫穩(wěn)定性理論及LaSalle不變?cè)碜C明本發(fā)明所設(shè)計(jì)的控制器能使得虛擬目標(biāo)軌跡收斂至實(shí)際目標(biāo)，代表輪式移動(dòng)機(jī)器人跟蹤上目標(biāo)。本發(fā)明所涉及的跟蹤控制方法能夠使得輪式移動(dòng)機(jī)器人系統(tǒng)漸近穩(wěn)定，機(jī)器人有效地跟蹤上目標(biāo)，仿真及實(shí)驗(yàn)結(jié)果驗(yàn)證了本發(fā)明所提出的控制方法的合理性。
【專利說(shuō)明】
一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及輪式移動(dòng)機(jī)器人伺服控制領(lǐng)域，尤其是一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī) 器人目標(biāo)跟蹤控制方法。
【背景技術(shù)】
[0002] 輪式移動(dòng)機(jī)器人在物料自動(dòng)搬運(yùn)、特殊人群服務(wù)、搶險(xiǎn)救災(zāi)，以及危險(xiǎn)地域探索等 方面的應(yīng)用具有不可比擬的優(yōu)勢(shì)，已廣泛地應(yīng)用于工業(yè)、農(nóng)業(yè)、服務(wù)業(yè)、國(guó)防和宇宙探索等 領(lǐng)域，對(duì)人類社會(huì)的生產(chǎn)和生活產(chǎn)生了積極而深遠(yuǎn)的影響。例如，美國(guó)國(guó)家宇航局研制的 "好奇號(hào)"火星探測(cè)車成功登陸火星，為人類探測(cè)外星生命打下堅(jiān)實(shí)的基礎(chǔ)。近年來(lái)，針對(duì)輪 式移動(dòng)機(jī)器人的控制研究產(chǎn)生諸多分支，其中一個(gè)重要的分支就是視覺(jué)伺服控制的研究。 隨著各類傳感器的發(fā)展，特別是視覺(jué)傳感器，為輪式移動(dòng)機(jī)器人視覺(jué)伺服控制研究提供了 更為廣闊的應(yīng)用空間。
[0003] 根據(jù)攝像機(jī)安裝位置的不同，機(jī)器人視覺(jué)伺服系統(tǒng)分為eye-to-hand(固定攝像 機(jī))系統(tǒng)及eye-in-hand (手眼)系統(tǒng)。eye-to-hand系統(tǒng)的攝像機(jī)安裝在機(jī)器人本體之外，通 過(guò)攝像機(jī)獲得目標(biāo)及機(jī)器人的控制位姿來(lái)控制機(jī)器人跟蹤目標(biāo)，此類系統(tǒng)不易出現(xiàn)目標(biāo)丟 失的現(xiàn)象，但機(jī)器人的運(yùn)動(dòng)容易造成目標(biāo)的遮擋。eye-in-hand系統(tǒng)的攝像機(jī)安裝于機(jī)器人 本體上，由機(jī)器人運(yùn)動(dòng)帶動(dòng)攝像機(jī)運(yùn)動(dòng)，此類系統(tǒng)對(duì)于防止目標(biāo)遮擋有較強(qiáng)的優(yōu)勢(shì)，但又容 易出現(xiàn)目標(biāo)丟失的情況。因此，如何更好地解決以上兩種問(wèn)題是輪式移動(dòng)機(jī)器人目標(biāo)跟蹤 控制研究的難點(diǎn)。
[0004] 綜上所述，研究如何令輪式移動(dòng)機(jī)器人在已知環(huán)境下，不出現(xiàn)目標(biāo)丟失和目標(biāo)遮 擋且快速有效地跟蹤上目標(biāo)，對(duì)于輪式移動(dòng)機(jī)器人的智能化、自主化，都具有重要的理論價(jià) 值和實(shí)際意義。

【發(fā)明內(nèi)容】

[0005] 發(fā)明目的：針對(duì)上述現(xiàn)有技術(shù)存在的缺陷，本發(fā)明旨在提供一種解決輪式移動(dòng)機(jī) 器人目標(biāo)跟蹤控制問(wèn)題的基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法。
[0006] 技術(shù)方案:一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，具體包括如下 步驟：
[0007] (1)對(duì)輪式移動(dòng)機(jī)器人進(jìn)行分析，建立輪式移動(dòng)機(jī)器人非完整運(yùn)動(dòng)學(xué)模型；
[0008] (2)利用單目攝像頭獲取目標(biāo)T對(duì)機(jī)器人本體的相對(duì)位置差p和相對(duì)姿態(tài)差a;
[0009] (3)建立虛擬跟蹤目標(biāo)I，并根據(jù)步驟(2)中所得的P和a，與步驟(1)中的運(yùn)動(dòng)學(xué)模 型結(jié)合，設(shè)計(jì)機(jī)器人的線速度v與角速度w ;
[0010] (4)將步驟(3)中設(shè)計(jì)的線速度v與角速度co代入李雅普諾夫函數(shù)，若李雅普諾夫 函數(shù)收斂至零，則驗(yàn)證了所設(shè)計(jì)的線速度v和角速度《使得系統(tǒng)漸近穩(wěn)定，且機(jī)器人已準(zhǔn)確 地跟蹤上目標(biāo)t若李雅普諾夫函數(shù)未收斂至零，則返回步驟(3)重新設(shè)計(jì)機(jī)器人的線速度v 與角速度《。
[0011] 進(jìn)一步的，步驟(1)中所述輪式移動(dòng)機(jī)器人非完整運(yùn)動(dòng)學(xué)模型具體為： Ix - v cos 9 y - v sin 0 9 ~ a>
[0013]其中（x，y)是機(jī)器人在世界坐標(biāo)系下的坐標(biāo)，0是機(jī)器人在世界坐標(biāo)系下的航向 角。
[0014] 進(jìn)一步的，步驟(2)中所述獲取目標(biāo);T對(duì)機(jī)器人本體的相對(duì)位置差P和相對(duì)姿態(tài)差 a具體為：
[0016]其中，（xb，yb)為目標(biāo)T在世界坐標(biāo)系下的坐標(biāo)，九)為目標(biāo)T在機(jī)器人本體笛 卡爾坐標(biāo)系下的坐標(biāo)且4 > &
[0017]進(jìn)一步的，步驟(3)中所述虛擬跟蹤目標(biāo)心在世界坐標(biāo)系下的坐標(biāo)為(x。，y。），滿 足： 剛[y:] = [y]+d=]
[0019]進(jìn)一步的，步驟⑶中所述機(jī)器人的線速度￥與角速度《為：
[0021 ]其中，Vmax、《 max分別為最大線速度和角速度，kv、ku G (〇，1 ]分別為線速度和角速 度控制增益，P^P分別為機(jī)器人本體與目標(biāo)之間的期望距離和實(shí)際距離，a為目標(biāo)在機(jī)器 人本體笛卡爾坐標(biāo)系下的偏向角。
[0022]進(jìn)一步的，在步驟(3)中，附加如下條件：
[0023] ku ? maxP* 多 kvVmax
[0024]進(jìn)一步的，步驟(4)中所述李雅普諾夫函數(shù)即：
[0025] V = - [(xc - Xf,)^ + (yc - yb)2] -
[0026]有益效果：本發(fā)明通過(guò)在輪式移動(dòng)機(jī)器人正前方P*處建立虛擬跟蹤目標(biāo)的方式， 將虛擬目標(biāo)巧?與目標(biāo):T:之間的誤差作為反饋控制量，控制虛擬目標(biāo)忑1的軌跡收斂到目標(biāo) T，解決了一類輪式移動(dòng)機(jī)器人目標(biāo)跟蹤問(wèn)題，相比于現(xiàn)有的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制 方法，本發(fā)明設(shè)計(jì)基于視覺(jué)的運(yùn)動(dòng)控制器結(jié)構(gòu)簡(jiǎn)單，硬件要求低，控制精度高，較好地解決 了目標(biāo)丟失等問(wèn)題。
【附圖說(shuō)明】
[0027]圖1是本發(fā)明中輪式移動(dòng)機(jī)器人運(yùn)動(dòng)學(xué)模型及其坐標(biāo)系示意圖；
[0028]圖2是本發(fā)明中輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制原理圖；
[0029]圖3是本發(fā)明中輪式移動(dòng)機(jī)器人跟蹤運(yùn)動(dòng)示意圖；
[0030]圖4是本發(fā)明中輪式移動(dòng)機(jī)器人運(yùn)動(dòng)控制器的速度控制量；
[0031]圖5是本發(fā)明中輪式移動(dòng)機(jī)器人運(yùn)動(dòng)控制器的角速度控制量；
[0032]圖6是本發(fā)明中輪式移動(dòng)機(jī)器人的實(shí)際距離曲線圖；
[0033] 圖7是本發(fā)明中輪式移動(dòng)機(jī)器人的偏差角曲線圖。
【具體實(shí)施方式】
[0034] 下面結(jié)合附圖對(duì)本發(fā)明作進(jìn)一步說(shuō)明：本發(fā)明適用于帶有單目攝像頭等外部傳感 器的輪式移動(dòng)機(jī)器人控制系統(tǒng)，其運(yùn)動(dòng)學(xué)模型及坐標(biāo)系建立如圖1所示。系統(tǒng)由目標(biāo)測(cè)距模 塊得到目標(biāo)偏差，與期望跟蹤距離結(jié)合設(shè)計(jì)運(yùn)動(dòng)控制器，控制輪式移動(dòng)機(jī)器人持續(xù)跟蹤目 標(biāo)。
[0035] 如圖2所示，一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，具體包括如下 步驟：
[0036] (1)對(duì)輪式移動(dòng)機(jī)器人進(jìn)行分析，建立輪式移動(dòng)機(jī)器人非完整運(yùn)動(dòng)學(xué)模型，具體 為： (X - V cos 0 y = v sin 0 0 - a)
[0038]其中（x，y)是機(jī)器人在世界坐標(biāo)系下的坐標(biāo)，0是機(jī)器人在世界坐標(biāo)系下的航向 角。
[0039] (2)利用單目攝像頭獲取目標(biāo)：T對(duì)機(jī)器人本體的相對(duì)位置差p和相對(duì)姿態(tài)差a，具體 為：
[0041 ]其中，（xb，yb)為目標(biāo)T:在世界坐標(biāo)系下的坐標(biāo)，(4,丸）為目標(biāo)T.在機(jī)器人本體笛 卡爾坐標(biāo)系下的坐標(biāo)且吞> 〇,則：
[0042] ^l = [cos0 Sin0ir^-^1 [yj i~sin9 cosG\bh-y\
[0043] (3)建立虛擬跟蹤目標(biāo);虛擬跟蹤目標(biāo)在世界坐標(biāo)系下的坐標(biāo)為（Xc，y c)，滿 足： \X{~\ X~\ 『COS0
[0044] / = cos〈 L^c-J Ly」 Lsm 0-
[0045]根據(jù)步驟(2)中所得的P和a，與步驟（1)中的運(yùn)動(dòng)學(xué)模型結(jié)合，設(shè)計(jì)機(jī)器人的線速 度v與角速度《為：
[0047]其中，vmax、《 max分別為最大線速度和角速度，kv、ku G (〇，1 ]分別為線速度和角速 度控制增益，pIPp分別為機(jī)器人本體與目標(biāo)之間的期望距離和實(shí)際距離，a為目標(biāo)在機(jī)器 人本體笛卡爾坐標(biāo)系下的偏向角。
[0048]為了確保機(jī)器人能快速有效地跟蹤上目標(biāo)T，還需附加如下條件：
[0049] ku 〇 maxP* 多 kvVmax
[0050] (4)將步驟(3)中設(shè)計(jì)的線速度v與角速度CO代入李雅普諾夫函數(shù)，BP:
[0052] 對(duì)其求導(dǎo)得：
[0053] V - (xL - x.b)(v cos 6 - p~a> sin 0) + (yc- - vb)(x? sin 9 + pxco cos 6)
[0054] 將控制器v、《代入f，得： V = -kJCx, - xb) cos 0 + (yc - yh) sin 9]2
[0055] -. /c2 [_(xL. _ h) sin 0 _ (yc - yfc) cos 0]2 + - xh)(p - xh) cos 0 + k^v,. - yB)(p - %)sina
[0056] 其中
均大于零且k2>ki。
[0057] 因_ .元fc) 2 〇.*若（xc-xb)cos9+(yc-yb)sin9<0,則坎 〇。'
[0058] 考慮(1。11))(3〇89+(5^-5^)8；[119>0情形，下面分兩種情況討論：
[0059]情形1:%20
[0060]由兔2 〇，可得p -知=Jib2 + fhZ -宏&幺fh，從而有： V < -/cjcos 6 (xL. - xb) + sin G {yc - yh)]2
[0061 ] - /cjsin 6 (xc - xb) - cos 9 (ye： - yb)]2 + k^x,. - xb)yh cos 9 + h(yc - y-n)% sin 9
[0062] 展開(kāi)整理得： V < -k1l(cos20 - sin 6 cos 6 + sin20)(xt. - xb)2
[0063] + (cos20 - sin20)(xc - xh)(yc - yb) + (cos20 + sin 0 cos 0 + sin26〇(yt.-凡)2]
[0064] 將不等式右邊整理為平方項(xiàng)和形式：
[0066] 當(dāng)飲=0時(shí)，有：
[0067] \}；c }，h ~ Un = 〇
[0068] 所以= 〇j|的最大不變集是{(xb，yb)}，根據(jù)LaSalle不變?cè)恚▁c，yc)的 軌跡會(huì)收斂到(xb，yb)。
[0069] 情形2:爲(wèi) < 0
[0070] 由為 <:〇，可得p: -為=J%2 + fb2 一％ S _&，從而有： V < -A：! [cos 6 {xc - + sin 6 {yc - yb)]2
[0071 ] - ^^[sin 9 (xc - xb) - cos 6 (v,. - y；,)]2 + k1{x(： - cos 6 ~ ^(yc - yb)yk sin 6
[0072] 展開(kāi)整理得： F < -ifejtCcos2^ + sin 9 cos 0 + sin20)(xc. - xb)2
[0073] 4- (sin20 - cos29)(xc - xb)(yc -- y^) + (cos2& - sin 6 cos 9 -f sin20)(yc- - ^)2]
[0074] 將不等式右邊整理為平方項(xiàng)和形式：
[0076] 當(dāng)玖=〇時(shí)，有：
[0077] 產(chǎn) "二 [xc - Xf, = 0
[0078] 所以= 0}的最大不變集是{(a，yb)}，根據(jù)LaSalle不變?cè)?拉薩爾不 變?cè)恚?，（x c，yc)的軌跡會(huì)收斂到(xb，yb)。
[0079] 因此，（x。，y。)的軌跡最終會(huì)收斂到(Xb，yb)，（x，y)的軌跡會(huì)收斂到以（ Xb，yb)為中 心、為半徑的圓環(huán)上。輪式移動(dòng)機(jī)器人最終會(huì)跟蹤上目標(biāo)且正對(duì)著目標(biāo):T，即：
[0080] 在控制器的作用下，李雅普諾夫函數(shù)最終收斂至零，證明所設(shè)計(jì)的線速度v和角速 度《能使得系統(tǒng)漸近穩(wěn)定，故可知虛擬跟蹤目標(biāo)?;的軌跡最終收斂至目標(biāo)T，代表輪式移動(dòng) 機(jī)器人最終跟蹤上目標(biāo):T。仿真實(shí)驗(yàn)結(jié)果如圖3-圖7所示。
[0081]綜合以上穩(wěn)定性分析，本發(fā)明設(shè)計(jì)的基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制 方法是穩(wěn)定的，且目標(biāo)跟蹤效果良好。
[0082]以上僅是本發(fā)明的優(yōu)選實(shí)施方式，應(yīng)當(dāng)指出：對(duì)于本技術(shù)領(lǐng)域的普通技術(shù)人員來(lái) 說(shuō)，在不脫離本發(fā)明原理的前提下，還可以做出若干改進(jìn)和潤(rùn)飾，這些改進(jìn)和潤(rùn)飾也應(yīng)視為 本發(fā)明的保護(hù)范圍。
【主權(quán)項(xiàng)】
1. 一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，其特征在于，具體包括如下 步驟： (1) 對(duì)輪式移動(dòng)機(jī)器人進(jìn)行分析，建立輪式移動(dòng)機(jī)器人非完整運(yùn)動(dòng)學(xué)模型； (2) 利用單目攝像頭獲取目標(biāo)y對(duì)機(jī)器人本體的相對(duì)位置差P和相對(duì)姿態(tài)差α; (3) 建立虛擬跟蹤目標(biāo)巧，并根據(jù)步驟(2)中所得的Ρ和α，與步驟（1)中的運(yùn)動(dòng)學(xué)模型結(jié) 合，設(shè)計(jì)機(jī)器人的線速度ν與角速度ω ; (4) 將步驟(3)中設(shè)計(jì)的線速度ν與角速度ω代入李雅普諾夫函數(shù)，若李雅普諾夫函數(shù) 收斂至零，則驗(yàn)證了所設(shè)計(jì)的線速度ν和角速度ω使得系統(tǒng)漸近穩(wěn)定，且機(jī)器人已準(zhǔn)確地跟 蹤上目標(biāo):Γ;若李雅普諾夫函數(shù)未收斂至零，則返回步驟(3)重新設(shè)計(jì)機(jī)器人的線速度ν與角 速度ω。2. 根據(jù)權(quán)利要求1所述的一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，其特 征在于，步驟(1)中所述輪式移動(dòng)機(jī)器人非完整運(yùn)動(dòng)學(xué)模型具體為：其中(x，y)是機(jī)器人在世界坐標(biāo)系下的坐標(biāo)，θ是機(jī)器人在世界坐標(biāo)系下的航向角。3. 根據(jù)權(quán)利要求1所述的一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，其特 征在于，步驟(2)中所述獲取目標(biāo):Γ對(duì)機(jī)器人本體的相對(duì)位置差P和相對(duì)姿態(tài)差α具體為：其中，U，yb)為目標(biāo)Γ在世界坐標(biāo)系下的坐標(biāo)，(?.?)為目標(biāo)『在機(jī)器人本體笛卡爾坐 標(biāo)系下的坐標(biāo)且％ 〇。4. 根據(jù)權(quán)利要求1所述的一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，其特 征在于，步驟(3)中所述虛擬跟蹤目標(biāo)3；在世界坐標(biāo)系下的坐標(biāo)為(X。， y。），滿足：5. 根據(jù)權(quán)利要求1所述的一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，其特 征在于，步驟(3)中所述機(jī)器人的線速度ν與角速度ω為：其中，vmax、〇max分別為最大線速度和角速度，kv、kue (〇，1]分別為線速度和角速度控制 增益，pIPp分別為機(jī)器人本體與目標(biāo)之間的期望距離和實(shí)際距離，α為目標(biāo)在機(jī)器人本體 笛卡爾坐標(biāo)系下的偏向角。6. 根據(jù)權(quán)利要求1所述的一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，其特 征在于，在步驟(3)中，附加如下條件： kcj ω maxp 多 kvVmax 〇7. 根據(jù)權(quán)利要求1所述的一種基于點(diǎn)鎮(zhèn)定的輪式移動(dòng)機(jī)器人目標(biāo)跟蹤控制方法，其特 征在于，步驟(4)中所述李雅普諾夫函數(shù)即：
【文檔編號(hào)】G05D1/12GK105929849SQ201610272043
【公開(kāi)日】2016年9月7日
【申請(qǐng)日】2016年4月28日
【發(fā)明人】翟軍勇, 肖大偉
【申請(qǐng)人】東南大學(xué)