MatthewPeterKelly提供了一个很好的非线性优化库matlab版本能让我接触如何进行轨迹优化，对于机器人来说轨迹优化的目的是对未来期望状态的规划控制，产生所需的前馈信号让系统完成复杂的轨迹运动，而反馈控制是来修正跟踪轨迹过程中的误差，这里的轨迹并不完全是指空间中一个三维轨迹，他也可以是系统状态，如机器人每个关节如何随时间运动，而对于机器人来说由于其自由度非常多要实现如后空翻或跳跃等行为，如果没有轨迹规划的参与那传统工程手段主要是采用状态机控制输出，即人工设定机器人行为的关键帧，并对参数进行调节，通过实验得到不同阶段的邦邦输出，其最大的问题是一般不考虑系统的动力学模型，连最简单的简化模型都可以不考虑，而是通过参数调节来实现，这样会造成以下几个问题：
（1）控制输出的不连续，这可能造成执行器保护，或损坏
（2）控制输出超出执行器能力
（3）控制输出不是最优
（4）模型参数变化后需要重新整定
（5）状态逻辑复杂，很可能没考虑周全，导致状态机卡死
因此通过轨迹规划是实现复杂机器人行为的必要手段，当然这里有划分为在线规划和离线规划，二者有区别但核心都是一样的，最基本的还是系统的动力学模型，而由于轨迹规划考虑了很长一段时间内的最优化问题，要求解对计算量要求很大因此很多时候都是采用简化模型，如跳跃我们采用SLIP模型产生轨迹，用反馈+前馈对各个时刻状态进行跟踪，对于传统轨迹优化的具体方法可以参考MatthewPeterKelly的项目：
其附带了一个PDF对基本轨迹优化如射点法等进行了介绍，当然要完全了解也挺难，我们目前还是直接采用现成的库以模板的形式描述这样一个优化问题，用优化库来求解，我们采用matlab求解微分方程获取系统的状态输出。
对于研究四足机器人首先是对其动力学模型进行研究，理论上最合适的研究路线从1维度SLIP模型开始着手，然后研究2D SLIP模型，然后扩展为矢量平面的5连杆模型，最后才是3D空间中的浮动基座模型，因此本文首先以1维SLIP模型来介绍如何采用matlab构建一个简单动力学模型，并对其开环模型进行仿真和动画绘制，首先需要构建1D-SLIP的动力学模型。
对于SLIP来说其分为两个典型状态，一个是飞行状态即弹簧末端离地（状态1），二是支撑状态即弹簧触地后长度小于原长（状态2），自身压缩和伸长的过程，对于状态1来说其主要受到重力加速度作用因此定义系统状态
即自身在全局坐标系下的高度和速度，设弹簧原始长度为l，最小压缩长度为l_min的话其微分动力学方程为：
在支撑过程中基于弹簧的基本模型可以构建如下的动力学方程：
上式中的K和D即弹簧的刚度和阻尼系数，基于上述方程我们进一步构建对应的代码如下：
function dz = cartPoleDynamics(z,u,p)
g=9.81;
if z(1,:)>p.l%控制动力学
dx = z(2,:);%速度积分模型
ddx=-g;
else%支撑
if(z(1,:)<p.l_min)%小于长度
dx = 1e-5;
ddx=-p.kp/p.m*(z(1,:)-p.l)-p.kd*z(2,:);
else
dx = z(2,:);%速度积分模型
ddx=-g - p.kp/p.m*(z(1,:)-p.l)-p.kd*z(2,:);
end
ddx=ddx+u;
end
dz = [dx;ddx];%一阶微分状态
end上面的代码即使对SLIP模型的描述，其通过判断当前高度是否小于弹簧原长来作为触地，另外在支撑中对最小压缩弹簧高度进行了限制，则基于该开环动力学模型可以采用matlab自带的ode45求解器进行仿真：
clc;
close all;
clear;
addpath ../../
z0 = [
1.0; %pos
0 %spd
];
tSpan = [0,5];
param.m = 0.5; % (kg)
param.g = 9.81; % (m/s^2) gravity
param.l = 0.3; % max_leg
param.l_min=0.05;
param.kp=100;
param.kd=2;
%%%% Function Handles
ctrlFun = @(z)( zeros(size(z(1,:))) ); %Passive controller for now
dynFun = @(t,z)( cartPoleDynamics(z, ctrlFun(z), param) );
%%%% Simulate the system!
options = odeset(...
'RelTol',1e-8, ...
'AbsTol',1e-8);
sol = ode45(dynFun, tSpan, z0, options);
%%%% Unpack the simulation
t = linspace(tSpan(1), tSpan(2), 200);
z = deval(sol,t);%模拟系统状态
u = ctrlFun(z);在获取仿真数据后结合运动学模型将系统状态进行动画绘制：
%%%% Draw Trajectory:
[p_com,p_leg] = cartPoleKinematics(z,param);%计算每个时刻各部分的运动学
xLow = -1;
xUpp = 1;
yLow = min(p_leg);
yUpp = max(p_com);
Limits = [xLow,xUpp,yLow,yUpp];
Anim.speed = 0.5;
Anim.plotFunc = @(t,z)( drawAnim(z,param,Limits) );
Anim.verbose = true;
animate(t,z,Anim);
其中运动学的函数如下，其主要读取了质心的系统状态，结合支撑和腾空相序计算腿的全局位置：
function [p_com,p_leg] = cartPoleKinematics(z,p)
x = z(1,:);%获取状态向量各通道
for i=1:length(x)
if x(i)>p.l
p_leg(i)=x(i)-p.l;
else
p_leg(i)=x(i)-p.l+p.l_min;
if p_leg(i)<p.l_min
p_leg(i)=0;
end
end
end
p_com=x;
end最终采用动画函数将结果进行绘制：
function drawAnim(z,param,Limits)
[p_com,p_leg] = cartPoleKinematics(z,param);
Cart_Width = 0.05;
Cart_Height = 0.05;
Pole_Width = 3; %pixels
hold off;
plot([Limits(1) Limits(2)],[0,0],'k-','LineWidth',2)
hold on;
% Compute color:
color = [0.2,0.7,0.1];
com = p_com;
pole = p_leg;
%Plot Cart %绘制每个采样时间的状态
x = - 0.5*Cart_Width;
y = com(1) - 0.5*Cart_Width;
w = Cart_Width;
h = Cart_Height;
hCart = rectangle('Position',[x,y,w,h],'LineWidth',2);
set(hCart,'FaceColor',color);
set(hCart,'EdgeColor',0.8*color);%画正方向
%Plot Pendulum 画直线棍子
Rod_X = [0,0];
Rod_Y = [com(1), pole(1)];
plot(Rod_X,Rod_Y,'k-','LineWidth',Pole_Width,'Color',color)
hold on;
axis(Limits);axis equal; axis on;
end最终运行该函数可以看到弹簧从空中下落，在触地后按弹簧模型进行运动，通过调节PD参数来模拟不同的弹簧柔顺性：
综上，本文中给出了与ODe45模拟1D弹簧运动的例子，基于该例子已经可以自己修改来实现其他的系统模型，也可以参考例子中提供的倒立摆，无人机等模型，当然最重要的是学习这样的仿真系统搭建框架方式，在上面的例子中是一个被动模型，即没有控制输入，仅依靠弹簧自身的模型来决定系统的运动，这里可以直接在支撑相代码部分构件一个简单的PD控制器来对期望高度进行控制，这也就是所谓的VMC虚拟模型，当然我们想做的是系统的轨迹规划控制，对于1D-SLIP模型来说就是让质量块跳跃到我们设定的高度，当然你可以基于能量的方式规划一个支撑过程中的恒定力来实现该过程，但是由于弹簧自身刚度的存在，这样的设计方法需要多次整定参数，而基于轨迹规划由于模型中考虑了弹簧自身刚度模型，则可以直接规划出连续的轨迹输出，另外该轨迹不仅仅是力还包括了质心的速度和高度，这样非常方便我们构建反馈+前馈的混合控制模型，使得对期望轨迹跟踪的精度更高。
链接：https://pan.baidu.com/s/1ZTseLqM9-5DWWbsDefGXOQ
提取码：73zr 
评论（0）
您还未登录，请登录后发表或查看评论