基于模型預(yù)測控制的雙足高效行走系統(tǒng)

摘" 要： 提出基于模型預(yù)測的雙足高效行走系統(tǒng). 首先，利用最優(yōu)能效算法對雙足行進(jìn)中的最優(yōu)步態(tài)進(jìn)行分析，包括行進(jìn)步長和步速，并以此生成參考軌跡；然后，通過模型預(yù)測控制的思路對機(jī)器人的最優(yōu)能效步態(tài)進(jìn)行跟蹤控制；最后，結(jié)合機(jī)器人逆運(yùn)動學(xué)構(gòu)建高效穩(wěn)定的雙足行走系統(tǒng). 通過仿真實驗驗證了該系統(tǒng)可以有效跟蹤、生成高效的行走步態(tài)，探尋了雙足機(jī)器人動態(tài)行走的新思路.

關(guān)鍵詞： 模型預(yù)測控制； 線性倒立擺； 高效行走步態(tài)； 零力矩點(diǎn)（ZMP）； 支撐多邊形

中圖分類號： TP 18""" 文獻(xiàn)標(biāo)志碼： A""" 文章編號： 1000-5137（2024）03-0308-07

An efficient bipedal walking system based on model predictive control

YAN Haixin， LI Guochen， SHANGGUAN Qianqian?， AN Kang?

（College of Information， Mechanical and Electrical Engineering， Shanghai Normal University， Shanghai 201418， China）

Abstract： A model-predictive， efficient bipedal walking system was presented in this paper. Initially， optimal efficiency algorithms were employed to analyze the optimal gait of bipedal locomotion， including stride length and walking speed， on which a reference trajectory was generated. Subsequently， employing the model predictive control paradigm， the robot’s optimal and efficient gait was tracked and controlled. Finally， an efficient and stable bipedal walking system was constructed by combining robot inverse kinematics. Simulation results validated that the system could effectively track the generated efficient walking gait， exploring new avenues for dynamic walking for bipedal robots.

Key words： model predictive control； linear inverted pendulum； efficient walking gait； zero moment point （ZMP）； support polygon

如何有效提高雙足機(jī)器人行走過程中的穩(wěn)定性和能量利用效率，進(jìn)一步增強(qiáng)機(jī)器人的待機(jī)能力和對環(huán)境的適應(yīng)能力是當(dāng)下雙足機(jī)器人研究領(lǐng)域亟待解決的重要難題. HU等［1］對人類的行走步態(tài)進(jìn)行深入分析，并將人類的行走機(jī)制應(yīng)用到人形機(jī)器人中，提高了機(jī)器人行走的能量利用效率. LUO等［2］對線性倒立擺模型進(jìn)行跟蹤控制，并通過巧妙的俯仰和偏航動量控制策略，顯著提升了雙足機(jī)器人的整體穩(wěn)定性. PARK等［3］引入預(yù)觀控制的方式，綜合考慮雙足機(jī)器人的參考軌跡、速度、加速度和穩(wěn)定性，提高了雙足機(jī)器人對穩(wěn)定步態(tài)的跟蹤控制能力.

目前的研究主要關(guān)注于提高雙足行走的穩(wěn)定性或能量利用效率中的單一指標(biāo)，而未對穩(wěn)定性和能耗指標(biāo)作有效融合，導(dǎo)致機(jī)器人易產(chǎn)生穩(wěn)定低效或高效不穩(wěn)的行走步態(tài). 本文作者分析了雙足機(jī)器人的高效行走步態(tài)，通過模型預(yù)測控制的方法對機(jī)器人的穩(wěn)定性進(jìn)行跟蹤控制，進(jìn)而設(shè)計出高效穩(wěn)定的雙足行走系統(tǒng).

1" 雙足機(jī)器人高效穩(wěn)定行走模型

1.1 雙足機(jī)器人兩桿三質(zhì)點(diǎn)模型

