基于重心動力學及虛擬模型的單腿平臺運動控制*

譚永營，晁智強，李華瑩，韓壽松

（陸軍裝甲兵學院車輛工程系，北京 100072）

0 引言

單腿平臺的運動控制是其實現多步態(tài)運動的基礎，步行平臺所呈現的多種步態(tài)可看作各單腿運動的不同組合［1］。不論雙腿、四腿還是六腿步行平臺，其行走過程中腿部的運動均可歸結為擺動和支撐兩個階段，在不同階段腿部控制的任務不同。在擺動段，腿部運動控制的主要目標是跟蹤足端期望軌跡；而在支撐段，腿部運動控制的主要目標是保證機身各自由度期望值的跟蹤。在擺動段腿部的運動可等效看作支點固定的多自由度機械臂的擺動，在支撐段腿部的運動可等效看作單腿著地緩沖及恒定高度下施加豎直干擾力兩種情況。當腿部各連桿重量較大時，腿部的運動將對機身的運動產生較大影響。而當前在單腿運動控制過程中多僅考慮機身的重量，忽略腿部各連桿重量的影響，無法適用于腿部重量較大的單腿平臺運動控制。

單腿平臺運動控制最常用的方法之一是基于SLIP（Spring Loaded Inverted Pendulum）的控制方法。Raibert 等在步行平臺動步態(tài)研究過程中，提出了直腿型及關節(jié)型單腿運動控制的SLIP 三分控制方法，成功地將此方法應用于雙足步行平臺、四足步行平臺等的行走控制［2-4］。SLIP 模型是生物學家在步行動物研究過程中，根據動物身體運動規(guī)律及腿長變化規(guī)律提出的一種簡化控制模型，其在單腿平臺控制中應用廣泛［5-8］。但基于SLIP 的控制方法僅僅抓住了單腿運動的最基本特點，較適合應用于機身重量較大、腿部重量較小的單腿平臺。當平臺各腿段重量較大時，由于腿部重量的影響，SLIP 控制方法將會產生極大的誤差。Pratt 等針對雙足步行平臺的運動控制，提出了虛擬模型控制算法，但其應用過程中也需要腿部重量較小的假設［9-10］。文獻［11-12］將足端空間虛擬模型控制算法應用于單腿平臺著地緩沖、連續(xù)彈跳等的控制中，取得了較好的效果。文獻［13-14］應用關節(jié)空間虛擬模型控制算法，實現了步行平臺單腿的著地緩沖。當腿部重量較大時，虛擬模型控制算法無法實現對單腿運動的準確控制。

針對上述問題，為實現腿部各桿件重量較大時單腿平臺運動的準確控制，在平臺的運動學及重心動力學建?；A上，結合重心動力學和虛擬模型控制方法，實現單腿平臺的運動控制，并對控制效果進行仿真，驗證控制方法的有效性。

1 單腿平臺的運動學及動力學建模

單腿平臺的模型如圖1 所示，包括機身和腿部兩部分。單腿平臺腿部有3 個自由度，從上到下依次是胯側擺關節(jié)、胯縱擺關節(jié)及膝關節(jié)，對應的3 個腿段分別為胯側擺段、大腿段及小腿段。

圖1 單腿平臺結構

單腿平臺結構參數包括腿部各腿段的重量以及腿部各桿件的長度等，如表1 所示。

表1 單腿平臺相關參數

2 單腿平臺的運動學及重心動力學建模

整個單腿平臺的運動與浮基系統(tǒng)類似。因此，運用浮基多體動力學的方法，對單腿平臺進行運動學及動力學建模。在此，借鑒Featherstone 提出的空間六維向量對單腿平臺各桿件運動學量進行計算［15］。首先對單腿平臺的各桿件和關節(jié)進行編號，而后建立各桿件的坐標系，如圖2 所示。

圖2 單腿平臺編號及坐標系建立

根據Roberson 與Schwertassek 提出的空間多連桿系統(tǒng)單關節(jié)模型，可對相鄰連桿的運動學量進行計算，如式（1）所示：

以上各式中，式（3）與式（4）用于計算機身的速度和加速度。式（5）和式（6）中的k=2，3，4，用于計算腿部各桿件的速度和加速度。

由文獻［16］可得，單腿平臺的速度可由各桿件的速度得到，如式（7）所示：

則單腿平臺在六維空間中的關節(jié)空間動力學方程可寫作以下形式：

