混動履帶式無人平臺軌跡跟蹤控制研究

張 彬，鄒 淵，張旭東，孫逢春，吳 喆，孟逸豪

（1.北京理工大學機械與車輛學院，北京 100081；2.電動車輛國家工程技術研究中心，北京 100081）

前言

履帶式無人平臺具有機械結構簡單、高通過性、高機動性等優點，其在軍事、消防、農業等領域得到廣泛應用［1-2］。履帶式無人平臺能夠完成環境感知、建圖定位、路徑規劃、自主決策和車輛橫縱向控制等任務。其中，在各種復雜環境下，保證精準而穩定的軌跡跟蹤控制是履帶式無人平臺實現無人化和智能化的關鍵之一［3］。

滑移轉向車輛通過兩側驅動系統與路面間的相互作用，產生速度差實現轉向。履帶式無人平臺屬于典型的滑移轉向車輛，控制算法的效果與模型的準確性密切相關，因此須對車輛建模進行研究［4］。在最初階段假設履帶與地面之間無相對滑動，將履帶車輛視為剛體，根據其幾何特性得到其理想模型［5-6］。隨著研究的深入，對履帶車輛模型的研究以滑移滑轉特性為主，將滑轉滑移現象通過庫侖摩擦力進行表達。在計算履帶車輛的受力時，根據工程經驗設置摩擦因數［7-8］。由于利用此方法建立的模型應用于無人履帶車輛控制時，對模型的描述存在經驗設定，缺乏足夠的準確性。在進一步的研究中，研究人員提出了利用滑移率來定量描述無人履帶車輛行駛過程中的滑移滑轉現象。文獻［9］和文獻［10］中提出使用剪切應力和位移的關系來計算獲得滑移率，但是仍沒有解決模型求解時間和經驗參數對真實車輛進行的近似處理問題。這些研究通過地面力學能夠較準確地分析履帶車輛的轉向過程，但并不適用于在軌跡跟蹤過程中對實時性要求較高的無人履帶車輛。

更進一步有研究人員使用滑動參數來定量表征無人履帶車輛的滑移轉向特性。為了降低模型復雜度，減少模型計算收斂時間，文獻［11］和文獻［12］中基于瞬時轉向中心提出了一種新型的滑移轉向車輛的運動學模型。履帶車輛在轉向過程中，其獨特的轉向方式，在車輛轉向時會發生履帶的滑移滑轉。復雜的履帶和地面作用關系導致車輛的實際運行軌跡不僅取決于主動輪的轉速，還取決于車輛所行駛的地面特征和車輛的運動狀態等。因此在分析履帶車輛轉向時須考慮履帶的滑移滑轉和車輛的質心側偏角。

目前用于無人車輛軌跡跟蹤控制的算法主要有純跟蹤控制、比例積分微分控制（PID）、前饋-反饋控制、線性二次型調節器控制（LQR）和模型預測控制（MPC）。純跟蹤控制是根據車輛當前自身狀態與預瞄點的橫向偏差和航向角偏差做對比，將偏差作為算法的控制目標進行求解的算法。該算法簡單易用，但其預瞄距離受到諸多的參數影響［13］。PID 控制器在工程項目中應用廣泛，其無須建立被控對象模型，主要通過試湊的方式獲得理想的控制參數，但是該過程耗時長且適應性較差，外界環境或者系統內部狀態的變化都有可能導致原有參數失效。MPC控制器可以同時處理多約束和多目標問題，為多個目標的集成優化提供聯合解決方案，并且可以綜合考慮不同系統的多個約束［14］。目前基于MPC 的軌跡跟蹤控制已有廣泛的研究，但研究對象多為輪式阿克曼轉向車輛。針對履帶式無人平臺的研究相對較少，且大多研究為某一地面條件下的軌跡跟蹤效果，沒有考慮縱向速度規劃和不同越野路面的適應性。

基于以上問題，在建立了考慮滑移滑轉和質心側偏角的履帶車輛模型的基礎上，提出一種考慮縱向速度規劃的分層軌跡跟蹤控制策略。上層策略根據道路特點對縱向車速進行優化，下層建立基于線性時變模型預測控制（LTV-MPC）的軌跡跟蹤控制算法，從而實現對目標路徑的跟蹤。最后通過聯合仿真和實車驗證了控制策略的軌跡跟蹤效果和對不同路面的適應性。

