基于滑動參數實時估計的履帶車輛運行軌跡預測方法研究

熊光明， 魯浩， 郭孔輝， 陳慧巖

(北京理工大學 機械與車輛學院, 北京 100081)

基于滑動參數實時估計的履帶車輛運行軌跡預測方法研究

熊光明， 魯浩， 郭孔輝， 陳慧巖

(北京理工大學 機械與車輛學院, 北京 100081)

要實現履帶車輛的無人駕駛，在軌跡規劃階段需要準確預測其未來一段時間內的運動軌跡，然而履帶與地面之間的滑動使車輛運動軌跡的準確預測變得非常困難。通過研究轉向過程中履帶接地段的運動，建立基于瞬時轉向中心的履帶車輛運動學模型。針對車輛的相對位姿是滑動參數的泛函，雅可比矩陣難以求解的問題，通過對泛函微分方程線性化，推導了雅可比矩陣的解析解。根據車輛相對位置計算值和測量值的差值，運用Levenberg-Marquardt算法迭代求解滑動參數，并結合給定控制序列預測未來一段時間內車輛的運動軌跡。該方法不需要提前知道土壤參數，并且能夠實時估計滑動參數，以適應路面變化。實車試驗結果表明，與傳統軌跡預測方法相比，利用該方法預測車輛軌跡時，車輛位置偏差減少30%以上。

兵器科學與技術； 履帶車輛； Levenberg-Marquardt算法； 滑動參數估計； 軌跡預測

0 引言

履帶車輛由于其優越的通過性和機動性，在軍事、農業、火星探測等領域得到廣泛應用。履帶車輛轉向過程始終伴隨著高速側履帶的滑轉與低速側履帶的滑移[1]。在自動駕駛車輛的局部路徑規劃中，需要根據車輛模型預測車輛行駛軌跡[2]。相比于輪式車輛，履帶車輛的滑動轉向特性使行駛軌跡的預測更加困難。因此，研究滑動參數的估計及其對車輛運動的影響對于實現履帶車輛的無人駕駛具有重要的理論及現實意義。

目前，國內外學者對履帶車輛的轉向過程做了大量研究。如國內文獻[1，3-5]、國外文獻[6-8]建立了履帶與地面之間存在剪切作用關系的轉向過程模型，并通過實車轉向試驗驗證了所建立模型的準確性。Martinez等[9]建立了履帶車輛的近似運動學模型，并利用遺傳算法離線計算出滑動參數。Moosavian 等[10]運用實車試驗的方法，將滑移率擬合成轉向半徑的指數函數，并成功應用于車輛的自適應控制。文獻[11-13]設計了基于濾波技術的滑動參數估計系統，能夠實時估計出車輛的滑動參數，從而提高控制精度。

在之前的文獻中，滑動參數的估計都是在控制階段，用于提高控制精度。本文在軌跡規劃階段估計滑動參數，用于精確預測車輛軌跡。如果在軌跡規劃階段忽略車輛的滑動，生成的待跟蹤軌跡將有可能是車輛不可執行的，無論控制精度多高，也將不可避免的產生跟蹤誤差。

在深入分析履帶車輛轉向過程的基礎上，提出一種基于瞬時轉向中心(轉向極)的履帶車輛運動學模型。通過將泛函微分方程在參考軌跡上線性化計算得到車輛相對位姿的雅可比矩陣，并根據車輛相對位置的計算值和測量值的偏差，運用Levenberg-Marquardt算法實現滑動參數(轉向極偏移量)的迭代求解。將當前時刻估計的滑動參數和預測時域內的主動輪轉速序列代入運動學模型，從而實現運動軌跡的準確預測。

1 包含滑動參數的履帶車輛運動學建模

為了方便分析，圖1中列出了履帶車輛轉向過程中各運動學參數[3,9]。假設履帶車輛在水平地面上作平面運動，車輛的質心與幾何中心重合，大地坐標系為OXY，X軸指向正東方向，Y軸指向正北方向，車體坐標系Oxy固結于車體上。

圖1 履帶車輛轉向運動學關系Fig.1 Steering kinematic relationships of tracked vehicles

1.1 傳統運動學模型

履帶車輛是一種典型的差速轉向車輛，通過調節左、右兩側主動輪轉速，實現航向的改變。定義車輛在大地坐標系下的坐標為P=[XYθ]T，其中(X,Y)表示車輛位置，θ表示車輛航向，fl表示低速側履帶滑移率，fr表示高速側履帶滑轉率，則車輛運動學模型為

