(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211010867.6 (22)申请日 2022.08.23 (71)申请人 北京理工大 学 地址 100081 北京市海淀区中关村南大街5 号 (72)发明人 黄强　赖俊杭　陈学超　余张国　 高峻峣　李庆庆　 (74)专利代理 机构 南京智造力知识产权代理有 限公司 32382 专利代理师 张明明 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 基于双层模型预测控制的双足机器人全向 行走质心轨 迹规划方法 (57)摘要 本发明公开了一种基于双层模型预测控制 的双足机器人全向行走质心轨迹规划方法， 上层 MPC在满足ZMP稳定性约束和质心运动可行域约 束的条件下， 得到稀疏的质心运动轨迹和ZMP轨 迹； 下层MPC以跟踪上层稀疏轨迹为目标， 得到密 集质心位置运动轨迹； 该密集轨迹经质心轨迹检 测器判定 无发散后， 输出当前时刻的质心运动位 置， 供机器人使用。 本发明采用变周 期双层模型 预测控制结构， 兼顾了生 成轨迹的准确性和实时 性， 通过质心轨迹检测可防止发散轨迹对机器人 硬件造成损伤。 权利要求书3页 说明书12页 附图2页 CN 115256396 A 2022.11.01 CN 115256396 A 1.基于双 层模型预测控制的双足机器人全向行 走质心轨 迹规划方法， 其特 征在于： 上层MPC接收落脚点规划和质心状态估计的输入后， 在满足零力矩点稳定性约束及可 行域约束的条件下， 使得上层MPC目标函数最小化， 解得稀疏最优控制序列 通过状 态空间方程的循环迭代， 得到预测区间内的X方向稀疏质心运动位置轨迹 Y方向稀疏 质心运动位置轨迹 X方向稀疏质心运动速度轨迹 Y方向稀疏质心运动速度轨迹 X方向稀疏ZMP轨 迹 和Y方向稀疏ZMP轨 迹 下层MPC以跟踪上层稀疏轨迹为目标， 解得密集最优控制序列 经状态空间方程 的循环迭代， 得到X 方向密集质心位置 轨迹 和Y方向密集质心位置 轨迹 质心轨迹检测器依据可行性判定准则判断密集质心位置轨迹是否发散， 如果没有发 散， 则将密集质心位置 轨迹的首项值 和 发送给双足机器人 执行。 2.根据权利要求1所述的双足机器人全向行走质心轨迹规划方法， 其特征在于， 所述可 行域约束的条件为： 其中 和 是与质心运动可行域几何参数相关的稀疏系数矩阵， 和 为 预测区间内质心运动可行域中心点的X、 Y方向稀疏位置向量， 为上层MPC预测区间内 的X方向质心位置向量， 为上层MPC预测区间内的Y方向质心位置向量。 3.根据权利要求2所述的双足机器人全向行走质心轨迹规划方法， 其特征在于， 所述可 行域具体为： 以双足机器人质心运动方向为姿态角， 分别作两条平行线穿过两只脚底板中 心点， 以两条平行线与脚 底板的交点作为质心 运动可行域的边界顶点， 构建封闭矩形， 即为 该时刻质心运动的可 行域。 4.根据权利要求1所述的双足机器人全向行走质心轨迹规划方法， 其特征在于， 所述最 优控制序列 为： 其中： 是上层MPC预测区间内的世界坐标系X方向和Y方 向的控制输入向量， 和 为上层MPC 输出的预测区间内的X 方向和Y方向落脚点 位置向量； 由校正后的X方向和Y方向的状态变量 和上层MPC的控制输入向量 得 到X方向稀疏质心运动位置轨迹 Y方向稀疏质心运动位置轨迹 X方向稀疏质心 运动速度轨迹 Y方向稀疏质心运动速度轨迹 X方向稀疏ZMP轨迹 和Y方向 稀疏ZMP轨 迹 权　利　要　求　书 1/3 页 2 CN 115256396 A 2其 中 ： 上 层 M P C 最 优 控 制 输 入 序 列 为上层MPC的系数矩阵。 5.根据权利要求4所述的双足机器人全向行走质心轨迹规划方法， 其特征在于， 所述校 正后的X方向和Y方向的状态变量 满足： 其中： 为X方向的状态变量， 为Y方向的状态变量， α为对质心状态估计的置信度， 和 为质心状态估计输入的质心状态估计向量。 6.根据权利要求1所述的双足机器人全向行走质心轨迹规划方法， 其特征在于， 所述下 层MPC的目标函数为： 其中： η， μ， λ为权 重系数， 为下层MPC求 解的密集 最优控制序列； 是下层MPC目标函数中跟踪上层MPC稀疏质心位置 轨迹 和 的部分： 是下层MPC目标函数中跟踪上层MPC稀疏质心速度轨 迹 和 的部分： 是下层MPC目标函数中跟踪上层MPC稀疏ZMP轨 迹 和 的部分： 是下层MPC目标函数中直接最小化控制输入的部分： 式中S为映射系数矩阵， 为下层MPC预测区间内的X方向质心位置向量， 为下 层MPC预测区间内的Y方向质心位置向量， 为下层MPC预测区间内的X方向质心速度向权　利　要　求　书 2/3 页 3 CN 115256396 A 3