一種主動懸架式火星車穩定裕度優化控制策略

唐 玲，劉 濤，李德倫，魏世民

(1. 北京郵電大學自動化學院，北京 100876；2. 北京空間飛行器總體設計部，北京100094；3. 空間智能機器人系統技術與應用北京市重點實驗室，北京 100094)

0 引 言

火星車是承載各種儀器執行火星表面探測任務的重要載體和直接工具。迄今為止，全世界共有4輛火星車成功完成在軌探測任務，分別為火星車旅居者、勇氣號、機遇號和好奇號(Curiosity)[1-3]，4輛火星車均為搖臂式懸架機構。為解決火星車在火星表面的高性能移動問題,我國的火星探測采用了一種六輪主動懸架式火星車，在傳統火星車的搖臂式懸架機構上加入了主動環節[4]。

行星車的穩定性反映了其在不同地形環境下的抗傾覆能力，是行星車移動系統的重要指標之一[5]?；鹦潜砻婢哂衅皆⑸矫}、丘陵、隕石坑等多種地形地貌特征，是一種典型的非結構化環境[6]。雖然火星車可以使用導航控制的方式在相對平坦的地形中行駛，但由于火星地形地貌復雜，陡峭坡面、地面凹凸不平、局部土壤沙化等非結構化環境均會引起火星車車體傾斜，火星車行駛過程中存在由于穩定性不足而導致傾覆的風險。因此，研究火星車在非結構化環境下的穩定性控制，對于火星探測任務的順利開展具有重要的實際意義。

許多學者開展了對行星車穩定性分析和控制的研究。Kilitd等[7]提出了一種三懸架式火星車，建立其運動學模型，并以火星車運動過程中傾斜角度為評價標準，分析火星車的穩定性，但火星車的極限傾斜角度還與火星車的結構尺寸相關，因此該穩定性評價標準不具有普適性。Zhu等[8]人使用穩定錐法分析了一種輪腿混合式機器人爬升過程中的穩定性。鄒懷武等[9]針對嫦娥三號巡視器在坡道轉移過程中，以不出現車輪翹起為穩定準則，分析巡視器的行駛穩定性，而在非結構化環境中，當各車輪速度不匹配時，也會出現車輪翹起的情況，因此該穩定性準則具有一定局限性。文獻[7-9]沒有開展針對行星車穩定性的優化控制。Mcdermott等[10]通過控制一種四輪行星車車體位姿平衡以增強其行駛穩定性，但該穩定性控制方法不適用于爬坡工況。Nakamura等[11]基于穩定錐法，通過控制四輪式行星車質心位置，以提升其在崎嶇地形行駛過程中的車輪驅動力以及抗傾覆能力，但該方法需要在行星車上加入一套移動裝置才能實現整車的質心位置調整。Cordes等[12]針對一種三輪腿式行星車，以各個輪腿受力平衡為目標，調整懸架構型來實現行星車在崎嶇地形上的平穩行駛。綜上可知，行星車穩定性控制均是針對懸架構型可調整的行星車，而這些行星車的懸架構型與我國的主動懸架式火星車有一定的區別，這類穩定性控制方法在主動懸架式火星車上適用性有限。

目前應用于機械系統的穩定性指標主要有穩定裕度法和穩定錐法。穩定裕度是指質心投影點與由機構支撐點組成的邊緣之間的最短距離[13]，穩定裕度相對于穩定錐法，物理意義明確，數學模型簡單。穩定裕度法廣泛應用于移動機器人領域，機器人通常以穩定裕度為標準，規劃得出穩定裕度最大的行進路徑[14-16]。主動懸架式行星車可以采用穩定裕度的方法，通過改變整車質心的位置，獲得最優穩定裕度的懸架構型，來提高車輛行駛的穩定性。非結構化環境下行星車質心位置控制的關鍵是建立行星車行駛過程中的準確運動學模型。Tarokh等[17]考慮到車輪的旋轉、縱向和橫向滑動影響，建立了一個通用的火星車Rocky7運動學模型，但沒有考慮火星車懸架機構構型調整的情況。Inotsume等[18]建立了一種四輪月球車的運動學模型，通過控制懸架機構構型來提升軟土坡面上的行駛牽引力。高海波等[19]建立了一種主動懸架式火星車的運動學模型，分析火星車車輪抬離地面的性能。文獻[17-19]中的運動學模型均沒有考慮星球車側傾時車輪與硬質地面的點接觸情況。Lou等[20]建立六輪星球探測運動學模型，分析星球車整車運動速度與車輪速度分量之間的關系，但是運動學模型簡略了懸架機構各個關節之間的運動關系。目前仍然缺乏足夠完善的運動學模型來控制主動懸架式行星車懸架機構的構型。

