基于前饋補償的輪腿式機動平臺姿態自適應控制

劉輝, 劉寶帥, 廖登廷, 韓立金*, 崔山

(1.北京理工大學 機械與車輛學院, 北京 100081; 2.北京理工大學 前沿技術研究院(濟南), 山東 濟南 250300)

0 引言

輪腿式機動平臺運動機構由腿和車輪組成,通過腿部關節與輪協同實現作動,具備輪式平臺高機動性、腿式平臺高通過性和環境適應性特性,在戰場偵察、廠區巡邏、災后救援及資源勘測等領域具有廣泛的應用前景[1-3]。國內外學者在輪腿式機動平臺運動控制領域開展了大量工作,如瑞士蘇黎世聯邦理工大學在ANYmal足式平臺上開發了四輪腿平臺[4]和Ascento兩輪腿平臺[5]、北京理工大學開發了四輪腿并聯液壓機器人[6]等。與腿足式機動平臺不同,輪腿式平臺由4個腿式結構和4個輪式結構串聯組成執行機構,運動過程中需要各個驅動電機協調工作,進而實現不同的動作與任務[7]。

當輪腿式機動平臺車輪滾動行駛時,可利用作動關節主動調整車身姿態角度與高度,能夠執行偵察、巡邏等任務,對平臺任務執行與功能拓展具有重要意義[8]。因此,國內外學者在輪腿式機動平臺姿態主動控制方面開展了大量研究。Grand等[9]針對Hylos輪腿機器人,基于運動學和逆運動學模型,提出一種路徑軌跡和姿態解耦控制策略。Du等[10]提出一種基于質心運動空間動量的分層控制框架,將腿式運動和輪式運動解耦,以實現不同模擬路面下的平臺質心運動、腿式運動和輪式運動。劉本勇等[11]基于空間機構學位姿變換推導質心姿態與輪心位置關系,并利用非線性規劃遺傳算法對姿態控制過程中的質心穩定性條件進行了優化。韓子勇等[12]基于質心動力學方法和二次優化方法,通過優化車輪-地面反作用力實現了對搖臂懸掛機動平臺姿態主動控制。此外,姿態控制也是腿足式機動平臺的研究重點。Carlo等[13]簡化了Cheetah 3四足機器人關于地面反作用力的動力學模型,利用凸優化算法求解關于地面反作用力的模型預測控制問題,提升了機器人高動態運動的穩健性和魯棒性。張國騰等[14]建立了作用于虛擬質心的虛擬力與支撐腿關節扭矩之間的數學模型,通過調整軀干虛擬力實現了軀干高度與姿態的控制。然而,輪腿式機動平臺運動過程中質心位姿與路面環境的關系是非線性并不可測的,尤其是未知環境下輪端易受到地面的沖擊、碰撞、不規則地面摩擦等問題影響,極易出現作動系統“虛腿”情況,進而增大了姿態誤差。

目前,開展輪腿式機動平臺姿態自適應控制的關鍵點主要包括:1)獲取輪胎與地面的接觸狀態,目前主流方法主要包括有觸地力傳感器[15]和無觸地力傳感器[16];2)閉環控制中獲取的傳感器反饋信息存在“毛刺”,需要通過狀態觀測器來提升反饋參數精確度。Camurri等[17]通過檢測關節力矩來估計地面對機器人的作用力,實現對HyQ四足機器人的沖擊檢測和狀態估計。在六足機器人方面,Yang等[18]采用平方根無跡卡爾曼濾波器融合處理多傳感器的信息,以接觸點為中心的腿部關節傳感器來實現精準的狀態估計,其抗噪聲干擾性能比以慣性測量單元(IMU)信號為中心的狀態觀測器更好。Bloesch等[19]利用擴展卡爾曼濾波器設計了一種基于IMU信號的狀態觀測器,用以對機器人進行狀態估計。Qin等[20]在雙足機器人足端配置了六軸力傳感器,利用足端與地面的接觸面受力狀態距離摩擦圓錐邊界的遠近來判斷發生相對滑動的概率,以提升輪-地接觸判別準確率。區別于腿足式機動平臺,輪腿式機動平臺的輪胎滾動行駛時,輪胎與地面摩擦產生的驅動力也會影響其輪端接觸點受力,進而影響接觸狀態的判斷。綜上,利用單一參數難以滿足輪腿式機動平臺接觸狀態估計。

