基于慣性測(cè)量單元的輪式機(jī)器人軌跡跟蹤仿真

孫正鳳，張 朋，劉小軍

(南京理工大學(xué)泰州科技學(xué)院，江蘇 泰州 225300)

1 引言

在航空航天、國防和空間探測(cè)等領(lǐng)域，輪式機(jī)器人因其具備速度快、效率高及可拓展性強(qiáng)等特點(diǎn)在以上領(lǐng)域得到了廣泛的應(yīng)用。輪式機(jī)器人工作中，其軌跡跟蹤問題是其任務(wù)可否完成的重要基礎(chǔ)。輪式機(jī)器人屬于一種非線性耦合性的運(yùn)動(dòng)目標(biāo)，由于實(shí)際工作環(huán)境的復(fù)雜性，對(duì)其進(jìn)行軌跡跟蹤過程中存在眾多難點(diǎn)，國內(nèi)相關(guān)學(xué)者對(duì)此進(jìn)行了大量的研究，并得出了一些較好的研究成果。文獻(xiàn)[1]提出了一種基于預(yù)測(cè)迭代學(xué)習(xí)的軌跡跟蹤方法。該方法首先設(shè)計(jì)具備反饋?zhàn)饔玫膶W(xué)習(xí)機(jī)模型，采用該模型來調(diào)整輪式移動(dòng)機(jī)器人移動(dòng)軌跡跟蹤輸出信息的誤差，將調(diào)整結(jié)果輸入到反饋控制器中從而完成軌跡跟蹤。該方法能夠有效消除所在環(huán)境外界擾動(dòng)的影響，但對(duì)其所處的工作環(huán)境的適應(yīng)性較差，存在軌跡跟蹤誤差較大的問題，實(shí)際應(yīng)用效果并不理想。文獻(xiàn)[2]為了使輪式移動(dòng)機(jī)器人的軌跡跟蹤更為精準(zhǔn)，建立了軌跡跟蹤動(dòng)態(tài)誤差模型?；诜€(wěn)定性理論，提出一種信息反饋狀態(tài)，并通過Lyapunov函數(shù)穩(wěn)定性控制，來提高輪式機(jī)器人軌跡跟蹤精準(zhǔn)度。但由于模型參數(shù)的不確定性會(huì)對(duì)軌跡跟蹤結(jié)果產(chǎn)生影響，導(dǎo)致其系統(tǒng)的跟蹤精度降低。文獻(xiàn)[3]為了加強(qiáng)輪式機(jī)器人的軌跡跟蹤效果，在輪式機(jī)器人運(yùn)動(dòng)學(xué)模型上設(shè)計(jì)了自適應(yīng)滑模控制機(jī)制，在此機(jī)制激勵(lì)下建立了一種傾斜參數(shù)的自適應(yīng)函數(shù)；基于自適應(yīng)函數(shù)提出了對(duì)等控制并改變控制器的角速度，將機(jī)器人的誤差以及軌跡辨識(shí)階段的不確定參數(shù)反饋到控制器中，計(jì)算出左右驅(qū)動(dòng)的期望角速度。但由于機(jī)器人工作環(huán)境的復(fù)雜性導(dǎo)致在反饋的信息中存在不穩(wěn)定因素，這種因素的存在導(dǎo)致輪式機(jī)器人的軌跡跟蹤精度還有待提高，因此難以應(yīng)用到實(shí)際生活中。

為了有效解決在復(fù)雜應(yīng)用環(huán)境下的輪式機(jī)器人軌跡跟蹤的質(zhì)量和精準(zhǔn)度問題，本文針對(duì)輪式機(jī)器人慣性測(cè)量單元進(jìn)行了深入研究，提出一種基于慣性測(cè)量單元的輪式機(jī)器人軌跡跟蹤方案。首先對(duì)輪式移動(dòng)機(jī)器人的軌跡建立位姿和空間坐標(biāo)，其次利用慣性測(cè)量單元的控制系統(tǒng)通過運(yùn)算公式來完成輪式機(jī)器人的軌跡跟蹤，最后在自主的輪式機(jī)器人平臺(tái)上進(jìn)行實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，本文提出的軌跡跟蹤方法可有效提高軌跡跟蹤精度，驗(yàn)證了本文方法的有效性。

2 輪式機(jī)器人運(yùn)動(dòng)學(xué)模型

2.1 輪式機(jī)器人運(yùn)動(dòng)學(xué)關(guān)系

