系列文章目录

提示：这里可以添加系列文章的所有文章的目录，目录需要自己手动添加
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曲线计算脚的摆动轨迹

原理

看看我另外的博客
https://blog.csdn.net/qq_35635374/article/details/120730023

代码

（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到之间1T 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)，进行三阶的贝塞尔曲线进行两点之间的插值，输出位置轨迹、进行求导即可或者速度轨迹和加速度轨迹【防盗标记–盒子君hzj】

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

（2）这里的起点和目标点不是三维的，而是一个标量，足端取的是Z轴的高度坐标【防盗标记–盒子君hzj】

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

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

参考资料

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

【四足机器人--摆动相足端位置速度轨迹规划】（4.1）FootSwingTrajectory（bezier曲线计算脚的摆动轨迹）代码解析相关推荐

【四足机器人--关节初始化时足端位置（速度、加速度）轨迹规划】（4.2）JPosInitializer（B样条曲线计算脚的摆动轨迹）代码解析
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.关节初始化构造函数(设置目标时间及间隔) 二.给定并B样条起始点 ...
四足机器人并联腿足端轨迹Matlab仿真
四足机器人并联腿足端轨迹Matlab仿真轨迹计算轨迹是分成两部分:摆线方程+水平线段首先,设置大腿关节轴心坐标为(0,0),根据机械设计的大腿小腿长度设置参数.此处设置大腿 l1=100mm,小 ...
【四足机器人步态状态FSM管理】电机失能模式、躯干翻身\收腿\恢复站立步态、WBC平衡站立步态、站立过程步态、对角小跑的运动步态
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.电机失能模式SM_State_Passive 功能 1.onEn ...
基于气动人工肌肉的双足机器人关节设计
介绍了一种由气动人工肌肉构建的双足机器人关节,该关节利用气动人工肌肉的柔性特性,可以有效控制双足机器人快速行走或跑步时的落地脚冲击问题. 详细给出了气动人工肌肉的工作原理以及由其构成的关节系统的硬件架 ...
四足论文《面对未知地形的四足机器人足端轨迹优化》解读
在笔者的关于足端轨迹规划的文章中,曾经提到了类正弦轨迹,该规划是根据斯坦福的四足机器人Doggo的开源代码中得出的.而该轨迹的缺点是对于地形的适应差,无法自适应的调节自身足端的轨迹规划. 基于该种问题 ...
[足式机器人]Part1 双足和四足的运动Ch04——【Legged Robots that Balance 读书笔记】
本文仅供学习使用本文结合个人理解体会作者的思路,也从个人科研角度出发,部分翻译参考由:QQ群(2群legged robot that balance)提供,感谢他们为本文英文版翻译所做的贡献(但有些 ...
四足机器人——步态规划
一.步态的概念步态就是描述动物走路的一种周期性现象.(描述动物是怎样走的) 二.四足机器人的步态足式运动的步态是指腿的摆动和支撑运动以及这些运动之间的相对时间关系.不同的步态规则决定了不同的足式运 ...
真·稳如狗：中国团队推出四足机器人，对标波士顿动力
今日宇树科技发布了他们的四足机器人莱卡狗(Laikago),这是国内首个正式对外发布的四足机器人. 在足式机器人领域,创建于1992年的波士顿动力已经成了世界上最具影响力的机器人公司之一,在过去的25 ...
STM32毕业设计——基于STM32+JAVA+Android的六足机器人控制系统设计与实现（毕业论文+程序源码）——六足机器人控制系统
基于STM32+JAVA+Android的六足机器人控制系统设计与实现(毕业论文+程序源码) 大家好,今天给大家介绍基于STM32+JAVA+Android的六足机器人控制系统设计与实现,文章末尾附有 ...

【四足机器人--摆动相足端位置速度轨迹规划】（4.1）FootSwingTrajectory（bezier曲线计算脚的摆动轨迹）代码解析