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

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

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

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

目前的研究主要關(guān)注于提高雙足行走的穩(wěn)定性或能量利用效率中的單一指標(biāo)，而未對(duì)穩(wěn)定性和能耗指標(biāo)作有效融合，導(dǎo)致機(jī)器人易產(chǎn)生穩(wěn)定低效或高效不穩(wěn)的行走步態(tài). 本文作者分析了雙足機(jī)器人的高效行走步態(tài)，通過(guò)模型預(yù)測(cè)控制的方法對(duì)機(jī)器人的穩(wěn)定性進(jìn)行跟蹤控制，進(jìn)而設(shè)計(jì)出高效穩(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為擺動(dòng)腿與支撐腿的夾角，x_0，y_0是支撐腿的位置.

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

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

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

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

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

計(jì)算所消耗的能量E_kick，

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

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

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

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

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

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

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

ZMP為機(jī)器人受到合力矩為零的點(diǎn)，是一個(gè)用于評(píng)估雙足機(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ū)域. 這個(gè)多邊形代表了機(jī)器人在地面上的支撐區(qū)域. 在單腿支撐狀態(tài)下，支撐多邊形通常涵蓋整個(gè)腳部，如圖2（c）所示的灰色區(qū)域；而在雙腿支撐狀態(tài)和切換狀態(tài)，支撐多邊形則為兩腳連線所形成的最小多邊形，如圖2（a），2（b）所示的灰色區(qū)域. 若機(jī)器人的ZMP位于這個(gè)支撐多邊形內(nèi)，則機(jī)器人處于穩(wěn)定狀態(tài). 因此，支撐多邊形作為機(jī)器人穩(wěn)定性的一個(gè)關(guān)鍵指標(biāo)，反映了機(jī)器人在特定支撐條件下的穩(wěn)定能力.

2.2 線性倒立擺

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

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

f sin θ=mx ¨， （6）

f cos θ=mg，"" （7）

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

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

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

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

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

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

在雙足機(jī)器人線性倒立擺模型中，k時(shí)刻的狀態(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為單位時(shí)間步長(zhǎng)；u（k）為k時(shí)刻的控制量.

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

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ù)測(cè)的ZMP軌跡盡可能精確地跟蹤參考軌跡，使控制量u和質(zhì)心狀態(tài)量X的變化幅度盡可能小，構(gòu)建評(píng)估函數(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時(shí)刻，如果預(yù)先知道未來(lái)N_p時(shí)間內(nèi)ZMP的參考軌跡，則使評(píng)估函數(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è)計(jì)和實(shí)驗(yàn)結(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)的距離最短、耗時(shí)最少的安全無(wú)碰撞路徑［11］. 數(shù)據(jù)處理模塊負(fù)責(zé)接收攝像頭的信息并加以處理，首先對(duì)攝像頭獲取的周圍環(huán)境視覺(jué)信息進(jìn)行分析，并標(biāo)注障礙物，然后將處理后的信息傳輸至路徑規(guī)劃模塊. 運(yùn)動(dòng)學(xué)模塊主要根據(jù)步態(tài)跟蹤控制模塊傳輸?shù)臋C(jī)器人位置信息，求解機(jī)器人的運(yùn)動(dòng)關(guān)節(jié)角度，進(jìn)而操控真實(shí)機(jī)器人進(jìn)行實(shí)時(shí)行走［12］. 高效步態(tài)生成模塊主要負(fù)責(zé)生成最優(yōu)能效步態(tài)，通過(guò)截取路徑規(guī)劃模塊生成的部分原始軌跡，結(jié)合機(jī)器人自身的結(jié)構(gòu)約束，計(jì)算不同步長(zhǎng)和步速下的指標(biāo). 在此基礎(chǔ)上，考慮機(jī)器人動(dòng)力學(xué)約束，選取最優(yōu)能效下的步長(zhǎng)、步速，生成ZMP參考軌跡.

設(shè)定雙足機(jī)器人參數(shù)：機(jī)器人腿長(zhǎng)度l為1 m；髖關(guān)節(jié)質(zhì)點(diǎn)到腿部質(zhì)點(diǎn)長(zhǎng)度l1為0.4 m；腿部質(zhì)點(diǎn)到足底長(zhǎng)度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.

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

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

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

4" 結(jié) 論

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

參考文獻(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é)任編輯：包震宇，顧浩然）