代码解释及MATLAB复现--灯哥四足机器人步态规划部分

某只旺仔

已于 2022-12-19 00:38:22 修改

阅读量1.3k

点赞数 3

分类专栏：四足机器人文章标签：机器人

于 2022-12-19 00:37:33 首次发布

本文链接：https://blog.csdn.net/qq_54543084/article/details/128367517

版权

四足机器人专栏收录该内容

7 篇文章 11 订阅

订阅专栏

笔者在看灯哥较新版的四足机器人控制程序中，观察到他没有向往常一样使用的传统的摆线轨迹，而是采用分段多项式进行足端轨迹规划。其中引入了摆动相抬腿速度作为轨迹规划的一个影响因素，提高了其自适应能力，这跟笔者之前写到的足端轨迹规划文章中的三次多项式规划方式有相同的思想。

一、规划方案：

考虑到灯哥的py-apple为八自由度四足机器人，因此我们只考虑机器人前进方向和竖直方向，将前进方向定为x轴，竖直方向定为z轴。

并定义下列参数含义：

t: 当前时间
Tf: 摆动相时间
xt: 摆动相在x轴方向上的目标位置
zh: 摆动相在z轴方向上的抬腿高度
x0: 摆动腿在x轴上初始位置
z0: 摆动相在z轴上初始位置
xv0:摆动相抬腿前腿的瞬时速度

1、x轴规划

（1）当 $t>0$ 并且 $t<\frac{T_{f}}{4}$ 时：

$x_{f}=(\frac{-4*xv0}{T_{f}})t^{2}+xv0*t+x_{0}$

（2）当 $t\geqslant \frac{T_{f}}{4}$ 并且 $t<\frac{3T_{f}}{4}$ 时：

$x_{f}=\frac{(-4T_{f}*xv0-16x_{t}+16x_{0})*t^{3}}{T_{f}^{3}}+\frac{(7T_{f}*xv0+24x_{t}-24x_{0})*t^{2}}{T_{f}^{2}}+\frac{(-15T_{f}*xv0-36x_{t}+36x_{0})*t}{4T_{f}})+\frac{(9*T_{f}*xv0+16x_{t})}{16}$

（3）当 $t\geqslant \frac{3T_{f}}{4}$ 时：

$x_{f}=x_{t}$

2、z轴规划

(1) 当 $t\geqslant 0$ 并且 $t<\frac{T_{f}}{2}$ 时：

$z_{f}=\frac{(16z_{0}-16z_{h})*t^{3}}{T_{f}^{2}}+\frac{(12z_{h}-12z_{0})*t^{2}}{T_{f}^{2}}+z_{0}$

（2）当 $t\geqslant \frac{T_{f}}{2}$ 时：

$z_{f}=\frac{(4z_{0}-4z_{h})*t^{2}}{T_{f}^{2}}-\frac{(4z_{0}-4z_{h})*t}{T_{f}}+z_{0}$

二、灯哥源代码：

def swing_curve_generate(t,Tf,xt,zh,x0,z0,xv0):
  # X Generator
  if t>=0 and t<Tf/4:
    xf=(-4*xv0/Tf)*t*t+xv0*t+x0
    
  if t>=Tf/4 and t<(3*Tf)/4:
    xf=((-4*Tf*xv0-16*xt+16*x0)*t*t*t)/(Tf*Tf*Tf)+((7*Tf*xv0+24*xt-24*x0)*t*t)/(Tf*Tf)+((-15*Tf*xv0-36*xt+36*x0)*t)/(4*Tf)+(9*Tf*xv0+16*xt)/(16)
    
  if t>(3*Tf)/4:
    xf=xt
  # Z Generator
  if t>=0 and t<Tf/2:
    zf=(16*z0-16*zh)*t*t*t/(Tf*Tf*Tf)+(12*zh-12*z0)*t*t/(Tf*Tf)+z0
  
  if t>=Tf/2:
    zf=(4*z0-4*zh)*t*t/(Tf*Tf)-(4*z0-4*zh)*t/(Tf)+z0
      
  #Record touch down position
  x_past=xf
  t_past=t
  
  # # Avoid zf to go zero
  if zf<=0:
    zf=0
  #x,z position,x_axis stop point,t_stop point;depend on when the leg stop
  
  return xf,zf,x_past,t_past

三、MATLAB复现

考虑到在该程序中，无法实时监测摆动腿速度，因此我们将xv0设置为定值，其他参数也任意初始化。

MATLAB源程序：

Tf = 0.5;
xv0 =1;
xt = 8;
zh = 4;
x0 = 0;
z0 = 0;
xf = zeros(1,length([0:0.01:Tf]));
zf = zeros(1,length([0:0.01:Tf]));
i=1;
k=1;
for t = 0:0.001:Tf
    if(t>=0&&t<Tf/4)
        xf(i) = (-4*xv0/Tf)*t*t+xv0*t+x0;
        i=i+1;
    elseif(t>=Tf/4&&t<(3*Tf)/4)
        xf(i) = ((-4*Tf*xv0-16*xt+16*x0)*t*t*t)/(Tf*Tf*Tf)+((7*Tf*xv0+24*xt-24*x0)*t*t)/(Tf*Tf)+((-15*Tf*xv0-36*xt+36*x0)*t)/(4*Tf)+(9*Tf*xv0+16*xt)/(16);
        i=i+1;
    elseif(t>=(3*Tf)/4)
        xf(i) =xt;
        i=i+1;
    end
    
    if(t>=0&&t<Tf/2)
        zf(k) = (16*z0-16*zh)*t*t*t/(Tf*Tf*Tf)+(12*zh-12*z0)*t*t/(Tf*Tf)+z0;
        if(zf(k)<0)
            zf(k)=0;
        end
        k=k+1;
    elseif(t>=Tf/2)
        zf(k) = (4*z0-4*zh)*t*t/(Tf*Tf)-(4*z0-4*zh)*t/(Tf)+z0;
        if(zf(k)<0)
            zf(k)=0;
        end
        k=k+1;
    end
end

subplot(3,1,1);
plot(xf,zf,'m-','LineWidth',2);
xlim([-1 max(xf)+1]);
ylim([0  max(zf)+1]);
xlabel('x');
ylabel('z');

subplot(3,1,2);
t = 0:0.001:Tf;
plot(t,xf,'b--','LineWidth',2);
xlabel('t');
ylabel('x');
ylim([0  max(xf)+1]);

subplot(3,1,3);
plot(t,zf,'g-','LineWidth',2);
xlabel('t');
ylabel('z');
ylim([0  max(zf)+1]);

想知道其他轨迹规划方法的友友可以看我的这一篇文章：
四足机器人学习笔记（足端轨迹规划）_某只旺仔的博客-CSDN博客_足端轨迹