針對以上問題，本文提出針對六輪主動懸架式火星車的穩定裕度優化控制方法。考慮火星車懸架機構調整過程中車輪與接觸地面滑移和側傾等運動關系，建立和完善六輪主動懸架式火星車的運動學模型，在此基礎上，推導火星車的穩定裕度模型和火星車穩定裕度優化控制模型，擬通過調整火星車懸架機構構型，來實現火星車在非結構化環境下穩定裕度的優化控制。最后，采用Adams和Matlab/Simulink軟件建立火星車動力學與控制聯合仿真平臺，對火星車的穩定裕度優化控制策略的有效性進行仿真校驗。

1 六輪主動懸架式火星車運動學模型

六輪主動懸架式火星車結構圖如圖1所示?；鹦擒囉绍噹⒉钏倨?、夾角調整機構、前主搖臂、后主搖臂、副搖臂、轉向機構、車輪、車輪驅動機構和離合器組成。離合器和差速器是被動關節，其它機構是主動關節。主動懸架式火星車可以通過驅動機構控制主動關節來改變懸架機構構型，被動關節和連桿根據地形拓撲改變其關節角度。

圖1 火星車結構示意圖Fig.1 Structure diagram of the Mars rover

差速器左右兩個旋轉關節的運動約束關系為d1=-d2=d/2，其中，d1和d2分別Jd1和Jd2的轉動角度，d是左側差速器軸相對右側差速器軸的轉動角度。夾角調整機構的兩個旋轉關節運動約束關系為hfi=-hri/2，其中，hfi與hri為Jhfi和Jhri的旋轉角度。ti,i=1,…,6為轉向機構i的旋轉角度wi,i=1,…,6是驅動關節轉動角度。b1為左離合器i的旋轉角度。

圖2 火星車坐標系示意圖Fig.2 Coordinate frames of the Mars rover

(1)

坐標系Oi相對于坐標系Oi-1的位置矢量與姿態變換矩陣可以表示為

(2)

(3)

坐標系Oi-1與坐標系Oi之間的齊次變換矩陣表示為

(4)

式中，Z13表示一個1×3的零向量矩陣。因此，火星車坐標系之間的相對關系可以通過式(4)得出。

δi=γi+βi

(5)

(6)

式中：r為車輪半徑。

圖3 車輪i輪地接觸坐標系Fig.3 Coordinate frames for terrain contact at wheel i

使用運動學模型描述t-Δt時刻至t時刻火星車構型調整過程中坐標系之間的關系。Oi0表示為t-Δt時刻坐標系，Oi表示為t時刻坐標系。因此，Tci0,ci,i=1,…,6表示為Oci(t)=Oci相對于Oci(t-Δt)=Oci0的齊次變換矩陣。坐標系Oci0與Oci之間的關系簡圖如圖4所示。因此，Tci0,ci可以表示為式(7)所示。

(7)

式中：ξi為車輪i滾動滑移，ηi為車輪i側向滑移，ζi為車輪i傾斜角度。

圖4 滑移與傾斜運動Fig.4 Incremental motion by rolling and slip

Tcm0,cm表示從t-Δt時刻至t時刻車廂質心坐標系Ocm的齊次變換矩陣，可以表示為

Tcm0,cm=Tcm0,ci0Tci0,ciTci,cm

(8)

以i=4為例，Tcm0,cm可以表示為

Tcm0,cm=Tcm0,c40Tc40,c4Tc4,cm

