系列文章目录

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

文章目录

系列文章目录
前言
一、关节初始化构造函数（设置目标时间及间隔）
二、给定并B样条起始点、中间点、落脚点，目标时间点四个参数
三、按照B样条曲线平稳移动跟踪到匍匐状态的函数（进行关节初始化）
四、B样条实现的库Basic Bspline（看成小黑盒子，会用API就能部署）
总结
参考资料

前言

认知有限，望大家多多包涵，有什么问题也希望能够与大家多交流，共同成长！

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

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

一、关节初始化构造函数（设置目标时间及间隔）

/*!* 功能：关节初始化构造函数（设置目标时间及间隔）*/
template <typename T>   //C++模板
JPosInitializer<T>::JPosInitializer(T end_time, float dt): _b_first_visit(true),_end_time(end_time),_curr_time(0.),_dt(dt),_ini_jpos(cheetah::num_act_joint)
{_UpdateParam();
}

/*!* 功能：关节初始化，更新控制器参数函数* 步骤：* （1）从yaml文件加载关节位置控制初始化参数，更新参数* （2）获得target_jpos矩阵* （3）获得mid_jpos矩阵*/
template <typename T>
void JPosInitializer<T>::_UpdateParam()
{ParamHandler handler(THIS_COM "config/initial_jpos_ctrl.yaml");   //从yaml文件加载关节位置控制初始化参数handler.getVector<T>("target_jpos", _target_jpos);                //_target_jpos矩阵handler.getVector<T>("mid_jpos", _mid_jpos);                      //_mid_jpos矩阵
}

.
.

二、给定并B样条起始点、中间点、落脚点，目标时间点四个参数

/*!* 功能：给定并B样条起始点、中间点、落脚点，目标时间点四个参数* 步骤：* （1）定义变量* （2）复位每条腿的参数* （3）设置每条腿的参数：起始点、中间点、落脚点，目标时间点* （4）根据上面填充的数据，使用B样条的方法，进行更新关节轨迹参数*/
template <typename T>
void JPosInitializer<T>::_UpdateInitial(const LegController<T>* ctrl)
{/*（1）定义起始点、中间点、落脚点变量*/T ini[3 * cheetah::num_act_joint];       //定义起始点T fin[3 * cheetah::num_act_joint];       //定义落脚点T** mid = new T*[1];                     //定义中间点mid[0] = new T[cheetah::num_act_joint];/*（2）复位每条腿起始点和落点的参数为0*/for (size_t i(cheetah::num_act_joint); i < 3 * cheetah::num_act_joint; ++i) /{ini[i] = 0.;fin[i] = 0.;}
/*（3）加载每条腿三个关节起始点、中间点和落点位置的参数*/
//起始点for (int leg(0); leg < 4; ++leg)          //四条腿{for (int jidx(0); jidx < 3; ++jidx)     //每条腿的三个关节{ini[3 * leg + jidx]       = ctrl->datas[leg].q[jidx];   //加载对应的参数_ini_jpos[3 * leg + jidx] = ctrl->datas[leg].q[jidx];}}
//中间点和落脚点for (size_t i(0); i < cheetah::num_act_joint; ++i)  //加载对应的关节位置参数{fin[i] = _target_jpos[i];mid[0][i] = _mid_jpos[i];}
/*（4）根据上面填充的数据，使用B样条的方法，进行更新关节轨迹参数*/_jpos_trj.SetParam(ini, fin, mid, _end_time);      delete[] mid[0];delete[] mid;
}

.
.

三、按照B样条曲线平稳移动跟踪到匍匐状态的函数（进行关节初始化）

