MIT miniCheetah mpc的简单matlab仿真

介绍：使用 Dynamic Locomotion in the MIT Cheetah 3 Through Convex Model-Predictive Control一文中介绍的凸模型进行mpc控制的简单仿真。物理参数和权重等基本和miniCheetah 相同；

框架如图；由于是单刚体仿真可以省略掉状态估计和腿部控制器；参考轨迹由人工通过速度指令累加生成，或将生成好的路径分段提供给CMPC控制器中进行轨迹跟踪；

代码较为简单，基本照搬MIT开源代码进行部分语言适配即可；MIT开源代码可以看我之前的贴子。原理讲解CSDN上也有很多人进行了分析，可以参考盒子君的文章

腿序号如图所示，左前腿为1号腿 ；坐标系参考如图

下面是四腿支撑时进行质心轨迹控制的效果：

后续工作：

仿照MIT开源项目的步态时间规划，为mpc提供接触信息；

将提前触地检测加入到mpc计算中，

演示设置为右前腿在摆动相相位0.7时触地，参考位姿设置高度0.28，其他均为0；

命令行中的为输入到仿真的力，可以证明其为对角步态，其稳定运行16秒的位姿如图所示；

 

后续工作2：

加入步态时间表和落足点调整：

后续工作3：

修正了bug，重写了绘制函数，加入了一个通过pid的路径跟踪代码：

next_des=max(min(nextPoint+1,length(linex)),1);
temp=[linex(next_des);liney(next_des)]-[linex(nextPoint);liney(nextPoint)];
des_yaw=atan2(temp(2),temp(1));
desyaw_plot=[desyaw_plot;des_yaw];
yaw_err=X_out(3,iter)-des_yaw;
yaw_turn_rate=-4*yaw_err;
kp=diag([0.03,0.03,0]);
kd=diag([0.003,0.003,0]);
err=[X_out(4:5,iter);0]-[linex(nextPoint);liney(nextPoint);0];
div=err-lasterr;
v_des_world=-(kp*err+kd*div);
lasterr=err;
v_des_world=v_des_world/norm(v_des_world)*1;

后续工作4：

更改路径跟踪方式为直接将路径提供给CMPC控制器中进行路径跟踪，如果参考路径经过插值处理使得更加合理后，跟踪效果应该效果不错；

轨迹跟踪效果如图：紫色为参考轨迹

%查找当前最近的路径点
for i=1:length(linex)
                if norm([X_out(4:5,iter)]-[linex(i);liney(i)])<last_norm
                    last_norm=norm([X_out(4:5,iter)]-[linex(i);liney(i)]);
                    lasti=i;
                    if last_norm<0.05 %提前结束查询
                        break;
                    end
                end
            end

%% 处理轨迹信息 根据自身情况进行更改
%%
%将生成的轨迹信息填入MPC参考
 for i =1:future_period
                X_ref_mpc((i-1)*13+1:(i-1)*13+13)=zeros(13,1);
                X_ref_mpc((i-1)*13+4)=xtr(i);
                X_ref_mpc((i-1)*13+5)=ytr(i);
                X_ref_mpc((i-1)*13+6)=0.28;

                X_ref_mpc((i-1)*13+3)=yaw_d(i);
                
                X_ref_mpc((i-1)*13+9)=dotyaw(i);
                X_ref_mpc((i-1)*13+10)=dotx(i);
                X_ref_mpc((i-1)*13+11)=doty(i);
            end

NMPC版本地址：非常不完善的版本，期望有大佬能优化一下

NaCl-1374/RPC_cheetah: Policy Regularized Model Predictive Control simulated by matlab (github.com)