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

孫正鳳，張 朋，劉小軍

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

1 引言

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

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

2 輪式機器人運動學模型

2.1 輪式機器人運動學關系

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

(1)

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

(2)

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

(3)

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

(4)

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

2.2 慣性測量單元運動學關系

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

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

(5)

(6)

(7)

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

(8)

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

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

(9)

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

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

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

(10)

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

(11)

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

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

(12)

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

令控制法則為

(13)

其中，常數k1>0，k2>0，滿足下列公式

(14)

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

4 仿真驗證

圖1 仿真結果

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

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

圖2 不同方法軌跡跟蹤仿真結果

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

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

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

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

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

5 結論

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