(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202211266298.1 (22)申请日 2022.10.17 (71)申请人 广州市优 普计算机有限公司 地址 510630 广东省广州市天河区天河北 路233号3203室 (72)发明人 袁进波　 (74)专利代理 机构 佛山市禾才知识产权代理有 限公司 4 4379 专利代理师 梁永健 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于MPC的四足机器人状态 修正方法及 系统 (57)摘要 本发明公开了一种基于MPC的四足机器人状 态修正方法及系统， 该方法包括步骤一是根据四 足机器人多自由度的冗余特性， 对控制任务进行 排序； 步骤二是将保持四足机器人身体平衡的控 制任务设置为最高优先级任务； 步骤三是获取四 足机器人的足底反力期待值， 在保持四足机器人 身体平衡的控制任务的雅可比矩 阵零空间内进 行投影， 得到次优先级任务的输入； 步骤四是根 据次优先级任务的输入， 对四足机器人的足底反 力期待值进行修正， 得到最终的足底反力期待 值， 以控制四足机器人下一步的运动； 本发明解 决了单使用MPC中单刚体动力学模型得到的足底 反力准确性不高， 而且无法对四足机器人的足底 反力期待值进行修正， 难以确保四足机器人运动 的平稳性的问题。 权利要求书3页 说明书9页 附图1页 CN 115488893 A 2022.12.20 CN 115488893 A 1.一种基于 MPC的四足机器人状态修 正方法， 其特 征在于： 包括以下步骤： 步骤S1： 根据四足机器人多自由度的冗余特性， 对 控制任务进行排序； 步骤S2： 将保持四足机器人身体平衡的控制任务设置为 最高优先级任务； 步骤S3： 获取四足机器人的足底反力期待值， 在保持四足机器人身体平衡的控制任务 的雅可比矩阵零空间内进行投影， 得到次优先级任务的输入； 步骤S4： 根据次优先级任务的输入， 对四足机器人的足底反力期待值进行修正， 得到最 终的足底反力期待值， 以控制四足机器人 下一步的运动。 2.根据权利要求1所述的一种基于MPC的四足机器人状态修正方法， 其特征在于： 在步 骤S3中， 具体包括以下步骤： 步骤S31： 计算保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子N0； 步骤S32： 计算 其他控制任务的雅可比矩阵零空间投影算子Ni|i‑1； 步骤S33： 根据所述保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算 子和所述其他控制任务的雅可比矩阵零空间投影算子， 得到修正的加速度 所述修正 的加速度作为次优先级任务的输入。 3.根据权利要求2所述的一种基于MPC的四足机器人状态修正方法， 其特征在于： 步骤 S4中， 具体是根据修正的加速度 使用二次规划求解得到最终的足底反力fr， 其中二次 规划求解的约束公式如下： subject to Wfr≥0    (4) 其中式(1)为浮动基系统动力学方程； 式(2)为加速度方程； 式(3)为足底反力方程； 式 (4)为接触力约束； Q1、 Q2为MPC控制器计算得到的地面反力； frMPC为MPC控制器计算得到的地 面反力； Sf为浮动基选择矩阵； W为增广的约束矩阵； δf为浮动基加速度的弛豫变量； 为浮 动基受到的地面反作用力的弛豫变量； 为修正的加速度； 为实际的关节角 加速度； A 为广义质量矩阵； b为科里奥利力； g为重力加速度； Jc为(足端)接触(地面)雅可比矩阵； 为零向量。 4.根据权利要求2所述的一种基于MPC的四足机器人状态修正方法， 其特征在于： 所述 保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子N0、 所述其他控制任务 的雅可比矩阵零空间投影算子Ni|i‑1以及所述 修正的加速度 的计算公式如下：权　利　要　求　书 1/3 页 2 CN 115488893 A 2其中 Ji|pre＝JiNi‑1 Ni‑1＝N0N1|0…Ni‑1|i‑2 因为i≥1， 所以 Δqi＝0, 其中， 为接触雅可比矩阵的广义逆矩阵； ei为位置误差， 其中 为期望位置， xi为实 际位置， ei定义为： 为第i个任务对应的加速度， 其中Kp和Kd分别是位置和速度反馈增益， 为期望 位置加速度， 为期望位置 速度， 为实际位置 速度， 定义为： Ji|pre为第i个任务 雅可比矩阵在前一个任务 雅可比矩阵零空间中的投影； 表示基于以下公式的广义逆矩阵， 其中A'为权 重矩阵， J为 雅可比矩阵， Δqi为第i个任务的关节误差； Δqi‑1为第i‑1个任务的关节误差； ei为 Ji为第 i个任务雅可比矩阵； 为第i个任务的期望关节速度； 为第i‑1个任务的期望关节 速度； 为第i个任务的期 望关节加速度； 为第i‑1个任务的期 望关节加速度； 为第i个任务的期望位置加速度； 为第i个任务雅可比矩阵的时间导数； 为实际关节角 速度； 为第i个任务任务雅可比矩阵在前面任务的零空间上 的投影矩阵的伪逆矩阵； 为第i个任务雅可比矩阵在前面任务零空间上的投影矩阵的动态一致性伪逆矩阵； 为接触雅可比矩阵在前面任务零空间上的投影矩阵的动态一致性伪逆矩阵； Jc为接触 雅可比矩阵。权　利　要　求　书 2/3 页 3 CN 115488893 A 3