一种基于MPC的四足机器人状态修正方法及系统技术方案

本发明专利技术公开了一种基于MPC的四足机器人状态修正方法及系统，该方法包括步骤一是根据四足机器人多自由度的冗余特性，对控制任务进行排序；步骤二是将保持四足机器人身体平衡的控制任务设置为最高优先级任务；步骤三是获取四足机器人的足底反力期待值，在保持四足机器人身体平衡的控制任务的雅可比矩阵零空间内进行投影，得到次优先级任务的输入；步骤四是根据次优先级任务的输入，对四足机器人的足底反力期待值进行修正，得到最终的足底反力期待值，以控制四足机器人下一步的运动；本发明专利技术解决了单使用MPC中单刚体动力学模型得到的足底反力准确性不高，而且无法对四足机器人的足底反力期待值进行修正，难以确保四足机器人运动的平稳性的问题。的平稳性的问题。的平稳性的问题。

【技术实现步骤摘要】
一种基于MPC的四足机器人状态修正方法及系统


[0001]本专利技术涉及四足机器人状态修正
，特别是一种基于MPC的四足机器人状态修正方法及系统。

技术介绍

[0002]四足机器人属于浮动基机器人，即基座可以自由移动的机器人，可以将四足机器人视为在世界坐标系下单刚体动力学模型。目前，在控制四足机器人运动过程中，通过控制器使四足机器人执行踏步、转弯、爬坡和越障等任务，具体地，使用MPC控制器求解四足机器人的足底反力期待值，实现四足机器人下一步运动的预测和控制。但单纯的使用MPC中单刚体动力学模型得到的足底反力准确性不高，而且无法对四足机器人的足底反力期待值进行修正，难以确保四足机器人运动的平稳性。

技术实现思路

