四足机器人MPC+WBC方案

四足机器人MPC+WBC方案

一、MPC:

系统方程：

状态变量：\(x_r = (h_{com}, q_b, q_j) \in R^{24}\), 包含6维质心点动量，6维机身位姿，3*4个关节角度
输入变量：\(u = (f_{c1}, ......, f_{c4}, v_j) \in R^{3*4+12}\), 包含4个腿末端的3维力，12个关节速度
动力学：
其中\(f_{ci}\)是4足末端的接触力，\(r_{fi}\)是足触地相对于质心的位置矢量, \(h_{com}\)是质心动量，A是质心动力学的刚体惯性矩阵

动量=mv；所以

动量求导后，变成ma，就是力。

cost function:

• 机体跟随误差(姿态，位置，角速度，速度)
• 足轨迹跟随误差(关节角，关节角速度，接触脚在世界坐标系下的位置和速度，接触力)
• 关节速度和接触力中的高频内容惩罚
• relaxed barrier functions 不等式约束

机体误差和足迹轨迹误差：

relaxed barrier functions 不等式约束：

• relaxed barrier functions：

• 全惩罚项：

• 关节角、角速度、关节力矩上下限：

• 落足区域：

• 摩擦锥约束：

• 障碍物约束：

等式约束：

• \(f_{ci} = 0\) 摆动腿的接触力为0
• \(v_{ci} = 0\) 支撑腿的接触速度为0
• \(v_{ci}n_z = v^{*}_z\) 对于摆动相，参考轨迹仅在地表法线方向约束(这样即保证了足部以指定的速度抬离并离地，同时又在切线方向有足够的灵活度)

二、WBC：