本文對輪腿式機動平臺姿態控制進行解耦,建立了基于空間向量的質心動力學模型,推導了運動空間內質心位姿與輪端位置的顯示數學關系。綜合考慮輪端的地面反力和縱向驅動力建立了輪端接觸狀態估計器,結合腿部高度觀測器和接觸狀態估計對腿部高度進行前饋補償控制,提高輪-地接觸狀態估計和質心位姿控制的精確度。在輪腿式機動平臺的浮動基坐標系構建整體簡化模型轉化為二次優化問題,實現支撐相下垂向力和縱向力的最優分配,進而獲取輪腿式機動平臺前饋力矩,提高平臺對崎嶇不平路面的適應能力。

1 輪腿式機動平臺結構與組成

輪腿式機動平臺主要由車身系統、感知系統、行走系統等組成,如圖1所示,車身系統內部安裝電池、DC、IMU、主控制器等部件;感知系統的感知控制套件Jetson Agx Xavier固定在車身內部,16線激光雷達、深度相機D435i、2個RTK天線等安裝在車身表面,可實時采集行進過程中的路況信息;行走系統由4個單輪腿系統組成,采用肘-膝式布置方案,大腿由髖關節電機直接驅動,小腿由膝關節電機驅動同步帶實現作動,車輪由輪邊電機直接驅動,關鍵參數見表1。在行走系統的髖關節位置設計由摩擦片和扭簧組成的減振系統,可在一定程度上降低來自地面對行走系統的振動,滿足不同路面環境下實現高效穩定運行。

表1 輪腿式機動平臺關鍵參數

圖1 輪腿式機動平臺整體構型

圖2 輪腿式機動平臺坐標系和運動空間定義

(1)

式中:qh和qk分別表示髖關節和膝關節的旋轉角度。根據式(1),運動學逆解模型可以表示為

(2)

2 輪腿式機動平臺質心動力學模型構建

本文建立了關于質心運動線動量和角動量的質心動力學模型。首先,通過控制質心運動實現輪腿式機動平臺的姿態調節控制,其中質心空間慣量由各個子部件的空間慣量組成,即

(3)

(4)

3 基于前饋補償的姿態自適應控制策略

輪腿式機動平臺在不規則障礙地形中行進時,需要保持質心姿態和高度,進而確保平臺平穩運行。下面主要介紹本文提出的基于前饋補償的姿態自適應控制策略,包括位姿狀態估計、姿態高度補償、運動規劃與控制(見圖3),確保輪腿式機動平臺在運動空間內輪腿與地面始終平穩接觸。

圖3 基于前饋補償的姿態自適應控制策略

為使平臺車身始終保持一定相對位置和姿態角度行駛,質心位置與姿態角度的變化率可以表示為

(5)

(6)

式中:Rc為質心平移時變換矩陣;P′c為初始質心位置向量;hc為質心位姿變換向量;j=1,2,3,4。在車身出現俯仰與側傾運動時,輪端位置變換矩陣為

(7)

(8)

(9)

式中:R為高度變換矩陣。因此,各個輪端位置向量代入逆運動學中,即可以獲得各個關節作動角度。

3.1 位姿狀態估計

輪腿式機動平臺配置了IMU傳感器,但由于其自身精度限制、零偏誤差以及其他振動、電磁等干擾的原因,導致采集的姿態角度信號中摻雜噪聲干擾,給閉環控制算法中信號差分和位姿解算帶來誤差。為提高控制精度,本文利用拓展Kalman濾波(EKF)算法對質心姿態角度進行估計,設計了質心位姿估計的狀態變量x,其表達式為

(10)

姿態角度和速度偏差的離散化微分方程可以表示為

xt+1=Ttxt+et

(11)

(12)

由于平臺無橫擺關節設計,本文僅考慮其縱向運動和垂向運動中的俯仰角和側傾角。為提升估計精度,狀態估計微分方程考慮了測量誤差和過程隨機誤差均值,即

(13)

式中:wt為過程誤差。基于式(11)～式(13)中給定的時序離散狀態微分方程,測量方程為

Yt+1=h(xi,ζt)

(14)

式中:ζt=et+wt為測量誤差與過程誤差之和。因此,基于EKF算法的狀態變量與觀測變量的離散時序模型可以表示為

(15)

3.2 觸地狀態檢測與車身姿態補償策略

圖4 左前腿坐標系定義

對于單個輪腿執行機構(見圖4),其腿部和車輪運動與地面之間構成一個串并聯機構,將腿部作動機構簡化為桿件,平穩運行狀態下的利用拉格朗日方法構建的動力學模型可以表示為

(16)

(17)

式中:PO1、PO2和PO3分別為大腿、小腿和車輪的質心位置坐標,如圖4所示。對位置坐標進行微分可求得各坐標原點的運動速度,可獲取各個桿件與車輪的動能Ek和勢能Ep,可得拉格朗日函數為