如圖1所示，M為機(jī)器人髖關(guān)節(jié)質(zhì)點(diǎn)，表示機(jī)器人上半身總質(zhì)量，m為機(jī)器人腿部質(zhì)點(diǎn)，l1為腿部質(zhì)點(diǎn)到髖關(guān)節(jié)質(zhì)點(diǎn)的距離，l2為腿部質(zhì)點(diǎn)到足底的距離，q1為支撐腿與地面法線的夾角，q2為擺動腿與支撐腿的夾角，x_0，y_0是支撐腿的位置.

雙足機(jī)器人行進(jìn)過程中的狀態(tài)可以分為三個階段：單腿支撐階段、擺動腿足底與地面碰撞階段、雙腿支撐階段. 如圖1所示，雙足機(jī)器人初始狀態(tài)為雙足支撐階段，然后擺動腿進(jìn)行切換運(yùn)動，當(dāng)擺動腿運(yùn)動快結(jié)束時，會與地面產(chǎn)生碰撞，可以將其簡化為一個等效沖量，碰撞結(jié)束后重新進(jìn)入雙腿支撐階段，完成擺動腿和支撐腿的切換，進(jìn)行下一周期的運(yùn)動. 由此建立兩桿三質(zhì)點(diǎn)動力學(xué)方程［4］，分析雙足機(jī)器人高效行走步態(tài).

1.2 高效行走步態(tài)分析求解

高效行走步態(tài)用于分析雙足機(jī)器人行進(jìn)能耗與其行進(jìn)步長、速度之間的關(guān)系. 定義每一步的行走能量效率

C_OT=E_step/（gd_step M） ，""" （1）

其中，E_step是機(jī)器人每一步行走所消耗的能量； g是重力加速度; d_step是雙足機(jī)器人的行進(jìn)步長.

計算所消耗的能量E_kick，

E_kick=1/2∑m_i [（v_i^+ ）^2-（v_i^- ）^2 ]，" （2）

其中，P_hip是擺動腿切換過程中髖關(guān)節(jié)消耗的功率; v_i^+， v_i^-是機(jī)器人第i步蹬地前后的速度.

將高效步態(tài)的求解問題轉(zhuǎn)換成數(shù)學(xué)形式如下：

（min"""" f（x），@s.t.l≤{（x@f（x）@Ax）}≤u，） （3）

其中，f（·）為目標(biāo)函數(shù)；x為對應(yīng)的求解參數(shù)，包括周期運(yùn)動時間t、行走的初始狀態(tài)量、時間t內(nèi)的髖關(guān)節(jié)力矩序列以及蹬地沖量；Ax為相應(yīng)的稀疏矩陣，表示相應(yīng)的約束關(guān)系；l，u為對應(yīng)約束的上下限.

2" 雙足機(jī)器人模型預(yù)測控制思想

2.1 零力矩點(diǎn)（ZMP）[5]穩(wěn)定性判據(jù)

ZMP為機(jī)器人受到合力矩為零的點(diǎn)，是一個用于評估雙足機(jī)器人行走穩(wěn)定性的重要判據(jù)，在線性倒立擺模型中，

P_x=x-z_c/g x ¨ ，" （4）

P_y=y-z_c/g y ¨ ， （5）

其中，P_x，P_y 分別是ZMP在x，y軸方向上的坐標(biāo)；x， y， x ¨， y ¨分別是質(zhì)心在x，y軸方向上的坐標(biāo)和加速度；z_c是質(zhì)心的高度.

支撐多邊形［6］為雙足機(jī)器人足部在地面上的投影所形成的多邊形區(qū)域. 這個多邊形代表了機(jī)器人在地面上的支撐區(qū)域. 在單腿支撐狀態(tài)下，支撐多邊形通常涵蓋整個腳部，如圖2（c）所示的灰色區(qū)域；而在雙腿支撐狀態(tài)和切換狀態(tài)，支撐多邊形則為兩腳連線所形成的最小多邊形，如圖2（a），2（b）所示的灰色區(qū)域. 若機(jī)器人的ZMP位于這個支撐多邊形內(nèi)，則機(jī)器人處于穩(wěn)定狀態(tài). 因此，支撐多邊形作為機(jī)器人穩(wěn)定性的一個關(guān)鍵指標(biāo)，反映了機(jī)器人在特定支撐條件下的穩(wěn)定能力.