(1)

式中：

(2)

(3)

由于滑轉率、滑移率很難測得，而履帶的卷繞速度卻可很容易得到，所以當履帶車輛在某種路面以不同的速度、不同轉向半徑轉向時，常常將滑轉率、滑移率假定為一定值[14]。

然而當低速側履帶卷繞速度為0 m/s，滑移率fl為1，即一側履帶完全制動，車輛做原地轉向運動時，無法根據(3)式反求得到牽連速度vMq，從而無法預測車輛的運動軌跡。為了解決這一問題，通過對轉向過程中履帶接地段的運動學分析，建立基于瞬時轉向中心的運動學模型。

1.2 履帶及車體瞬時轉向中心

履帶車輛轉向時的一些重要運動學參數都和兩側履帶和車體的瞬時轉向中心有關[1]。本小節通過分析履帶接地段的運動，揭示了各運動學參數與瞬時轉向中心的關系。

履帶接地段在任一時刻作平面運動，可看作繞瞬時轉向中心的旋轉運動，所以M、N點的絕對速度可表示為

(4)

(5)

另一方面，M、N點的絕對速度是牽連速度和相對速度的矢量和，此處絕對速度的標量形式為

(6)

(7)

車體作繞瞬時轉向中心(xc,yc)的旋轉運動，車體上M、N點的牽連速度為

(8)

(9)

由(4)式～(9)式可以計算得到各瞬時轉向中心的坐標值為

(10)

(11)

(12)

(13)

(10)式～(13)式描述了車輛轉向時，兩側履帶及車體的瞬時轉向中心與其他運動學參數之間的關系。即使低速側履帶卷繞速度為0 m/s，通過(10)式～(13)式仍然可以反求得到車輛縱向速度、側向速度和轉向角速度，具體推導見(14)式～(16)式。

由于高速側履帶接地段總是滑轉，其上任意一點絕對速度的縱向分量與車體的運動方向相反，所以高速側履帶接地段的瞬時轉向中心(xr,yr)總是在高速側履帶接地段以外，即yr小于-B/2. 同理，低速側履帶接地段總是滑移，其上任意一點絕對速度的縱向分量與車體的運動方向相同，所以低速側履帶接地段的瞬時轉向中心(xl,yl)總是在低速側履帶接地段以內，即yl大于B/2. 當車輛直線行駛時，角速度接近于0 rad/s，(12)式中的yc趨向無窮，而(10)式～(13)式分子分母為同階無窮小量，所以其他幾個瞬時轉向中心坐標值為有限值[8]。

1.3 基于瞬時轉向中心的運動學模型

通過對履帶接地段運動分析得到，車體的側、縱向速度及角速度不僅與兩側履帶卷繞速度有關，還和履帶及車體的瞬時轉向中心位置密切相關。由(10)式、(11)式及(13)式可以得到車體坐標系下車體縱向速度、側向速度和轉向角速度與瞬時轉向中心坐標之間的關系：

(14)

(15)

(16)

于是，履帶車輛轉向時，基于瞬時轉向中心的車輛運動微分方程為

(17)

2 滑動參數估計

2.1 求解思路

由(10)式～(13)式可知，滑動參數(yl,yr,xc)能夠通過車體速度和角速度計算得到，但是精確的速度很難通過傳感器直接測量得到，而通過差分全球定位系統(DGPS)能夠很容易的測量得到車輛的精確位置。本文根據車輛相對位置計算值和測量值的偏差，運用Levenberg-Marquardt方法估計滑動參數，如圖2所示。相比于利用速度估計滑動參數，利用相對位置估計滑動參數時，由于相鄰兩次測量更新時間可以較長，因此能夠降低傳感器的采樣頻率[15]。

圖2 滑動參數估計框圖Fig.2 Block diagram of slippage parameters estimation

假設t1為當前時刻，t0為過去某一時刻，首先計算t0～t1的車輛相對位置計算值和測量值的偏差，并計算雅可比矩陣，然后通過引入代價函數，將滑動參數的求解轉換為非線性最小二乘求解問題，最后利用Levenberg-Marquardt方法迭代求解未知的滑動參數。

2.2 車輛相對位姿的計算

車輛位姿包括位置和航向，t1相對t0時刻的車輛位姿可通過對(17)式積分得到：

(18)

