基于模型预测控制的双足高效行走系统

摘" 要： 提出基于模型预测的双足高效行走系统. 首先，利用最优能效算法对双足行进中的最优步态进行分析，包括行进步长和步速，并以此生成参考轨迹；然后，通过模型预测控制的思路对机器人的最优能效步态进行跟踪控制；最后，结合机器人逆运动学构建高效稳定的双足行走系统. 通过仿真实验验证了该系统可以有效跟踪、生成高效的行走步态，探寻了双足机器人动态行走的新思路.

关键词： 模型预测控制； 线性倒立摆； 高效行走步态； 零力矩点（ZMP）； 支撑多边形

中图分类号： TP 18""" 文献标志码： 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

如何有效提高双足机器人行走过程中的稳定性和能量利用效率，进一步增强机器人的待机能力和对环境的适应能力是当下双足机器人研究领域亟待解决的重要难题. HU等［1］对人类的行走步态进行深入分析，并将人类的行走机制应用到人形机器人中，提高了机器人行走的能量利用效率. LUO等［2］对线性倒立摆模型进行跟踪控制，并通过巧妙的俯仰和偏航动量控制策略，显著提升了双足机器人的整体稳定性. PARK等［3］引入预观控制的方式，综合考虑双足机器人的参考轨迹、速度、加速度和稳定性，提高了双足机器人对稳定步态的跟踪控制能力.

目前的研究主要关注于提高双足行走的稳定性或能量利用效率中的单一指标，而未对稳定性和能耗指标作有效融合，导致机器人易产生稳定低效或高效不稳的行走步态. 本文作者分析了双足机器人的高效行走步态，通过模型预测控制的方法对机器人的稳定性进行跟踪控制，进而设计出高效稳定的双足行走系统.

1" 双足机器人高效稳定行走模型

1.1 双足机器人两杆三质点模型

如图1所示，M为机器人髋关节质点，表示机器人上半身总质量，m为机器人腿部质点，l1为腿部质点到髋关节质点的距离，l2为腿部质点到足底的距离，q1为支撑腿与地面法线的夹角，q2为摆动腿与支撑腿的夹角，x_0，y_0是支撑腿的位置.

双足机器人行进过程中的状态可以分为三个阶段：单腿支撑阶段、摆动腿足底与地面碰撞阶段、双腿支撑阶段. 如图1所示，双足机器人初始状态为双足支撑阶段，然后摆动腿进行切换运动，当摆动腿运动快结束时，会与地面产生碰撞，可以将其简化为一个等效冲量，碰撞结束后重新进入双腿支撑阶段，完成摆动腿和支撑腿的切换，进行下一周期的运动. 由此建立两杆三质点动力学方程［4］，分析双足机器人高效行走步态.

1.2 高效行走步态分析求解

高效行走步态用于分析双足机器人行进能耗与其行进步长、速度之间的关系. 定义每一步的行走能量效率

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

其中，E_step是机器人每一步行走所消耗的能量； g是重力加速度; d_step是双足机器人的行进步长.

计算所消耗的能量E_kick，

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

其中，P_hip是摆动腿切换过程中髋关节消耗的功率; v_i^+， v_i^-是机器人第i步蹬地前后的速度.

将高效步态的求解问题转换成数学形式如下：

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

其中，f（·）为目标函数；x为对应的求解参数，包括周期运动时间t、行走的初始状态量、时间t内的髋关节力矩序列以及蹬地冲量；Ax为相应的稀疏矩阵，表示相应的约束关系；l，u为对应约束的上下限.

2" 双足机器人模型预测控制思想

2.1 零力矩点（ZMP）[5]稳定性判据

ZMP为机器人受到合力矩为零的点，是一个用于评估双足机器人行走稳定性的重要判据，在线性倒立摆模型中，

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

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

其中，P_x，P_y 分别是ZMP在x，y轴方向上的坐标；x， y， x ¨， y ¨分别是质心在x，y轴方向上的坐标和加速度；z_c是质心的高度.

支撑多边形［6］为双足机器人足部在地面上的投影所形成的多边形区域. 这个多边形代表了机器人在地面上的支撑区域. 在单腿支撑状态下，支撑多边形通常涵盖整个脚部，如图2（c）所示的灰色区域；而在双腿支撑状态和切换状态，支撑多边形则为两脚连线所形成的最小多边形，如图2（a），2（b）所示的灰色区域. 若机器人的ZMP位于这个支撑多边形内，则机器人处于稳定状态. 因此，支撑多边形作为机器人稳定性的一个关键指标，反映了机器人在特定支撑条件下的稳定能力.

2.2 线性倒立摆

人类行走分别使用肌肉和神经作为驱动元件和控制元件，如果没有了肌肉和神经，人类的行走就像一个不断前进的倒立摆［7］. 因此，在双足机器人中，可以用线性倒立摆来描述机器人的动态行为，尤其是在平衡和稳定性控制方面. 基于线性倒立摆的动力学模型，可以控制双足机器人按照预设轨迹进行跟踪行走. 如图3所示，将双足机器人全身质量简化为一个质心，腿部简化为两条无质量且可以伸缩的腿，维持质心高度不变，可以构建线性倒立摆模型［8］.

当线性倒立摆处于稳定状态时，设质心质量为m，合力为f，则质心在x轴上的投影x ¨的关系如下：

f sin θ=mx ¨， （6）

f cos θ=mg，"" （7）

其中，θ为支撑腿与Z轴的夹角.

可以推导出在x方向上线性倒立摆的动力学方程：

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

2.3 模型预测控制算法

模型预测控制［9］广泛应用于自动化、自动驾驶、机器人控制等领域. 该算法基于一个动态模型，在每个控制周期内，通过对未来一系列可能的输入进行优化，选择当前时刻的最优控制输入［10］.

如图4所示，在k时刻进行采样预测，选取未来N_p个时间间隔内的参考轨迹和模型状态量作为系统输入，求解一定约束条件下的目标函数，得到[k，k+N_p]时刻系统的预测控制量和预测轨迹，如图4中轨迹2，4所示；接着选取k+1时刻的控制量对机器人进行控制，重新执行上述步骤，预测[k+1，k+N_p+1]时域内的控制量，并计算预测轨迹. 通过这种迭代的方式，不断计算预测时域内的最优控制量，形成连续的控制过程.

在双足机器人线性倒立摆模型中，k时刻的状态量X（k）=[（xamp;x ˙amp;x ¨ ）]^T，x， x ˙， x ¨分别是x方向上的位置、速度、加速度. 设控制量u为质心水平方向上的加速度，则

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时刻的控制量.

联立线性倒立摆的动力学模型，可以得到ZMP的预测轨迹

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

选取双足机器人高效行走步态作为ZMP参考轨迹p^ref进行输入系统，为使系统预测的ZMP轨迹尽可能精确地跟踪参考轨迹，使控制量u和质心状态量X的变化幅度尽可能小，构建评估函数

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及状态量X的权重系数.

在k时刻，如果预先知道未来N_p时间内ZMP的参考轨迹，则使评估函数最小化的最优控制器为：

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所得到的增益系数.

3" 系统分析设计和实验结果

双足机器人行走系统的总体架构如图5所示，分为路径规划、步态生成、步态跟踪控制和传感器数据处理模块.

路径规划模块的关键功能是在考虑环境约束、机器人自身能力和任务需求的情况下，生成一条起点到终点的距离最短、耗时最少的安全无碰撞路径［11］. 数据处理模块负责接收摄像头的信息并加以处理，首先对摄像头获取的周围环境视觉信息进行分析，并标注障碍物，然后将处理后的信息传输至路径规划模块. 运动学模块主要根据步态跟踪控制模块传输的机器人位置信息，求解机器人的运动关节角度，进而操控真实机器人进行实时行走［12］. 高效步态生成模块主要负责生成最优能效步态，通过截取路径规划模块生成的部分原始轨迹，结合机器人自身的结构约束，计算不同步长和步速下的指标. 在此基础上，考虑机器人动力学约束，选取最优能效下的步长、步速，生成ZMP参考轨迹.

设定双足机器人参数：机器人腿长度l为1 m；髋关节质点到腿部质点长度l1为0.4 m；腿部质点到足底长度l2为0.6 m；机器人上半身质点质量M为10 kg；机器人腿部质点质量m为3 kg；重力加速度g为9.8 m·s-2.

因为目标函数和约束方程都呈非线性，该问题属于非线性规划问题，通过二次序列规划求解不同速度、步长下的最优能量利用率（COT），结果如图6所示. 图6为COT与机器人步长、步速的关系. 图7为步长分别与COT、所需蹬地冲量的关系. 可以看出：对于固定的步长，随着速度的增加，双足机器人的COT呈上升趋势；对于固定的速度，随着行走步长的增加，双足机器人的COT先降低后增加，所需蹬地冲量呈上升趋势.

步态跟踪控制模块以线性倒立摆模型为基础，提供了ZMP和质心的映射关系以及相应的动力学关系. 将最优能效步态和数据处理模块得到的ZMP真实数据作为输入，迭代优化评估函数，输出未来1 s的控制量，进而得到位置信息.

对目标函数u（k）进行实时求解，将k+1时刻的控制量作为模型本次周期的控制输入量，不断迭代这个步骤，得到ZMP的预测轨迹如图8所示. 双足机器人行走过程中越贴近ZMP参考轨迹，则步态稳定性越好，能效越优. 图8中绿色曲线为仿真环境输入的最优ZMP参考轨迹，通过计算得到ZMP的预测轨迹为蓝色曲线，可以看到模型可以高效跟踪输入轨迹.

4" 结 论

本文作者提出双足机器人高效稳定行走系统，可以实时生成机器人的最优行进步态，包括行进步长和步速，并以此生成下一模块的最优参考轨迹. 在步态跟踪控制模块引入基于线性倒立摆的预测控制算法，实时跟踪控制生成的参考轨迹，有效结合了高效行走步态和模型预测控制思想，最终通过仿真实验验证了该模块的有效性. 然而，在真实场景中，周围环境信息往往是复杂多样的，因此实现双足机器人的高效行走仍面临着很大的挑战，如何适应不平整路面的行走是未来需要探索的方向.

参考文献：

［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.

（责任编辑：包震宇，顾浩然）