(9)

式(9)中Tcm0,c40為

(10)

式(9)中Tc4,cm為

(11)

2 火星車穩定裕度優化控制系統

2.1 火星車穩定裕度計算模型

為了便于分析穩定性，定義了全局坐標系Ot。在t時刻，Ot的坐標系原點與Ocm原點重合。Ot坐標系的x軸方向與重力方向平行且相反。z軸方向與水平面平行，并指向火星車車廂前板。y軸滿足右手定則?；鹦擒嚪€定裕度示意圖如圖5所示。OR為火星車整車質心坐標系，可以通過桿件質量和火星車構型得到，OR的相對位置不受構型調整的影響。因此，假定OR相對于Ocm的位置和姿態始終不變化。OR′是OR在水平面上的投影；Oci′是Oci,i=1,3,4,6在水平面上的投影。h13,h36,h14,h46表示OR′與由Oci′,i=1,3,4,6組成的邊的距離。

圖5 穩定裕度示意圖Fig.5 Diagram of stability margin

Oc1,Oc3,OR相對于Ot的位置矢量可以分別表示為Pt,c1=Ct,cm0Ccm0,cmPcm,c1，Pt,c3=Ct,cm0Ccm0,cmPcm,c3，Pt,R=Ct,cm0Ccm0,cmPcm,R。Ct,cm0為坐標系Ocm0相對于全局坐標系Ot的旋轉矩陣。Ct,cm0可以通過車廂陀螺儀獲得。因此，三角形ΔOR′Oc1′Oc3′的三邊矢量可以表示為:

(12)

三角形ΔOR′Oc1′Oc3′的面積S可以通過海倫公式獲得

S=

(13)

其中

(14)

則穩定裕度h13可寫作

(15)

穩定裕度h14,h36,h46的求解方法與h13相同，因此，火星車穩定裕度為

hm=min(h13,h14,h36,h46)

(16)

其中，h13，h14，h36，h46是關于火星車懸架機構初始和調整后各關節角度的函數。

2.2 火星車穩定裕度的優化控制模型

設ti0，hfj0，hrj0，dj0，bj0，δi0和ζi0是火星車運動學模型變量參數在t-Δt時刻的值。ti，hfj，hrj，dj，bj，δi，ζi，ξi和ηi是運動學變量參數在t時刻的值。根據式(15)和式(16)可知期望的穩定裕度hm是ti0，hfj0，hrj0，dj0，bj0，δi0，ζi0，ti，hfj，hrj，dj，bj，δi，ζi和ηi,i=1,…,6,j=1,2的函數。以下對每個變量進行分析和計算。

為了提高控制系統計算效率，在火星車穩定裕度優化控制模型推導過程中，對模型作以下簡化：(1)假設火星車t-Δt時刻各車輪與地面為水平面接觸，設置δi0=ζi0=0；(2)假設火星車構型調整過程中車輪沒有滾動滑移和側向滑移，設置ξi=ηi=0(i=1,…,6)；(3)設定構型調整過程中轉向機構不參與構型調整，即ti=ti0(i=1,…,6)；(4)構型調整過程中差速器角度影響很小，設定d=d0。

此外，hfj和hrj變量參數為控制目標；ti0,hfj0,hrj0,dj0和bj0參數變量可以通過火星車傳感器測量獲得；δi,bj和ζi是火星車被動關節角度，是火星車夾角調整機構關節角度和地形參數的函數，需要使用火星車運動學模型推導獲得。

根據上述關系得出式(17)所示的關系式。

(17)

通過式(17)，可推導等b1和δ1，如式(18)和(19)所示。

(18)

(19)

通過使用這些變量，火星車初始構型的坐標系Ob1,Oh1,Ot1,Oc1可以表示為

(20)

b2與δ2的求解方式和b1與δ1的求解方式相同。

圖6 火星車非結構環境下構型調整示意圖Fig.6 The configuration adjustment diagram of the rover

ζi,i=1,…,6表示在構型調節過程中火星車的側傾角度，且各車輪的側傾角度值均相同，如圖7所示。ζi可以通過Oc1與Oc4的相對位置求得，如式(21)所示。