假設(shè)，分別用L(X，Y，Z)，R(x，y，z)表示空間坐標(biāo)系與輪式機(jī)器人坐標(biāo)系，從空間角度來看，空間坐標(biāo)系采用東-北-天坐標(biāo)系，即(ENU)。采用歐拉角描述不同坐標(biāo)系之間的轉(zhuǎn)換關(guān)系，設(shè)Θ=[φ，θ，ψ]T表示輪式機(jī)器人坐標(biāo)系相對(duì)于空間坐標(biāo)系的位姿角，φ表示空間坐標(biāo)東的滾角(繞X軸)，使用θ表示坐標(biāo)北(繞Y軸)，ψ表示空間坐標(biāo)系的位姿角(繞Z軸)。為了表述輪式機(jī)器人的軌跡跟蹤性質(zhì)，將空間坐標(biāo)系的圖表用斜視圖代替。由Z-Y-X歐拉角[4]的移動(dòng)順序可以獲得輪式機(jī)器人的空間位姿，使用H和W來分別表示輪式機(jī)器人前后輪心距和左右輪心距，在坐標(biāo)系R中，將輪式機(jī)器人的重心軌跡跟蹤速度設(shè)定為vG，輪式機(jī)器人前后輪轉(zhuǎn)動(dòng)角速度為wRz，結(jié)合跟蹤速度vG和轉(zhuǎn)動(dòng)角速度wRz，可將目標(biāo)機(jī)器人左右輪心線速度[5]在x軸方向描述為

(1)

其中，ICRG、ICRl、ICRr分別表示機(jī)器人本體、機(jī)器人左側(cè)車輪落地和右側(cè)車輪落地的瞬心速度，將點(diǎn)R在坐標(biāo)上表達(dá)為(xGc，yGc，0)、(xlc，ylc，0)、(xrc，yrc，0)，這三個(gè)點(diǎn)位在y軸的水平方向，即

(2)

設(shè)定目標(biāo)機(jī)器人四向輪的縱向滑移率為λi，則有

(3)

式中：r是目標(biāo)機(jī)器人車輪半徑，Δvix=vix-rωi。對(duì)于左右兩邊的機(jī)器人車輪來說有λl=λ1=λ2，λr=λ3=λ4?？紤]到輪式機(jī)器人的實(shí)際移動(dòng)環(huán)境，取λ∈[-1，1]。λ∈[-1，0]表示目標(biāo)機(jī)器人車輪保持制動(dòng)狀態(tài)，λ∈[0，1]表示目標(biāo)機(jī)器人車輪保持驅(qū)動(dòng)狀態(tài)。假設(shè)，Δvi表示輪式機(jī)器人車輪接觸到地面時(shí)的滾動(dòng)速度，根據(jù)輪式機(jī)器人速度瞬心和轉(zhuǎn)動(dòng)角速度可得

(4)

當(dāng)輪式機(jī)器人在進(jìn)行逆向運(yùn)動(dòng)時(shí)，滿足Δv1x=Δv2x≥0，且Δv3x=Δv4x≤0，左體車輪處于靜止?fàn)顟B(tài)時(shí)，右側(cè)位置處于緊急制動(dòng)狀態(tài)。

2.2 慣性測(cè)量單元運(yùn)動(dòng)學(xué)關(guān)系

在上述分析輪式機(jī)器人運(yùn)動(dòng)學(xué)關(guān)系的基礎(chǔ)上，需要進(jìn)一步分析慣性測(cè)量單元運(yùn)動(dòng)學(xué)關(guān)系。使用(xR1，yR1，0)、(xR2，yR2，0)分別描述慣性測(cè)量單元IMU1、慣性測(cè)量單元IMU2在空間坐標(biāo)系R上的坐標(biāo)。

因?yàn)閮蓚€(gè)慣性測(cè)量單元連接在輪式機(jī)器人實(shí)體上，因此在輪式機(jī)器人移動(dòng)的過程中，兩個(gè)慣性測(cè)量單元的速度瞬心與輪式機(jī)器人重心G的速度瞬心重疊，因此考慮到輪式機(jī)器人的移動(dòng)過程中存在加速和減速度的情況，慣性測(cè)量單元測(cè)量出的x、y平面加速量Ah1、Ah2包含了向心加速度和切向加速度。假設(shè)，向心加速度為ap1、ap2；切向加速度為aq1，aq2，設(shè)速度瞬心ICRG到慣性測(cè)量單元IMU1的長度為r1，速度瞬心到慣性測(cè)量單元IMU2的長度為r2，則有

