本篇将讲解MiniCheetah中所使用的的足端轨迹方程-贝塞尔曲线方程

一、贝塞尔曲线

贝塞尔曲线就是这样的一条曲线，它是依据四个位置任意的点坐标绘制出的一条光滑曲线。
【四足机器人那些事】足端轨迹规划（二）在历史上，研究贝塞尔曲线的人最初是按照已知曲线参数方程来确定四个点的思路设计出这种矢量曲线绘制法。贝塞尔曲线的有趣之处更在于它的“皮筋效应”，也就是说，随着点有规律地移动，曲线将产生皮筋伸引一样的变换，带来视觉上的冲击。1962年，法国数学家Pierre Bézier第一个研究了这种矢量绘制曲线的方法，并给出了详细的计算公式，因此按照这样的公式绘制出来的曲线就用他的姓氏来命名是为贝塞尔曲线。

二、数学形式

1、线性公式

给定点P0、P1，线性贝兹曲线只是一条两点之间的直线。这条线由下式给出：
取P₀ = [0, 0], P₁ = [1, 1]。可以从图中看出，其效果等同于线性插值。
【四足机器人那些事】足端轨迹规划（二）

2、二次方公式

对于给定点P0=[0, 0]、P1=[0.5, 0.5]、P2=[1, 0]，曲线的函数如下：
其图像如下：
【四足机器人那些事】足端轨迹规划（二）

3、三次方公式

P0、P1、P2、P3四个点在平面或在三维空间中定义了三次方贝兹曲线。曲线起始于P0走向P1，并从P2的方向来到P3。一般不会经过P1或P2；这两个点只是在那里提供方向信息。P0和P1之间的间距，决定了曲线在转而趋进P3之前，走向P2方向的“长度有多长”。函数形式如下：

三、Cheetah中所使用的轨迹函数

这里我们先着眼于位置，把源码中的速度项，加速项去掉，对其进行分析。

1.1 代码部分

轨迹生成函数调用：

template <typename T>
void FootSwingTrajectory<T>::computeSwingTrajectoryBezier(T phase, T swingTime) {
 _p = Interpolate::cubicBezier<Vec3<T>>(_p0, _pf, phase);
 T zp, zv, za;
 if(phase < T(0.5)) {
  zp = Interpolate::cubicBezier<T>(_p0[2], _p0[2] + _height, phase * 2);
 } else {
  zp = Interpolate::cubicBezier<T>(_p0[2] + _height, _pf[2], phase * 2 - 1);
 }
 _p[2] = zp;
}

方法实现

template <typename y_t, typename x_t>
y_t cubicBezier(y_t y0, y_t yf, x_t x) {
 static_assert(std::is_floating_point<x_t>::value,"must use floating point value");
 assert(x >= 0 && x <= 1);
 y_t yDiff = yf - y0;
 x_t bezier = x * x * x + x_t(3) * (x * x * (x_t(1) - x));
 return y0 + bezier * yDiff;
}

1.2 数学模型

1.2.1位置

规定以下符号 – 任意起始点p₀=[x₀ y₀ z₀]^T – 结束点p_f = [x_f y_f z_f]^T – 抬腿高度h。将方法实现中的代码转换成数学表达式如下：
【四足机器人那些事】足端轨迹规划（二）将上式合并成一条：
需要注意的是，该计算可针对一维变量(例如对Z轴坐标单独计算)，也可以是三维向量之间的运算(例如坐标点)，对于三维点计算，其展开形式如下(实际上与上式无异)：
【四足机器人那些事】足端轨迹规划（二）下面结合图像更加直观地对其进行分析。先分析式1的图像，在 t ∈ [0, 1] 的情况下，分别取两组数据进行绘图比较：

p₀ = 0.5 , p_f = 0.2

p₀,p_f的值互换:

p₀ = 0.2 , p_f = 0.5

可以看到如下图像：
【四足机器人那些事】足端轨迹规划（二） 因此可以得出结论，源码中，该部分定义了，摆动相上行与下行的轨迹：

  if(phase < T(0.5)) {
    zp = Interpolate::cubicBezier<T>(_p0[2], _p0[2] + _height, phase * 2);
  } else {
    zp = Interpolate::cubicBezier<T>(_p0[2] + _height, _pf[2], phase * 2 - 1);
  }

下面来看整体轨迹，设定以下数据： – 起始点p0=[0, 0, 0.2] – 结束点pf=[1.5, 1.5, 0] – 抬腿高度h=0.5 空间轨迹如下：
【四足机器人那些事】足端轨迹规划（二）各轴坐标随时间的图像：

1.2.2速度

接下来看该轨迹的速度，对时间进行微分，得到：
化简得到下式：
绘制其图像：
【四足机器人那些事】足端轨迹规划（二）

1.2.3加速度

对1.2.2得到的速度公式微分，得到：
从公式可以看出，此时加速度方程为线性方程：
【四足机器人那些事】足端轨迹规划（二）

总结

以上对MiniCheetah中所使用的的足端轨迹方程进行了位置，速度，加速度的分析，尽管该轨迹方程形式上非常简洁易用，但是仍存在一定缺点，例如对地面冲击较大，容易造成电机损坏以及姿态稳定上的问题，因此还有一定的提升空间，有兴趣的小伙伴可以参考上一篇文章，将摆线运动轨迹融入到cheetah四足机器人的足端轨迹当中去，这里暂时不展开细说。