1 混動履帶式無人平臺參數和系統建模

1.1 車輛配置參數

圖1為課題組自研的混動履帶式無人平臺，表1為無人平臺主要參數。該無人平臺是一款線控的串聯式混合動力履帶底盤，采用模塊化和動力履帶設計，將組件全部置于兩邊的履帶艙內，為中間平臺省出更多的承載空間。用于軌跡跟蹤控制的硬件主要包括：IMU 與GPS 等的組合定位設備、信號轉換設備、交換機和工控機等。組合定位設備可以實時獲得的測量信號如表2 所示。無人履帶車輛的硬件架構如圖2 所示。車輛上層軌跡跟蹤系統的信息通過CANET 實現與底盤系統的CAN 通信，下發相關的控制指令并接收底盤反饋的信息。

表1 混動履帶式無人平臺主要參數

圖1 混動履帶式無人平臺

表2 傳感器測量信號列表

圖2 履帶式無人平臺硬件架構

1.2 履帶式無人平臺運動學模型

履帶車輛轉向時，車體可以看作繞瞬時轉向中心（instantaneous center of rotation，ICR）旋轉的平面運動，而兩側履帶由于其繞主動輪的卷繞運動比車體多一個自由度，其運動由牽連運動和相對運動組成，其中的牽連運動指履帶隨車體的平面運動，相對運動指履帶相對于車體的旋轉運動。當履帶車輛轉向時，內側履帶的滑移和外側履帶的滑轉是其固有特性。基于ICR 的履帶車輛運動學模型如圖3 所示，圖中ωz為橫擺角速度，B為履帶車輛軌距，Oc（xc，yc）為車體的瞬時轉向中心，Ol（xl，yl）和Oh（xh，yh）分別為低速側和高速側履帶接地段的瞬時轉向中心。

圖3 履帶車輛轉向運動學模型

采用滑移率fl、滑轉率fh來表示低速側和高速側履帶的滑移滑轉程度。

由于在橫向上履帶和車體沒有相對運動，高速側和低速測履帶接地點和車體的瞬時轉向中心縱向的坐標值一樣，即瞬時轉向中心在車輛的縱坐標方向上在一條直線上。

在全局坐標系下，考慮履帶滑移滑轉的最終運動學微分方程為

2 考慮縱向速度規劃的履帶式無人平臺軌跡跟蹤控制

本文所提出的考慮縱向速度規劃的無人平臺分層軌跡跟蹤控制策略如圖4 所示，上層根據道路特點采用偽譜法得到縱向參考車速，同時根據感知和規劃層獲得期望參考軌跡。將參考軌跡和參考車速作為軌跡跟蹤的期望目標下發給下層。下層是根據車輛的運動學模型所搭建的基于線性時變模型預測控制的軌跡跟蹤控制器，將優化求解得到的轉速控制命令下發給底盤整車控制器（VCU），最終實現對期望路徑的跟隨。車輛實際運動狀態通過擴展卡爾曼濾波算法（EKF）進行估計并反饋給控制算法，控制算法根據車輛反饋的車速信息、橫擺角速度和左右驅動輪的轉速計算得到車輛的滑移滑轉率和質心側偏角。

圖4 軌跡跟蹤控制算法框架

2.1 上層縱向速度規劃控制器

偽譜法在收斂速度和計算精度方面具有明顯優勢，在車輛軌跡跟蹤控制方面的應用較少［15］。雖然偽譜法無法像模型預測控制一樣進行實時求解，但可以在一定周期內根據規劃層獲得的參考軌跡進行縱向速度優化，因此本文利用偽譜法實現履帶車輛的分層軌跡跟蹤控制。

在上層速度規劃層面，優化的控制目標為縱向車速vx和橫擺角速度ωz，并將優化得到的縱向車速作為下層基于LTV-MPC的控制器的期望車速，其基本運動學的狀態方程如下：

為了能夠對車輛縱向加速度添加過程約束和縱向車速添加終端約束，將車速vx作為狀態量，并將縱向加速度作為控制量。初始時刻車輛位于大地坐標系下的坐標原點，經過t時刻后位于點（X，Y）處，當前狀態為P（X，Y，φ）。期望位置在全局坐標系下為Pref（Xref，Yref，φref），則履帶車輛的軌跡跟蹤誤差可以表示為