[0003]针对上述缺陷，本专利技术提出了一种基于MPC的四足机器人状态修正方法及系统，其目的在于解决了单使用MPC中单刚体动力学模型得到的足底反力准确性不高，而且无法对四足机器人的足底反力期待值进行修正，难以确保四足机器人运动的平稳性的问题。
[0004]为达此目的，本专利技术采用以下技术方案：
[0005]一种基于MPC的四足机器人状态修正方法，包括以下步骤：
[0006]步骤S1：根据四足机器人多自由度的冗余特性，对控制任务进行排序；
[0007]步骤S2：将保持四足机器人身体平衡的控制任务设置为最高优先级任务；
[0008]步骤S3：获取四足机器人的足底反力期待值，在保持四足机器人身体平衡的控制任务的雅可比矩阵零空间内进行投影，得到次优先级任务的输入；
[0009]步骤S4：根据次优先级任务的输入，对四足机器人的足底反力期待值进行修正，得到最终的足底反力期待值，以控制四足机器人下一步的运动。
[0010]优选地，在步骤S3中，具体包括以下步骤：
[0011]步骤S31：计算保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子N0；
[0012]步骤S32：计算其他控制任务的雅可比矩阵零空间投影算子N
i|i
‑1；
[0013]步骤S33：根据所述保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子和所述其他控制任务的雅可比矩阵零空间投影算子，得到修正的加速度所述修正的加速度作为次优先级任务的输入。
[0014]优选地，步骤S4中，具体是根据修正的加速度使用二次规划求解得到最终的足底反力f
r
，其中二次规划求解的约束公式如下：
[0015][0016]subject to
[0017][0018][0019][0020]Wf
r
≥0
ꢀꢀ
(4)
[0021]其中式(1)为浮动基系统动力学方程；式(2)为加速度方程；式(3)为足底反力方程；式(4)为接触力约束；Q1、Q2为MPC控制器计算得到的地面反力；f
rMPC
为MPC控制器计算得到的地面反力；S
f
为浮动基选择矩阵；W为增广的约束矩阵；δ
f
为浮动基加速度的弛豫变量；为浮动基受到的地面反作用力的弛豫变量；为修正的加速度；为实际的关节角加速度；A为广义质量矩阵；b为科里奥利力；g为重力加速度；J
c
为(足端)接触(地面)雅可比矩阵；为零向量。
[0022]优选地，所述保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子N0、所述其他控制任务的雅可比矩阵零空间投影算子N
i|i
‑1以及所述修正的加速度的计算公式如下：
[0023][0024][0025][0026]其中
[0027]J
i|pre
＝J
i
N
i
‑1[0028]N
i
‑1＝N0N
1|0
…
N
i
‑
1|i
‑2[0029][0030][0031]因为i≥1，所以
[0032][0033][0034]其中，为接触雅可比矩阵的广义逆矩阵；e
i
为位置误差，其中为期望位置，x
i
为实际位置，e
i
定义为：
[0035][0036]为第i个任务对应的加速度，其中K
p
和K
d
分别是位置和速度反馈增益，为期望位置加速度，为期望位置速度，为实际位置速度，定义为：
[0037][0038]J
i|pre
为第i个任务雅可比矩阵在前一个任务雅可比矩阵零空间中的投影；
[0039]表示基于以下公式的广义逆矩阵，其中A
′
为权重矩阵，J为雅可比矩阵，
[0040][0041]Δq
i
为第i个任务的关节误差；Δq
i
‑1为第i
‑
1个任务的关节误差；e
i
为为J
i
为第i个任务雅可比矩阵；为第i个任务的期望关节速度；为第i
‑
1个任务的期望关节速度；为第i个任务的期望关节加速度；为第i
‑
1个任务的期望关节加速度；为第i个任务的期望位置加速度；为第i个任务雅可比矩阵的时间导数；为实际关节角速度；为第i个任务任务雅可比矩阵在前面任务的零空间上的投影矩阵的伪逆矩阵；为第i个任务雅可比矩阵在前面任务零空间上的投影矩阵的动态一致性伪逆矩阵；为接触雅可比矩阵在前面任务零空间上的投影矩阵的动态一致性伪逆矩阵；J
c
为接触雅可比矩阵。
[0042]优选地，所述四足机器人运动是通过关节扭矩控制的，其中，所述四足机器人的运动学方程如下：
[0043][0044]其中，A为广义质量矩阵；b为科里奥利力；g为重力加速度；τ为关节扭矩，J
c
为地面接触雅可比矩阵；f
r
’
为地面反作用力；表示浮动基在世界坐标系下的加速度；表示四足机器人关节数量；表示四足机器人关节角位置；06表示零向量。
[0045]本申请的另一方面提供了一种基于MPC的四足机器人状态修正系统，所述系统包括
[0046]排序模块，用于根据四足机器人多自由度的冗余特性，对控制任务进行排序；
[0047]设置模块，用于将保持四足机器人身体平衡的控制任务设置为最高优先级任务；
[0048]获取模块，用于获取四足机器人的足底反力期待值；
[0049]投影模块，用于在保持四足机器人身体平衡的控制任务的雅可比矩阵零空间内进行投影，得到次优先级任务的输入；
[0050]修正模块，用于根据次优先级任务的输入，对四足机器人的足底反力期待值进行修正，得到最终的足底反力期待值，以控制四足机器人下一步的运动。
[0051]优选地，所述投影模块包括
[0052]第一投影算子计算子模块，用于计算保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于MPC的四足机器人状态修正方法，其特征在于：包括以下步骤：步骤S1：根据四足机器人多自由度的冗余特性，对控制任务进行排序；步骤S2：将保持四足机器人身体平衡的控制任务设置为最高优先级任务；步骤S3：获取四足机器人的足底反力期待值，在保持四足机器人身体平衡的控制任务的雅可比矩阵零空间内进行投影，得到次优先级任务的输入；步骤S4：根据次优先级任务的输入，对四足机器人的足底反力期待值进行修正，得到最终的足底反力期待值，以控制四足机器人下一步的运动。2.根据权利要求1所述的一种基于MPC的四足机器人状态修正方法，其特征在于：在步骤S3中，具体包括以下步骤：步骤S31：计算保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子N0；步骤S32：计算其他控制任务的雅可比矩阵零空间投影算子N
i|i
‑1；步骤S33：根据所述保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子和所述其他控制任务的雅可比矩阵零空间投影算子，得到修正的加速度所述修正的加速度作为次优先级任务的输入。3.根据权利要求2所述的一种基于MPC的四足机器人状态修正方法，其特征在于：步骤S4中，具体是根据修正的加速度使用二次规划求解得到最终的足底反力f
r
，其中二次规划求解的约束公式如下：subject tototoWf
r
≥0
ꢀꢀꢀꢀ
(4)其中式(1)为浮动基系统动力学方程；式(2)为加速度方程；式(3)为足底反力方程；式(4)为接触力约束；Q1、Q2为MPC控制器计算得到的地面反力；f
rMPC
为MPC控制器计算得到的地面反力；S
f
为浮动基选择矩阵；W为增广的约束矩阵；δ
f
为浮动基加速度的弛豫变量；为浮动基受到的地面反作用力的弛豫变量；为修正的加速度；为实际的关节角加速度；A为广义质量矩阵；b为科里奥利力；g为重力加速度；J
c
为(足端)接触(地面)雅可比矩阵；为零向量。4.根据权利要求2所述的一种基于MPC的四足机器人状态修正方法，其特征在于：所述保持四足机器人身体平衡的控制任务的雅可比矩阵零空间投影算子N0、所述其他控制任务的雅可比矩阵零空间投影算子N
i|i
‑1以及所述修正的加速度的计算公式如下：
其中J
i|pre
＝J
i
N
i
‑1N
i
‑1＝N0N
1|0
…
N
i
‑
1|i
‑
22
因为i≥1，所以Δq
i
＝0,＝0,其中，为接触雅可比矩阵的广义逆矩阵；e
i
为位置误差，其中为期望位置，x
i
为实际位置，e
i
定义为：：为第i个任务对应的加速度，其中K
p
和K
d
分别是位置...

【专利技术属性】
技术研发人员：袁进波，
申请(专利权)人：广州市优普计算机有限公司，
类型：发明
国别省市：