(21)

將火星車運動學模型變量參數ti0,hfj0,hrj0,dj0,bj0,δi0,ζi0,ti,hfj,hrj,dj,bj,δi,ζi,ηi,i=1,…,6,j=1,2的數值和表達式代入式(16)，可得出火星車期望的穩定裕度優化控制目標如式(22)所示。

圖7 火星車側傾角度Fig.7 Incline angle of the rover

(22)

針對式(22)的火星車穩定裕度優化控制目標函數，以火星車夾角調整機構角度hf1,hf2為變量，使用內點法求解，最終獲得火星車最大穩定裕度hm及期望的夾角調整機構角度hf1d,hf2d。

2.3 火星車穩定裕度控制系統

火星車穩定裕度控制系統主要由火星車運動學計算模塊、火星車穩定裕度計算模塊、火星車穩定裕度優化控制模塊、懸架機構主動關節路徑規劃模塊，以及懸架機構主動關節PI控制器組成。火星車穩定裕度控制系統框架與流程如圖8所示，圖中，Ii,i=1,2為夾角調整機構的控制電流。

圖8 火星車穩定裕度控制系統Fig.8 The frame of the stability margin control system of the rover

各模塊的主要功能描述如下：

1)火星車運動學計算模塊，用于計算火星車懸架機構支撐車輪i與地面的接觸坐標系Oci，i=1,3,4,6相對于車廂質心坐標系Ocm的位姿矩陣Pcm,ci。

2)火星車穩定裕度計算模塊，根據火星車傳感器獲取的數據信息計算火星車的穩定裕度值，可實時監測火星車行駛過程中的穩定性。

3)火星車穩定裕度優化控制模塊，該模塊為控制系統的核心，使用內點法求解式(22)的穩定裕度函數，計算輸出火星車的最優穩定裕度對應的期望的夾角調整機構角度hf1d,hf2d。

4)懸架機構主動關節路徑規劃模塊和懸架機構主動關節控制器采用常規的方法實現，其中懸架機構主動關節路徑規劃模塊采用速度梯形法對夾角調整機構的角速度進行規劃，輸出夾角調整機構的期望角速度ωf1d,ωf2d；懸架機構關節角速度采用PI控制器進行控制。

由于六角系統獨特的圖結構,因此研究它的點可區別邊染色還是有一定難度的,文中所研究的圖為“長”為n個相鄰六邊形,“寬”為m個相鄰六邊形構成的形為平行四邊形的六角系統,并記作H(m,n),如圖1所示。當m=1時,首先使用π(H(1,n))+1種顏色通過染色算法對H(1,n)(n≥6)中所有2度點進行邊著色。其次,通過分析3度點的色集合,對個別邊進行調色,證明了H(1,n)是滿足點可區別邊染色猜想的。當m≥2時,用k(關于m,n的函數)種顏色通過算法對H(m,n)進行邊著色,并對相關邊的顏色進行局部調整，從而給出關于H(m,n)的點可區別邊色數的一個上界。

3 仿真校驗

3.1 火星車動力學與控制仿真模型說明

為了驗證火星車穩定裕度優化控制算法的有效性，本文分別使用Adams和Matlab/Simulink軟件建立火星車高精度的動力學和控制模型，并在Matlab/Simulink軟件中對火星車單側輪越障、爬坡、坡面越障三種典型工況進行動力學與控制系統的聯合仿真。其中，在火星車單側輪越障工況中，車廂會發生側向傾斜；在火星車爬坡工況中，車廂會發生縱向傾斜；在火星車坡面越障工況中，車廂會發生側向傾斜并側翻。

火星車典型工況的動力學模型主要包括火星車的多柔體動力學模型和非結構化地形模型。火星車動力學模型的主要參數設置為：火星車的長度為1600 mm，寬為1300 mm，高為800 mm，總重量為240 kg；在初始時刻，火星車整車質心位置Pcm,R=(-51.58，-13.80，-14.73) mm；火星車車輪采用運動模式(MOTION)驅動，角速度為6 °/s?；鹦擒嚪€定裕度優化控制系統的主要參數設置為：火星車夾角調整機構采用PI控制器控制關節角速度，控制周期為10 ms，關節角度運動范圍為[-π/4，π/4]；火星車穩定裕度優化控制模塊控制周期為5 s。