2.2 線性倒立擺

人類行走分別使用肌肉和神經(jīng)作為驅(qū)動元件和控制元件，如果沒有了肌肉和神經(jīng)，人類的行走就像一個不斷前進(jìn)的倒立擺［7］. 因此，在雙足機(jī)器人中，可以用線性倒立擺來描述機(jī)器人的動態(tài)行為，尤其是在平衡和穩(wěn)定性控制方面. 基于線性倒立擺的動力學(xué)模型，可以控制雙足機(jī)器人按照預(yù)設(shè)軌跡進(jìn)行跟蹤行走. 如圖3所示，將雙足機(jī)器人全身質(zhì)量簡化為一個質(zhì)心，腿部簡化為兩條無質(zhì)量且可以伸縮的腿，維持質(zhì)心高度不變，可以構(gòu)建線性倒立擺模型［8］.

當(dāng)線性倒立擺處于穩(wěn)定狀態(tài)時，設(shè)質(zhì)心質(zhì)量為m，合力為f，則質(zhì)心在x軸上的投影x ¨的關(guān)系如下：

f sin θ=mx ¨， （6）

f cos θ=mg，"" （7）

其中，θ為支撐腿與Z軸的夾角.

可以推導(dǎo)出在x方向上線性倒立擺的動力學(xué)方程：

x ¨=g tanθ=g/z_c" x-P_x ."" （8）

2.3 模型預(yù)測控制算法

模型預(yù)測控制［9］廣泛應(yīng)用于自動化、自動駕駛、機(jī)器人控制等領(lǐng)域. 該算法基于一個動態(tài)模型，在每個控制周期內(nèi)，通過對未來一系列可能的輸入進(jìn)行優(yōu)化，選擇當(dāng)前時刻的最優(yōu)控制輸入［10］.

如圖4所示，在k時刻進(jìn)行采樣預(yù)測，選取未來N_p個時間間隔內(nèi)的參考軌跡和模型狀態(tài)量作為系統(tǒng)輸入，求解一定約束條件下的目標(biāo)函數(shù)，得到[k，k+N_p]時刻系統(tǒng)的預(yù)測控制量和預(yù)測軌跡，如圖4中軌跡2，4所示；接著選取k+1時刻的控制量對機(jī)器人進(jìn)行控制，重新執(zhí)行上述步驟，預(yù)測[k+1，k+N_p+1]時域內(nèi)的控制量，并計算預(yù)測軌跡. 通過這種迭代的方式，不斷計算預(yù)測時域內(nèi)的最優(yōu)控制量，形成連續(xù)的控制過程.

在雙足機(jī)器人線性倒立擺模型中，k時刻的狀態(tài)量X（k）=[（xamp;x ˙amp;x ¨ ）]^T，x， x ˙， x ¨分別是x方向上的位置、速度、加速度. 設(shè)控制量u為質(zhì)心水平方向上的加速度，則

X（k+1）=AX（k）+Bu（k） ，""""" （9）

其中，A=[（1amp;Tamp;T^2/2@0amp;1amp;T@0amp;0amp;1）]; B=[（T^3/6@T^2/2@T）]；T為單位時間步長；u（k）為k時刻的控制量.

聯(lián)立線性倒立擺的動力學(xué)模型，可以得到ZMP的預(yù)測軌跡

P（k）=[（1amp;Tamp;T^2/2）][（x@x ˙@x ¨ ）]+（T^3/6-（Tz_c）/g）u（k） ." （10）

選取雙足機(jī)器人高效行走步態(tài)作為ZMP參考軌跡p^ref進(jìn)行輸入系統(tǒng)，為使系統(tǒng)預(yù)測的ZMP軌跡盡可能精確地跟蹤參考軌跡，使控制量u和質(zhì)心狀態(tài)量X的變化幅度盡可能小，構(gòu)建評估函數(shù)

J=（p-p^ref ）^T W_p （p-p^ref）+u^T W_u u+X^T W_X X，"" （11）

其中，W_p， W_u， W_X分別是ZMP參考軌跡誤差、控制量u及狀態(tài)量X的權(quán)重系數(shù).

在k時刻，如果預(yù)先知道未來N_p時間內(nèi)ZMP的參考軌跡，則使評估函數(shù)最小化的最優(yōu)控制器為：

u（k）=-Q_p ∑_（i=0）^k?〖（p（i）-p^ref （i））-Q_X 〗 X（k）-∑_（j=1）^（N_p）?〖Q_j p^ref" 〗（k+j），"""" （12）