(5)

(6)

(7)

當(dāng)輪式機(jī)器人在向左旋轉(zhuǎn)且xR1>xGc>xR2時(shí)，結(jié)合1.1節(jié)所描述的輪式機(jī)器人運(yùn)動(dòng)學(xué)關(guān)系，采用式(7)表示具有非完整性約束的輪式機(jī)器人運(yùn)動(dòng)學(xué)特性：

(8)

式中：v和ω分別表示輪式機(jī)器人的水平速度和輪式機(jī)器人的車輪轉(zhuǎn)動(dòng)速度，均為控制量；(x，y)是運(yùn)動(dòng)平面上輪式機(jī)器人在笛卡爾空間[6]坐標(biāo)中的坐標(biāo)位置，θ是輪式機(jī)器人對(duì)于x軸的姿態(tài)角。

假設(shè)，輪式機(jī)器人的車輪不存在打滑現(xiàn)象，其移動(dòng)軌跡約束項(xiàng)可描述為

(9)

相對(duì)于任意制定的幾何軌跡f(x，y)=0，設(shè)軌跡跟蹤的信息控制反饋為U=(v，ω)T=(v(s)，ω(s))T。此公式使得輪式機(jī)器人可以在指定的幾何軌跡上運(yùn)動(dòng)，存在S>0，使得s>S時(shí)，有f(x(s)，y(s))<ξ。由于S是時(shí)間t的單一遞增函數(shù)，因此反饋的控制信息在時(shí)間上滿足：對(duì)任意給定ξ>0，存在T>0，使t

3 基于運(yùn)動(dòng)學(xué)特性的軌跡跟蹤控器設(shè)計(jì)

在分析輪式機(jī)器人運(yùn)動(dòng)學(xué)關(guān)系與慣性測(cè)量單元運(yùn)動(dòng)學(xué)關(guān)系的基礎(chǔ)上，提出基于運(yùn)動(dòng)學(xué)特性的軌跡跟蹤控器設(shè)計(jì)。

(10)

根據(jù)式(20)即可獲得輪式移動(dòng)機(jī)器人的期望軌跡跟蹤路徑

(11)

輪式機(jī)器人的軌跡跟蹤時(shí)間t和機(jī)器人的軌跡長度s參考之間的關(guān)系是s=v(t)。

為了使輪式機(jī)器人有效跟蹤期望路徑[10]，需要相關(guān)公式推導(dǎo)出輪式機(jī)器人的實(shí)際平行移動(dòng)速度v和實(shí)際轉(zhuǎn)速ω的反饋控制率[11]。根據(jù)式(11)有ωd(s)=vd(s)c(s)。其中vd(s)表示期望點(diǎn)的期望速度，c(s)表示期望點(diǎn)的曲率。設(shè)定輪式機(jī)器人偏離指定軌跡的誤差為

(12)

式中，dx為x軸的方向誤差，dy表示y軸的方向誤差。

令控制法則為

(13)

其中，常數(shù)k1>0，k2>0，滿足下列公式

(14)

綜上所述，完成對(duì)基于慣性測(cè)量矩陣的輪式機(jī)器人軌跡跟蹤方法的設(shè)計(jì)，通過選擇線速度和角速度作為控制器參數(shù)，在此基礎(chǔ)上，結(jié)合動(dòng)力學(xué)模型設(shè)計(jì)輪式機(jī)器人軌跡跟蹤控制器，可以使輪式移動(dòng)機(jī)器人在指定的軌跡上進(jìn)行進(jìn)跟蹤，能夠有效提高輪式機(jī)器人軌跡跟蹤精度。

4 仿真驗(yàn)證

圖1 仿真結(jié)果

圖1(a)表示輪式機(jī)器人從起始點(diǎn)出發(fā)，到達(dá)目標(biāo)點(diǎn)的實(shí)際軌跡跟蹤結(jié)果?？梢钥闯?，在本文方法控制下，輪式機(jī)器人的實(shí)際移動(dòng)軌跡高精度跟蹤了期望軌跡。同時(shí)觀測(cè)圖1(b)、(c)、(d)可知，輪式機(jī)器人的軌跡跟蹤收斂性誤差等于0，并且軌跡收斂速度非常快，軌跡跟蹤也更為完整。

