基于慣性測量單元的輪式機器人軌跡跟蹤仿真

孫正鳳，張 朋，劉小軍

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

1 引言

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

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

2 輪式機器人運動學模型

2.1 輪式機器人運動學關(guān)系

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

(1)

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

(2)

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

(3)

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

(4)

當輪式機器人在進行逆向運動時，滿足Δv1x=Δv2x≥0，且Δv3x=Δv4x≤0，左體車輪處于靜止狀態(tài)時，右側(cè)位置處于緊急制動狀態(tài)。

2.2 慣性測量單元運動學關(guān)系

在上述分析輪式機器人運動學關(guān)系的基礎(chǔ)上，需要進一步分析慣性測量單元運動學關(guān)系。使用(xR1，yR1，0)、(xR2，yR2，0)分別描述慣性測量單元IMU1、慣性測量單元IMU2在空間坐標系R上的坐標。

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

(5)

(6)

(7)

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

(8)

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

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

(9)

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

3 基于運動學特性的軌跡跟蹤控器設(shè)計

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

(10)

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

(11)

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

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

(12)

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

令控制法則為

(13)

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

(14)

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

4 仿真驗證

圖1 仿真結(jié)果

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

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

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

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

4.1 面向時延的軌跡跟蹤控制分析

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

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

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

5 結(jié)論

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