基于双层模型预测控制的双足机器人全向行走质心轨迹规划方法

1.本发明涉及双足机器人技术领域，具体涉及一种基于双层模型预测控制的双足机器人全向行走质心轨迹规划方法。


背景技术：

2.双足机器人具有与人类相似的运动模式，采用双足交替与地面接触，以实现行走运动。在家庭服务、国防军事等应用场景中，灵活全向行走是双足机器人提升环境探索能力，完成作业任务的必要前提。但双足机器人质心较高、支撑面积小、全身自由度多、全身运动规划和稳定控制的难度较大。有效快速地生成符合机器人动力学特征的行走质心运动轨迹是实现双足机器人行走全身运动规划和稳定控制的重要基础，对实现双足机器人灵活全向行走具有重要意义。
3.模型预测控制(model predictive control，mpc)因预观未来区间的状态，使得其计算负担较重。在实际应用时，往往依赖于高性能的计算机以提高mpc的计算速度。为实现行走质心运动轨迹的在线规划，现有技术一般对mpc预测区间采用稀疏的采样时间间隔(远大于控制周期)以降低矩阵维度，减轻对计算机性能的依赖，但这样处理同样会增大求解得到的最优控制序列间隔，使得最优控制序列不够准确，降低了控制的精度，使得生成轨迹不够准确。
4.现有技术在生成质心运动轨迹时，缺乏对轨迹执行可行性的检查。如果让实际机器人执行发散的不合理的质心运动轨迹，将会造成机器人关节失能，引起硬件故障，损伤机器人机械电气本体。


技术实现要素：

