四足论文《面对未知地形的四足机器人足端轨迹优化》解读

        在笔者的关于足端轨迹规划的文章中，曾经提到了类正弦轨迹，该规划是根据斯坦福的四足机器人Doggo的开源代码中得出的。而该轨迹的缺点是对于地形的适应差，无法自适应的调节自身足端的轨迹规划。

        基于该种问题，《在面对未知地形的四足机器人足端轨迹优化》中，对于该轨迹进行了优化，将不同地形下的理想足端轨迹分段融合，得到适应多种地形的足端轨迹。

支撑相：  将支撑相划分为水平支撑相，下坡面支撑相，上坡面支撑相。原则均是将支撑相足端轨迹调整到与坡面平行。

                                    （a） 水平运动               （b）上坡面运动            （c）下坡面运动

将这三种地形下的足端轨迹分段融合，即可得到该种轨迹：

 Y方向位置约束：

μ1	摆动相在迈步周期T中的占比
μ2	向上坡面支撑相在迈步周期T中的占比
μ3	水平支撑相在迈步周期T中的占比
μ4	向下坡面支撑相在迈步周期T中的占比
q1	向上坡面支撑相在Y轴方向上移动距离占迈步总长S的比例
q2	水平支撑相在Y轴方向上移动距离占迈步总长S的比例
q3	向下坡面支撑相在Y轴方向上移动距离占迈步总长S的比例
S	迈步总长
H	抬腿高度

     在此条件下，Y方向速度约束和加速度约束均为0.

Z方向位置约束：

         在此条件下，Z方向速度约束和加速度约束均为0.

μ1	摆动相在迈步周期T中的占比
μ2	向上坡面支撑相在迈步周期T中的占比
μ4	向下坡面支撑相在迈步周期T中的占比
T	迈步周期
H	迈步高度
k2	支撑相在Z方向移动距离占迈步高度H的比例

对于Y，Z方向的位置约束求解：

对应的matlab代码：

clear;clc
% y轴指向前进方向
% z轴指向竖直向上方向
T = 1;
u1 = 0.25;    %摆动相在迈步周期的占比
u2 = 0.25;    %向上坡面支撑相在迈步周期的占比
u3 = 0.25;    %水平支撑相在迈步周期的占比
u4 = 0.25;    %向下坡面支撑相在迈步周期的占比

q1 = 0.333;   %向上坡面支撑相在Y轴方向上移动距离占迈步总长S的比例
q2 = 0.333;   %水平支撑相在Y轴方向上移动距离占迈步总长S的比例
q3 = 0.333;   %向下坡面支撑相在Y轴方向上移动距离占迈步总长S的比例

S = 0.1;      %迈步总长
H = 0.03;     %跨步最大高度
k1 = 0.667;   %摆动相在Z方向移动距离占迈步高度H的比例
k2 = 0.333;   %支撑相在Z轴方向上移动距离占迈步高度H的比例

num_y = 1;
for t=0:0.01:T   
    if (t=0)
        y = S*(t/u1/T-1/2/pi*sin(2*pi*t/u1/T));
        z = k1*H*(1/2-1/2*cos(2*pi*t/u1/T))+k2*H;
        y_all(num_y) = y;
        z_all(num_y) = z;
        num_y = num_y+1;
    elseif (t>=u1*T)&&(t<(u1+u2)*T)
        y = S - q3*S*((t-u1)/u2/T-1/2/pi*sin(2*pi*(t-u1*T)/u2/T));
        z = k2*H - k2*H*((t-u1*T)/u2/T - 1/2/pi*sin(2*pi*(t-u1*T)/u2/T));
        y_all(num_y) = y;
        z_all(num_y) = z;
        num_y = num_y+1;
    elseif(t>=(u1+u2)*T)&&(t<(1-u4)*T)
        y = (q1+q2)*S - q2*S*((t-(u1+u2)*T)/u3/T-1/2/pi*sin(2*pi*(t-(u1+u2)*T)/u3/T));
        z = 0;
        y_all(num_y) = y;
        z_all(num_y) = z;
        num_y = num_y+1;
    elseif(t>=(1-u4)*T)&&(t<=T)
        y = q1*S - q1*S*((t-(1-u4)*T)/u4/T-1/2/pi*sin(2*pi*(t-(1-u4)*T)/u4/T));
        z = k2*H*((t-(1-u4))*T/u4/T - 1/2/pi*sin(2*pi*(t-(1-u4)*T)/u4/T));
        y_all(num_y) = y;
        z_all(num_y) = z;
        num_y = num_y+1;
    end 
end
plot(y_all,z_all);

对应的画出的轨迹：

当更改参数的时候： 比如 减小斜坡支撑相在迈步周期中的时间占比，增大水平支撑相的时间占比，增加支撑相在Z轴方向上移动距离占迈步高度H的比例  结果如图所示

因此 可以根据斜坡的斜率和倾斜角度，使用该步态规划，得到适应该地形的轨迹从而避免：

1. 从水平地面向上坡面过渡时，摆动相还未结束，足端速度未降到0便与上坡面接触，引起冲击 。2. 从水平地面向下坡面过渡时，支撑相方向与坡面方向不一样而带来的冲击。

从原论文中得出：（TROT步态）

 PItch  和 Roll 的幅值减小,机器人稳定性增强

足端力矩减小

其他的可视化结论可以参考该篇论文：

《面向未知地形的四足机器人足端轨迹优化》