/*!* 功能：按照B样条曲线平稳移动跟踪到匍匐状态的函数（进行关节初始化）*/
template <typename T>
bool JPosInitializer<T>::IsInitialized(LegController<T>* ctrl)
{_curr_time += _dt;// 初始设置if (_b_first_visit) //在第一次进来的时候更新控制指令{_UpdateInitial(ctrl); //给定并设置B样条起始点、中间点、落脚点，目标时间点四个参数_b_first_visit = false;}// 进行B样条曲线跟踪if (_curr_time < _end_time) {T jpos[cheetah::num_act_joint];//获取给定时间的B样条线位置,存到jpos_jpos_trj.getCurvePoint(_curr_time, jpos);//遍历4条腿的三个电机，进行位置控制B样条曲线跟踪for (size_t leg(0); leg < cheetah::num_leg; ++leg)              {for (size_t jidx(0); jidx < cheetah::num_leg_joint; ++jidx) {ctrl->commands[leg].tauFeedForward[jidx] = 0.;            //设置关节前馈力矩为0ctrl->commands[leg].qDes[jidx] = jpos[3 * leg + jidx];    //设置关节期望位置ctrl->commands[leg].qdDes[jidx] = 0.;                     //设置关节期望速度=0}}return false;}return true;
}

.
.

四、B样条实现的库Basic Bspline（看成小黑盒子，会用API就能部署）

我把整个库放上来把，有点多

