四足机器人跳跃轨迹优化

Na-Cl

已于 2022-05-14 14:41:13 修改

阅读量3.7k

点赞数 10

分类专栏：四足机器人仿真文章标签： matlab 线性代数动态规划

于 2022-05-11 02:22:12 首次发布

本文链接：https://blog.csdn.net/qq_29735067/article/details/124701652

版权

本文通过对比Matlab内置的fmincon优化器和Casadi求解器，展示了在四足机器人动态运动轨迹规划任务中，Casadi显著优于Matlab的计算速度。在0.5秒的时间窗口内，采用16步计算的简单线性插值参考轨迹，尽管最终未达到期望终点，但Casadi仅需半秒就能完成优化，而fmincon则耗时约3分钟。这一结果突显了Casadi在实时优化问题中的高效性能。

摘要由CSDN通过智能技术生成

matlab自带优化工具和casadi对相同参考轨迹进行优化的结果

废话不多说，直接上图，规划时间0.5s 分16步计算，期望终点位置[1,0,0.2]' 由于时间较短，且给定的参考轨迹为简单的线性插值，规划出的轨迹并没有到期望的终点；

参考论文为[Online Trajectory Optimization for Dynamic Aerial Motions of a Quadruped Robot]，[Implementing Regularized Predictive Control for Simultaneous Real-Time Footstep and Ground Reaction Force Optimization]

图为优化问题设置部分参考，引子第二篇文献，两篇文献优化问题非常类似

图一为matlab自带的fmincon函数求解，速度奇慢，这个过程大概算了3分钟

图二为casadi求解器求解，过程大概只需要半秒

三维动画为慢放后效果

casadi代码可以参考se-hwan/landing-controller: Trajectory optimization in MATLAB for MIT Cheetah (github.com)

#使用matlab自带求解器 运算奇慢无比
clc;
clear;
warning off
%%
%物理参数
Body.m = 9;%机器人质量
%机身惯量
Body.Ib = [0.07 0     0;
    0    0.26   0;
    0    0     0.242];%转动惯量矩阵
Body.length_body=0.38;
Body.width_body=0.22;
Body.hipPos=[0.2,0.2,-0.2,-0.2;
    0.1,-0.1,0.1,-0.1;
    0,  0,   0, 0];
g = 9.8;%重力加速度
mu=0.4;%摩擦系数
%%
foot_pos=[0.2,0.2,-0.2,-0.2;
    0.1,-0.1,0.1,-0.1;
    0,      0,   0,      0];

%状态的权重
weight.QX = [10 10 10, 10 10 10, 10 10 10, 10 10 10 0]';
weight.QN = [50 50 100, 10 10 100, 10 10 10, 10 10 10 0]';
weight.Qc = [0.001 0.001 0.001]';
weight.Qf = [0.0001 0.0001 0.001]';
%%


N = 16; % N = 11
T = 0.5; % T = 0.22
dt_val = repmat(T/(N-1),1,N-1)';
cs_val = [repmat([1 1 1 1]', 1, 2) repmat([1 1 1 1]', 1, 2) repmat([0 0 0 0]', 1, 8) repmat([1 1 1 1]', 1, 3)]';
cs_TD_val = zeros(4,N-1)';


q_init_val = [0 0 0 0 0 0.2]';
qd_init_val = [0 0 0 0 0 0]';

q_term_ref = [0 0 0 1 0 0.6]';
qd_term_ref = [0 0 0, 0 0 0]';

c_init_val = repmat(q_init_val(4:6),4,1)+...
    diag([1 1 1, 1 -1 1, -1 1 1, -1 -1 1])*repmat([0.2 0.1 -q_init_val(6)],1,4)';

c_ref = diag([1 1 1, 1 -1 1, -1 1 1, -1 -1 1])*repmat([0.2 0.1