L=Ek1+Ek2+Ek3-Ep1-Ep2-Ep3

(18)

聯立式(16)～式(18),考慮小腿端部外力f∈R3×1,可得拉格朗日方程中的各系數矩陣為

(19)

(20)

(21)

G=[G1,G2,G3]T

(22)

(23)

式中:m2為表示小腿質量;I3為輪端旋轉慣量;l=m2L1L2mκ=(0.5m1+0.5m2+m3)gL1,m3為輪端電機和輪胎質量;mν=(0.5m2+m3)gL2;qhk=qh-qk。

根據式(19)、式(23),f∈R3×1可以表示為

(24)

式中:f=[fx,fy,fz]T;τ*為關節和驅動輪力矩。根據式(24),可以獲取輪腿式機動平臺小腿端部與輪腿軸心的垂向支撐力fz。與足式平臺不同,輪腿式機動平臺通過帶驅動電機的輪腿與地面實現接觸,輪邊驅動電機產生驅動力矩τw,進而產生縱向驅動力fxw克服與行進速度相反方向的縱向摩擦力實現期望運動,即

(25)

式中:Rw表示輪胎半徑。本文僅考慮平臺縱向和垂向運動,輪端受力可以表示為

(26)

(27)

(28)

(29)

根據分析,本文設定閾值σm判斷輪腿式機動平臺中輪胎與地面接觸狀態,即

(30)

根據分析,獲取當前各個腿部高度對控制器高度補償具有重要作用。因此,根據平臺腿部作動系統反饋實時角度位置關系,建立平臺各個腿部高度值觀測器,如式(31)所示:

(31)

圖5 車身姿態實時補償算法

3.3 運動控制策略

圖6 輪腿式機動平臺整體質心虛擬模型

(32)

(33)

式中:I∈R2×2為單位矩陣;pi×∈R2×2表示各個腿部作動機構相對于質心的位置坐標;fm、τm分別為作用于機身質心的廣義力和廣義力矩。基于式(33),質心虛擬力分配任務可以轉化為二次規劃問題,即

min (Af-b)TS(Af-b)+αfTWf
s.t.Cf≤dm

(34)

根據上文對輪-地接觸穩定分析,在開展各腿部作動機構虛擬力分配時需要考慮其所需虛擬力的取值范圍,不僅可使輪端與地面實現穩定接觸,亦可確保各關節和輪邊驅動電機穩定可靠輸出,即

(35)

式中:f*,min和f*,max分別表示不同方向分配虛擬力的最小值和最大值;μ為輪胎與接觸路面的靜摩擦系數,不等式矩陣Cf≤dm可以表示為

(36)

基于二次規劃算法求得的各個腿部作動機構的虛擬力分配結果,結合運動學和力平衡分析求得的雅克比矩陣,得到作用至關節電機的驅動力矩,該力矩作為驅動關節的前饋力矩,其表達式如下:

(37)

(38)

利用PD控制算法可以求得反饋力矩,即

(39)

4 仿真分析

本文利用數學仿真軟件建立了輪腿式無人機動平臺多剛體模型,根據設計構型和傳感器配置調用模型庫進行參數設置,模擬車身中配置的IMU獲取質心側傾、俯仰和橫擺姿態角度和角速度,組合導航獲取車身實時位置和速度,關節連接模塊獲取關節角度、角速度和驅動力矩等參數。為驗證所提出的控制算法,本文建立了一個由雙邊橋、波形障礙及側坡組成的路面環境,如圖7所示。在仿真過程中,設定期望運動軌跡,其中在起步階段0～1 s時平臺縱向速度從0 m/s加速至0.3 m/s,然后平臺以期望俯仰角為0°、側傾角為0°、質心高530 mm、縱向速度為0.3 m/s的速度勻速前進通過障礙。

圖7 輪腿式機動平臺運行及工況示意圖

圖8為在不同障礙場景下的輪腿式機動平臺的姿態自適應仿真效果,首先在t=0 s時仿真開始,然后在t=4 s時進入第①階段的雙邊橋障礙,在本階段中機身姿態角度和高度始終與初始狀態一致,通過第①階段后進入兩個波形障礙的第②階段,在本階段中僅有一個車輪越過波形障礙,其余車輪均在平直路面,驗證平臺在短波形障礙下機身姿態調整水平。此外,采用側坡工況驗證輪腿式機動平臺側傾姿態控制效果,輪腿式機動平臺在第③階段中為左側車輪開始上側坡,等左側前后兩個車輪都上坡后進入第④階段,此時右側兩個車輪在水平路面上,左側兩個車輪在側坡障礙行駛,通過調整左右側腿部高度,實現姿態角度和機身質心高度始終保持與初始狀態保持一致,然后在圖8中第⑤階段左側車輪依次離開側坡,兩側車輪均進入水平路面。

