變長度柔性雙足機器人行走控制及穩定性分析

廖發康，周亞麗，張奇志

（北京信息科技大學 自動化學院，北京 100192）

0 引言

1990 年，McGeer［1］提出了被動動態行走（Passive Dynamic Walking，PDW）模型，在雙足機器人領域取得了里程碑意義的研究成果。與主動控制系統［2］相比，基于PDW原理設計的機器人效率要高得多，因為此類機器人的運動是通過腿的自然擺動來維持的，而不是通過安裝在每個關節上的電機來驅動的。但是被動行走步態單一，抗擾性能較差，容易出現混沌特性，進入不穩定狀態［3］；而且由于擺動腿落地瞬間，與地面之間的沖擊存在能量損失，采用剛性腿的被動行走機器人不能在水平面上持續行走，且剛性腿太僵硬，不具有擬人性。1989 年，Blickhan［4］首次提出了彈簧-質點模型，用以描述人類的奔跑和跳躍。研究表明觸地時間和跳躍頻率不是簡單地由彈簧-質量系統的固有頻率決定的，而是很大程度上受著陸速度矢量的影響。雖然人類的腿具有復雜的神經控制系統，但在跑步階段，腿會表現出簡單的彈簧狀行為，可以用彈簧-質點模型來描述。2006 年，Geyer 等［5］對彈簧-質點模型進行擴展，該模型將雙支撐作為行走過程中的重要組成部分，用簡單的彈簧負載倒立擺（Spring-Loaded Inverted Pendulum，SLIP）模型模擬了人類在平面上的運動。Geyer 的研究表明：帶彈簧的柔性腿可以準確地再現人體行走過程中髖關節運動軌跡和站立時的動力學特性。雙足機器人采用柔性腿代替剛性腿，借助于彈簧的彈性勢能，可以實現平地上的類人行走。2010 年，Rummel 等［6］通過對機器人行走時腿部剛度與魯棒性之間的關系研究，得出了可變腿剛度能有效地抑制外部干擾的結論。2012 年，Visser等［7］把SLIP 擴展為變腿剛度模型（Variable leg stiffness-SLIP，V-SLIP），顯著提高了雙足機器人對外界干擾的魯棒性。2013 年，Visser 等［8］將V-SLIP 模型擴展為考慮擺動腿動力學的V-SLIP 模型（V-SLIP model with Swing Leg dynamics，V-SLIP-SL），即在每條腿的末端增加了一個腳質量。仿真結果表明，在控制器的作用下，機器人具有足夠的魯棒性，能夠抑制由擺動腿動態所引起的擾動。2020 年，Pelit 等［9］以被動SLIP-SL 模型作為模板，為5 連桿全驅動雙足機器人的質心和擺動腳提供所需的軌跡。仿真結果表明，該控制器可以跟蹤由SLIP-SL 模型獲得的軌跡，并且簡單的SLIP-SL 模型比高自由度模型在計算方面更有優勢。

盡管雙足機器人的擴展及變剛度控制能使機器人擁有良好的魯棒性能，但變剛度實驗裝置的設計和制造［10-11］都比較復雜，而變長度的實驗裝置要簡單得多［12］。2008 年，Asano 等［13］針對平面伸縮腿雙足機器人模型，提出了一種簡單的控制律，在不考慮零力矩點（Zero Moment Point，ZMP）條件的情況下，通過伸縮擺動腿即可輕松實現水平步態生成。2012 年，Asano［14］引入了一個由8 個伸縮腿組成的平面無緣輪模型，并將該方法推廣到帶有制動彈簧的平面伸縮腿雙足機器人，通過數值仿真驗證了該方法的有效性。2018年，Harata 等［15］研究了包含伸縮腿的欠驅動雙足機器人的穩定性。將輸出跟隨控制律應用于機器人的線性化運動方程中，將被控機器人的運動方程指定為一個線性時變系統，建立了誤差轉移方程來評估穩定性，通過數值計算說明了腿部伸縮運動對穩定性的影響。

上述文獻的變長度伸縮腿控制基本采用的都是剛性腿，本課題組在此基礎上，對雙足機器人變長度的柔性伸縮腿控制展開研究。2019 年，張奇志等［16］研究半被動伸縮腿雙足機器人行走控制和周期解的全局穩定性問題，設計了腿伸縮長度的支撐腿角度反饋控制率，證明了伸縮腿雙足機器人行走過程不動點的全局穩定性；同年，宋嘉琦等［17］采用變長度柔性伸縮腿結構，研究了半被動雙足機器人的平面穩定行走的控制問題，結果表明變長度控制算法可以實現機器人的穩定行走，并具有較強的抗干擾性。2022 年，張瑞等［18］用半被動變長度彈性伸縮腿雙足機器人模型，提出了一種基于能量的仿人控制方法。結果表明，所提出的控制策略可使雙足機器人在水平面上實現起步行走過程，并對外部的擾動具有一定的抗擾性。