式中：ye為在車輛坐標系下與參考路徑的橫向偏差；φe為車輛與參考路徑的航向角偏差。

上層縱向速度優化問題的約束條件為

2.2 下層基于線性時變模型預測控制的軌跡跟蹤控制設計

采用車輛在全局坐標系下的橫坐標、縱坐標和橫擺角x=［XYφ］為系統狀態量，左右側主動輪轉速μ=［ωlωh］為控制量，并且將實時的左右側履帶的滑移滑轉率fl、fh和質心側偏角β作為狀態方程更新參數。基于式（7）得到履帶車輛基于ICR 的非線性運動學系統狀態方程：

對每一采樣時刻t對車輛當前狀態（xr=xˉt(k)，μr=μˉt(k)）在名義工作點進行泰勒展開，得到近似1階線性化系統：

式中：m為控制量的維度；n為狀態量的維度；r為輸出量的維度。

系統的預測時域為Np，控制時域為Nc，為了降低模型預測控制的計算復雜度，作出如下假設：

2.3 優化目標函數與控制約束

為使無人履帶車輛在軌跡跟蹤和速度跟隨過程中保證控制精度和平順性，構造以下優化目標函數：

式中：首項反映車輛對軌跡的跟蹤能力；第2 項反映控制所需能量的要求；第3 項是為了防止出現方程無解的情況；Q和R為權重矩陣；ρ為權重系數；ε為松弛因子。通過增大Q矩陣或者減小R矩陣，能夠提高系統的軌跡跟蹤能力；如果增大R矩陣，則系統的動態性能降低，穩定性提高。

為滿足實時性的要求，對方程進行高效快速的求解，將上述目標函數轉換為標準的二次型形式：

在每一個采樣周期內采用內點法完成上述的二次規劃問題求解，可以實時獲得最優控制序列，并將序列的第1項與當前的控制量求和作為下一步的量。

在下一個控制周期重復上述過程，在線滾動優化實現對期望路徑的跟蹤控制。

3 仿真與驗證

通過Matlab/Simulink 和RecurDyn 的聯合仿真，驗證所提出的算法在各種路面下的軌跡跟蹤效果。在此基礎上又加入基于偽譜法的縱向速度優化，進一步提高履帶車輛的軌跡跟蹤效果。期望路徑采用雙移線工況，該工況包含了車輛的直線行駛和轉向行駛工況。初始狀態量設置為［0 0 0］，控制器采樣周期為0.05 s，預測時域為20，控制時域為15，權重矩陣參數為：Q= diag（50，100，500），R=diag（500，500）。車輛在初始時刻速度為零，當履帶車輛模型在RecurDyn 中達到穩態之后，模型預測控制器開始輸出控制量進行軌跡跟蹤，縱向的期望車速為恒定的15 km/h。本文充分考慮車輛與地面作用引起的地面沉陷（參數見表3），主要研究履帶車輛在黏質土（clayey soil）、砂壤土（sandy loam）和雪地等典型越野路面的軌跡跟蹤效果。

表3 各種路面的地面沉陷參數

根據路面參數在RecurDyn 的Contact parameter下設置路面模型，如圖5所示。

圖5 路面參數設置

圖6 為黏質土壤地面條件下無人履帶車輛軌跡跟蹤效果和橫向偏差。由圖6 可以看出在黏質土壤地面條件下不考慮滑移滑轉會產生較大的橫向誤差，而所提出的控制算法在該路面仍然可以保證良好的軌跡跟蹤效果。考慮滑移滑轉的車輛軌跡與期望軌跡的橫向偏差最大值為 0.049 5 m，不考慮滑移滑轉的車輛軌跡與期望軌跡的橫向偏差最大值為0.135 2 m。因此，所提出的軌跡跟蹤算法在黏質土壤地面條件下能夠明顯提高車輛的軌跡跟蹤精度。

圖6 黏質土壤地面條件下軌跡跟蹤效果

圖7 為砂壤地面條件下履帶車輛的軌跡跟蹤效果和橫向偏差。可以看出考慮滑移滑轉的車輛軌跡與期望軌跡的橫向偏差最大值為0.045 9 m，不考慮滑移滑轉的車輛軌跡與期望軌跡的橫向偏差最大值為0.124 5 m，在砂壤條件下仍然可以保證良好的軌跡跟蹤效果。

