研究项目,无人机的路径规划,需要用到B样条,所以在此写下B-spline的结合C++代码的理解以及在项目中的应用
一、阶数p_
阶数 = 所有权重中t的最高次幂=控制点数量-1;
二、节点表u_
节点表数量m_ = n_ + p_ + 1(控制点数量+阶数+1),节点表用于生成基函数,节点表的生成一般有两种方法,顺序方法和Clamped方法,下面的代码选其一种。举例如下
假设曲线有6个控制点,阶数是3阶,那么节点表大小=6+3+1=10。
如果是顺序列表,只需要按顺序设置
如果是Clamped列表,由于是3阶,前面3+1个参数均设置为0,后面3+1个参数均设置为1,然后剩余参数均匀递增
void NonUniformBspline::setUniformBspline(const Eigen::MatrixXd& points, const int& order,const double& interval) {//设置节点表
control_points_ = points;
p_ = order;// B-spline阶数
interval_ = interval;//interval = ts = pp_.ctrl_pt_dist / pp_.max_vel_
n_ = points.rows() - 1;// 控制点Q: Q0, Q1, ... , Qn
m_ = n_ + p_ + 1;// 节点u: U0, U1, ... , Un+p+1
u_ = Eigen::VectorXd::Zero(m_ + 1);
for (int i = 0; i <= m_; ++i) {//Clamped列表,由于是3阶,前面3+1个参数均设置为0,后面3+1个参数均设置为1,然后剩余参数均匀递增
if (i <= p_) {
u_(i) = double(-p_ + i) * interval_;// 前p+1个节点, 好像没什么用? 只要保证up = 0即可
} else if (i > p_ && i <= m_ - p_) {
u_(i) = u_(i - 1) + interval_;// 中间n-p个节点, up+1 = ts , up+2 = 2 * ts
} else if (i > m_ - p_) {
u_(i) = u_(i - 1) + interval_;// 后p+1个节点
}
}
三、基本函数表d_ = N
有上面的阶数和节点表,则可以开始计算基本函数表了,优化的路径 = 控制点*基本函数
基本函数表公式:
其中,是第i个k次B样条的基函数,也可以理解为各个控制点的权重。
图中为基函数的运算关系,从左往右,从上到下计算。
基本函数,表示基函数的次数(degree)为k,这是第i个k次B样条基函数,i=0,1,...
基本函数表的C++代码实现如下:
Eigen::VectorXd NonUniformBspline::evaluateDeBoor(const double& u) {//返回基本函数表
double ub = min(max(u_(p_), u), u_(m_ - p_)); //p degree, n+1 control points, m = n+p+1
// determine which [ui,ui+1] lay in
int k = p_;
while (true) {
if (u_(k + 1) >= ub) break;
++k;
}
/* deBoor's alg */
vector<Eigen::VectorXd> d;
for (int i = 0; i <= p_; ++i) {// [Ui, Ui+1]的轨迹由p+1的控制点(Qi-p, ... ,Qi)共同决定
d.push_back(control_points_.row(k - p_ + i));
// cout << d[i].transpose() << endl;
}
// https://blog.csdn.net/Hachi_Lin/article/details/89812126
for (int r = 1; r <= p_; ++r) {// 三角递归的横向 r = 1 -> i = 3, 2, 1 // r = 2 -> i = 3, 2
for (int i = p_; i >= r; --i) {// 三角递归的纵向 r = 3 -> i = 3
double alpha = (ub - u_[i + k - p_]) / (u_[i + 1 + k - r] - u_[i + k - p_]);
// cout << "alpha: " << alpha << endl;
d[i] = (1 - alpha) * d[i - 1] + alpha * d[i];
}
}
return d[p_];
}
四、计算B-spline的曲线
根据基本函数表和控制点,我们可以求得B-spline
其中Q为控制点,是第i个k次B样条的基函数
五、根据运动规划的速度和加速度调整时间
因为生成的路径有可能会不符合动力学约束,所以接下来进行时间的调整,来满足动力学约束~~~未完待续~~~
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)