式中，H 為關節(jié)空間慣性矩陣，C 為偏差力矩陣，為關節(jié)驅動力矩陣，g為各連桿重力矩陣，J 為雅克比矩陣，fc為足端與地面之間的相互作用力矩陣。單腿平臺關節(jié)空間動力學的H 和C 項可由Featherstone提出的復合剛體算法與逆向動力學算法求得［14］，在此不再贅述。關節(jié)1 為假想的虛擬關節(jié)，并未施加直接作用力，因此，可將關節(jié)空間動力學方程分為機身部分和腿部各連桿部分，如式（9）中所示：

3 基于重心動力學的單腿平臺運動控制方法

單腿平臺的運動控制本質上是對重心運動的控制。當腿部重量較小時，可僅考慮機身的重量，將單腿平臺的重心近似認為在機身的重心處。但在腿部重量較大時，腿部的運動導致平臺整體重心的移動。因此，此處采用重心動力學及虛擬模型控制的方式，實現單腿平臺的運動控制。

3.1 單腿平臺支撐段的運動控制

物體所受合外力等于其質量與加速度的乘積，虛擬模型控制中控制參數易受重量變化的影響，導致無法實現平臺的期望運動。在此首先對單腿平臺的期望加速度進行計算。由虛擬模型控制可得，單腿平臺機身的期望加速度計算如下：

上式中，x¨d、y¨d及z¨d為單腿平臺機身的期望加速度。xd、yd及zd為機身的期望位置，x˙d、y˙d及z˙d為機身期望速度，kx、ky、kz及kx˙、ky˙、kz˙分別為各方向相應的位置及速度增益系數。

由于重心動力學控制方法中需要用到單腿平臺腿部各關節(jié)的加速度，因此，在得到單腿平臺期望加速度后，結合雅克比矩陣可得腿部關節(jié)的期望加速度如下：

單腿平臺重心動力學控制的優(yōu)點是可以將腿部的重量考慮到控制過程中。因此，結合單腿平臺的重心動力學式（12）～式（14）可得，在支撐階段實現期望運動所需的足端力為：

在得到單腿平臺足端與地面接觸力后，運用雅克比矩陣，將足端力轉換到關節(jié)空間，如式（18）所示：

式中，為單腿平臺腿部各關節(jié)的力矩1，包括胯側擺關節(jié)的力矩2、胯縱擺關節(jié)的力矩及膝關節(jié)的力矩3。

3.2 單腿平臺擺動段的運動控制

在單腿平臺運動的過程中，腿部在擺動段相對機身邁出一定距離，以實現支撐點的切換。在擺動段采用虛擬模型的控制方法實現擺動腿的控制，通過虛擬元件的作用使足端沿期望軌跡運動。單腿平臺支撐段足端受力計算方法如下：

式中，fsx、fsy、fsz分別為擺動腿足端受力沿1 坐標系各軸的分量，xfd、yfd、zfd及x˙fd、y˙fd、z˙fd為足端在機身坐標系中的期望位置及期望速度，不帶下標fd 的變量為各量的實際值，k 為增益系數。

在得到足端受力后，仍然應用雅克比矩陣，將足端受力轉換到驅動關節(jié)，如式（20）所示：

4 步行平臺單腿控制仿真

在步行平臺行走過程中，單腿在支撐段的運動，可看作著地緩沖及機身對單腿施加干擾力兩種情況，下面分別用文中控制算法及虛擬模型控制算法［17］對單腿平臺進行控制仿真，并對控制效果進行對比分析。

4.1 單腿平臺運動過程分析

單腿平臺的運動主要包括著地緩沖恢復以及豎直干擾兩種。此處對單腿在兩種情況下的運動進行簡單分析，以找到合理的狀態(tài)機。單腿平臺著地緩沖與恢復的過程如圖3 所示。單腿平臺整個運動過程可分為下落、緩沖及恢復3 個階段。在單腿平臺開始運動后，足端與地面之間接觸力fz的有無可作為判斷是否開始緩沖的狀態(tài)機。在足端著地后，緩沖段與恢復段的區(qū)別是單腿平臺豎直速度v1z的方向，因此，可利用豎直速度v1z的正負來判斷恢復階段開始與否。

圖3 運動過程分析

單腿豎直干擾過程包括豎直下壓與高度恢復兩個階段，因此，僅通過平臺機身豎直速度v1z的正負即可對平臺所處的階段進行判斷。

4.2 單腿平臺運動控制仿真

為驗證重心動力學與虛擬模型控制的有效性，下面運用Adams 與Simulink 對單腿平臺豎直干擾及下落緩沖過程進行仿真。在Adams 中建立單腿平臺虛擬樣機模型（如圖4 所示），將控制算法寫入Simulink，并建立兩者的接口模塊。Simulink 中控制算法計算出的力矩施加于Adams 中單腿平臺驅動關節(jié)處，通過關節(jié)的運動實現單腿平臺的著地緩沖及抵抗豎直干擾。