5.针对现有技术中存在不足，本发明提供了一种基于双层模型预测控制的双足机器人全向行走质心轨迹规划方法，该方法能够兼顾最优控制序列的准确性和计算速度，从而在线生成较为可靠的质心运动轨迹；另外，本发明还提供了一种基于质心运动可行域的质心轨迹可行性判定准则，可捕捉生成轨迹的发散现象，在必要时可终止运动轨迹的生成进程，防止对机器人硬件造成巨大损伤。
6.本发明是通过以下技术手段实现上述技术目的的。
7.基于双层模型预测控制的双足机器人全向行走质心轨迹规划方法：
8.上层mpc接收落脚点规划和质心状态估计的输入后，在满足零力矩点稳定性约束及可行域约束的条件下，使得上层mpc目标函数最小化，解得稀疏最优控制序列通过状态空间方程的循环迭代，得到预测区间内的x方向稀疏质心运动位置轨迹y方向稀疏质心运动位置轨迹x方向稀疏质心运动速度轨迹y方向稀疏质心运动速度轨迹x方向稀疏zmp轨迹和y方向稀疏zmp轨迹
9.下层mpc以跟踪上层稀疏轨迹为目标，解得密集最优控制序列经状态空间方程的循环迭代，得到x方向密集质心位置轨迹和y方向密集质心位置轨迹
10.质心轨迹检测器依据可行性判定准则判断密集质心位置轨迹是否发散，如果没有发散，则将密集质心位置轨迹的首项值和发送给双足机器人执行。
11.上述技术方案中，所述可行域约束的条件为：
[0012][0013]
其中和是与质心运动可行域几何参数相关的稀疏系数矩阵，和为预测区间内质心运动可行域中心点的x、y方向稀疏位置向量，为上层mpc预测区间内的x方向质心位置向量，为上层mpc预测区间内的y方向质心位置向量。
[0014]
上述技术方案中，所述可行域具体为：以双足机器人质心运动方向为姿态角，分别作两条平行线穿过两只脚底板中心点，以两条平行线与脚底板的交点作为质心运动可行域的边界顶点，构建封闭矩形，即为该时刻质心运动的可行域。
[0015]
上述技术方案中，所述最优控制序列为：
[0016][0017]
其中：是上层mpc预测区间内的世界坐标系x方向和y方向的控制输入向量，和为上层mpc输出的预测区间内的x方向和y方向落脚点位置向量；
[0018]
由校正后的x方向和y方向的状态变量和上层mpc的控制输入向量得到x方向稀疏质心运动位置轨迹y方向稀疏质心运动位置轨迹x方向稀疏质心运动速度轨迹y方向稀疏质心运动速度轨迹x方向稀疏zmp轨迹和y方向稀疏zmp轨迹
[0019][0020][0021][0022]
其中：上层mpc最优控制输入序列其中：上层mpc最优控制输入序列为上层mpc的系数矩阵。
[0023]
上述技术方案中，所述校正后的x方向和y方向的状态变量满足：
[0024][0025]
其中：为x方向的状态变量，为y方向的状态变量，α为对质心状态估计的置信度，和为质心状态估计输入的质心状态估计向量。
[0026]
上述技术方案中，所述下层mpc的目标函数为：
[0027][0028]
其中：η，μ，λ为权重系数，为下层mpc求解的密集最优控制序列；
[0029]
是下层mpc目标函数中跟踪上层mpc稀疏质心位置轨迹和的部分：
[0030][0031]
是下层mpc目标函数中跟踪上层mpc稀疏质心速度轨迹和的部分：
[0032][0033]
是下层mpc目标函数中跟踪上层mpc稀疏zmp轨迹和的部分：
[0034][0035]
是下层mpc目标函数中直接最小化控制输入的部分：
[0036][0037]
式中s为映射系数矩阵，为下层mpc预测区间内的x方向质心位置向量，为下层mpc预测区间内的y方向质心位置向量，为下层mpc预测区间内的x方向质心速度向量，为下层mpc预测区间内的y方向质心速度向量，为下层mpc预测区间内的x方向zmp位置向量，为下层mpc预测区间内的y方向zmp位置向量，是下层mpc预测区间内的世界坐标系x方向和y方向的控制输入向量。
[0038]
上述技术方案中，所述密集最优控制序列通过以下解析式进行求解：
[0039][0040]
其中：ψ为解析系数矩阵，ξ
x
，ξy是与上层mpc稀疏轨迹相关的系数矩阵；且：
[0041]
[0042][0043][0044]
其中：为下层mpc的系数矩阵，为校正后的x方向和y方向的状态变量。
[0045]
上述技术方案中，所述x方向密集质心位置轨迹和y方向密集质心位置轨迹满足：
[0046][0047]
上述技术方案中，所述可行性判定准则为：
[0048][0049]
其中和是与质心运动可行域几何参数相关的密集系数矩阵，和为预测区间内的质心运动可行域的中心点的密集位置向量。
[0050]
上述技术方案中，当前时刻为t
k+1
时，更新落脚点规划和质心状态估计，计算得到t
k+1
时刻的质心密集位置轨迹和判定其满足可行性判定准则后，将所述轨迹首项和给双足机器人执行，重复上述过程直至机器人停止运动。
[0051]
本发明的有益效果为：
[0052]
(1)本发明的双足机器人全向行走质心轨迹规划方法，上层mpc采用稀疏的时间间隔，下层mpc采用密集的时间间隔，离散周期上疏下密，能够兼顾最优控制序列的准确性和计算速度，保证双足机器人行走质心轨迹规划的实时性和准确性，从而在线生成机器人行走准确的质心运动轨迹；
[0053]
(2)本发明设计了双足机器人全向行走质心运动可行域，基于该可行域还设计了质心轨迹可行性的判定准则，可捕捉机器人质心轨迹的发散现象，在必要时可终止运动轨迹的生成进程，防止对机器人硬件造成巨大损伤。
附图说明
[0054]
图1为本发明所述双足机器人行走质心运动轨迹规划流程图；
[0055]
图2为本发明所述双足机器人单双足支撑期支撑域示意图；
[0056]
图3为本发明所述安全域约束说明示意图；
[0057]
图4为本发明所述质心运动可行域构建说明示意图。
具体实施方式
[0058]
下面结合附图以及具体实施例对本发明作进一步的说明，但本发明的保护范围并不限于此。
[0059]
模型预测控制是一种滚动时域控制算法，其核心思想是基于动态系统的当前状态，使用预测模型在每个时刻在线求解未来有限区间内的开环最优控制问题，并将求解得到的最优控制序列的首项作用于系统。模型预测控制常常包含预测模型、目标函数和约束条件三个部分。预测模型是描述系统过程变化的数学模型，目标函数和约束条件是最优控制问题的基本组成部分，其中目标函数用于描述未来有限区间内的控制成本，也称为成本函数，约束条件用于描述控制系统在变化过程中需满足的限制条件。
[0060]
如图1所示，本发明提出一种基于双层模型预测控制的双足机器人全向行走质心轨迹规划方法，双足落脚点规划和质心状态估计输入质心轨迹规划器，其中双足落脚点规划和质心状态估计的方法为现有技术；在tk时刻，双足落脚点规划为质心规划规划器提供mpc预测区间[tk，tk+t
p
](t
p
为时间间隔)内的期望落脚点平面全维信息，包括期望落脚点的x方向平面位置向量期望落脚点的y方向平面位置向量期望落脚点的姿态向量以及平面位置向量和姿态向量对应的步行周期向量而质心状态估计部分则准确估计行走运动过程中机器人质心的x方向状态估计向量y方向状态估计向量用于校正更新mpc的预测模型tk时刻的状态变量；质心轨迹规划器包括上层模型预测控制器(上层mpc)、下层模型预测控制器(下层mpc)和质心轨迹检测器；上层mpc在接收到落脚点规划和质心状态估计的输入后，在满足零力矩点(zero moment point，zmp)稳定性约束及可行域约束的条件下，使得上层mpc目标函数最小化，解得稀疏最优控制序列通过状态空间方程的循环迭代，得到预测区间内的x方向稀疏质心运动位置轨迹y方向稀疏质心运动位置轨迹x方向稀疏质心运动速度轨迹y方向稀疏质心运动速度轨迹x方向稀疏zmp轨迹和y方向稀疏zmp轨迹下层mpc以跟踪上层稀疏轨迹(包括稀疏质心运动位置轨迹、稀疏质心运动速度轨迹和稀疏zmp轨迹)为目标，解得密集最优控制序列经状态空间方程的循环迭代，得到x方向密集质心位置轨迹和y方向密集质心位置轨迹质心轨迹检测器依据可行性判定准则判断密集质心位置轨迹是否发散，如果没有发散，则将该轨迹的首项值和发送给双足机器人执行。
[0061]
使用三维线性倒立摆模型(3d-lipm)作为上层mpc和下层mpc的预测模型，该模型假设质心高度恒定，将机器人的质心运动分解为在世界坐标系下x轴和y轴两个方向完全解耦且相似的线性倒立摆模型，其离散状态空间表达式为：
[0062][0063]
其中：a为系统矩阵，b为输入矩阵，c为输出矩阵，其表达式分别为：
[0064][0065]
其中t为状态的离散周期，且上层mpc与下层mpc使用不同的离散周期值；g为重力加速度常值，hc为世界坐标系z轴方向质心高度常值；
[0066]
为tk时刻c方向(表示世界坐标系下x轴或y轴方向)状态变量，uk为tk时刻c方向控制输入，zk为tk时刻解耦后的线性倒立摆模型输出量，其表达式分别为：
[0067][0068]
其中ck为tk时刻c方向的质心位置，为tk时刻c方向的质心速度，为tk时刻c方向的质心加速度，为tk时刻c方向的质心加加速度，为tk时刻c方向的zmp位置。
[0069]
在tk时刻，基于当前状态变量通过循环迭代(式(1))，可在预测区间[tk，tk+t
p
]内预测任意时刻的状态变量，其计算公式为：
[0070][0071]
其中：ai和aj分别表示i、j个系统矩阵相乘，u
k+i-1-j
为t
k+i-1-j
时刻c方向控制输入；n为预测区间内离散采样点的个数，其计算公式为：
[0072][0073]
基于式(4)，将预测区间[tk，tk+t
p
]内的所有状态变量进行拆解合并，可用当前状态变量和预测区间内的控制输入向量分别表征预测区间内c方向的质心位置向量质心速度向量质心加速度向量以及zmp位置向量即：
[0074][0075][0076]
[0077][0078]
其中φ
ps
，φ
vs
，φ
as
为当前状态变量对应的系数矩阵，φ
pu
，φ
vu
，φ
au
为质心加速度向量对应的系数矩阵，由式(4)迭代计算得到系数组成，且系数矩阵均与离散周期相关。代表实数域下的n
×
3维线性空间，其余表示以此类推。
[0079]
对上层mpc，采取稀疏采样的策略，以提高该部分的计算速度；取上层mpc的离散周期大于控制周期tc，则有：
[0080][0081][0082]
其中为大于1的整数，n
up
为上层mpc预测区间内的采样点个数。
[0083]
当采用上层mpc的离散周期时，式(6)-(9)的系数矩阵为和
[0084]
设计上层mpc的二次型目标函数为：
[0085][0086]
其中α，β，γ，ω为权重系数，是上层mpc需求解的最优控制序列，其表达式为：
[0087][0088]
其中是上层mpc预测区间内的世界坐标系x方向和y方向的控制输入向量，和为上层mpc输出的预测区间内的x方向和y方向落脚点位置向量；
[0089]
是上层mpc目标函数直接最小化控制输入的部分：
[0090][0091]
是上层mpc目标函数中跟踪期望质心平均速度输入的部分：
[0092][0093]
是上层mpc目标函数中跟踪期望zmp的部分：
[0094][0095]
是上层mpc目标函数中跟踪期望落脚点位置的部分：
[0096]
[0097]
其中：||
·
||代表欧式范数，为上层mpc预测区间内的x方向质心速度向量，为上层mpc预测区间内的y方向质心速度向量，为上层mpc预测区间内的x方向zmp位置向量，为上层mpc预测区间内的y方向zmp位置向量；为x方向期望的质心运动速度向量，为y方向期望的质心运动速度向量；为x方向期望的zmp轨迹，为y方向期望的zmp轨迹，均可通过双足落脚点规划部分输入的落脚点平面全维信息计算得到。
[0098]
对上层mpc，基于zmp稳定性判据设计稳定性约束条件。由于双足机器人的脚底板只能受到地面的支持力，因此zmp稳定性判据要求机器人运动过程中，zmp需要始终处于支撑多边形内以保证运动的稳定性，支撑多边形是双足机器人足底与地面之间的所有接触点的最小凸多边形区域，当机器人处于单足支撑期时，支撑多边形区域(简称支撑域)即为其单足脚底板形状，而双足支撑期，支撑域则为包含两只脚底板在内的最小凸多边形；参见图2。
[0099]
为保留一定稳定裕度，将支撑域向内收缩一定距离，转变为安全域，约束生成的质心运动轨迹要满足相应的zmp轨迹在行走过程中始终处于安全域范围内的约束条件，考虑单足支撑期支撑域，该约束条件可表达为：
[0100][0101]
其中和是与脚底板几何参数、支撑域向内收缩距离以及落脚点在世界坐标系下z轴姿态角相关的系数矩阵。以预测区间内的第j步落脚点为例，安全域约束示意图如图3所示，其中xw和yw为世界坐标系的x轴方向和y轴方向，x
foot
和y
foot
为落脚点坐标系的x轴方向和y轴方向，是期望落脚点的姿态向量中第j个落脚点的z轴姿态角，安全域为支撑域向内收缩一定距离的区域。
[0102]
对上层mpc，为避免质心运动轨迹发散，基于落脚点平面全维信息设计质心平面运动范围约束条件，要求在线规划的质心运动轨迹需始终处于允许的运动范围内。在双足机器人的行走过程中，质心位置位于两脚位置之间。在左右方向上，当质心位于左脚落脚点左侧或右脚落脚点右侧，可判定质心轨迹发散。当质心位于靠前脚前侧或靠后脚后侧时，可判定质心轨迹发散。因此，本发明设计了一种基于落脚点全维信息的质心运动可行域，如图4所示。在tk时刻，双足机器人处于全向行走状态，两只脚的姿态并不相同，此时质心的位置和质心运动方向如图4所示，以质心运动方向为姿态角，分别作两条平行线穿过两只脚底板中心点，以两条平行线与脚底板的交点作为质心运动可行域的边界顶点，构建封闭矩形，该矩形即为该时刻质心的运动可行域。在全向行走过程中，机器人支撑腿脚底板地面投影为支撑腿脚底板本身，摆动腿脚底板向地面垂直映射可得到摆动腿脚底板地面投影。在机器人全向行走过程中，质心运动可行域会随着机器人双腿的运动而改变几何参数，但不会改变形状(仍然保持为矩形)，运动可行域的几何参数(包括矩形中心点在世界坐标系下的位置、以及矩形的长和宽)可由上述确定质心运动可行域的构建方法计算得到。
[0103]
质心运动可行域是对机器人质心前进方向和横移方向的平面位置约束，要求在全向行走过程中质心的位置均需要处于质心运动可行域内，结合上述可行域构建说明，可得到预测区间内质心位置向量应满足的可行域约束的表达式为：
[0104][0105]
其中和是与质心运动可行域几何参数相关的稀疏系数矩阵，和为预测区间内质心运动可行域中心点的x、y方向稀疏位置向量。
[0106]
对机器人行走过程中的状态变量其初始值为：
[0107][0108]
在tk时刻，利用质心状态估计输入的质心状态估计向量对质心当前状态变量进行校正更新，即：
[0109][0110]
其中和为x方向和y方向校正后的状态变量，为x方向的状态变量，为y方向的状态变量，α为对质心状态估计的置信度，且有0≤α≤1。
[0111]
整合上层mpc的目标函数、稳定性约束和可行域约束，基于校正更新后状态变量，可通过二次规划求解器(为现有技术)求解最优控制序列如式(13)所示，即可得到上层mpc的控制输入向量和输出的落脚点位置向量输出的落脚点位置向量可用于双足机器人踝关节运动轨迹的在线规划(为现有技术)。将中的合并，可得到上层mpc最优控制输入序列为：
[0112][0113]
由校正后的x方向和y方向的状态变量和上层mpc求得的基于公式(6)、(7)和(9)，得到上层mpc的稀疏质心位置轨迹和稀疏质心速度轨迹和稀疏zmp轨迹和其计算式分别为：
[0114][0115][0116][0117]
上层mpc因目标函数和约束的复杂性，所以采用了稀疏的采样间隔，以加快上层mpc优化求解的速度，但求得的最优控制序列和上述轨迹均为稀疏的，其时间间隔(上层mpc的离散周期)比控制周期大，该最优控制序列首项代表区间内的控制输入，如直接使用该最优控制序列首项代替[tk，tk+tc]区间的控制输入计算下一个控制
周期的质心轨迹，将会造成较大的误差，且越大，误差也越大，因此，本发明设计了下层mpc来计算密集的最优控制序列，从而得到较为精确的质心轨迹。
[0118]
下层mpc以跟踪上层mpc求解得到的稀疏轨迹为目标，求解密集最优控制序列即下层mpc的离散周期应与控制周期tc相同，则：
[0119][0120][0121]
其中n
down
为下层mpc预测区间内的采样点个数。
[0122]
当采用下层mpc的离散周期时，式(6)-(9)的系数矩阵为和
[0123]
下层mpc的目标函数为：
[0124][0125]
其中η，μ，λ为权重系数，为下层mpc求解的密集最优控制序列，即：
[0126][0127]
分别是下层mpc预测区间内的世界坐标系x方向和y方向的控制输入向量；
[0128]
是下层mpc目标函数中跟踪上层mpc稀疏质心位置轨迹和的部分：
[0129][0130]
是下层mpc目标函数中跟踪上层mpc稀疏质心速度轨迹和的部分：
[0131][0132]
是下层mpc目标函数中跟踪上层mpc稀疏zmp轨迹的部分：
[0133][0134]
是下层mpc目标函数中直接最小化控制输入的部分：
[0135][0136]
式(30)-(33)中为映射系数矩阵，可将预测区间内下层mpc中需要跟踪上层mpc稀疏轨迹的采样点抽离出；为下层mpc预测区间内的x方向质心位置向量，为下层mpc预测区间内的y方向质心位置向量；为下层mpc预测区间内的x方向质
心速度向量，为下层mpc预测区间内的y方向质心速度向量；为下层mpc预测区间内的x方向zmp位置向量，为下层mpc预测区间内的y方向zmp位置向量。
[0137]
下层mpc以跟踪上层稀疏轨迹为目标，不设置额外的约束，为无约束mpc，可直接求取密集最优控制序列解析解，其解析解表达式为：
[0138][0139]
其中为解析系数矩阵，其计算式为：
[0140][0141]
其中为单位矩阵，是与上层mpc稀疏轨迹相关的系数矩阵，其计算式分别为：
[0142][0143][0144]
当目标函数的权重参数为常值时，ψ为常值矩阵，在线运行本发明设计的质心轨迹规划器前可提前计算该矩阵及其逆矩阵备用，从而提高在线质心运动轨迹规划的计算速度；ξ
x
，ξy中因上层mpc稀疏轨迹和校正后的状态变量会随机器人的全向行走而变化，是动态矩阵，计算式中除稀疏轨迹和状态变量以外的矩阵同样可提前计算。
[0145]
由校正后的x方向和y方向的状态变量和下层mpc求得的密集最优控制序列中的控制输入向量基于式(6)，得到下层mpc的密集质心位置轨迹，其计算可整合为：
[0146][0147]
因外部输入较多，双层mpc的结构较为复杂，本发明还设计了一种质心轨迹检测准则和保护机制，检测质心轨迹规划器是否因计算错误、外部输入不合理等原因导致输出的和出现发散现象，防止因关节超限等原因对机器人硬件造成巨大损伤。
[0148]
质心轨迹检测器利用上层mpc的质心运动可行域约束对和进行检测，如果和中有任意一个质心运动位置不满足质心运动可行域约束，则判定该质心运动轨迹存在发散迹象，需要终止机器人的行走运动，如均满足质心运动可行域约束，则在tk时刻将密集质心运位置轨迹的首项和下发给机器人执行。如上所述，依据式(19)，可行性判定准则为：
[0149]
[0150]
其中和是与质心运动可行域几何参数相关的密集系数矩阵，和为预测区间内的质心运动可行域的中心点的密集位置向量。当密集质心位置轨迹和满足式(39)，判定该轨迹不存在发散迹象，可以执行，输出该轨迹首项下发给机器人执行，如果不满足式(39)，则判定该轨迹发散，终止机器人行走运动过程。
[0151]
当前时刻为t
k+1
时，更新落脚点规划和质心状态估计给质心轨迹规划器提供的输入，重复上述过程计算得到t
k+1
时刻的质心密集位置轨迹和判定其满足可行性判定准则后，下发轨迹首项和给双足机器人执行。随时间的累进，重复上述过程直至机器人停止运动。
[0152]
本发明规划的双足机器人全向行走质心运动轨迹规划可配合踝关节轨迹规划(为现有技术)经逆运动学得到双足机器人全向行走的全身运动轨迹，实现双足机器人行走全身运动在线规划。
[0153]
所述实施例为本发明的优选的实施方式，但本发明并不限于上述实施方式，在不背离本发明的实质内容的情况下，本领域技术人员能够做出的任何显而易见的改进、替换或变型均属于本发明的保护范围。