本文在上述研究成果的基礎上，提出了考慮軀干影響和擺動腿動力學的變腿長度柔性雙足機器人（Variable Leg length SLIP with Torso and Swing Legs dynamics，VL-TSLIPSL）模型。首先，建立柔性雙足機器人的動力學模型，仿真求解出雙足機器人被動行走的自然步態，將其作為期望步態；其次，采用反饋線性化控制方法，通過對雙足機器人腿長度變量和髖關節力矩的控制，實現機器人的穩定周期行走；最后，采用Newton-Raphson 迭代法和龐加萊映射相結合，求解不動點并分析系統的穩定性。

1 雙足機器人動力學模型

1.1 機器人模型描述

雙足機器人的模型如圖1 所示。由質量為M的髖關節質點、中心質量為m的剛性桿及兩條長度可變的柔性腿組成，每條腿的末端各有一個腳質量mf。髖關節質點坐標為[x，y]=：qh∈Qh，其中Qh表示二維的實數域。軀干長度為l，轉動慣量為I；腿長度為L0+ui，L0為原長度，ui(i=1，2)為可控的長度增量。兩腿各有一個彈性系數為k0的單向彈簧。髖關節力矩τ1和τ2用來控制軀干姿態角α和擺動腿與法線夾角β。

圖1 VL-TSLIP-SL模型Fig.1 VL-TSLIP-SL model

圖2 為機器人的運動過程。其初始狀態是左腿作支撐腿，右腿作擺動腿，呈垂直站立（Vertical Leg Orientation，VLO）狀態，擺動腿不與地面接觸，此時為單支撐（Single Support，SS）階段。當髖關節下降到高度S時，此時的S＝L0sinθ稱為切換高度，擺動腿以原始長度L0和角度θ接觸地面，然后機器人進入雙支撐（Double Support，DS）階段。當髖關節升高到高度S時，系統再次進入SS 階段，左腿變為擺動腿，右腿變為支撐腿，考慮到雙腿的對稱性，圖2 中所表示的從左腿作支撐腿到右腿作支撐腿的過程稱為機器人的一個步態周期，如此循環反復。

圖2 雙足機器人的運動過程Fig.2 Moving process of biped robot

由此可得，從SS 過渡到DS 的條件是：

DS 切換為SS 的條件為：

其中Li是后支撐腿的長度。

1.2 系統動力學模型

在圖1 中，以支撐腿與地面的接觸點為坐標原點O，建立圖中所示直角坐標系，利用拉格朗日方程建立系統的動力學方程。定義拉格朗日函數為：

其中：K為系統的動能，P為系統的勢能。

系統動力學方程為：

其中：n為系統的自由度，qi為系統的變量，Fi為作用于系統的廣義力。

1.2.1 單支撐階段的動力學方程

SS 階段的簡化模型如圖3 所示。右腿是擺動腿，長度為L0，與法線夾角為β，因為在此階段不需要對右腿進行變長度控制，所以假設為剛性腿。在這個階段，左腿長度增量為u1，力矩τ1和τ2是控制髖關節質點運動，軀干姿態角和擺動腿夾角的控制輸入，系統狀態變量為［x，y，α，β］T=：qSS∈QSS（QSS={qSS∈QSS|L0sinθ＜y＜L0}）。

圖3 SS階段的簡化模型Fig.3 Simplified model of SS phase

分別計算系統的動能K和勢能P，可得拉格朗日函數的表達式為：

1.2.2 雙支撐階段的動力學方程

DS 階段的簡化模型如圖4 所示。此時雙腿接觸地面，由于右腿與法線的夾角β可以通過髖關節位置和腳觸地點計算得到，因此系統的狀態變量為[x，y，α]T=：qDS∈QDS。在這個階段中，雙腿長度增量u1、u2和力矩τ1是控制髖關節運動和軀干姿態角的控制輸入，其中QDS為QDS={qDS∈QDS| 0 ＜y≤L0sinθ}。

圖4 DS階段的簡化模型Fig.4 Simplified model of DS phase

采用與SS 階段相同的推導方法，得到DS 階段的動力學方程：

由式（9）～（10）可知，系統在SS 階段為欠驅動模型，在DS 階段為全驅動模型，但總的來說，系統是欠驅動系統。

1.3 狀態空間表達式