圖4 Adams 中建立的模型

4.2.1 單腿平臺豎直干擾仿真

單腿平臺初始時刻以一定姿態(tài)站立在地面上，從1 s 開始在機身上施加豎直向下并線性增大的干擾力，在2 s 左右干擾力達到最大值300 N，而后突然撤去干擾力，如圖5 中所示。分別用重心動力學及虛擬模型、僅虛擬模型的控制方法，對單腿平臺豎直干擾過程進行仿真，并記錄平臺機身高度的變化。仿真過程中機身的目標高度為0.6 m。圖5 中，以VM 表示僅虛擬模型控制時單腿平臺相關參數的變化，CG 表示重心動力學及虛擬模型控制時相關參數的變化。

圖5 豎直干擾力

由圖6 可得，在重心動力學及虛擬模型控制中，未受干擾力時機身豎直高度的誤差在-0.013 m～-0.012 m 之間變化，在豎直干擾過程中機身豎直高度誤差在-0.044 m～-0.013 m 之間變化；而在虛擬模型控制中，未受干擾力時機身高度的誤差在-0.021 m～-0.019 m 之間變化，在豎直干擾的過程中機身高度誤差在-0.071 m～-0.019 m 之間變化。因此，與僅虛擬模型控制時相比，重心動力學及虛擬模型的控制方法使機身高度誤差減小約37%。單腿平臺豎直干擾仿真證明了重心動力學及虛擬模型控制的優(yōu)越性。

圖6 單腿平臺高度誤差

4.2.2 單腿平臺著地緩沖仿真

為實現單腿平臺的著地緩沖與恢復，首先參考文獻［18］中的方法，規(guī)劃單腿平臺機身的軌跡。采用恒加速度的緩沖方式對單腿平臺著地過程進行規(guī)劃，實現一定距離內平臺速度的緩沖。在恢復階段，由于初始及結束時的加速度、速度與位置均為已知量，因此，采用5 次曲線對恢復階段進行規(guī)劃。假設tc1與tc2分別為緩沖過程及恢復過程所用時間，則單腿平臺的軌跡如式（21）所示：

式中，h1、h2及h3分別為平臺緩沖相初始、緩沖結束時及恢復過程結束時機身的高度，v1為緩沖相初始機身的速度，a1為緩沖相初始的加速度。假設單腿平臺下落高度為0.1 m，h1、h2、h3分別為0.6 m、0.53 m、0.6 m，tc1為0.1 s，均tc2為0.2 s，則單腿平臺機身的軌跡如圖7 所示。

圖7 機身軌跡

分別用重心動力學及虛擬模型、僅虛擬模型的控制方法，使單腿平臺的機身跟蹤期望軌跡，并記錄相關參數的變化，如圖8～圖9 所示。

圖8 機身高度誤差

圖9 機身速度誤差

由圖8～圖9 可得，兩種控制方法均能實現單腿平臺的著地緩沖，但控制效果有較大差異。由圖8可得，在重心動力學及虛擬模型控制中，機身高度誤差的絕對值保持在0～0.004 m 范圍內，而在僅虛擬模型控制中機身高度誤差在0～0.008 m 范圍內，且平臺高度出現較大波動。因此，與虛擬模型控制時相比，重心動力學及虛擬模型控制方法使高度跟蹤誤差減小約50%。由圖9 可得，在重心動力學及虛擬模型控制中，單腿平臺豎直速度跟蹤誤差的絕對值在0～0.001 6 m/s 范圍內變化，并且豎直速度跟蹤誤差波動較??；而在僅虛擬模型控制中，豎直速度跟蹤誤差的絕對值在0～0.2 m/s 范圍內變化，且速度跟蹤誤差的波動較大。單腿平臺著地緩沖仿真結果進一步證明了重心動力學及虛擬模型控制的有效性及優(yōu)越性。

5 結論

本文針對腿部重量較大的單腿平臺，提出一種基于重心動力學及虛擬模型的控制方法，并運用動力學仿真驗證了控制方法的有效性。得到的結論如下：

1）重心動力學及虛擬模型的控制方法，既保留了虛擬模型控制實現簡單的優(yōu)點，又能依靠重心動力學提高單腿平臺位置及速度的跟蹤精度；

2）重心動力學及虛擬模型控制方法，能夠減小單腿平臺位置及速度跟蹤時的波動；

3）重心動力學及虛擬模型控制方法，通過重心動力學實現了對較大腿部重量的適應性。