式中：g(θ(τ),R(τ))=dP(τ)/dt為車輛運動微分方程；R(τ)=[yl(τ)yr(τ)xc(τ)]T為未知的滑動參數；h(R(ξ))=wz(ξ)為ξ時刻車輛的橫擺角速度。

τ時刻車輛的航向θ(τ)不僅依賴于當前τ時刻履帶滑動參數，還依賴于歷史時刻的滑動參數，因此，車輛相對位姿ΔP是滑動參數R(·)的泛函。需要注意的是，此處R(·)是滑動參數隨時間變化的函數，而不是對應于某一特定時刻的函數值。

2.3 雅可比矩陣

雅可比矩陣的計算是滑動參數迭代求解的關鍵。在任意τ(t0<τ

圖3 滑動參數擾動引起的車輛相對位姿誤差Fig.3 The vehicle’s relative pose error due to slippage parameters perturbation

(19)

式中：

(20)

(21)

(19)式是一線性時變系統，由于F(t)和

(22)

式中：δP(t)為t時刻車輛位姿擾動量；δR(τ)為τ時刻滑動參數擾動量；Φ(t,τ)為狀態轉移矩陣

于是雅可比矩陣為

2.4 滑動參數迭代求解

盡管履帶接地段瞬時轉向中心隨時間和空間不斷變化，為了便于求解，假設滑動參數在短時間內保持不變，即R(τ)≡常量(t0≤τ≤t)。

將(18)式離散化為

(23)

式中：ΔT為采樣周期。

(24)

參數估計問題轉化為求使代價函數最小的未知參數，即

(25)

由于Levenberg-Marquardt算法是一種有效的求解非線性最小二乘問題的迭代解法，尤其適合參數較少的情況下，它吸收了最速下降法和牛頓- 高斯法的優點，既能使總體沿著下降方向進行搜索，又能保證收斂速度較快，所以本文采用該方法求解滑動參數。其迭代公式為

Rj+1=Rj+[(Jj)TJj+λjI]-1(Jj)Tfj,

(26)

式中：j為迭代次數；Jj是雅可比矩陣；I為單位矩陣；λ為阻尼因子。

(27)

阻尼因子λ的引入是Levenberg-Marquardt算法成功的關鍵。當迭代過程中代價函數變大，即F(Rn)>F(R)時，λ快速變大，Levenberg-Marquardt算法近似于最速下降法，使下一步的迭代沿梯度方向進行；當達到全局最小值時，λ迅速減小，算法近似于牛頓- 高斯法，大大提升了收斂速度。

3 軌跡預測

(28)

根據履帶車輛的轉向特性，將滑動參數限制在一定范圍內，yl0

4 實車試驗

為了驗證算法的有效性，利用某型號履帶裝甲車輛進行了實車試驗，試驗路面為土路，如圖4所示。

圖4 試驗履帶車輛與試驗路面Fig.4 Test tracked vehicle and experimental road

數據采集系統包括安裝在主動輪上的輪速傳感器、高精度DGPS系統、慣性測量單元。其中輪速傳感器測量兩側主動輪轉速，經換算可以得到左右側履帶的線速度；DGPS系統用來測量車輛的運動軌跡，能精確地記錄車輛質心的經緯度，經換算可轉化為大地坐標系下的坐標值；慣性測量單元用來測量車輛的航向。履帶車輛的結構參數如表1所示。試驗中，由駕駛員駕駛車輛，并由傳感器同步記錄下車輛的兩側主動輪轉速、行駛軌跡及航向。試驗過程如下：履帶車輛以一定速度直線行駛，然后拉動左側操縱桿，使左側主動輪完全制動，同時加大油門，使車輛有足夠的功率克服轉向阻力，實現原地轉向，車輛運行軌跡如圖5所示。

表1 履帶車輛結構參數Tab.1 Structural parameters of tracked vehicle

圖5 車輛運行軌跡Fig.5 Moving trajectory of vehicle

4.1 與傳統軌跡預測方法比較

傳統軌跡預測方法是將滑轉、滑移率看作一常量，其數值根據經驗確定，然后將未來一段時間內的主動輪轉速序列代入運動微分方程式，向前預測車輛運動軌跡；本文提出的方法是基于實時估計的滑動參數對軌跡進行預測。

圖6 兩種方法預測得到的位置偏差Fig.6 Position errors of vehicle predicted by two methods

從圖6中可以看出，忽略履帶的滑轉、滑移，即f=0時，距離偏差最大。考慮滑轉、滑移時，利用傳統方法預測軌跡時，滑轉率、滑移率為0.3，平均距離偏差最小。利用本文提出的方法，基于實時估計的滑動參數對軌跡進行預測時，軌跡誤差最小，平均位置偏差由f=0.3時的0.76 m下降為0.29 m，下降幅度達到61.8%，具體見表2.

表2 平均距離偏差Tab.2 Mean distance errors

車輛運行過程中，滑動參數實時更新，其隨時間的變化如圖7所示，從圖7中可以看出，左、右側履帶橫向偏移量Δyl、Δyr的絕對值在0～1 m內波動，車體瞬時轉向中心的縱向偏移量xc在1 m上下波動。其中，Δyl=yl-B/2， Δyr=yr+B/2.

圖7 滑動參數估計值Fig.7 Estimated slippage parameters

4.2 預測時域長度與預測軌跡誤差的關系

在履帶車輛自主駕駛過程中，每一個周期規劃的路徑長度是影響其自主性能的一個重要因素。規劃的路徑越遠，系統越能夠提前判斷前方情況從而做出決策。圖8描述了分別利用傳統方法和基于實時估計滑動參數的方法預測軌跡時，得到的位置偏差及偏差減少率與預測時域長度的關系。由4.1節可知，利用傳統方法預測軌跡時，滑轉率、滑移率為0.3，平均距離偏差最小，所以圖8中令f=0.3. 由于向前預測軌跡時所用模型與真實車輛模型不可避免的存在誤差，所以隨著預測時域長度的增大，預測的車輛軌跡與真實軌跡間的偏差不斷增大，但是相比于傳統方法，利用當前估計的滑動參數進行軌跡預測，預測誤差顯著減小，偏差減少率隨預測時域增大而減小，偏差減少率在30%～70%之間。

圖8 位置偏差及偏差減少率Fig.8 Position and reduction rate errors

5 結論

1)提出了一種基于瞬時轉向中心的運動學建模方法，解決了當低速側履帶主動輪轉速為0 r/min時，傳統的基于滑轉率、滑移率的運動學模型不能根據主動輪轉速反求車體速度的問題。