為了便于后續的反饋線性化控制器的設計，在這里將動力學方程轉換為標準的狀態空間表達式。在SS 階段，令：

2 控制器設計

2.1 控制目標

對于文中所要研究的雙足機器人，設定的控制目標是：在保證軀干直立的情況下，使髖關節質點跟蹤期望高度y*(x)和期望水平速度，并使擺動腿按期望軌跡β*（x）擺動。由于機器人在穩定行走過程中，水平位置x在時間上是嚴格單調遞增的，因此本文采用x替代時間t作為自變量，即采用空間軌跡方法。

為了避免符號混亂，省略函數參數z1。定義控制目標的誤差函數為：

控制目標是使誤差盡可能收斂到0 的鄰域。

2.2 反饋線性化控制

本文通過反饋線性化方法設計控制器，來控制腿的伸縮長度和髖關節力矩，使機器人的行走能夠逐漸逼近期望的步態，軀干傾角和擺動腿夾角收斂到期望角度。需要說明的是：由于系統在SS 階段有4 個自由度，但是只有3 個控制輸入，屬于欠驅動狀態，因此采用部分反饋線性化（Partial Feedback Linearization，PFL）的控制方式［19］，水平方向速度誤差ev在SS 階段不受控制。在DS 階段，兩腳都與地面接觸，不需要對誤差eβ的動態控制，屬于全驅動狀態，因此采用標準反饋線性化的控制方式?？紤]到在單支撐階段，不是用3個控制輸入控制4 個被控變量；而是用3 個控制輸入控制3個被控變量，另一個被控量不控制，所以單支撐欠驅動和雙支撐全驅動的反饋線性化控制器的設計是一樣的。

2.2.1 單支撐階段

在SS 階段，控制變量為髖關節高度軌跡y、軀干姿態角α和擺動腿夾角β，則系統的狀態空間表達式為：

為了將該非線性系統轉換為線性標準型，首先需要確定誤差的關系度γSS={γp，γα，γβ}。

對于位置誤差ep：

所以關系度γp=2，同理可得γα和γβ的關系度也為2。因此，在SS 階段中，誤差的關系度γSS=｛γp，γα，γβ｝=｛2，2，2｝。

根據反饋線性化原理，可得誤差動力學方程為：

2.2.2 雙支撐階段

在DS 階段，控制變量為髖關節高度y、軀干姿態角α和前進速度x˙。采用與SS 階段相同的方法，可以得到關系度γDS={γp，γα，γv}={2，2，1}，則誤差動力學方程為：

3 軌道穩定性分析

被動行走周期步態的特點是系統的狀態變量每隔一個周期重復取值一次，在狀態空間中表現為周期軌道，又稱極限環，因此被動行走周期步態的局部穩定性采用軌道穩定性來定義。從直觀上講，周期步態是軌道穩定的是指周期軌道對應的某個狀態受到有限擾動后偏離了原有的周期軌道，但系統的狀態最終運動到其附近的一條周期軌道上。如果狀態受到擾動后，系統在自身的作用下仍能回到原來所在的周期軌道上則稱周期步態是軌道漸近穩定的。

分析周期軌道穩定性的常用辦法是通過龐加萊映射（Poincaré map）將周期軌道穩定性問題轉化為不動點的穩定性問題［20-21］。本文將擺動腿觸地瞬間所在的超平面取為龐加萊截面Σ，兩個連續觸地事件之間的關系可以用龐加萊映射表示為P：Σ→Σ，即

其中：P代表龐加萊映射，為龐加萊截面上的6 維狀態變量。狀態變量不包含髖關節位置（x，y），這是因為水平位置x為自變量，垂直位置y可由y=L0sinθ計算得到。因此，選擇合適的龐加萊節截面及采用空間軌跡的分析方法，可以將狀態變量維數由8 維降低到6 維，可以有效降低算法的復雜性及運算量。

對于周期相位軌跡，龐加萊截面上的狀態變量為常數，即

其中：φ*為龐加萊映射的不動點。因此，通過龐加萊映射，雙足機器人的穩定性問題轉換為龐加萊映射不動點的穩定性問題。

3.1 求解不動點

求解龐加萊映射不動點的常用方法是Newton-Raphson迭代法。用φk表示軌道第k次穿過龐加萊截面的點，由式（19）可知φk+1=P(φk)，設φk+1=φk+Δφk，利用一階泰勒公式對龐加萊映射函數P（x）進行局部線性化，可以得到如下關系：

使用φk+1=φk+Δφk進行迭代，直到Δφk小于給定的閾值誤差，便求得不動點φ*。

3.2 軌道穩定性判據