3.2 火星車單側車輪越障工況仿真與分析

火星車單側車輪越障過程為：火星車左側三個車輪依次從水平地面越過障礙，右側車輪始終在水平地面行駛。單側障礙物高度為296 mm，障礙前后坡度均為20°火星車與地面摩擦系數設置為0.3。分別對使用和未使用穩定裕度優化控制算法兩種情況進行仿真，仿真時間為385 s，仿真步長為0.05 s。

仿真結果如圖9和圖10所示。圖9為使用穩定裕度優化控制算法時，火星車越障過程中夾角調整機構關節角度hf1與hf2的時間歷程曲線。未使用穩定裕度優化控制算法時夾角調整機構不運動，角度始終為0 rad，以下夾角調整機構轉動角度曲線圖中均省略該曲線。圖10為使用和未使用穩定裕度優化控制算法兩種情況下，火星車越障過程中穩定裕度的時間歷程曲線。

圖9 火星車單側車輪越障過程中hf1和hf2曲線Fig.9 Curve of hf1 and hf2 during Mars rover one-sidewheels crossing obstacle

圖10 火星車單側車輪越障過程中穩定裕度曲線Fig.10 Stability margin curve of Mars rover during Marsrover one-side wheels crossing obstacle

從圖9可以看出，使用穩定裕度優化控制算法時，在0～190 s火星車逐漸爬上障礙，hf1轉動角度從初始狀態0 rad逐漸增加至0.22 rad，hf2轉動角度從初始狀態0 rad逐漸減小至-0.58 rad，使得火星車左側懸架機構的高度降低，右側懸架機構的高度升高，減小了由于障礙物引起的車廂傾斜。375 s后，火星車駛入水平地面，夾角調整架構關節運動至初始角度0 rad，火星車懸架機構恢復為常規構型。

從圖10可以看出，未使用穩定裕度優化控制算法情況下，火星車在水平地面時，穩定裕度初始值為600 mm；之后，火星車左側車輪上坡，穩定裕度逐漸減小，火星車左側三個車輪均在障礙物平面上行駛時，穩定裕度保持445 mm左右不變；之后，火星車左側車輪下坡，穩定裕度逐漸升高，直至增加初始值600 mm。使用穩定裕度優化控制算法，整個運動過程中穩定裕度最小值為589 mm。在該過程中，使用穩定裕度優化控制算法相對于未使用穩定裕度優化控制算法，火星車的穩定裕度提高了32.35%。

3.3 火星車爬坡工況仿真與分析

火星車爬坡過程為：火星車從高度為0 mm的地面爬上30°坡，之后駛入高度為993 mm的地面。火星車與地面摩擦系數設置為0.6。分別對使用和未使用穩定裕度優化控制算法兩種情況進行仿真，仿真時間為285 s，仿真步長為0.05 s。

仿真結果如圖11和圖12所示。圖11為使用穩定裕度優化控制算法時，火星車爬坡過程中夾角調整機構關節角度hf1與hf2的時間歷程曲線。圖12為使用和未使用穩定裕度優化控制算法兩種情況下，火星車爬坡過程中穩定裕度的時間歷程曲線。

圖11 火星車爬坡過程中hf1和hf2曲線Fig.11 Curve of hf1 and hf2 during Mars rover climbing

圖12 火星車爬坡過程中穩定裕度曲線Fig.12 Stability margin curve of Mars rover duringMars rover climbing

從圖12可以看出，未使用穩定裕度優化控制算法情況下，火星車初始時刻位于水平地面，穩定裕度為600 mm；之后，隨著火星車駛入坡面，穩定裕度逐降低至441 mm；當火星車再次駛入高度為993 mm平地后，穩定裕度又逐漸增加至600 mm。使用穩定裕度優化控制算法時，火星車穩定裕度最小值為529 mm。在火星車爬坡過程中，使用穩定裕度優化控制算法相對于未使用穩定裕度優化控制算法，火星車的穩定裕度提高了19.95%。