2)將滑動參數的求解轉化為非線性最小二乘求解問題，利用Levenberg-Marquardt算法迭代求解未知的滑動參數。實車試驗表明，在土壤參數未知的情況下，本文提出的方法能夠顯著降低車輛的預測軌跡誤差。

3)在1～4 s的預測時域內，本文方法顯著優于傳統方法，但是優勢隨著時域的增大而減弱。因此，如何在較大時域內提高車輛軌跡的預測精度是下一步的研究重點。

References)

[1] 芮強, 王紅巖, 王欽龍，等. 履帶車輛轉向性能參數分析與試驗研究[J].機械工程學報，2015,51(12)：127-136. RUI Qiang, WANG Hong-yan, WANG Qin-long, et al. Research on the acquisition of steering performance parameters of armored vehicle based on experiments[J]. Journal of Mechanical Engineering, 2015,51(12)：127-136. (in Chinese)

[2] 姜巖, 王琦, 龔建偉，等. 無人駕駛車輛局部路徑規劃的時間一致性與魯棒性研究[J].自動化學報, 2015, 41(3):518-527 JIANG Yan, WANG Qi, GONG Jian-wei, et al. Research on temporal consistency and robustness in local planning of intelligent vehicles[J]. Acta automatica Sinica, 2015, 41(3):518-527. (in Chinese)

[3] 王紅巖, 王欽龍, 芮強，等. 高速履帶車輛轉向過程分析與試驗驗證[J].機械工程學報，2014,50(16)：162-171 WANG Hong-yan, WANG Qin-long, RUI Qiang, et al. Analyzing and testing verification the performance about high-speed tracked vehicles in steering process[J]. Journal of Mechanical Engineering, 2014,50(16)：162-171. (in Chinese)

[4] 程軍偉, 高連華, 王紅巖，等. 履帶車輛轉向分析[J].兵工學報，2007,28(9):1110-1115. CHENG Jun-wei, GAO Lian-hua, WANG Hong-yan, et al. Analysis on the steering of tracked vehicles[J]. Acta Armamentarii, 2007, 28(9):1110-1115. (in Chinese)

[5] 李軍魁, 王紅巖, 芮強. 履帶車輛穩態轉向性能分析與試驗[J].機械工程師，2016 (1): 82-85. LI Jun-kui, WANG Hong-yan, RUI Qiang. Analysis and testing verification the steady state steering performance of tracked vehicle[J]. Mechanical Engineer, 2016 (1): 82-85. (in Chinese)