對于不動點φ*附近的小擾動Δφ*，采用泰勒級數展開，龐加萊映射函數可以表示為：

其中：?P為龐加萊映射函數相對于系統狀態的梯度，稱為雅可比矩陣。?P其表達式為：

該矩陣的特征值λ稱為被動行走系統的Floquet 乘子。由于Δφk+1=(?P)Δφk，所以，當雅可比矩陣的所有特征值滿足|λ| ＜1 時，必有＜1，即隨著k的增加，Δφk→0，誤差會隨著步長的增加而逐漸減小，極限環將逐漸收斂到原周期軌道，由軌道穩定性定義可知系統的周期步態是軌道穩定的。

4 仿真分析

表1 系統參數和初始條件Tab.1 System parameters and initial conditions

4.1 步態分析

圖5 給出的是髖關節軌跡、軀干姿態角α和擺動腿夾角β的軌跡圖?？梢钥闯?，所提出的控制器能夠維持穩定的周期步態，并保持軀干直立。從圖6 中可以看出，得到的誤差曲線收斂于零的鄰域。在DS 階段，因為擺動腿角度β在此階段沒有定義，所以eβ≡0。此外，如前所述，系統在SS 階段處于欠驅動狀態，采用的是部分反饋線性化的控制方式。即僅在DS 階段控制髖關節質點的水平速度。從圖6（d）中可以看出：誤差ev在DS 階段趨于零，而在單支撐階段，存在一定的誤差。

圖5 髖關節軌跡、軀干傾角軌跡和擺動腿夾角軌跡Fig.5 Hip trajectories，torso attitude angle trajectory，and swing leg included angle trajectory

圖6 穩態誤差變化曲線（灰色區域為DS階段）Fig.6 Error change curves in steady state（the areas printed in grey are DS phases）

圖7 給出了穩定狀態下的控制輸入變化曲線，黑色虛線對應腿原長度（L0=1 m）。理想的SLIP 參考模型可以在沒有任何控制輸入的情況下（即u1=u2=0）維持穩定的步態。本文提出的VL-TSLIP-SL 模型包括軀干姿態和擺動腿控制，這些非理想動態都被認為是系統的外部擾動，因此需要較大的控制輸入來抑制這些擾動。在圖7 中可以清楚地看到控制輸入隨著機器人的行走作出的變化，由于擺動腿夾角β在DS 階段沒有定義，所以在DS 階段τ2≡0。此外，由于在行走的過程中，左右腿交替作為擺動腿，所以在SS 階段，控制輸入u1和u2交替為零。

圖7 穩態控制輸入變化曲線Fig.7 Control input change curves in steady state

系統的能量曲線如圖8 所示，其中虛線為理想SLIP 模型的能量水平?？梢钥闯?，總能量在SLIP 模型的能量水平上波動。一方面原因是控制器作用在軀干和擺動腿上，需要消耗能量；另一方面原因是期望軌跡由傅里葉級數擬合而來，存在著一定的偏差。因此，實際上能量水平不是恒值，存在一定的波動。

圖8 系統的能量曲線Fig.8 Energy curves of system

4.2 穩定性驗證

4.2.1 特征值分布

對于雅可比矩陣，由于無法得到解析解，在實際的機器人系統中，通常采用數值方法進行求解。數值解法的步驟如下：

步驟1 用Newton-Raphson 迭代法得到不動點φ*=

步驟2 每次對一個狀態變量施加擾動，其他狀態變量保持不變。

步驟3 觀察龐加萊映射的首次返回值。

步驟4 計算式（25）中對應列的元素值。

步驟5 重復6 次，即每個狀態變量各1 次。

步驟6 計算雅可比矩陣的特征值，判斷系統是否穩定。

對于本文提出的雙足機器人系統，存在一個不動φ*=（0.000 3，1.090 8，1.147 6，-1.058 8，0.000 6，0.010 5）T，對不動點施加一個大小為1 × 10-3的小擾動，則雅可比矩陣為：

計算特征值為：-0.050 7+0.000 0i；-0.000 2+0.0000i；0.482 9+0.427 5i；0.482 9-0.427 5i；0.347 1；0.0042。

其最大特征值的模為0.644 9＜1，因此不動點φ*是穩定的，該系統是軌道穩定的。

4.2.2 極限環

圖9 給出了雙足機器人以穩定的周期步態行走足夠長的時間后的相位軌跡。由于機器人雙腿步態的對稱性，雙腿交替為支撐腿與擺動腿，因此只畫出了從左腿以原長觸地瞬間到右腿以原長觸地瞬間的極限環。在極限環之外，本文用小棍圖來表示雙足機器人行走的步態，腳末端的黑點表示腿與地面接觸。相位軌跡的循環方向用箭頭表示。