為了進(jìn)一步驗(yàn)證本文方法應(yīng)用于輪式機(jī)器人軌跡跟蹤的綜合性能，將本文方法與文獻(xiàn)[3]提出的基于自適應(yīng)滑?？刂频能壽E跟蹤方法進(jìn)行對(duì)比，圖2(a)本文方法的軌跡跟蹤結(jié)果，圖2(b)是文獻(xiàn)[3]方法迭代800次的軌跡跟蹤仿真結(jié)果。

圖2 不同方法軌跡跟蹤仿真結(jié)果

本文提出的方法與基于自適應(yīng)滑?？刂频能壽E跟蹤方法的跟蹤結(jié)果對(duì)比如圖2(b)。與其它方法進(jìn)行比較，本文方法有著更好的軌跡跟蹤的精準(zhǔn)度和聚斂速度。因此對(duì)仿真的結(jié)果進(jìn)行分析能夠得到：本文通過慣性測(cè)量單元進(jìn)行軌跡跟蹤的效果比傳統(tǒng)軌跡跟蹤方法更為精準(zhǔn)，跟蹤性能更好。

4.1 面向時(shí)延的軌跡跟蹤控制分析

通過以下實(shí)驗(yàn)對(duì)比軌跡跟蹤控制器，遭遇時(shí)間延遲后的跟蹤能力。針對(duì)實(shí)驗(yàn)環(huán)境，設(shè)定輪式移動(dòng)機(jī)器人初始狀態(tài)為(-1，0，π/4)。跟蹤時(shí)間為t=100ms時(shí)，輪式機(jī)器人遭到突發(fā)的時(shí)間延遲。本文提出的方法和文獻(xiàn)[2]方法的比對(duì)結(jié)果如圖3所示，設(shè)Z軸表示為時(shí)間t。

圖3 面向時(shí)延發(fā)生時(shí)不同方法軌跡跟蹤效果比較

圖3表示突發(fā)的時(shí)間延遲后，輪式機(jī)器人的跟蹤效果三維展示圖。圖3中較細(xì)實(shí)線代表本文方法控制下的機(jī)器人實(shí)際軌跡，加粗實(shí)線代表文獻(xiàn)[2]方法的跟蹤軌跡，圓點(diǎn)為障礙物，虛線表示期望軌跡。圖3中時(shí)間t=100ms時(shí)，即不同方法遭遇突發(fā)的時(shí)間延遲，此時(shí)輪式機(jī)器人停止移動(dòng)，t仍然在持續(xù)增加。當(dāng)時(shí)延t=500ms時(shí)，輪式機(jī)器人行動(dòng)恢復(fù)正常，并繼續(xù)按照制定的軌跡進(jìn)行跟蹤。文獻(xiàn)[2]方法在不確定性時(shí)延發(fā)生后，已無法按照原制定的軌跡進(jìn)行跟蹤；而本文方法通過設(shè)計(jì)輪式機(jī)器人工作軌跡參考量，結(jié)合動(dòng)力學(xué)模型設(shè)計(jì)的輪式機(jī)器人軌跡跟蹤控制器，通過控制器的調(diào)整仍然可以有效跟蹤期望軌跡。說明本文方法跟蹤過程不依賴于時(shí)間參量，軌跡跟蹤效果更為優(yōu)異。

5 結(jié)論

本文針對(duì)傳統(tǒng)輪式機(jī)器人軌跡跟蹤控制方法存在的多方面問題，提出一種基于慣性測(cè)量單元(IMU)的輪式機(jī)器人軌跡跟蹤方法。通過建立輪式機(jī)器人的空間坐標(biāo)來定位輪式機(jī)器人的移動(dòng)軌跡；而后建立機(jī)器人運(yùn)動(dòng)模型，在通過連接在機(jī)器人實(shí)體上的兩個(gè)慣性測(cè)量單元給出速度瞬心與兩個(gè)慣性測(cè)量量之間的關(guān)系。在此基礎(chǔ)上設(shè)計(jì)輪式機(jī)器人輪速控制器，通過該控制器來完成軌跡跟蹤。實(shí)驗(yàn)結(jié)果表明，所提軌跡跟蹤方法能夠有效克服時(shí)延參量問題，驗(yàn)證了所提方法在輪式機(jī)器人相關(guān)應(yīng)用領(lǐng)域中具備魯棒性。所提方法不僅改進(jìn)了傳統(tǒng)輪式機(jī)器人軌跡跟蹤控制方法存在的問題，還為機(jī)器人軌跡跟蹤控制技術(shù)的發(fā)展奠定了堅(jiān)實(shí)的基礎(chǔ)，提高了軌跡控制精度的同時(shí)為輪式機(jī)器人的使用者提供了極大便利。