系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
TODO:写完再整理

文章目录

• 系列文章目录
• 前言
• 一、FootSwingTrajectory（bezier曲线）的内容
• 二、FootSwingTrajectory（bezier曲线）的设置/获取相关函数
• • 1、设置脚的起点位置函数
  • 2、设置脚的终点位置函数
  • 3、设置摆动腿的最大高度函数
  • 4、获取此时轨迹坐标，获得摆动腿的当前点位置的函数
  • 5、获取此时轨迹导数，得到摆动腿上当前的脚速度函数
  • 6、获取此时轨迹二次导数，得到脚在当前点上的加速函数
• 三、用（三阶）bezier曲线计算脚的摆动轨迹
• • 原理
  • 代码
  • • （1）用bezier曲线分抬脚、落脚两个阶段计算脚的摆动轨迹
    • （2）y0和yf两点之间的三次bezier插值（x在0和1之间）
    • （3）y0和yf两点之间的三次bezier插值导数（x在0和1之间）
    • （4）y0和yf两点之间的三次bezier插值导数（x在0到之间1）
• 总结
• 参考资料

前言

本文先对FootSwingTrajectory（bezier曲线计算脚的摆动轨迹）代码解析做个简单的介绍，具体内容后续再更，其他模块可以参考去我其他文章

提示：以下是本篇文章正文内容

一、FootSwingTrajectory（bezier曲线）的内容

  Vec3<T> _p0, _pf, _p, _v, _a;
  T _height;

  /*!
   * 功能：创建一个新的脚摆动轨迹，一切设置为零函数
   */
  FootSwingTrajectory() 
  {
    _p0.setZero();    //初始点
    _pf.setZero();    //终点
    _p.setZero();     //轨迹点
    _v.setZero();     //轨迹速度
    _a.setZero();     //轨迹加速度
    _height = 0;      //轨迹高度
  }

.
.

二、FootSwingTrajectory（bezier曲线）的设置/获取相关函数

1、设置脚的起点位置函数

  /*!
   * 功能：设置脚的起点位置函数
   * @param p0 : 脚的初始位置
   */
  void setInitialPosition(Vec3<T> p0) 
  {
    _p0 = p0;
  }

2、设置脚的终点位置函数

  /*!
   * 功能：设置脚的终点位置函数
   * @param pf :最后的脚姿势
   */
  void setFinalPosition(Vec3<T> pf) 
  {
    _pf = pf;
  }

3、设置摆动腿的最大高度函数

  /*!
   * 功能：设置摆动腿的最大高度函数
   * @param h :摆动腿的最大高度，在摆动腿进行到一半时达到
   */
  void setHeight(T h) 
  {
    _height = h;
  }

4、获取此时轨迹坐标，获得摆动腿的当前点位置的函数

  /*!
   * 功能：获得轨迹坐标，获得摆动腿的当前点位置的函数
   * @return :脚的位置
   */
  Vec3<T> getPosition() 
  {
    return _p;
  }

5、获取此时轨迹导数，得到摆动腿上当前的脚速度函数

  /*!
   * 功能：获得此时轨迹导数，得到摆动腿上当前的脚速度函数
   * @return : 足部速度
   */
  Vec3<T> getVelocity() 
  {
    return _v;
  }

6、获取此时轨迹二次导数，得到脚在当前点上的加速函数

  /*!
   * 功能： 获取此时轨迹二次导数，得到脚在当前点上的加速函数
   * @return : 脚加速度
   */
  Vec3<T> getAcceleration() 
  {
    return _a;
  }

三、用（三阶）bezier曲线计算脚的摆动轨迹

原理

代码

（1）用bezier曲线分抬脚、落脚两个阶段计算脚的摆动轨迹

/*!
 * 用bezier曲线计算脚的摆动轨迹
 * @tparam T
 * @param phase
 * @param swingTime
 */
template <typename T>
void FootSwingTrajectory<T>::computeSwingTrajectoryBezier(T phase, T swingTime) 
{
  _p = Interpolate::cubicBezier<Vec3<T>>(_p0, _pf, phase);                                          //y0和yf之间的线性插值。x在0和1之间在两个值之间插值  
  _v = Interpolate::cubicBezierFirstDerivative<Vec3<T>>(_p0, _pf, phase) / swingTime;               //y0和yf之间的三次bezier插值导数。x在0和1之间
  _a = Interpolate::cubicBezierSecondDerivative<Vec3<T>>(_p0, _pf, phase) / (swingTime * swingTime);//y0和yf之间的三次bezier插值导数。x在0到之间1

  T zp, zv, za;
  if(phase < T(0.5)) //相位小于0.5，抬脚阶段
  {
    zp = Interpolate::cubicBezier<T>(_p0[2], _p0[2] + _height, phase * 2);
    zv = Interpolate::cubicBezierFirstDerivative<T>(_p0[2], _p0[2] + _height, phase * 2) * 2 / swingTime;
    za = Interpolate::cubicBezierSecondDerivative<T>(_p0[2], _p0[2] + _height, phase * 2) * 4 / (swingTime * swingTime);
  } 
  else              //相位大于0.5，放脚阶段
  {
    zp = Interpolate::cubicBezier<T>(_p0[2] + _height, _pf[2], phase * 2 - 1);
    zv = Interpolate::cubicBezierFirstDerivative<T>(_p0[2] + _height, _pf[2], phase * 2 - 1) * 2 / swingTime;
    za = Interpolate::cubicBezierSecondDerivative<T>(_p0[2] + _height, _pf[2], phase * 2 - 1) * 4 / (swingTime * swingTime);
  }

  _p[2] = zp;
  _v[2] = zv;
  _a[2] = za;
}

template class FootSwingTrajectory<double>;
template class FootSwingTrajectory<float>;

（2）y0和yf两点之间的三次bezier插值（x在0和1之间）

/*!
 * y0和yf两点之间的三次bezier插值。x在0和1之间
 */
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;
}

（3）y0和yf两点之间的三次bezier插值导数（x在0和1之间）

/*!
 * y0和yf两点之间的三次bezier插值导数。x在0和1之间
 */
template <typename y_t, typename x_t>
y_t cubicBezierFirstDerivative(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_t(6) * x * (x_t(1) - x);
  return bezier * yDiff;
}

（4）y0和yf两点之间的三次bezier插值导数（x在0到之间1）

/*!
 *  y0和yf两点之间的三次bezier插值导数。x在0到之间1
 */
template <typename y_t, typename x_t>
y_t cubicBezierSecondDerivative(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_t(6) - x_t(12) * x;
  return bezier * yDiff;
}

总结

输入起点、目标点和控制占空比因子(0,1)，进行三阶的贝塞尔曲线进行两点之间的插值，输出位置轨迹、进行求导即可或者速度轨迹和加速度轨迹

注意：
（1）这里的求导方式不是求得位置轨迹后对其进行求导，而是在计算的时候已经对三阶的贝塞尔曲线公式进行求导了

（2）这里的起点和目标点不是三维的，而是一个标量，足端取的是Z轴的高度坐标

（3）这里的轨迹规划是先从起点到抬腿高度点（抬脚）进行规划，再从抬腿高度点到目标点（落脚）进行规划

（4）该工程的起点、目标点、高度、和控制占空比因子(0,1)是从外部用API给定的
.
.

参考资料

理论部分可以参考一下我的博客
https://blog.csdn.net/qq_35635374/article/details/120730023

标签：四足,p0,轨迹,bezier,yf,摆动,y0
来源： https://blog.csdn.net/qq_35635374/article/details/122062464