階段Ⅰ：A點到B點（DS階段）。

如圖9（a）所示，從標記為A 的瞬間，即機器人單雙支撐切換瞬間開始行走，（也就是左腿以原長剛接觸地面變為支撐腿時刻），此時機器人處于DS 狀態。A 點位于龐加萊截面上，因此髖關節質點的水平速度==1.147 6 m/s。由于系統有足夠的能量，它將繼續向前移動，伸展左腿，直到達到其原長度（點B）。

圖9 系統的相位軌跡Fig.9 Phase trajectories of system

階段Ⅱ：B點到C點（SS階段）。

在B 點瞬間，左腿失去與地面的接觸，以原長向前擺動，機器人從DS 階段切換到右腿SS 階段?？梢夿 點也在龐加萊截面上，因此髖關節質點的水平速度=1.147 6 m/s。當機器人到達C 點時，右腿垂直站立，左腿處于擺動階段。

階段Ⅲ：C點到D點（SS階段）。

在C 點，機器人系統處于VLO 狀態。為保證步態對稱，髖關節質點的垂直速度=0 m/s。右腿保持與地面接觸，左腿繼續向前擺動。當機器人到達D 點時，左腿以其原長度接觸地面，雙足機器人從右腿SS 階段切換到DS 階段。然后兩條腿互換角色，相位軌跡回到下一個行走階段的A 點（實際上，A 點和D 點是同一點）。

圖9（b）為三維相位軌跡，可以更直觀地看出，隨著機器人按照x軸方向的行走，極限環依次螺旋上升，系統狀態變量已形成穩定的極限環，雙足機器人處于穩定運行狀態。

從特征值的計算及圖9 的相軌跡可以看出，雖然在單支撐階段采用了部分反饋線性化控制方法，但是雅可比矩陣的特征值均位于單位圓內，形成穩定的極限環，證明系統是軌道穩定的。

4.3 擾動抑制性能

為了驗證控制策略在擾動下的有效性，在x=3.692 m處，施加Fd=［Fx，Fy］T=（10，20）TN 的擾動，持續0.644 3 m，結果如圖10 所示。

在圖10 中，虛線表示對系統施加擾動的瞬間?？梢钥闯?，系統在擾動下能夠保持穩定的步態，經過一個短暫的過渡階段后，系統恢復到穩定的周期步態。對于髖關節高度y，由于采用的是變腿長度控制，機器人在受到擾動時，控制器控制伸縮腿，使系統能保持在原有的期望軌跡上；對于前進速度x˙和軀干α，大約在2 步內收斂到期望軌跡；對于擺動腿夾角β，大約在4 步內收斂到期望軌跡?？傮w上來看，機器人能夠抵抗外部擾動，繼續平穩地行走。

圖10 系統受到擾動后的響應Fig.10 Response of system after disturbance

為了進一步研究變長度控制器的性能，對機器人施加不同的干擾，通過重復性實驗研究其擾動抑制區域?？紤]到實際情況，干擾力限制在±100 N 內。結果如圖11 所示。深灰色區域為無變長度情況下的抗干擾區域，淺灰色區域為變長度情況下的抗干擾區域。對于圖11（a），當機器人處于SS 階段時，在3.692 m 處施加擾動；對于圖11（b），當機器人處于DS 階段時，在5.996 m 處施加擾動?？傻贸?，系統處于DS 階段的抗干擾性能優于SS 階段，這是因為系統在SS 階段為欠驅動，在DS 階段為全驅動。總體而言，該控制方法對擾動具有較好的魯棒性。

圖11 系統對不同擾動力的響應Fig.11 Response of system to different disturbance force

5 結語

本文提出了VL-TSLIP-SL 模型，該模型由一個髖關節質點、一個剛性軀干和兩條柔性腿組成，每條腿的末端都有一個腳質量。本模型包含了軀干姿勢控制和擺動腿動力學。利用拉格朗日第二方程構造機器人的動力學方程，通過控制雙腿長度，控制機器人的實際步態軌跡逐漸收斂到期望步態軌跡。由于系統的高耦合和非線性特點，采用反饋線性化的方法將非線性系統轉換為線性系統，實現機器人的步態漸近穩定。利用Newton-Raphson 迭代法結合龐加萊映射分析可知，雙足機器人周期軌道的穩定性等價于龐加萊映射不動點的穩定性。最后，通過數值仿真驗證了其軌道穩定性和抗干擾性能。未來的工作將進一步研究機器人的參數對穩定性的影響。此外，在本文理論研究的基礎上進行實際樣機的驗證是需要進一步研究的課題。