#ifndef B_SPLINE_BASIC
#define B_SPLINE_BASIC#include <assert.h>
#include <stdio.h>
#include <iostream>#define SP_IS_EQUAL(x, y) (((x) - (y)) * ((x) - (y)) < 1.e-10)
#define SP_SAFE_DELETE_AR(p) \if (p) {                   \delete[] p;              \(p) = NULL;              \}/*!* Basic Bspline  <br/>* DIM : Dimension of control points <br/>* DEGREE : Derivation is going to be 0 when it is over DEGREE <br/>* NUM_MIDDLE : Num middle points (the points except initial and final) <br/>* <br/>* CONST_LEVEL_INI/FIN : constraint level <br/>* 0: position <br/>* 1: + velocity <br/>* 2: + acceleration <br/>* <br/>* ******************************  WARNING* *********************************************** <br/> NumKnots(DEGREE +* NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN) >= 2 * (DEGREE + 1) <br/>* ******************************  WARNING* *********************************************** <br/>*/template <typename T, int DIM, int DEGREE, int NUM_MIDDLE, int CONST_LEVEL_INI,int CONST_LEVEL_FIN>
class BS_Basic
{public:BS_Basic(): NumKnots_(DEGREE + NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN +1),NumCPs_(NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN) {for (int i(0); i < NumKnots_; ++i) Knots_[i] = 0.;for (int i(0); i < NumCPs_; ++i) {for (int j(0); j < DIM; ++j) CPoints_[i][j] = 0.;}if (NumKnots_ < 2 * (DEGREE + 1)) {printf("Invalid setup (num_knots, degree): %d, %d\n", NumKnots_, DEGREE);}}~BS_Basic() {}/*!* size of T: DIM * CONST_LEVEL_INI (or CONST_LEVEL_FIN) <br/>* ex) if dim:3, const level ini: 3(pos, vel, acc) <br/>* ini[0 ~ 2]: pos <br/>* ini[3 ~ 5]: vel <br/>* ini[6 ~ 8]: acc <br/>* @param init : 初始点信息（矢量） <br/>* @param fin : 终点信息（矢量） <br/>* @param middle_pt   : 中间点信息（矩阵） <br/>* @param fin_time : 样条曲线的持续时间 <br/>* @return boolean : success <br/>*/bool SetParam(T* init, T* fin, T** middle_pt, T fin_time) {_CalcKnot(fin_time);                            //计算_CalcConstrainedCPoints(init, fin, fin_time);_CalcCPoints(middle_pt);return true;}/*!* 获取给定时间的样条线位置 <br/>* 如果输入时间在0之前，则返回初始值 <br/>* 如果输入时间在最后一次之后，则返回最后一次 <br/>* @param u : time <br/>* @return ret : position at the given time.  <br/>*/bool getCurvePoint(T u, T* ret) {int _span;if (u < Knots_[0])u = Knots_[0];else if (u > Knots_[NumKnots_ - 1]) {u = Knots_[NumKnots_ - 1];}if (!_findSpan(_span, u)) return false;T _N[DEGREE + 1];_BasisFuns(_N, _span, u);T _C[DIM];for (int j(0); j < DIM; ++j) {_C[j] = 0.0;for (int i(0); i <= DEGREE; ++i) {_C[j] += _N[i] * CPoints_[_span - DEGREE + i][j];}}for (int i(0); i < DIM; ++i) {ret[i] = _C[i];}return true;}/*!* 得到给定时刻的样条导数信息<br/>* 如果输入时间在0之前，则返回初始值 <br/>* 如果输入时间在最后一次之后，则返回最后一次 <br/>* @param u : time <br/>* @param d : drivative level (e.g. 1: velocity, 2: acceleration) <br/>* @return ret : derivative information at the given time.  <br/>*/bool getCurveDerPoint(T u, int d, T* ret) {if (d > DEGREE) return 0.0;if (u < Knots_[0])u = Knots_[0];else if (u > Knots_[NumKnots_ - 1]) {u = Knots_[NumKnots_ - 1];}T** _CK = new T*[d + 1];for (int i(0); i < d + 1; ++i) {_CK[i] = new T[DIM];}if (_CurveDerivsAlg1V(_CK, u, d)) {for (int m(0); m < DIM; ++m) ret[m] = _CK[d][m];for (int p(0); p < d + 1; ++p) delete[] _CK[p];SP_SAFE_DELETE_AR(_CK);return true;} else {for (int p(0); p < d + 1; ++p) delete[] _CK[p];SP_SAFE_DELETE_AR(_CK);}return false;}// protected:private:inline void _CalcKnot(T Tf) {int _i(0);int _j(0);int _NumMidKnot(NumKnots_ - 2 * DEGREE - 2);T _TimeStep = Tf / (_NumMidKnot + 1);// 增加初始部分的结序列// # of order ( degree + 1 )for (_j = 0; _j < DEGREE + 1; ++_j) Knots_[_i++] = 0.0;// 中间部分均匀的结序，// #: NumKnot - degree - degree = NumKnot - order - order + 2for (_j = 0; _j < _NumMidKnot; ++_j) {Knots_[_i] = Knots_[_i - 1] + _TimeStep;++_i;}// 为最后一部分增加结序列// # of order ( degree + 1 )for (_j = 0; _j < DEGREE + 1; ++_j) Knots_[_i++] = Tf;// for(int i(0); i< NumKnots_; ++i)// std::cout<<Knots_[i]<<std::endl;}bool _CurveDerivsAlg1V(T** CK, T u, int d) {assert(d <= DEGREE);int _k(0);int _j(0);T** _nders = new T*[d + 1];for (_k = 0; _k < d + 1; ++_k) _nders[_k] = new T[DEGREE + 1];int _span;if (!_findSpan(_span, u)) return false;_BasisFunsDers(_nders, _span, u, d);for (_k = 0; _k <= d; ++_k) {// Clean Up Columnfor (int m(0); m < DIM; ++m) CK[_k][m] = 0.;for (_j = 0; _j <= DEGREE; ++_j) {for (int m(0); m < DIM; ++m) {CK[_k][m] += _nders[_k][_j] * CPoints_[_span - DEGREE + _j][m];}}}for (_k = 0; _k < d + 1; ++_k) delete[] _nders[_k];delete[] _nders;return true;}bool _BasisFunsDers(T** ders, T u, int n) {// int _span = FindSpan(u);int _span;if (!_findSpan(_span, u)) return false;_BasisFunsDers(ders, _span, u, n);return true;}bool _BasisFunsDers(T** ders, int span, T u, int n) {int _j, _r, _k;int _s1, _s2;int _j1, _j2;int _rk;int _pk;T _saved = 0.0;T _left = 0.0;T _right = 0.0;T _temp = 0.0;T _d = 0.0;// to store the basis functions and knot differencesT** _ndu = new T*[DEGREE + 1];for (_j = 0; _j <= DEGREE; ++_j) _ndu[_j] = new T[DEGREE + 1];// to store (in an alternating fashion) the two most recently computed// rows a(k,j) and a(k-1,j)T** _a = new T*[2];for (_j = 0; _j < 2; ++_j) _a[_j] = new T[DEGREE + 1];_ndu[0][0] = 1.0;for (_j = 1; _j <= DEGREE; ++_j) {_saved = 0.0;for (_r = 0; _r < _j; ++_r) {_left = _Left(span, _j - _r, u);_right = _Right(span, _r + 1, u);// Lower triangle_ndu[_j][_r] = _right + _left;_temp = _ndu[_r][_j - 1] / _ndu[_j][_r];// Upper triangle_ndu[_r][_j] = _saved + _right * _temp;_saved = _left * _temp;}_ndu[_j][_j] = _saved;}// Load the basis functionsfor (_j = 0; _j <= DEGREE; ++_j) ders[0][_j] = _ndu[_j][DEGREE];// This section computes the derivatives (Eq. [2.9])for (_r = 0; _r <= DEGREE; ++_r) {_s1 = 0;_s2 = 1;_a[0][0] = 1.0;// Loop to compute k-th derivativefor (_k = 1; _k <= n; ++_k) {_d = 0.0;_rk = _r - _k;_pk = DEGREE - _k;if (_r >= _k) {_a[_s2][0] = _a[_s1][0] / _ndu[_pk + 1][_rk];_d = _a[_s2][0] * _ndu[_rk][_pk];}if (_rk >= -1)_j1 = 1;else_j1 = -_rk;if (_r - 1 <= _pk)_j2 = _k - 1;else_j2 = DEGREE - _r;for (_j = _j1; _j <= _j2; ++_j) {_a[_s2][_j] =(_a[_s1][_j] - _a[_s1][_j - 1]) / _ndu[_pk + 1][_rk + _j];_d += _a[_s2][_j] * _ndu[_rk + _j][_pk];}if (_r <= _pk) {_a[_s2][_k] = -_a[_s1][_k - 1] / _ndu[_pk + 1][_r];_d += _a[_s2][_k] * _ndu[_r][_pk];}ders[_k][_r] = _d;// Switch rows_j = _s1;_s1 = _s2;_s2 = _j;}}// Multiply through by the correct factors// (Eq. [2.9])_r = DEGREE;for (_k = 1; _k <= n; ++_k) {for (_j = 0; _j <= DEGREE; ++_j) ders[_k][_j] *= _r;_r *= (DEGREE - _k);}// Deallocatefor (_j = 0; _j <= DEGREE; ++_j) delete[] _ndu[_j];delete[] _ndu;for (_j = 0; _j < 2; ++_j) delete[] _a[_j];delete[] _a;return true;}void _BasisFuns(T* N, T u) {// Original/*int _span = FindSpan(u);BasisFuns(N, _span, u);*/int _span;if (_findSpan(_span, u)) {_BasisFuns(N, _span, u);}}void _BasisFuns(T* N, int span, T u) {int _j, _r;T _left = 0.0;T _right = 0.0;T _saved = 0.0;T _temp = 0.0;N[0] = 1.0;for (_j = 1; _j <= DEGREE; ++_j) {_saved = 0.0;for (_r = 0; _r < _j; ++_r) {_left = _Left(span, _j - _r, u);_right = _Right(span, _r + 1, u);if ((_right + _left) != 0) {_temp = N[_r] / (_right + _left);}N[_r] = _saved + _right * _temp;_saved = _left * _temp;}N[_j] = _saved;}}inline T _Left(int i, int j, T u) { return u - Knots_[i + 1 - j]; }inline T _Right(int i, int j, T u) { return Knots_[i + j] - u; }bool _findSpan(int& ret, T u) {if (u < Knots_[0] || Knots_[NumKnots_ - 1] < u) return false;if (SP_IS_EQUAL(u, Knots_[NumKnots_ - 1])) {for (int i(NumKnots_ - 2); i > -1; --i) {if (Knots_[i] < u && u <= Knots_[i + 1]) {ret = i;return true;}}return false;}// Binary searchint _low = 0;int _high = NumKnots_ - 1;int _mid = (_low + _high) >> 1;while (u < Knots_[_mid] || u >= Knots_[_mid + 1]) {if (u < Knots_[_mid])_high = _mid;else_low = _mid;_mid = (_low + _high) >> 1;}ret = _mid;return true;}void _CalcConstrainedCPoints(T* init, T* fin, T Tf) {// Positionfor (int m(0); m < DIM; ++m) {CPoints_[0][m] = init[m];CPoints_[NumCPs_ - 1][m] = fin[m];}// Initial ConstraintsT** d_mat = new T*[CONST_LEVEL_INI + 1];for (int i(0); i < CONST_LEVEL_INI + 1; ++i)d_mat[i] = new T[CONST_LEVEL_INI + 2];_BasisFunsDers(d_mat, 0., CONST_LEVEL_INI);T ini_const[DIM];// Vel, Acc, ...for (int j(1); j < CONST_LEVEL_INI + 1; ++j) {for (int k(0); k < DIM; ++k) {ini_const[k] = init[j * DIM + k];for (int h(j); h > 0; --h) {ini_const[k] -= d_mat[j][h - 1] * CPoints_[h - 1][k];}CPoints_[j][k] = ini_const[k] / d_mat[j][j];}}for (int p(0); p < CONST_LEVEL_INI + 1; ++p) delete[] d_mat[p];SP_SAFE_DELETE_AR(d_mat);// Final ConstraintsT** c_mat = new T*[CONST_LEVEL_FIN + 1];for (int i(0); i < CONST_LEVEL_FIN + 1; ++i)c_mat[i] = new T[CONST_LEVEL_FIN + 2];_BasisFunsDers(c_mat, Tf, CONST_LEVEL_FIN);// Vel, Acc, ...int idx(1);for (int j(NumCPs_ - 2); j > NumCPs_ - 2 - CONST_LEVEL_FIN; --j) {for (int k(0); k < DIM; ++k) {ini_const[k] = fin[idx * DIM + k];for (int h(idx); h > 0; --h) {ini_const[k] -=c_mat[idx][CONST_LEVEL_FIN + 2 - h] * CPoints_[NumCPs_ - h][k];}CPoints_[j][k] = ini_const[k] / c_mat[idx][CONST_LEVEL_FIN + 1 - idx];}++idx;}for (int p(0); p < CONST_LEVEL_FIN + 1; ++p) delete[] c_mat[p];SP_SAFE_DELETE_AR(c_mat);}void _PrintCP(int i) {printf("%i th CP check:\n", i);for (int m(0); m < DIM; ++m) {for (int j(0); j < NumCPs_; ++j) {printf("%f \t", CPoints_[j][m]);}printf("\n");}printf("\n");}void _CalcCPoints(T** middle_pt) {for (int i(0); i < NUM_MIDDLE; ++i) {for (int m(0); m < DIM; ++m) {CPoints_[CONST_LEVEL_INI + 1 + i][m] = middle_pt[i][m];}}}T fin_time_;int NumKnots_;int NumCPs_;T Knots_[DEGREE + NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN + 1];T CPoints_[NUM_MIDDLE + 2 + CONST_LEVEL_INI + CONST_LEVEL_FIN][DIM];
};#endif

总结

（1）第一步：给定并设置B样条轨迹起始点、中间点、落脚点，目标时间点四个参数，即可以通过库Basic Bspline插值规划得到连续的足端位置轨迹、速度轨迹、加速度（力矩）轨迹【防盗标记–盒子君hzj】

（2）第二步：仅仅把足端位置轨迹发送给腿部控制器，这就完成轨迹规划的任务了，至于腿部控制器怎么把足端位置轨迹分解到每个关节的转角我再后面的博客再进行论述【防盗标记–盒子君hzj】

.
.

参考资料

如果想看懂库Basic Bspline插值规划原理，可以看看我的这篇博客
【路径生成–插值拟合方法】B样条曲线

【四足机器人--关节初始化时足端位置（速度、加速度）轨迹规划】（4.2）JPosInitializer（B样条曲线计算脚的摆动轨迹）代码解析相关推荐

【四足机器人--摆动相足端位置速度轨迹规划】（4.1）FootSwingTrajectory（bezier曲线计算脚的摆动轨迹）代码解析
系列文章目录提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理文章目录系列文章目录前言一.FootSwingTrajectory(bezier曲线)的内容 ...
【四足机器人】学习笔记足端轨迹规划和步态规划
[四足机器人]学习笔记足端轨迹规划和步态规划一.足端轨迹规划(摆线) 二.步态规划 1.Walk步态 2.Trot步态近期,博主在古月居学习关于四足机器人的相关部分知识,从阳炼老师的四足机器人控 ...
基于气动人工肌肉的双足机器人关节设计
介绍了一种由气动人工肌肉构建的双足机器人关节,该关节利用气动人工肌肉的柔性特性,可以有效控制双足机器人快速行走或跑步时的落地脚冲击问题. 详细给出了气动人工肌肉的工作原理以及由其构成的关节系统的硬件架 ...
四足机器人关节锁死故障的容错问题
好久没写博客了,冒个泡. 四足机器人容错方面的运动控制很少被研究.现在能找到的,国内有上海交大做的特殊腿部结构的四足运动学容错方案,国外有韩国Jung-Min Yang所做的一系列的步态容错方法(很多 ...
四足机器人学习笔记（足端轨迹规划）
不管是基于位置控制还是力矩控制亦或是其他一些控制方式,均需要进行足端的轨迹规划,来使四足机器人能够成功跨越障碍物.因此对于四足机器人足端轨迹方法进行了汇总. 1. 摆线轨迹若使用位置控制模式,可以将 ...
双足机器人的稳定性判据_足式机器人稳定性判据
［1］王立权,王海龙,陈曦,等. 八足仿蟹机器人行走稳定性分析［J］. 中南大学学报(自然科学版),2014,45(10):3416-3422. ［2］宣奇波,张怀相,戴国骏. 四足步行机器人稳定性步 ...
相对全面的四足机器人驱动规划MATLAB和Simulink实现方式（足端摆线规划，Hopf-CPG，Kimura-CPG）
许久没更新四足机器人相关的博客文章,由于去年一整年都在干各种各样的~活,终于把硕士毕业论文给写好,才有点时间更新自己的所学和感悟.步态规划和足端规划只是为了在运动学层面获取四足机器人各关节的期望角位移 ...
四足论文《面对未知地形的四足机器人足端轨迹优化》解读
在笔者的关于足端轨迹规划的文章中,曾经提到了类正弦轨迹,该规划是根据斯坦福的四足机器人Doggo的开源代码中得出的.而该轨迹的缺点是对于地形的适应差,无法自适应的调节自身足端的轨迹规划. 基于该种问题 ...
四足机器人|机器狗|仿生机器人|多足机器人|Adams仿真|Simulink仿真|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|绝对干货！需要资料及指导的可以联系我！
四足机器人|机器狗|仿生机器人|多足机器人|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|绝对干货!需要资料及指导的可以联系我!QQ:1096474659 基于CPG的四 ...

【四足机器人--关节初始化时足端位置（速度、加速度）轨迹规划】（4.2）JPosInitializer（B样条曲线计算脚的摆动轨迹）代码解析