其中，Q_p， Q_u， Q_j是基于W_p， W_u， W_X所得到的增益系數(shù).

3" 系統(tǒng)分析設(shè)計和實驗結(jié)果

雙足機(jī)器人行走系統(tǒng)的總體架構(gòu)如圖5所示，分為路徑規(guī)劃、步態(tài)生成、步態(tài)跟蹤控制和傳感器數(shù)據(jù)處理模塊.

路徑規(guī)劃模塊的關(guān)鍵功能是在考慮環(huán)境約束、機(jī)器人自身能力和任務(wù)需求的情況下，生成一條起點(diǎn)到終點(diǎn)的距離最短、耗時最少的安全無碰撞路徑［11］. 數(shù)據(jù)處理模塊負(fù)責(zé)接收攝像頭的信息并加以處理，首先對攝像頭獲取的周圍環(huán)境視覺信息進(jìn)行分析，并標(biāo)注障礙物，然后將處理后的信息傳輸至路徑規(guī)劃模塊. 運(yùn)動學(xué)模塊主要根據(jù)步態(tài)跟蹤控制模塊傳輸?shù)臋C(jī)器人位置信息，求解機(jī)器人的運(yùn)動關(guān)節(jié)角度，進(jìn)而操控真實機(jī)器人進(jìn)行實時行走［12］. 高效步態(tài)生成模塊主要負(fù)責(zé)生成最優(yōu)能效步態(tài)，通過截取路徑規(guī)劃模塊生成的部分原始軌跡，結(jié)合機(jī)器人自身的結(jié)構(gòu)約束，計算不同步長和步速下的指標(biāo). 在此基礎(chǔ)上，考慮機(jī)器人動力學(xué)約束，選取最優(yōu)能效下的步長、步速，生成ZMP參考軌跡.

設(shè)定雙足機(jī)器人參數(shù)：機(jī)器人腿長度l為1 m；髖關(guān)節(jié)質(zhì)點(diǎn)到腿部質(zhì)點(diǎn)長度l1為0.4 m；腿部質(zhì)點(diǎn)到足底長度l2為0.6 m；機(jī)器人上半身質(zhì)點(diǎn)質(zhì)量M為10 kg；機(jī)器人腿部質(zhì)點(diǎn)質(zhì)量m為3 kg；重力加速度g為9.8 m·s-2.

因為目標(biāo)函數(shù)和約束方程都呈非線性，該問題屬于非線性規(guī)劃問題，通過二次序列規(guī)劃求解不同速度、步長下的最優(yōu)能量利用率（COT），結(jié)果如圖6所示. 圖6為COT與機(jī)器人步長、步速的關(guān)系. 圖7為步長分別與COT、所需蹬地沖量的關(guān)系. 可以看出：對于固定的步長，隨著速度的增加，雙足機(jī)器人的COT呈上升趨勢；對于固定的速度，隨著行走步長的增加，雙足機(jī)器人的COT先降低后增加，所需蹬地沖量呈上升趨勢.

步態(tài)跟蹤控制模塊以線性倒立擺模型為基礎(chǔ)，提供了ZMP和質(zhì)心的映射關(guān)系以及相應(yīng)的動力學(xué)關(guān)系. 將最優(yōu)能效步態(tài)和數(shù)據(jù)處理模塊得到的ZMP真實數(shù)據(jù)作為輸入，迭代優(yōu)化評估函數(shù)，輸出未來1 s的控制量，進(jìn)而得到位置信息.