圖7 砂壤地面條件下軌跡跟蹤效果

圖8 為雪地條件下無人履帶車輛的軌跡跟蹤效果和橫向偏差。可以看出考慮滑移滑轉的車輛軌跡與期望軌跡的橫向偏差最大值為 0.038 8 m，不考慮滑移滑轉的車輛軌跡與期望軌跡的橫向偏差最大值為0.148 4 m，在雪地條件下仍然可以保證良好的軌跡跟蹤效果。

圖8 雪地條件下軌跡跟蹤效果

圖9 所示為履帶車輛在黏質土、砂壤土和雪地條件下，采用考慮滑移滑轉和質心側偏角的軌跡跟蹤控制得到的質心側偏角曲線。在整個仿真過程中質心側偏角始終低于1°，在特殊的越野路面仍然能保持良好的平順性。

圖9 3種路況下的質心側偏角

以上仿真實驗驗證了所提出的算法在越野路面的良好適應性，縱向的期望車速為定值。下面所做的實驗為考慮偽譜法的縱向速度規劃軌跡跟蹤效果。參考路徑同樣采用雙移線工況，通過偽譜法依據局部參考路徑獲得的優化后的縱向期望車速如圖10 所示。可見當車輛參考路徑處曲率較大時通過降速以達到進一步提高軌跡跟蹤效果的目的。

通過偽譜法獲得車速的序列vref（t），然后再將其轉換為關于車輛位置的期望車速序列vref（X，Y），根據反饋的車輛位置信息經過最大似然估計將當前位置下的期望車速發送給下層LTV-MPV 的軌跡跟蹤控制器，通過求解二次規劃問題獲得當前時刻左右側主動輪的期望轉速，最終實現基于分層控制策略的軌跡跟蹤。

從圖10 的仿真結果可以看出，加入縱向速度規劃可以進一步提高軌跡跟蹤的精度，橫向偏差的最大值由原來的0.049 5 縮減為0.038 9 m。可以看出考慮縱向速度規劃的軌跡跟蹤控制具有更好的跟蹤效果。

圖10 考慮縱向速度規劃的軌跡跟蹤效果

圖11 所示為課題組自研的混動履帶式無人平臺實車。將實車在圖12 所示的越野泥濘路面進行實車測試，驗證履帶式無人平臺的軌跡跟蹤效果。根據采集獲得的期望路點，通過偽譜法進行縱向速度規劃，車輛在軌跡跟蹤過程中的車速信息以及跟蹤狀況如圖13所示。從實車測試的結果可以看出實車的橫向偏差均值都在0.3 m 以下。在實車實驗過程中在轉彎處有一定的超調，最大均值偏差不超過0.6 m，低于履帶中心距的一半。此實驗驗證了所提出的算法在越野路面具有良好的軌跡跟蹤效果。

圖11 履帶式無人平臺實車

圖12 實車測試用的越野泥濘路面

圖13 考慮縱向速度規劃的實車軌跡跟蹤效果

4 結論

為了提高無人電傳動履帶車輛的軌跡跟蹤性能，提出了一種考慮縱向速度規劃的分層軌跡跟蹤算法，具體結論如下。

（1）上層的速度規劃根據獲得的參考路徑，通過偽譜法優化得到一系列縱向車速序列vref（X，Y），經過最大似然估計將當前位置下的期望車速下發給下層控制器。

（2）建立考慮履帶滑移滑轉和質心側偏角的瞬時轉向中心運動學模型，通過模型的線性化、離散化處理以及構建二次規劃問題，完成下層線性時變模型預測控制器的設計。

（3）通過RecurDyn 和Matlab/Simulink 的聯合仿真，驗證了所提出的控制算法在黏質土壤、砂壤地面和雪地等越野路面的軌跡跟蹤效果，結果表明所提出的控制算法滿足良好的控制精度、適應性和平順性。通過實車在泥濘路面的測試，同樣也驗證了所提出算法的軌跡跟蹤效果。

下一步工作將開展更加充分的實車測試。同時將軌跡跟蹤算法和能量管理算法相結合實現車輛的能效協同控制，使混動履帶式無人平臺在滿足軌跡跟蹤精度的同時能夠實現較低的燃油消耗率。