3.4 火星車坡面越障工況仿真與分析

火星車坡面越障過程為：火星車行駛在30°的坡面上，火星車左側三個車輪依次越過障礙，右側車輪始終在坡面行駛。單側障礙物高度為500 mm，障礙前后坡度均為20°火星車與地面摩擦系數設置為0.8。分別對使用和未使用穩定裕度優化控制算法兩種情況進行仿真，仿真時間為550 s，仿真步長為0.05 s。

仿真結果如圖13和圖14所示。圖13為使用穩定裕度優化控制算法時，火星車越障過程中夾角調整機構關節角度hf1與hf2的時間歷程曲線。圖14為使用和未使用穩定裕度優化控制算法兩種情況下，火星車越障過程中穩定裕度的時間歷程曲線。

圖13 火星車坡面越障過程中hf1和hf2曲線Fig.13 Curve of hf1 and hf2 during Marsrover crossing obstacle on slope

圖14 火星車坡面越障過程中穩定裕度曲線Fig.14 Stability margin curve of Mars rover during Marsrover crossing obstacle on slope

從圖13可以看出，使用穩定裕度優化控制算法時，初始時刻火星車位于30°的坡面上，火星車開始調節姿態以增加其穩定裕度，hf1轉動角度從初始狀態0 rad逐漸增加至0.78 rad，hf2轉動角度從初始狀態0 rad逐漸減小至-0.78 rad，使得火星車左側懸架機構的高度降低，右側懸架機構的高度升高，減小了由于坡面引起的車廂傾斜。41 s后，火星車開始進行單側越障，由于夾角調整機構轉動角度已達到可調節最大值，因此，火星車懸架機構繼續保持當前構型行駛。

從圖14可以看出，未使用穩定裕度優化控制算法情況下，火星車在坡面時，穩定裕度初始值為244 mm；之后，火星車左側車輪上坡，穩定裕度逐漸減小，在149 s時，穩定裕度降低至0 mm，火星車發生側翻。使用穩定裕度優化控制算法時，0～41 s整車穩定裕度逐漸升高，41～234 s間，火星車夾角調整機構角度達到運動邊界值，不發生變化，火星車穩定裕度逐漸降低，234 s左側車輪全部位于障礙物上，穩定裕度達到最低161mm。273 s開始，左側車輪依次離開障礙物，穩定裕度逐漸升高。

通過仿真分析可知，火星車穩定裕度優化控制算法可有效提高火星車行駛過程中的穩定裕度，以及爬坡和越障能力。相對于文獻[10-12]中采用的穩定性控制方法，本文采用的控制方法適用于單側障礙、坡面、坡面障礙等多種的非結構化環境，適用性更加廣泛。

4 結 論

針對火星車在非結構化環境行駛過程中可能存在穩定性不足而引起傾覆的問題，提出了一種基于懸架機構構型調整的火星車穩定裕度優化控制方法。得出以下結論：

1)考慮火星車車輪與地面的滾動滑移、側向滑移、車輪傾斜角度等因素，建立并完善了六輪主動懸架式火星車行駛的運動學模型。

2)推導得出非結構環境下的主動懸架式火星車穩定裕度的模型，可應用于火星車在非結構化環境行駛過程中穩定裕度實時計算和監測。

3)推導了火星車穩定裕度優化控制模型，并對控制模型做了有效簡化。通過仿真與分析得出：該控制模型適用于火星車行駛的多種非結構化環境，并有效提高了火星車的行駛穩定裕度，以及爬坡和越障能力。在火星車單側輪越障和爬坡兩種典型工況中，使用穩定裕度優化控制算法，火星車穩定裕度分別提高了32.35%和19.95%；在坡面越障工況中，未使用穩定性優化算法，火星車產生了側翻，使用穩定性優化算法后，火星車平穩地完成了坡面越障工況的行駛。