[6] Wong J Y，Chiang C F. A general theory for skid steering of tracked vehicles on firm ground[J]. Proceedings of Institution of Mechanical Engineers, Part D: Journal of Automobile Engineering,2001, 215(3): 343-355.

[7] Said A M，Lakmal D S，Kaspar A. Track-terrain modelling and traversability prediction for tracked vehicles on soft terrain[J]. Journal of Terramechanics, 2010, 47(3):151-160.

[8] Wong J Y. Theory of ground vehicles[M]. 3rd ed. NY, US：John Wiley & Sons,2001.

[9] Martinez J L, Mandow A, Morales J. Kinematic modeling of tracked vehicles by experimental identification[C]∥ Proceedings of 2004 IEEE/RSJ International Conference on Intelligent Robots and Systems. Sendal, Japan: IEEE,2004:1487-1492.

[10] Moosavian S, Ali A, Kalantari A. Experimental slip estimation for exact kinematics modeling and control of a tracked mobile robot[C]∥ 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems. Nice, France :IEEE,2008: 95-100.

[11] 焦俊，汪宏喜，陳祎瓊，等．基于 UKF的農用履帶機器人滑動參數估計[J]．農業機械學報，2014,45(4):55-60 JIAO Jun, WANG Hong-xi, CHEN Yi-qiong, et al. Estimation sliding parameter of agricultural tracked robot based on UKF[J]. Transactions of the Chinese Society for Agricultural Machinery, 2014,45(4):55-60. (in Chinese)

[12] Le A T, Rye D C, Durrant-Whyte H F. Estimation of track-soil interactions for autonomous tracked vehicles[C]∥ Proceedings of the 1997 IEEE International Conference on Robotics and Automation. Albuquerque, NM, US: IEEE, 1997:1388-1393.

[13] Yi J, Wang H, Zhang J, et al. Kinematic modeling and analysis of skid-steered mobile robots with applications to low-cost inertial-measurement-unit-based motion estimation[J]. IEEE Transactions on Robotics, 2009, 25(5): 1087-1097.

[14] 吳紹斌.遙控履帶車輛的轉向控制研究[D]. 北京：北京理工大學，2002. WU Shao-bin. Research on steering control of remote tracked vehicle[D]. Beijing: Beijing Institute of Technology, 2002. (in Chinese)

[15] Seegmiller N, Rogers-Marcovitz F, Miller G, et al. Vehicle model identification by integrated prediction error minimization[J]. International Journal of Robotics Research, 2013, 32(32):912-931.

[16] Kelly A. Linearized error propagation in odometry[J]. The International Journal of Robotics Research,2004, 23(1): 179-218.

[17] Kelly A. General solution for linearized systematic error propagation in vehicle odometry[C]∥Proceedings of the 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems. Maui, HI, US: IEEE,2001:1938-1945.

Research on Trajectory Prediction of Tracked Vehicles Based on Real Time Slip Estimation

XIONG Guang-ming， LU Hao， GUO Kong-hui， CHEN Hui-yan

(School of Mechanical Engineering, Beijing Institute of Technology, Beijing 100081, China)

In order to realize the unmanned driving of tracked vehicle, its future motion trajectory within a period of time should be accurately predicted in trajectory planning. It is difficult to predict the future motion trajectory of vehicle due to the slippage between tracks and ground. A kinematics model based on the instantaneous steering center is developed by studying the interaction of track and ground. The relative pose of vehicle is a function of the slippage parameters, and the Jacobi matrix is difficult to solve. For this problem, the analytical solutions of Jacobi matrix are deduced by linearizoffing the functional differential equations. Slippage parameters are solved iteratively using Levenberg-Marquardt method according to the calculated and measured pose errors, and a set of input commands is given to predict the future motion trajectory within a period of time. The proposed method is used to update the slip parameters in real time without prior knowledge of terrain parameters. The real vehicle tests show that the position errors predicted using this method are reduced by more than 30% compared to the traditional trajectory prediction method.

ordnance science and technology; tracked vehicle; Levenberg-Marquardt method; slippage parameters estimation; trajectory prediction

2016-07-04

國家部委預先研究項目(40401060302)

熊光明(1975—), 男, 副教授。E-mail: xiongguangming@bit.edu.cn

TJ810.2

A

1000-1093(2017)03-0600-08

10.3969/j.issn.1000-1093.2017.03.025