基于ROS的Minimun Snap闭式求解算法代码学习
本文是在学习路径规划算法中的一篇学习记录,由于对c++掌握粗浅,将整个代码进行了详细的梳理,方便后来者参考学习.代码需要和理论部分结合起来看更容易理解,理论部分: 《Minimun Snap轨迹优化学习记录》.
(代码注释中英夹杂,英文注释是原来作者的注释,中文注释是本人后来添加的)
1.过程演示
1.进入到工作空间下运行catkin_make编译;
2.运行source devel/setup.bash;
3.打开一个新的终端运行roscore;
4.回到原终端窗口运行roslaunch waypoint_trajectory_generator test.launch;
5.rviz运行起来之后,按如下步骤操作:
结束的操作
最终的效果
2.trajectory_generator_waypoint.h解析
#ifndef _TRAJECTORY_GENERATOR_WAYPOINT_H_
#define _TRAJECTORY_GENERATOR_WAYPOINT_H_
//#include <Eigen/Eigen>
#include <eigen3/Eigen/Eigen>
#include <vector>
class TrajectoryGeneratorWaypoint {
private:
// 原作者给出的，闭式求解没有用到
double _qp_cost{};
Eigen::MatrixXd _Q;
Eigen::VectorXd _Px, _Py, _Pz;
public:
TrajectoryGeneratorWaypoint();
~TrajectoryGeneratorWaypoint();
// 生成多项式系数
Eigen::MatrixXd PolyQPGeneration(
int order,
const Eigen::MatrixXd &Path,
const Eigen::MatrixXd &Vel,
const Eigen::MatrixXd &Acc,
const Eigen::VectorXd &Time);
// Factorial阶乘
static int Factorial(int x);
// 获得Ct矩阵
Eigen::MatrixXd getCt(int n_seg, int d_order);
// 获得M矩阵
Eigen::MatrixXd getM(int n_seg, int d_order, int p_num1d, const Eigen::VectorXd &ts);
// 获得Q矩阵
Eigen::MatrixXd getQ(int n_seg, int d_order, int p_num1d, const Eigen::VectorXd &ts);
};
#endif
3.trajectory_generator_waypoint.cpp解析
3.1 Factorial函数
实现阶乘;
int TrajectoryGeneratorWaypoint::Factorial(int x)
{
int fac = 1;
for(int i = x; i > 0; i--)
fac = fac * i;
return fac;
}
3.2 getCt函数
下面的代码是原作者给出的，我贴到这里，但是我并没有理解作者的思路，所以后面又自己写了两种获得Ct矩阵的方法便于后来者理解，对于Ct矩阵的理论部分可参考上一篇博文《Minimun Snap轨迹优化学习记录》中第三小节<3.Minimun Snap闭式求解(Closed-form Solution for Minimun Snap)>的第四张图片;
MatrixXd TrajectoryGeneratorWaypoint::getCt(int const n_seg, int const d_order){ // d_order is 4
/*
* get Selection Matrix
* args:
* n_seg: the number of segments
* d_order: the deferential order: if minimal jerk. it is 3,
* return:
* Ct: a matrix,
* row corresponds to original variable vector d ( 2 * d_order * n_seg )
* column corresponds to [ dF, dP ]' ( d_order*2*n_seg - (n_seg-1)*d_order )
* Note: the variables implicitly eliminates same variables""
*/
int ct_rows = d_order*2*n_seg;
int ct_cols = d_order*2*n_seg - (n_seg-1)*d_order;
MatrixXd Ct = MatrixXd::Zero(ct_rows, ct_cols);
vector<int> d_vector;
for(int k = 0; k < n_seg; k ++){
//
for(int t = 0; t < 2; t++){
for(int d = 0; d < d_order; d++){
d_vector.push_back(k*100+t*10+d);
}
}
}
// cout << "---------------" << endl;
// cout << "d_vector:" << endl;
// for(vector<int>::iterator it = d_vector.begin(); it != d_vector.end(); it++)
// cout << *it << endl;
// cout << "---------------" << endl;
int val, row;
int col = 0; // column of one in Ct
// fixed starting point at segment 0 in [ dF, dP ]'
int k = 0;
int t = 0;
int d = 0;
for(d = 0; d < d_order; d++){
val = k * 100 + t * 10 + d;
auto it = std::find(d_vector.begin(), d_vector.end(), val);
// std::distance,返回两个迭代器之间的距离
row = std::distance(d_vector.begin(), it);
Ct(row, col) = 1;
col += 1;
}
// fixed final point at segment 0, 1, 2, n_seg-2 in [ dF, dP ]'
t = 1;
d = 0;
for(k = 0; k < n_seg - 1; k++){
val = k * 100 + t * 10 + d;
auto it = std::find(d_vector.begin(), d_vector.end(), val);
row = std::distance(d_vector.begin(), it);
Ct(row, col) = 1;
val = (k + 1) * 100 + (t - 1) * 10 + d;
it= std::find(d_vector.begin(), d_vector.end(), val);
row = std::distance(d_vector.begin(), it);
Ct(row, col) = 1;
col += 1;
}
// fixed final point at segment n_seg-1 in [ dF, dP ]'
k = n_seg - 1;
t = 1;
d = 0;
for(d = 0; d < d_order; d++){
val = k * 100 + t * 10 + d;
auto it = std::find(d_vector.begin(), d_vector.end(), val);
row = std::distance(d_vector.begin(), it);
Ct(row, col) = 1;
col += 1;
}
// free variables at segment 0, 1, 2, n_seg-1 in [ dF, dP ]'
k = 0;
t = 1;
d = 1;
for(k = 0; k < n_seg - 1; k++){
for(d = 1; d < d_order; d++){
val = k * 100 + t * 10 + d;
auto it = std::find(d_vector.begin(), d_vector.end(), val);
row = std::distance(d_vector.begin(), it);
Ct(row, col) = 1;
val = (k + 1) * 100 + (t - 1) * 10 + d;
it = std::find(d_vector.begin(), d_vector.end(), val);
row = std::distance(d_vector.begin(), it);
Ct(row, col) = 1;
col += 1;
}
}
return Ct;
}
比较容易理解的方法一：
MatrixXd TrajectoryGeneratorWaypoint::getCt(int const n_seg, int const d_order){ // d_order is 4
/*
* get Selection Matrix
* args:
* n_seg: the number of segments
* d_order: the deferential order: if minimal jerk. it is 3,
* return:
* Ct: a matrix,
* row corresponds to original variable vector d ( 2 * d_order * n_seg )
* column corresponds to [ dF, dP ]' ( d_order*2*n_seg - (n_seg-1)*d_order )
* Note: the variables implicitly eliminates same variables""
*/
/*
* 阅读代码之前，请仔细理解 d = Ct*[dF dP]t，以及[dF dP]和矩阵d的内容.
* 对于 d_order=4 n_seg=3 的情况：
* [dF dP]t = [p0 v0 a0 j0 p1 p2 p3 v3 a3 j3, v1 a1 j1 v2 a2 j2]
* dt = [p0 v0 a0 j0 p1 v1 a1 j1 p1 v1 a1 j1 p2 v2 a2 j2 p2 v2 a2 j2 p3 v3 a3 j3]
*/
// 确定Ct矩阵尺寸，可参考https://blog.csdn.net/weixin_44558122/article/details/116173197#t3中<<3.Minimun Snap闭式求
// 解(Closed-form Solution for Minimun Snap)>>部分第四张图片的例子
int ct_rows = d_order*2*n_seg;
int ct_cols = d_order*2*n_seg - (n_seg-1)*d_order;
MatrixXd Ct = MatrixXd::Zero(ct_rows, ct_cols);
string p("p");
string v("v");
string a("a");
string j("j");
// 构造dfdp
vector<string> dfdp;
for(int k = 0; k < n_seg+1; k++){
if(k == 0 || k == n_seg){
dfdp.push_back(p+to_string(k));
dfdp.push_back(v+to_string(k));
dfdp.push_back(a+to_string(k));
dfdp.push_back(j+to_string(k));
}
else
{
dfdp.push_back(p+to_string(k));
}
}
for(int i = 1; i < n_seg; i++){
dfdp.push_back(v+to_string(i));
dfdp.push_back(a+to_string(i));
dfdp.push_back(j+to_string(i));
}
// for(auto it = dfdp.begin(); it != dfdp.end(); it++)
// cout << *it << " ";
// cout << endl;
// 构造d
vector<string> d;
for(int k = 0; k < n_seg+1; k++){
if(k == 0 || k == n_seg){
d.push_back(p+to_string(k));
d.push_back(v+to_string(k));
d.push_back(a+to_string(k));
d.push_back(j+to_string(k));
}
else
{
d.push_back(p+to_string(k));
d.push_back(v+to_string(k));
d.push_back(a+to_string(k));
d.push_back(j+to_string(k));
d.push_back(p+to_string(k));
d.push_back(v+to_string(k));
d.push_back(a+to_string(k));
d.push_back(j+to_string(k));
}
}
// for(auto it = d.begin(); it != d.end(); it++)
// cout << *it << " ";
// 通过查找约束(如p0、a1)在d和dfdp中的位置，对应Ct矩阵的行和列，对对应的位置赋1
for(auto it = d.begin(); it != d.end(); it++){
int row = distance(d.begin(),it);
// 在dfdp中取找当前it迭代对象对应的d中的值
auto it1 = std::find(dfdp.begin(), dfdp.end(), *it);
int col = distance(dfdp.begin(),it1);
Ct(row,col) = 1;
// cout << row << " " << col << endl;
}
return Ct;
}
稍微有些绕的方法二：
MatrixXd TrajectoryGeneratorWaypoint::getCt(int const n_seg, int const d_order){ // d_order is 4
/*
* get Selection Matrix
* args:
* n_seg: the number of segments
* d_order: the deferential order: if minimal jerk. it is 3,
* return:
* Ct: a matrix,
* row corresponds to original variable vector d ( 2 * d_order * n_seg )
* column corresponds to [ dF, dP ]' ( d_order*2*n_seg - (n_seg-1)*d_order )
* Note: the variables implicitly eliminates same variables""
*/
// 阅读代码之前，请仔细理解 d = Ct*[dF dP]t，以及[dF dP]和矩阵d的内容.
// 对于 d_order=4 n_seg=3 的情况：
// [dF dP]t = [p0 v0 a0 j0 p1 p2 p3 v3 a3 j3, v1 a1 j1 v2 a2 j2]
// dt = [p0 v0 a0 j0 p1 v1 a1 j1 p1 v1 a1 j1 p2 v2 a2 j2 p2 v2 a2 j2 p3 v3 a3 j3]
int ct_rows = d_order*2*n_seg;
int ct_cols = d_order*2*n_seg - (n_seg-1)*d_order;
MatrixXd Ct = MatrixXd::Zero(ct_rows, ct_cols);
// p0,v0,a0,j0
for(int i = 0; i < d_order; i++)
Ct(i,i) = 1;
// middle point p1,p2,...p(n-1)
for(int i = 1; i < n_seg; i++){
// d_order+(i-1)*8-1+1:
// d_order,起点约束个数;
// i-1,表示这个中间点之前还有几个中间点;
// *8,表示每个中间点的p,v,a,j都有两套;
// -1,是因为在c++中,矩阵的下标表示是从0开始的,我们在下面推倒的时候用到矩阵下标都是从1开始;
// +1,表示当前操作的是p，举个例子，比如对p1操作的时候，前面有4个约束，p0,v0,a0,j0,即d_order数量
// 的约束个数，+1就当前表示对p1操作，也就是第[dF dP]中的第五个;
// d_order+(i-1)*1: (注:这里写成这样没做化简只是为了方便理解过程和思路)
// d_order,起点约束个数;
// i-1,表示这个中间点之前还有几个中间点;
// *1表示每个中间点的p;
Ct(d_order+(i-1)*8-1+1,d_order+(i-1)*1) = 1;
Ct(d_order+(i-1)*8+4-1+1,d_order+(i-1)*1) = 1;
}
// pn,vn,an,jn
for(int i = 0; i < d_order; i++)
Ct(d_order+2*d_order*(n_seg-1)+i,d_order+n_seg-1+i) = 1;
// v1,a1,j1,...,v(n-1),a(n-1),j(n-1)
// k表示第几个中间点
for(int k = 1; k < n_seg; k++){
// i表示中间点需要求的dP,d_order=4时表示v,a,j,而p是需要给定的,属于dF;
for(int i = 1; i < d_order; i++){
// ((k-1)*2+1)*d_order+1-1+i: (注:这里写成这样没做化简只是为了方便理解过程和思路)
// k-1表示这个中间点之前还有几个中间点,比如k=1,表示第一个中间点,前面就是起点,没有中间点了,即k-1=0;
// (k-1)*2,*2表示每个中间点的p,v,a,j都有两套(上一段轨迹的终点一套,下一段轨迹的起点一套);
// ((k-1)*2+1)*d_order,*d_order表示每个点都有d_order个约束,+1表示起点的约束;
// ((k-1)*2+1)*d_order+1,+1代表约束p,因为我们现在要处理的是约束vaj,p是给定的不需要在这里处理;
// ((k-1)*2+1)*d_order+1-1,-1是因为在c++中,矩阵的下标表示是从0开始的,我们在下面推倒的时候用到矩阵下标都是从1开始;
// ((k-1)*2+1)*d_order+1-1+i,+i表示当前是对vaj的哪一个约束操作
// (2*d_order+n_seg-1)-1+(k-1)*3+i):
// 2*d_order,表示起点和终点;
// n_seg-1,表示中间点的个数,中间点有几个,就有几个约束p;
// (2*d_order+n_seg-1)-1,-1是因为在c++中,矩阵的下标表示是从0开始的,我们在下面推倒的时候用到矩阵下标都是从1开始;
// (k-1)*3,k-1表示这个中间点之前还有几个中间点,*3表示每个点的vaj约束;
// ((k-1)*2+1)*d_order+1-1+4+i: +4表示现在处理的是中间点两套pvaj约束的后面一套.
Ct(((k-1)*2+1)*d_order+1-1+i,(2*d_order+n_seg-1)-1+(k-1)*3+i) = 1;
Ct(((k-1)*2+1)*d_order+1-1+4+i,(2*d_order+n_seg-1)-1+(k-1)*3+i) = 1;
}
}
return Ct;
}
3.3 getM函数
对于M矩阵的理论部分可参考上一篇博文《Minimun Snap轨迹优化学习记录》中第三小节<3.Minimun Snap闭式求解(Closed-form Solution for Minimun Snap)>的第一张图片,请结合后面的图片理解,代码如下：
MatrixXd TrajectoryGeneratorWaypoint::getM(int const n_seg, int const d_order, int const p_num1d, const Eigen::VectorXd &ts){
/*
* get Mapping Matrix
* args:
* n_seg: the number of segments
* d_order: the deferential order: if minimal jerk. it is 3,
* p_num1d: the number of variables in each segment, if minimal jerk, it is 6
* ts: time allocation as a column vector, size: n_seg x 1,
* return:
* M: a matrix, size: n_seg * p_num1d x n_seg * p_num1d
*/
// MatrixXd M, M_temp, zeros;
MatrixXd M = MatrixXd::Zero(n_seg * p_num1d, n_seg * p_num1d);
MatrixXd coeff(d_order, p_num1d);
if(d_order == 4){
coeff << 1, 1, 1, 1, 1, 1, 1, 1,
0, 1, 2, 3, 4, 5, 6, 7,
0, 0, 2, 6, 12, 20, 30, 42,
0, 0, 0, 6, 24, 60, 120,210;
}
else if(d_order == 3){
coeff << 1, 1, 1, 1, 1, 1,
0, 1, 2, 3, 4, 5,
0, 0, 2, 6, 12, 20;
}
else{
cout << "This derivatieve order is not provided getM!!!" << endl;
}
double t;
for(int k = 0; k < n_seg; k++){
// calculate M_k of the k-th segment
MatrixXd M_k = MatrixXd::Zero(p_num1d, p_num1d); // Matrix size: p_num1d x p_num1d
// time of the k-th segment
t = ts(k);
for(int i = 0; i < d_order; i++){
M_k(i, i) = coeff(i, i);
}
for(int i = 0; i < d_order; i++){
for(int j = i; j < p_num1d; j++){
if( i == j){
M_k(i+d_order, j) = coeff(i, j) ;
}
else{
M_k(i+d_order, j) = coeff(i, j) * pow(t, j - i);
}
}
}
// cout << "---------------" << endl;
// cout << "M_old:" << endl;
// cout << M << endl;
M.block(k*p_num1d, k*p_num1d, p_num1d, p_num1d) = M_k;
// cout << "---------------" << endl;
// cout << "M_new:" << endl;
// cout << M << endl;
}
return M;
}
循环之前原始M矩阵：
第一次循环后添加的部分：
上面M_new图中上面的红色框部分的解释：
上面M_new图中下面的红色框部分的解释：
选取M_new图中的箭头指向的315.182作为例子计算一下：
最终的到的M矩阵
3.4 getQ函数
对于Q矩阵的理论部分可参考上一篇博文《Minimun Snap轨迹优化学习记录》中第三小节<2.Minimun Snap轨迹生成(Minimun Snap Optimization))>的第二张和第三张图片,请结合后面的图片理解,代码如下：
Eigen::MatrixXd TrajectoryGeneratorWaypoint::getQ(int const n_seg, int const d_order, int const p_num1d, const Eigen::VectorXd &ts){
/*
* get Q matrix
* args:
* n_seg: the number of segments
* d_order: the deferential order: if minimal snap. it is 4
* p_num1d: the number of variables in each segment
* ts: time allocation as a column vector, size: n_seg x 1,
* return:
* Q: a matrix, size: n_seg * p_num1d x n_seg * p_num1d
* Note: p = [p0, p1, p2,...pn-1]'
*/
MatrixXd Q = MatrixXd::Zero(n_seg * p_num1d, n_seg * p_num1d);
for(int k = 0; k < n_seg; k++) {
Eigen::MatrixXd Q_k = Eigen::MatrixXd::Zero(p_num1d, p_num1d); // Matrix size: p_num1d x p_num1d
for (int i = 0; i < p_num1d; i++)
{
for (int j = 0; j < p_num1d; j++)
{
// 只有当i和j都同时大于等于4的时候(i,j是从0开始的，正常我们表示矩阵的时候下标是从1开始算的，要注意这点)，才会到else里执行计算
if (i < d_order || j < d_order)
continue;
else
// Factorial,表示阶乘;下面公式参考:https://blog.csdn.net/weixin_44558122/article/details/116173197#t3中<<2.Minimun Snap轨迹
// 生成(Minimun Snap Optimization)>>部分第三张图片,对于pow(ts(k), i + j - 2 * d_order + 1),这里为什么没有 （t下标i-1）^(r+c-7)
// 这一项了？是因为我们采用的时间分配方法是相对时间,每一段轨迹的开头都是0.
Q_k(i, j) = Factorial(i) / Factorial(i - d_order) * Factorial(j) / Factorial(j - d_order) *
1 / (i + j - d_order * 2 + 1) * pow(ts(k), i + j - 2 * d_order + 1);
}
}
// 矩阵块操作,可参考:https://gaoyichao.com/Xiaotu/?book=eigen&title=chapter4
// cout << "---------------" << endl;
// cout << "Q_old:" << endl;
// cout << Q << endl;
Q.block(k * p_num1d, k * p_num1d, p_num1d, p_num1d) = Q_k;
// cout << "Q_new:" << endl;
// cout << Q << endl;
// cout << "---------------" << endl;
}
return Q;
}
对于Q1的求解：
最终得到的Q矩阵
3.5 PolyQPGeneration函数
作用：求解多项式系数;
Eigen::MatrixXd TrajectoryGeneratorWaypoint::PolyQPGeneration(
const int d_order, // the order of derivative
const Eigen::MatrixXd &Path, // waypoints coordinates (3d)
const Eigen::MatrixXd &Vel, // boundary velocity
const Eigen::MatrixXd &Acc, // boundary acceleration
const Eigen::VectorXd &Time) // time allocation in each segment
{
// enforce initial and final velocity and acceleration, for higher order derivatives, just assume them be 0;
int p_order = 2 * d_order - 1; // the order of polynomial
int p_num1d = p_order + 1; // the number of variables in each segment
int n_segment = Time.size(); // the number of segments
// 3 * p_num1d, 三维坐标x,y,z, 分别对每一个维度去求解
MatrixXd PolyCoeff = MatrixXd::Zero(n_segment, 3 * p_num1d); // position(x,y,z), so we need (3 * p_num1d) coefficients
// 这部分后面没有用到
VectorXd Px(p_num1d * n_segment), Py(p_num1d * n_segment), Pz(p_num1d * n_segment);
// get Q
MatrixXd Q = getQ(n_segment, d_order, p_num1d, Time);
cout << "Matrix Q is:\n"<< endl << Q << endl;
/* Produce Mapping Matrix to the entire trajectory,
* M is a mapping matrix that maps polynomial coefficients to derivatives. */
MatrixXd M = getM(n_segment, d_order, p_num1d, Time);
cout << "Mapping matrix M is:\n" << endl << M << endl;
// compute Ct
MatrixXd Ct = getCt(n_segment, d_order);
cout <<"Ct: \n"<< endl << Ct << endl;
MatrixXd C = Ct.transpose();
cout << "C is:\n" << endl << C << endl;
MatrixXd M_inv = M.inverse();
MatrixXd M_inv_t = M_inv.transpose();
cout << "M_inv is:" << endl << M_inv << endl;
cout << "M_inv_transp is:" << endl << M_inv_t << endl;
cout << "size of C is: " << C.rows() << ", " << C.cols() << endl;
cout << "size of M_inv_t is: " << M_inv_t.rows() << ", " << M_inv_t.cols() << endl;
cout << "size of Q is: " << Q.rows() << ", " << Q.cols() << endl;
cout << "size of M_inv is: " << M_inv.cols() << ", " << M_inv.rows() << endl;
cout << "size of Ct is: " << Ct.rows() << ", " << Ct.cols() << endl;
MatrixXd R = C * M_inv_t * Q * M_inv * Ct; // M is not changed
cout << "R is:\n" << endl << R << endl;
int num_d_F = 2 * d_order + n_segment - 1;
int num_d_P = (n_segment - 1) * (d_order - 1);
// 矩阵的角相关操作:https://blog.csdn.net/qq_33160678/article/details/112567335
MatrixXd R_pp = R.bottomRightCorner(num_d_P, num_d_P); // 6*6
cout << "R_pp is:\n" << endl << R_pp << endl;
MatrixXd R_fp = R.topRightCorner(num_d_F, num_d_P); // 10*6
cout << "R_fp is:\n" << endl << R_fp << endl;
// STEP 3: compute dF for x, y, z respectively
for(int dim = 0; dim < 3; dim++){
VectorXd wayPoints = Path.col(dim);
cout << "waypoints: " << endl << wayPoints << endl;
VectorXd d_F = VectorXd::Zero(num_d_F);
d_F(0) = wayPoints(0); //p0
// v0,0 a0,0 ... are 0
// pT,0, pT,1, ,,PT,n_seg-2
for(int i = 0; i < n_segment - 1; i++ ){
d_F(d_order + i) = wayPoints(i + 1);
}
// pT,n_seg-1
d_F(d_order + n_segment - 1) = wayPoints(n_segment);
cout << "d_F is:" << endl << d_F << endl;
VectorXd d_P = -1.0 * R_pp.inverse() * R_fp.transpose() * d_F;
cout << "d_P is:" << endl << d_P << endl;
VectorXd d_total(d_F.rows() + d_P.rows());
d_total << d_F, d_P;
cout << "d_total is:" << endl << d_total << endl;
// M.inverse() * Ct * d_total也即QP问题中的P矩阵，P矩阵就是x(t)多项式中各项的系数
VectorXd poly_coef_1d = M.inverse() * Ct * d_total;
cout<< "Dimension " << dim <<" coefficients: "<< endl << poly_coef_1d << endl;
MatrixXd poly_coef_1d_t = poly_coef_1d.transpose();
cout << "poly_coef_1d.transpose() :" << endl << poly_coef_1d_t << endl;
// PolyCoeff(n_segment, 3 * p_num1d)
for(int k = 0; k < n_segment; k++){
PolyCoeff.block(k, dim*p_num1d, 1, p_num1d) = poly_coef_1d_t.block(0,k*p_num1d, 1, p_num1d);
}
}
cout << "PolyCoeff :" << endl << PolyCoeff << endl;
return PolyCoeff;
}
4.trajectory_generator_node.cpp解析
4.1 rcvWaypointsCallBack函数
作用：获得输入的所有点坐标，即演示过程中的第五部分;
void rcvWaypointsCallBack(const nav_msgs::Path & wp)
{
vector<Vector3d> wp_list;
wp_list.clear();
for (int k = 0; k < (int)wp.poses.size(); k++)
{
// 记录所有路标点
Vector3d pt( wp.poses[k].pose.position.x, wp.poses[k].pose.position.y, wp.poses[k].pose.position.z);
wp_list.push_back(pt);
// 如果路标点的Z小于0,退出循环
if(wp.poses[k].pose.position.z < 0.0)
break;
}
// MatrixXd：表示任意大小的元素类型为double的矩阵变量，其大小只有在运行时被赋值之后才能知道。
MatrixXd waypoints(wp_list.size() + 1, 3);
// 将所有路标点的坐标放到waypoints矩阵里
waypoints.row(0) = _startPos;
for(int k = 0; k < (int)wp_list.size(); k++)
waypoints.row(k+1) = wp_list[k];
//Trajectory generation: use minimum snap trajectory generation method
//waypoints is the result of path planning (Manual in this homework)
trajGeneration(waypoints);
}
4.2 trajGeneration函数
作用：轨迹生成;
void trajGeneration(Eigen::MatrixXd path)
{
TrajectoryGeneratorWaypoint trajectoryGeneratorWaypoint;
MatrixXd vel = MatrixXd::Zero(2, 3);
MatrixXd acc = MatrixXd::Zero(2, 3);
vel.row(0) = _startVel;
// give an arbitraty time allocation, all set all durations as 1 in the commented function.
_polyTime = timeAllocation(path);
// generate a minimum-snap piecewise monomial polynomial-based trajectory
_polyCoeff = trajectoryGeneratorWaypoint.PolyQPGeneration(_dev_order, path, vel, acc, _polyTime);
// 可视化path，即将所有点用直线连接起来
visWayPointPath(path);
//After you finish your homework, you can use the function visWayPointTraj below to visulize your trajectory
// 可视化优化过的轨迹
visWayPointTraj( _polyCoeff, _polyTime);
}
4.3 visWayPointTraj函数
作用：可视化优化过的轨迹;
void visWayPointTraj( MatrixXd polyCoeff, VectorXd time)
{
// visualization_msgs相关参考:https://blog.csdn.net/u013834525/article/details/80447931
// 或者直接参考官网:http://wiki.ros.org/rviz/DisplayTypes/Marker#The_Marker_Message
visualization_msgs::Marker _traj_vis;
_traj_vis.header.stamp = ros::Time::now();
_traj_vis.header.frame_id = "/map";
_traj_vis.ns = "traj_node/trajectory_waypoints";
_traj_vis.id = 0;
_traj_vis.type = visualization_msgs::Marker::SPHERE_LIST;
_traj_vis.action = visualization_msgs::Marker::ADD;
_traj_vis.scale.x = _vis_traj_width;
_traj_vis.scale.y = _vis_traj_width;
_traj_vis.scale.z = _vis_traj_width;
_traj_vis.pose.orientation.x = 0.0;
_traj_vis.pose.orientation.y = 0.0;
_traj_vis.pose.orientation.z = 0.0;
_traj_vis.pose.orientation.w = 1.0;
_traj_vis.color.a = 1.0;
_traj_vis.color.r = 1.0;
_traj_vis.color.g = 0.0;
_traj_vis.color.b = 0.0;
double traj_len = 0.0;
int count = 0;
Vector3d cur, pre;
cur.setZero();
pre.setZero();
_traj_vis.points.clear();
Vector3d pos;
// 这个是在序列、点集中才会用到，指明序列中每个点的位置
geometry_msgs::Point pt;
for(int i = 0; i < time.size(); i++ )
{
for (double t = 0.0; t < time(i); t += 0.01, count += 1)
{
// 得到当前t对应的三维空间坐标位置
pos = getPosPoly(polyCoeff, i, t);
cur(0) = pt.x = pos(0);
cur(1) = pt.y = pos(1);
cur(2) = pt.z = pos(2);
_traj_vis.points.push_back(pt);
if (count) traj_len += (pre - cur).norm();
pre = cur;
}
}
_wp_traj_vis_pub.publish(_traj_vis);
}
4.4 getPosPoly函数
作用：获得当前时间t对应的三维空间点坐标;
Vector3d getPosPoly( MatrixXd polyCoeff, int k, double t )
{
Vector3d ret;
// 3 dimension: x,y,z
for ( int dim = 0; dim < 3; dim++ )
{
// 获取当前维度对应的polyCoeff，polyCoeff即公式中的M.inverse()*Ct*d,也即QP问题中的P矩阵，P矩阵就是x(t)多项式中各项的系数
VectorXd coeff = (polyCoeff.row(k)).segment( dim * _poly_num1D, _poly_num1D );
VectorXd time = VectorXd::Zero( _poly_num1D );
// for(int j = _poly_num1D-1; j >= 0; j--)
cout << "------------" << endl;
cout << polyCoeff << endl;
cout << coeff << endl;
cout << "------------" << endl;
// x(t) = p0 + p1*t^1 + p2*t^2 + p3*t^3 + p4*t^4 + p5*t^5 + p6*t^6 + p7*t^7
for(int j = 0; j < _poly_num1D; j ++)
if(j==0)
// t的0次=1
time(j) = 1.0;
else
time(j) = pow(t, j);
// 得到当前time下,各个维度对应的x(time)的值,即当前时间下对应的坐标点位置
ret(dim) = coeff.dot(time);
cout << "dim:" << dim << " coeff:" << coeff << endl;
}
return ret;
}
4.5 timeAllocation函数
作用：时间分配，理论部分可参考:https://blog.csdn.net/q597967420/article/details/77623235,下面会给出原作者的代码，不过我并没完全理解，所以采用了另一种更好理解的解法;
原作者代码：
VectorXd timeAllocation( MatrixXd Path)
{
VectorXd time(Path.rows() - 1);
/*
STEP 1: Learn the "trapezoidal velocity" of "TIme Allocation" in L5, then finish this timeAllocation function
variable declaration: _Vel, _Acc: _Vel = 1.0, _Acc = 1.0 in this homework, you can change these in the test.launch
You need to return a variable "time" contains time allocation, which's type is VectorXd
The time allocation is many relative timeline but not one common timeline
*/
double _Vel, _Acc;
_Vel = 1.0, _Acc = 1.0;
double t_slope = _Vel / _Acc;
double max_s_triangle = t_slope * _Vel;
std::cout <<"Path: "<< Path << std::endl;
time = VectorXd::Ones(Path.rows() - 1);
cout << "----------" << time << endl;
for(int k = 0;k < Path.rows()-1;k++){
Vector3d p1_2 = Path.row(k) - Path.row(k+1);
double s = std::sqrt(p1_2.dot(p1_2));
if(s <= max_s_triangle){
time(k) = 2.0 * s / _Acc;
}
else{
time(k) = (s - max_s_triangle) / _Vel + t_slope;
}
}
std::cout <<"time: "<< time << std::endl;
return time;
}
另一种解法：
VectorXd timeAllocation( MatrixXd Path)
{
VectorXd time(Path.rows() - 1);
double _Vel, _Acc;
_Vel = 1.0, _Acc = 1.0;
double accInv = 1.0 / _Acc;
double velInv = 1.0 / _Vel;
// 加速时间与距离，减速需要耗费相同的资源
double accTime = _Vel * accInv; // v = v0 + at, v0 = 0
// accDist = 0.5*_Vel*accTime = 1/2*_Vel*_Vel*accInv = 1/2*_Vel*_Vel*1/_Acc = 2/1 * _Vel^2 / _Acc
double accDist = 0.5 * _Vel * accTime; // v^2 - (v0)^2 = 2ax, v0 = 0
double accTime2 = 2.0 * accTime;
double accDist2 = 2.0 * accDist; // 包含加速和减速的总距离
for(int i = 0; i < Path.rows()-1; i++)
{
double t = 0.0;
// n = norm(v) 返回向量 v 的欧几里德范数。此范数也称为 2-范数、向量模或欧几里德长度。
double dist = (Path.row(i+1) - Path.row(i)).norm();
if(dist <= accDist2)
t = 2.0 * std::sqrt(dist*accInv);
else
t = accTime2 + (dist - accDist2)*velInv;
time(i) = t;
}
cout << "--------------------" << time << endl;
return time;
}
5.参考引用
代码来源：https://github.com/KailinTong/Motion-Planning-for-Mobile-Robots/tree/master/hw_5（从该地址下载的代码是原作者的代码，其中并未包含我后来自己写的解法部分，需要用这部分代码请从本文中直接复制替换）.
相关知识来源：深蓝学院<<移动机器人运动规划>>
2445
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
