四足机器人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\) 对于摆动相，参考轨迹仅在地表法线方向约束（这样即保证了足部以指定的速度抬离并离地，同时又在切线方向有足够的灵活度)