在笔者的关于足端轨迹规划的文章中，曾经提到了类正弦轨迹，该规划是根据斯坦福的四足机器人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<u1*T)&&(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 的幅值减小,机器人稳定性增强
足端力矩减小
其他的可视化结论可以参考该篇论文：
《面向未知地形的四足机器人足端轨迹优化》