圖8 輪腿式機動平臺仿真運動過程

圖9(a)和圖9(b)為輪腿式機動平臺車身質心姿態角度仿真效果,平臺在第4.4 s開始接近障礙,在第37.2 s時平臺左后輪離開障礙。從圖9中可以看出,實際俯仰角和側傾角的最大跟蹤誤差分別為-0.862°和0.369°,平均誤差分別為-0.276°和 0.102°。圖9中,通過對比姿態角度的實際值與估計值,其俯仰角與側傾角估計值最大誤差為0.014°和0.009°,平均誤差值分別為-0.005°和 0.001°。

圖9 車身質心姿態角度結果

圖10和圖11為輪腿式機動平臺縱向與垂向的速度與位置的跟蹤仿真結果。由圖10可以看出,輪腿式機動平臺質心的期望縱向速度為0.3 m/s,實際行駛速度會在越障過程中出現波動,縱向跟蹤終止位置為11 754 mm,與期望最終位置為11 850 mm,最終跟蹤誤差96 mm。從圖11中可以看出,輪腿式機動平臺垂向位置的最大跟蹤誤差為5 mm,平均跟蹤誤差為-2 mm。圖12(a)和圖12(b)分別表示在該工況下左前腿和右前腿的各個關節運行角度和力矩結果。在行駛過程中,輪腿式機動平臺關節力矩作動平緩,關節力矩最大值<60 N·m,滿足關節力矩閾值約束。

圖10 平臺質心縱向速度與位置仿真結果

圖11 平臺質心垂向速度與位置仿真結果

圖12 輪腿式機動平臺左/右前腿關節運動角度與力矩結果

圖13(a)、圖13(b)分別為不同腿部作動機構下輪-地接觸檢測指令結果,經統計,無前饋補償下不同輪腿作動系統中輪-地接觸指令為0情況占比分別為0%、5.717%、2.632%和5.080%,而帶有前饋補償下指令0情況占比0%、4.380%、2.790%和4.367%。對比可知,帶有前饋補償策略可以明顯降低輪-地接觸為0的概率。

圖13 輪腿作動機構的輪-地接觸指令結果對比

通過Simscape模型中的接觸工具箱模型,獲取了車輪與地面的垂向支撐力,如圖14所示,對比了帶有前饋補償策略和不帶前饋補償策略下的輪-地接觸力。表2對比了兩種策略對俯仰角和側傾角的影響情況,其中所提出的控制算法對俯仰姿態精度具有明顯的改善。與地面處于不穩定接觸狀態時,車輪具有離地騰空的趨勢。所提出的前饋補償策略提高了車輪與地面的垂向支撐力,提升了與地面的接觸概率,可有效降低作動過程中不穩定接觸的情況,提升其行駛過程中驅動輪的穩定性,同時有效提升了其姿態保持的控制效果與精度。

表2 仿真結果對比

圖14 輪腿式機動平臺輪-地接觸力仿真實驗結果

仿真在CPU Inter Corei7-10750H 2.6 GHz、RAM 16G的筆記本電腦上進行。本文統計了所提出算法中耗時最長的二次規劃算法,如圖15所示,其中計算耗時最長為0.872 ms,平均耗時為0.225 ms,這就表明所提出的算法具備應用于實時控制的潛力。

圖15 二次規劃算法求解時間

5 結論

本文以輪腿式機動平臺為研究對象,針對復雜路面下姿態控制精度與輪-地接觸力分配的問題,研究了機身姿態自適應控制算法。得出主要結論如下:

1) 分析了輪胎與地面接觸時輪-地接觸點摩擦約束條件,在考慮輪端垂向支撐力和縱向驅動力的基礎上構建了觸地狀態估計器。

2) 結合高度估計器對運動狀態下輪腿平臺腿部高度進行前饋補償。為輪腿機動平臺在非結構路面環境下姿態自適應控制的精確性和平穩性提供基本依據和支撐。

3) 為驗證所提出方法的可行性與有效性,利用數學仿真軟件進行仿真,對比了無前饋補償策略和帶前饋補償的機身姿態自適應控制策略,仿真結果符合預期。

4)在未來工作重點中,將對輪腿式機動平臺在未知全地形環境下的魯棒控制開展研究,進一步考慮抑制多源未知干擾信息對運動控制算法的影響。