對目標(biāo)函數(shù)u（k）進(jìn)行實時求解，將k+1時刻的控制量作為模型本次周期的控制輸入量，不斷迭代這個步驟，得到ZMP的預(yù)測軌跡如圖8所示. 雙足機(jī)器人行走過程中越貼近ZMP參考軌跡，則步態(tài)穩(wěn)定性越好，能效越優(yōu). 圖8中綠色曲線為仿真環(huán)境輸入的最優(yōu)ZMP參考軌跡，通過計算得到ZMP的預(yù)測軌跡為藍(lán)色曲線，可以看到模型可以高效跟蹤輸入軌跡.

4" 結(jié) 論

本文作者提出雙足機(jī)器人高效穩(wěn)定行走系統(tǒng)，可以實時生成機(jī)器人的最優(yōu)行進(jìn)步態(tài)，包括行進(jìn)步長和步速，并以此生成下一模塊的最優(yōu)參考軌跡. 在步態(tài)跟蹤控制模塊引入基于線性倒立擺的預(yù)測控制算法，實時跟蹤控制生成的參考軌跡，有效結(jié)合了高效行走步態(tài)和模型預(yù)測控制思想，最終通過仿真實驗驗證了該模塊的有效性. 然而，在真實場景中，周圍環(huán)境信息往往是復(fù)雜多樣的，因此實現(xiàn)雙足機(jī)器人的高效行走仍面臨著很大的挑戰(zhàn)，如何適應(yīng)不平整路面的行走是未來需要探索的方向.

參考文獻(xiàn)：

［1］""" HU B H， KRAUSZ N E， HARGROVE L J. A novel method for bilateral gait segmentation using a single thigh-mounted depth sensor and IMU ［C］// 2018 7th IEEE International Conference on Biomedical Robotics and Biomechatronics.Enschede： IEEE， 2018：807-812.

［2］""" LUO J W， FU Y L， WANG S G. 3D stable biped walking control and implementation on real robot［J］. Advanced Robotics， 2017，31（12）：634-649.

［3］""" PARK J， YOUM Y. General ZMP preview control for bipedal walking ［C］// Proceedings 2007 IEEE International Conference on Robotics and Automation. Rome： IEEE， 2007：2682-2687.

［4］""" AN K， CHEN Q J. Dynamic optimization of a biped model： energetic walking gaits with different mechanical and gait parameters ［J］. Advances in Mechanical Engineering， 2015，7（5）：1-13.

［5］""" WANG Z T， HARADA K， WAN W W. Multi-contact stability of humanoids using ZMP and CWC ［C］// 2019 IEEE-RAS 19th International Conference on Humanoid Robots （Humanoids）. Toronto：IEEE， 2019：126-131.

［6］""" KIMURA K， IMAOKA N， NODA S， et al. Locomotion approach of bipedal robot utilizing passive wheel without swing leg based on stability margin maximization and fall prevention functions ［J］. Robomech Journal， 2020，7：35.

［7］""" MIKOLAJCZYK T， MIKO?AJEWSKA E， AL-SHUKA HFN， et al. Recent advances in bipedal walking robots： review of gait， drive， sensors and control systems ［J］. Sensors， 2022，22（12）：4440.

［8］""" KAJITA S， KANEHIRO F， KANEKO K， et al. The 3D linear inverted pendulum mode： a simple modeling for a biped walking pattern generation ［C］// Proceedings 2001 IEEE/RSJ International Conference on Intelligent Robots and Systems.Maui： IEEE，2001：239-246.

［9］""" RAWLINGS J B. Tutorial overview of model predictive control ［J］. IEEE Control Systems Magazine， 2000，20（3）： 38-52.

［10］" CAMACHO EF， BORDONS C. Introduction to Model Based Predictive Control ［M］. London：Springer， 1999.

［11］" CAMPBELLS， O'MAHONYN， CARVALHOA， et al. Path planning techniques for mobile robots a review ［C］// 6th International Conference on Mechatronics and Robotics Engineering. Barcelona： ICMRE， 2020：12-16.

［12］" MANDAVAR K， VUNDAVILLIP R. Forward and inverse kinematic based full body gait generation of biped robot ［C］//International Conference on Electrical， Electronics， and Optimization Techniques. Chennai： ICEEOT， 2016：3301-3305.

（責(zé)任編輯：包震宇，顧浩然）