由于最近的研究需要，需要对Fast-planner和Ego-planner的代码了解，所以写出这篇代码解读文章，本文持续更新。废话不多说了，上干货！
本文基于以下大佬的代码解析基础上去阅读、理解、总结而成，对我的帮助真的特别大。觉得有帮助的朋友记得给大佬点赞！
Fast-Planner代码阅读-1. Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight_fast planner b样条_养生少年小余的博客-CSDN博客
本文之所以成就之高，原因在于其框架的完整性，代码主要解读包含三大板块：kinodynamic a_star 路径搜索、non_uniform_bspline均匀B样条轨迹优化、非均匀B样条轨迹优化三大主要部分。
一、kinodynamic a_star 路径搜索
实现该功能的文件为/fast_planner/path_searching/src/kinodynamic_astar.cpp，将围绕其主要循环函数展开。
int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v, Eigen::Vector3d start_a,
Eigen::Vector3d end_pt, Eigen::Vector3d end_v, bool init, bool dynamic, double time_start)该函数传入参数为分别为start_pt（起点位置）、start_v（起点速度）、start_a（起点加速度）、end_pt（终点位置）、end_v（终点速度）、init（初始化成功标志位）、dynamic（动静规划标志位）、time_start（起始时间）。
1. 读取并初始化参数、计算起点代价、优化时间，将起点加入到openlist中：
该步骤是为了将传入的vector转化为可带入运算的状态矩阵
//函数传入参数为起始点的位置、速度、加速度、终点位置、速度、初始化标志位、动态（可行）标志位？、起始时间
start_vel_ = start_v;//取出传入的起始点的速度
start_acc_ = start_a;//传入起始点的加速度
//path_node_pool_ 可能在初始化时被分配一定数量的节点，这些节点在运行时被重复使用，而不是每次需要节点时都动态地分配内存。这有助于提高性能，避免频繁的内存分配和释放。
PathNodePtr cur_node = path_node_pool_[0];//取出第一个路径点赋给当前节点
cur_node->parent = NULL;//父节点
cur_node->state.head(3) = start_pt;//state矩阵前3列记录位置
cur_node->state.tail(3) = start_v;//state矩阵后三列记录速度
cur_node->index = posToIndex(start_pt);//获取全剧坐标系下的位置索引
cur_node->g_score = 0.0;//记录当前点成本代价值
Eigen::VectorXd end_state(6);//初始化目标点状态的
Eigen::Vector3i end_index;//记录终点索引值
double time_to_goal;//路径规划时间
end_state.head(3) = end_pt;
end_state.tail(3) = end_v;
end_index = posToIndex(end_pt);
cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal);//记录当前节点的代价函数
cur_node->node_state = IN_OPEN_SET;//标记为待探索列表
open_set_.push(cur_node);//将当前节点添加到openlist中
use_node_num_ += 1;//已探索点个数记录其中，estimateHeuristic用于计算两点之间的启发函数代价，传入参数为起点与终点的状态，返回值为计算所得的代价值并且同时更新到达终点的最优时间optimal_time
double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2, double& optimal_time)
{
const Vector3d dp = x2.head(3) - x1.head(3);//位置变化量
const Vector3d v0 = x1.segment(3, 3);//起点速度矩阵
const Vector3d v1 = x2.segment(3, 3);//终点速度矩阵
double c1 = -36 * dp.dot(dp);//算一下alpha，belta带入后就能够清楚带入参数的含义了
double c2 = 24 * (v0 + v1).dot(dp);
double c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1));
double c4 = 0;
double c5 = w_time_;
std::vector<double> ts = quartic(c5, c4, c3, c2, c1);//对下列c式子求导数，令其为0
double v_max = max_vel_ * 0.5;
double t_bar = (x1.head(3) - x2.head(3)).lpNorm<Infinity>() / v_max;//最短时间限制
ts.push_back(t_bar);
double cost = 100000000;
double t_d = t_bar;
for (auto t : ts)
{
if (t < t_bar)//小于最小时间不合理则跳出
continue;
double c = -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + w_time_ * t;//将alpha，belta带入到J*中化简得
if (c < cost)
{
cost = c;
t_d = t;
}
}
optimal_time = t_d;
return 1.0 * (1 + tie_breaker_) * cost;//返回启发式函数代价并且将对应的路径时间赋给optimal_time
}这部分对应论文的：
庞特里亚金极大值原理的基本思想是，如果在某个时间点，存在一个最优轨迹或最优控制策略， 本论文中的启发函数利用庞特里亚金原理解决两点边值问题，得到最优解后用最优解的控制代价作为启发函数。quartic函数是求解4次方程的函数，四次方程函数是由（5）将alpha，belta带入后求导所得到的，quartic函数用于返回最优的控制时间量。关于时间的一元四次方程是通过费拉里方法求解的，需要嵌套一个元三次方程进行求解，也就是代码中应的cubic函数。
//四次方程的费拉里解法，https://zh.wikipedia.org/wiki/%E5%9B%9B%E6%AC%A1%E6%96%B9%E7%A8%8B，降次为三次方程电泳cubic
vector<double> KinodynamicAstar::quartic(double a, double b, double c, double d, double e)
{
vector<double> dts;
double a3 = b / a;
double a2 = c / a;
double a1 = d / a;
double a0 = e / a;
vector<double> ys = cubic(1, -a2, a1 * a3 - 4 * a0, 4 * a2 * a0 - a1 * a1 - a3 * a3 * a0);
double y1 = ys.front();
double r = a3 * a3 / 4 - a2 + y1;
if (r < 0)
return dts;
double R = sqrt(r);
double D, E;
if (R != 0)
{
D = sqrt(0.75 * a3 * a3 - R * R - 2 * a2 + 0.25 * (4 * a3 * a2 - 8 * a1 - a3 * a3 * a3) / R);
E = sqrt(0.75 * a3 * a3 - R * R - 2 * a2 - 0.25 * (4 * a3 * a2 - 8 * a1 - a3 * a3 * a3) / R);
}
else
{
D = sqrt(0.75 * a3 * a3 - 2 * a2 + 2 * sqrt(y1 * y1 - 4 * a0));
E = sqrt(0.75 * a3 * a3 - 2 * a2 - 2 * sqrt(y1 * y1 - 4 * a0));
}
if (!std::isnan(D))
{
dts.push_back(-a3 / 4 + R / 2 + D / 2);
dts.push_back(-a3 / 4 + R / 2 - D / 2);
}
if (!std::isnan(E))
{
dts.push_back(-a3 / 4 - R / 2 + E / 2);
dts.push_back(-a3 / 4 - R / 2 - E / 2);
}
return dts;
}
vector<double> KinodynamicAstar::cubic(double a, double b, double c, double d)
{
vector<double> dts;
double a2 = b / a;
double a1 = c / a;
double a0 = d / a;
double Q = (3 * a1 - a2 * a2) / 9;
double R = (9 * a1 * a2 - 27 * a0 - 2 * a2 * a2 * a2) / 54;
double D = Q * Q * Q + R * R;
if (D > 0)
{
double S = std::cbrt(R + sqrt(D));
double T = std::cbrt(R - sqrt(D));
dts.push_back(-a2 / 3 + (S + T));
return dts;
}
else if (D == 0)
{
double S = std::cbrt(R);
dts.push_back(-a2 / 3 + S + S);
dts.push_back(-a2 / 3 - S);
return dts;
}
else//<0
{
double theta = acos(R / sqrt(-Q * Q * Q));
dts.push_back(2 * sqrt(-Q) * cos(theta / 3) - a2 / 3);
dts.push_back(2 * sqrt(-Q) * cos((theta + 2 * M_PI) / 3) - a2 / 3);
dts.push_back(2 * sqrt(-Q) * cos((theta + 4 * M_PI) / 3) - a2 / 3);
return dts;
}
}cubic函数中区分了Delta的情况采用了不同的方法求根，判别式>=0的情况利用了求根公式，判别式<0的情况使用了三角函数解法。最终求得到最优的控制量t，并且更新了最优时间。
关于dynamic标志位（我猜的，水平有限暂时这么理解，有清楚的大佬评论教我）
if (dynamic)//为真时记录时间、将当前节点时间信息记录到expande_node中
{
time_origin_ = time_start;
cur_node->time = time_start;
cur_node->time_idx = timeToIndex(time_start);
expanded_nodes_.insert(cur_node->index, cur_node->time_idx, cur_node);
// cout << "time start: " << time_start << endl;
}
else
expanded_nodes_.insert(cur_node->index, cur_node);//仅将当前节点的空间信息插入到 expanded_nodes_ 中
PathNodePtr neighbor = NULL;//初始化邻居节点为空
PathNodePtr terminate_node = NULL;//终点节点为空
bool init_search = init;//初始化标记位
const int tolerance = ceil(1 / resolution_);//容差，变换前每1单位表示的距离-
如果
dynamic为true：- 该搜索算法会考虑时间信息，即路径规划是在动态环境中进行的。
- 每个节点除了空间状态外，还包含了时间信息（如
cur_node->time和cur_node->time_idx）。 - 节点被插入到
expanded_nodes_中时，时间信息也会被考虑。 - 最终搜索的路径是在三维空间和时间维度上的。
-
如果
dynamic为false：- 该搜索算法只考虑空间状态，即路径规划是在静态环境中进行的。
- 时间信息对于节点的存储和搜索不被考虑。
- 最终搜索的路径仅包含在三维空间中。
2.主要循环：
while循环是大头，包含了轨迹检查、节点拓展、节点剪枝、轨迹细化等功能。当open_set待搜索列表不为空时就一直执行（备注很清楚，是主观解读，如果有错误还请评论区指正）：
(1). 终止条件：
//基本循环函数
while (!open_set_.empty())
{
cur_node = open_set_.top();//在open_set中取节点。
// Terminate?
/*标志位在这个搜索算法中的主要作用是为了提前终止搜索。它表示当前节点是否已经离起点足够远，
超过了预定的 horizon_ 阈值。这个设计可能是为了在搜索空间较大时，避免过度的搜索，以提高算法的效率。
，当 reach_horizon 为真时，意味着当前节点已经离起点足够远，可能是因为在搜索空间中没有更好的路径，
或者当前路径不太可能变得更好。在这种情况下，算法可以提前结束搜索，以减少计算时间。
*/
bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= horizon_;
bool near_end = abs(cur_node->index(0) - end_index(0)) <= tolerance &&//判断是否到达终点了。
abs(cur_node->index(1) - end_index(1)) <= tolerance &&
abs(cur_node->index(2) - end_index(2)) <= tolerance;
if (reach_horizon || near_end)//离起点足够远或者判定到达起点了，停止条件当超出一定范围或者已经找到终点
{
terminate_node = cur_node;//将当前节点设为终点
retrievePath(terminate_node);//反向搜索路径
if (near_end)//当确实是靠近终点时
{
// Check whether shot traj exist
estimateHeuristic(cur_node->state, end_state, time_to_goal);//计算优化的最小时间
computeShotTraj(cur_node->state, end_state, time_to_goal);//带入优化的时间判断轨迹是否合理
if (init_search)//如果初始化成功
ROS_ERROR("Shot in first search loop!");
}
}
if (reach_horizon)//先判断是否超出范围
{
if (is_shot_succ_)//搜索到的路径安全可行的
{
std::cout << "reach end" << std::endl;//此处终点是最远点（其实也不算是规划成功，因为超出距离后到达的是最远处路径可行，并非是原来要找的终点）
return REACH_END;
}
else
{
std::cout << "reach horizon" << std::endl;//路径动态不可行（规划失败）
return REACH_HORIZON;
}
}
if (near_end)
{
if (is_shot_succ_)
{
std::cout << "reach end" << std::endl;
return REACH_END;
}
else if (cur_node->parent != NULL)//规划路径动态不可行，但是上一节点是可行的，此时为规划到接近终点的状态
{
std::cout << "near end" << std::endl;
return NEAR_END;
}
else
{
std::cout << "no path" << std::endl;
return NO_PATH;
}
}
/*
上述代码皆是单个循环内单个openlist节点内部的终止的判定条件，类似于递归代码的逻辑
下方代码可以看作是递归单个节点不满足逻辑时候需要处理的步骤（反正我是这么理解的）
*/(2).节点扩张：
当节点不满足终止条件（到达终点或超出探索距离）时，以当前节点为根，离散输入控制量，得到下一状态。
//当前节点无法到达，进行节点扩张
open_set_.pop();//弹出该节点
cur_node->node_state = IN_CLOSE_SET;//将已经探索的节点加入到close_list中，标记为已探索
iter_num_ += 1;//搜索完的点+1
double res = 1 / 2.0, time_res = 1 / 1.0, time_res_init = 1 / 20.0;//初始化时间常数
Eigen::Matrix<double, 6, 1> cur_state = cur_node->state;//记录当前节点状态
Eigen::Matrix<double, 6, 1> pro_state;//下一状态
vector<PathNodePtr> tmp_expand_nodes;//临时扩展节点列表
Eigen::Vector3d um;//离散的控制量，这里指的是三维上加速度
double pro_t;//拓展节点的时间戳
vector<Eigen::Vector3d> inputs;//输入
vector<double> durations;//单位输入控制量的持续时间
/*
这里大家要首先明确一点，init_search标志位用来标志是否用不连续的初始状态重试搜索。
本人的理解是如果有初始的节点输入状态存储，但是由于路径规划失败导致无法到达终点
所以需要拓展新的节点重新规划路径。
*/
if (init_search)//具有上次搜索失败的离散状态量
{
inputs.push_back(start_acc_);//存储起始状态的加速度
for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_ + 1e-3;
tau += time_res_init * init_max_tau_)
durations.push_back(tau);
init_search = false;
}//离散化时间
else//没有初始离散状态，未初始化成功，通过最大最小加速度初始化离散加速度后再离散时间
{
for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res)
for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res)
for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res)
{
um << ax, ay, az;
inputs.push_back(um);
}
for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_)
durations.push_back(tau);//离散化时间
}
// cout << "cur state:" << cur_state.head(3).transpose() << endl;
//组合不同的时间与加速度组合，得到不一样的下一状态集合
for (int i = 0; i < inputs.size(); ++i)
for (int j = 0; j < durations.size(); ++j)
{
um = inputs[i];
double tau = durations[j];
stateTransit(cur_state, pro_state, um, tau);//输入时间与加速度离散值，更新pro_state
pro_t = cur_node->time + tau;//拓展状态时间记录
Eigen::Vector3d pro_pos = pro_state.head(3);//拓展位置向量
// Check if in close set
Eigen::Vector3i pro_id = posToIndex(pro_pos);//拓展位置世界坐标系下的索引
int pro_t_id = timeToIndex(pro_t);//拓展位置时间id
PathNodePtr pro_node = dynamic ? expanded_nodes_.find(pro_id, pro_t_id) : expanded_nodes_.find(pro_id);//根据是否带时间戳记录拓展节点
if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET)//当存在于closet中，跳过此次拓展
{
if (init_search)
std::cout << "close" << std::endl;
continue;
}
//剩下NULL或不存在与closelist中的点
// Check maximal velocity
Eigen::Vector3d pro_v = pro_state.tail(3);//检查速度限制超过则跳过该次检查
if (fabs(pro_v(0)) > max_vel_ || fabs(pro_v(1)) > max_vel_ || fabs(pro_v(2)) > max_vel_)
{
if (init_search)
std::cout << "vel" << std::endl;
continue;
}
//剩下不超过速度限制的点，且不存在于close中，但可能为空
// Check not in the same voxel
Eigen::Vector3i diff = pro_id - cur_node->index;//检查拓展的栅格是否为当前栅格相同
int diff_time = pro_t_id - cur_node->time_idx;//检查栅格
if (diff.norm() == 0 && ((!dynamic) || diff_time == 0))//带时间戳时时间相同，或者时间相同跳出循环
{
if (init_search)
std::cout << "same" << std::endl;
continue;
}
//剩下不超过速度限制、且不是重复的点，且不存在于close中，但可能为空或者存在同一栅格中（但花费的时间不同的点）
// Check safety
Eigen::Vector3d pos;
Eigen::Matrix<double, 6, 1> xt;
bool is_occ = false;
for (int k = 1; k <= check_num_; ++k)//分辨率提高检查点，比如a到b我检查5个点，判定每个点是否占用
{
double dt = tau * double(k) / double(check_num_);
stateTransit(cur_state, xt, um, dt);//前向积分得到扩展节点的位置和速度
pos = xt.head(3);
if (edt_environment_->sdf_map_->getInflateOccupancy(pos) == 1 )//检查是否占用
{
is_occ = true;//占用标志位记为true
break;
}
}
if (is_occ)
{
if (init_search)
std::cout << "safe" << std::endl;//当检测到被占用时，跳过
continue;
}
//更新拓展状态的代价
double time_to_goal, tmp_g_score, tmp_f_score;//定义变量暂时存储代价函数值
tmp_g_score = (um.squaredNorm() + w_time_) * tau + cur_node->g_score;
tmp_f_score = tmp_g_score + lambda_heu_ * estimateHeuristic(pro_state, end_state, time_to_goal);(3)剪枝：
剪枝对同一栅格内所得到的采样点进行更新，仅保留最低代价的状态
//节点剪枝
// Compare nodes expanded from the same parent
/*
prune为剪枝标志位
如果 prune 为 true，表示需要对当前生成的节点进行剪枝。这说明当前节点与之前扩展的节点在相同的体素内（位置索引相同且时间索引相同，如果是动态规划）。
如果当前节点的代价（tmp_f_score）比之前扩展的相同节点的代价更小，则更新之前扩展的节点的信息，以保留更优的路径信息。
如果 prune 为 false，表示不需要对当前生成的节点进行剪枝。这说明当前节点与之前扩展的节点在不同的体素内。
如果当前节点是一个新的节点（pro_node == NULL），将其添加到搜索队列中，并更新相关信息。
如果当前节点已经在开放集合中，检查新路径的代价是否更小，如果是，则更新节点信息。
*/
bool prune = false;
for (int j = 0; j < tmp_expand_nodes.size(); ++j)//遍历所有临时拓展节点列表，用来检查拓展的点中有没有同个节点内的点，有的话标记剪枝操作
{
PathNodePtr expand_node = tmp_expand_nodes[j];
if ((pro_id - expand_node->index).norm() == 0 && ((!dynamic) || pro_t_id == expand_node->time_idx))//当前遍历节点与本次循环拓展节点在相同的体素内
{
prune = true;//标记需要剪枝（即比较代价函数）
if (tmp_f_score < expand_node->f_score)//比较代价函数，选取代价小的节点并保存。
{
expand_node->f_score = tmp_f_score;//将原来计算好的代价记录下来
expand_node->g_score = tmp_g_score;
expand_node->state = pro_state;//记录状态值
expand_node->input = um;//往前拓展状态的输入
expand_node->duration = tau;//往前拓展的积分的时间
if (dynamic)
expand_node->time = cur_node->time + tau;//如果是带时间戳的路径规划则记录时间戳信息
}
break;
}
}
// tem中存有的点是所有离散点？expand中存的是剪枝后每个节点的最优点
// This node end up in a voxel different from others
if (!prune)//当前节点为全新节点
{
if (pro_node == NULL)//如果 pro_node 为空，说明这是一个新的节点，需要将其添加到搜索队列中。更新节点的信息，包括索引、状态、代价等。
{
pro_node = path_node_pool_[use_node_num_];
pro_node->index = pro_id;
pro_node->state = pro_state;
pro_node->f_score = tmp_f_score;
pro_node->g_score = tmp_g_score;
pro_node->input = um;
pro_node->duration = tau;
pro_node->parent = cur_node;
pro_node->node_state = IN_OPEN_SET;//将其加入到openset中
if (dynamic)//带时间戳的规划需要带时间
{
pro_node->time = cur_node->time + tau;
pro_node->time_idx = timeToIndex(pro_node->time);
}
open_set_.push(pro_node);
//将其加入到探索列表节点中
if (dynamic)
expanded_nodes_.insert(pro_id, pro_node->time, pro_node);
else
expanded_nodes_.insert(pro_id, pro_node);
//加入临时探索列表中
tmp_expand_nodes.push_back(pro_node);//更新tmp
//已使用点数量
use_node_num_ += 1;
if (use_node_num_ == allocate_num_)//超过原定义好的ptr存储序列
{
cout << "run out of memory." << endl;
return NO_PATH;
}
}
else if (pro_node->node_state == IN_OPEN_SET)//如果 pro_node 已经在开放集合中，检查新路径的代价是否更小，如果是，则更新节点信息。
{
if (tmp_g_score < pro_node->g_score)
{
// pro_node->index = pro_id;
pro_node->state = pro_state;
pro_node->f_score = tmp_f_score;
pro_node->g_score = tmp_g_score;
pro_node->input = um;
pro_node->duration = tau;
pro_node->parent = cur_node;
if (dynamic)
pro_node->time = cur_node->time + tau;
}
}
else//如果 pro_node 的状态不是在开放集合中，可能存在错误。
{
cout << "error type in searching: " << pro_node->node_state << endl;
}
}
}
// init_search = false;
}3.流程分析 ：
流程分析如下（字丑，别骂了，爱看不看，有错误欢迎指出）
二、non_uniform_bspline均匀B样条轨迹优化
该部分的主要文件为：/fast_planner/bspline/src/non_uniform_bspline.cpp，用于实现B样条初始化，主要包含以下几个函数：B样条曲线参数初始化函数、节点处轨迹计算函数、轨迹拟合获取控制点函数、轨迹求导函数、动态可行性检查函数、轨迹节点调整函数。接下来将按照顺序介绍以上函数，写得备注很详细！
关于B样条轨迹的讲解可以阅读笔者下方这篇文章：
路径规划算法曲线篇（二）—— B样条曲线轨迹表示详解-CSDN博客
1.B样条初始化函数
读完上面那片文章应该对B样条有了初步认识了，此函数传入控制点、阶数、节点间隔实现对B样条参数的初始化。已知控制点数、阶数即可得求节点数。Fast中采用的是3次B样条曲线拟合轨迹，本代码初始化时候，节点向量从-3*interval开始的，一共有m_个节点。
/*
输入参数为，点，阶数，时间间隔
功能：初始化成员变量：控制点矩阵、B样条次数、时间间隔、多样条阶数、节点向量
*/
void NonUniformBspline::setUniformBspline(const Eigen::MatrixXd& points, const int& order,
const double& interval) {
//control_points_是动态大小的矩阵，元素为双浮点数，每行代表一个控制点，维度由列号确定即N*3矩阵
control_points_ = points;
//p为B样条次数
p_ = order;
//节点之间的是检查，即delta_t
interval_ = interval;
//B样条控制点个数-1得到阶数
n_ = points.rows() - 1;
//节点数为阶数+次数
m_ = n_ + p_ + 1;
//初始化存储节点的向量
u_ = Eigen::VectorXd::Zero(m_ + 1);
//B样条节点初始化，初始化为均匀B样条，即间隔相等。
/*
前p_+1个节点被称为开头节点或者起始节点，他们决定了B样条的起始点，这样定义出来的起始节点为小于0的区间
中间的p_ + 1 到 m_ - p_ 之间节点为中间节点，由累加而成。确保连续
后m_ - p_ 个节点称为尾节点（trailing nodes）或结束节点，它们决定了 B 样条曲线的结束点，初始化时，这些节点的值与前一个节点相等，确保在这一段区间内的节点是连续的。这个区间的长度是 (m_ - p_)
自己写出来很好理解
*/
for (int i = 0; i <= m_; ++i) {
if (i <= p_) {
u_(i) = double(-p_ + i) * interval_;
} else if (i > p_ && i <= m_ - p_) {
u_(i) = u_(i - 1) + interval_;
} else if (i > m_ - p_) {
u_(i) = u_(i - 1) + interval_;
}
}2.求B样条时间节点u_处值
由B样条的性质可知，B样条上某一点的值可以由De Boor递归公式推导得到，代码中的做法是将需要求的点值映射到距离其最近的时间节点，然后将其带入计算得到传入时间节点u处的值。
/*De Boor递归算法，计算传入参数u处的B样条曲线值，返回B-样条曲线在参数 u 处的B样条曲线函数*/
Eigen::VectorXd NonUniformBspline::evaluateDeBoor(const double& u) {
//限制参数 u 的范围： 将参数 u 限制在 B-样条曲线有效范围内，即 [u_(p_), u_(m_ - p_)]。
double ub = min(max(u_(p_), u), u_(m_ - p_));
// determine which [ui,ui+1] lay in
//找到输入的u是第几个节点处
int k = p_;
while (true) {
if (u_(k + 1) >= ub) break;
++k;
}
//通过k找到与当前节点相关的控制点k-p_到k
/* deBoor's alg */
vector<Eigen::VectorXd> d;
for (int i = 0; i <= p_; ++i) {
d.push_back(control_points_.row(k - p_ + i));
// cout << d[i].transpose() << endl;
}
//De Boor递归计算控制点，计算过程中使用了混合参数 alpha，该参数用于混合现有控制点以生成新的控制点。
//数学推导https://en.wikipedia.org/wiki/De_Boor%27s_algorithm
for (int r = 1; r <= p_; ++r) {//外循环次数
for (int i = p_; i >= r; --i) {//内循环控制节点，次数高一次，控制节点少一个（自己写以下就能理解）
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];
}
}
//返回计算得到的 B-样条曲线在参数 u 处的点
return d[p_];
}3.轨迹拟合获取控制点函数
这个函数很重要，起着承上启下的作用，在第一部分动态搜索得到了离散的路经点，此函数作用为求取控制点矩阵，该控制点矩阵生成的B样条曲线拟合路径点的路径。
其中，M矩阵表示了控制点与时间节点值之间的关系，来自于文献K. Qin, “General matrix representations for b-splines,” The Visual Computer, vol. 16, no. 3, pp. 177–186, 2000.
第一行表示了位置约束关系，第二行是速度、第三行是加速度。当传入的离散轨迹点为K时，有K-1段轨迹，由于是三次B样条曲线，（）所以算上头部3个节点，此外为了保证曲线的连续性后面添2个节点（因为要保证速度与加速度约束），共有K+5个时间节点，可以得到所求控制点个数应为K+5-次数3为K+2个。然后通过上式构建AX=B方程即可求解出控制点向量X。
关于s(t)，需要注意的是，当选择的约束为位置约束时为常数1，速度约束为1/t，加速度为1/t^2以此类推，需要高阶导数可以自己添加。另外添加了两个速度约束与两个加速度约束，此时的A矩阵大小应该为K+4*K+2。具体的代码含义我都以及备注了：
/*
函数参数：ts：轨迹执行时间、point_set：原轨迹点集合、start_end_derivative：起点与终点的高阶约束
输出：更新ctrl_pts控制点矩阵的值
函数功能：将给定点集和起始/终止导数转换为 B-样条曲线的控制点矩阵，通过对原始轨迹的拟合得到B样条轨迹的控制点
*/
void NonUniformBspline::parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,
const vector<Eigen::Vector3d>& start_end_derivative,
Eigen::MatrixXd& ctrl_pts) {
//判断输入的时间是否合理
if (ts <= 0) {
cout << "[B-spline]:time step error." << endl;
return;
}
//判断输入的轨迹点是否合理
if (point_set.size() < 2) {
cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl;
return;
}
//起点与终点的导数限制（即一阶和二阶导数，有此信息才可以确保得到一个具有足够信息的线性方程组，从而可以求解出 B-样条曲线的控制点。）
if (start_end_derivative.size() != 4) {
cout << "[B-spline]:derivatives error." << endl;
}
//记录原曲线路经点个数
int K = point_set.size();
// write A
//该矩阵用来构建线性方程组， B-样条曲线参数化过程中求解控制点
//一阶 B-样条基函数的系数向量、一阶 B-样条基函数的导数系数向量、二阶 B-样条基函数的系数向量
//取该数值的原因是B样条曲线矩阵表示论文中提出的，以满足 B-样条曲线的一些良好性质。
Eigen::Vector3d prow(3), vrow(3), arow(3);
prow << 1, 4, 1;
vrow << -1, 0, 1;
arow << 1, -2, 1;
//K+4是因为添加了四行导数约束信息
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);
for (int i = 0; i < K; ++i) A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();
A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose();
A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
// cout << "A:\n" << A << endl;
// A.block(0, 0, K, K + 2) = (1 / 6.0) * A.block(0, 0, K, K + 2);
// A.block(K, 0, 2, K + 2) = (1 / 2.0 / ts) * A.block(K, 0, 2, K + 2);
// A.row(K + 4) = (1 / ts / ts) * A.row(K + 4);
// A.row(K + 5) = (1 / ts / ts) * A.row(K + 5);
// write b
Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);
for (int i = 0; i < K; ++i) {
bx(i) = point_set[i](0);
by(i) = point_set[i](1);
bz(i) = point_set[i](2);
}
for (int i = 0; i < 4; ++i) {
bx(K + i) = start_end_derivative[i](0);
by(K + i) = start_end_derivative[i](1);
bz(K + i) = start_end_derivative[i](2);
}
// solve Ax = b
//求解线性方程
Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);
// convert to control pts
//控制点赋值
ctrl_pts.resize(K + 2, 3);
ctrl_pts.col(0) = px;
ctrl_pts.col(1) = py;
ctrl_pts.col(2) = pz;
// cout << "[B-spline]: parameterization ok." << endl;
}
4.轨迹求导函数
轨迹求导函数不难理解，主要用途是用于检测轨迹上的速度与加速度限制。利用B样条曲线的递归公式，先计算控制点，在通过控制点得到求导后的矩阵
/*得到求导之后的区域显得控制点矩阵*/
Eigen::MatrixXd NonUniformBspline::getDerivativeControlPoints() {
// The derivative of a b-spline is also a b-spline, its order become p_-1
// control point Qi = p_*(Pi+1-Pi)/(ui+p_+1-ui+1)
Eigen::MatrixXd ctp = Eigen::MatrixXd::Zero(control_points_.rows() - 1, control_points_.cols());
for (int i = 0; i < ctp.rows(); ++i) {
ctp.row(i) =
p_ * (control_points_.row(i + 1) - control_points_.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
}
return ctp;
}
/*计算求导后的曲线，并且将新的控制点、次数、节点向量记录下来*/
NonUniformBspline NonUniformBspline::getDerivative() {
Eigen::MatrixXd ctp = getDerivativeControlPoints();
NonUniformBspline derivative(ctp, p_ - 1, interval_);
/* cut the first and last knot */
Eigen::VectorXd knot(u_.rows() - 2);
knot = u_.segment(1, u_.rows() - 2);
derivative.setKnot(knot);
return derivative;
}
5. 速度、加速度检查函数：
速度加速度这里计算的是控制点之间的值，因为B样条的特殊性质，限制控制点的速度与加速度可以间接控制B样条的速度与加速度，计算公式如论文中所示：
/*轨迹安全性检查，并更新超阈值比例*/
bool NonUniformBspline::checkFeasibility(bool show) {
bool fea = true;//可行性标记位
// SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;
Eigen::MatrixXd P = control_points_;//取出控制点
int dimension = control_points_.cols();//求控制点列数即维度，x，y，z
/* check vel feasibility and insert points */
double max_vel = -1.0;//初始化最大速度为负
for (int i = 0; i < P.rows() - 1; ++i) {//遍历每一个控制点
Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));//根据控制点计算速度
if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||//判断是否有任一维度上的速度超过阈值
fabs(vel(2)) > limit_vel_ + 1e-4) {
if (show) cout << "[Check]: Infeasible vel " << i << " :" << vel.transpose() << endl;
fea = false;//标记为不可行
for (int j = 0; j < dimension; ++j) {
max_vel = max(max_vel, fabs(vel(j)));//采用计算出来的速度更新最大速度
}
}
}
/* acc feasibility */
double max_acc = -1.0;
for (int i = 0; i < P.rows() - 2; ++i) {//遍历n-2个点
Eigen::VectorXd acc = p_ * (p_ - 1) *
((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
(P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
(u_(i + p_ + 1) - u_(i + 2));//计算加速度
if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 ||
fabs(acc(2)) > limit_acc_ + 1e-4) {//判断是否有任一维度上的加速度超过阈值
if (show) cout << "[Check]: Infeasible acc " << i << " :" << acc.transpose() << endl;
fea = false;
for (int j = 0; j < dimension; ++j) {//记录更新最大加速度
max_acc = max(max_acc, fabs(acc(j)));
}
}
}
double ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));//计算最大速度或加速度超过约束的
return fea;
}
/*获取当前控制点与时间向量节点下计算出来的超阈值比例*/
double NonUniformBspline::checkRatio() {
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
// find max vel
double max_vel = -1.0;
for (int i = 0; i < P.rows() - 1; ++i) {
Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
for (int j = 0; j < dimension; ++j) {
max_vel = max(max_vel, fabs(vel(j)));
}
}
// find max acc
double max_acc = -1.0;
for (int i = 0; i < P.rows() - 2; ++i) {
Eigen::VectorXd acc = p_ * (p_ - 1) *
((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
(P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
(u_(i + p_ + 1) - u_(i + 2));
for (int j = 0; j < dimension; ++j) {
max_acc = max(max_acc, fabs(acc(j)));
}
}
double ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));
ROS_ERROR_COND(ratio > 2.0, "max vel: %lf, max acc: %lf.", max_vel, max_acc);
return ratio;
}
/*获取当前控制点与时间向量节点下计算出来的超阈值比例*/
double NonUniformBspline::checkRatio() {
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
// find max vel
double max_vel = -1.0;
for (int i = 0; i < P.rows() - 1; ++i) {
Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
for (int j = 0; j < dimension; ++j) {
max_vel = max(max_vel, fabs(vel(j)));
}
}
// find max acc
double max_acc = -1.0;
for (int i = 0; i < P.rows() - 2; ++i) {
Eigen::VectorXd acc = p_ * (p_ - 1) *
((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
(P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
(u_(i + p_ + 1) - u_(i + 2));
for (int j = 0; j < dimension; ++j) {
max_acc = max(max_acc, fabs(acc(j)));
}
}
double ratio = max(max_vel / limit_vel_, sqrt(fabs(max_acc) / limit_acc_));
ROS_ERROR_COND(ratio > 2.0, "max vel: %lf, max acc: %lf.", max_vel, max_acc);
return ratio;
}
/*检查可行性，重新分配时间节点*/
bool NonUniformBspline::reallocateTime(bool show) {
// SETY << "[Bspline]: total points size: " << control_points_.rows() << endl;
// cout << "origin knots:\n" << u_.transpose() << endl;
bool fea = true;
Eigen::MatrixXd P = control_points_;
int dimension = control_points_.cols();
double max_vel, max_acc;
/* check vel feasibility and insert points */
for (int i = 0; i < P.rows() - 1; ++i) {
Eigen::VectorXd vel = p_ * (P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1));
if (fabs(vel(0)) > limit_vel_ + 1e-4 || fabs(vel(1)) > limit_vel_ + 1e-4 ||
fabs(vel(2)) > limit_vel_ + 1e-4) {
fea = false;
if (show) cout << "[Realloc]: Infeasible vel " << i << " :" << vel.transpose() << endl;
max_vel = -1.0;
for (int j = 0; j < dimension; ++j) {
max_vel = max(max_vel, fabs(vel(j)));
}
double ratio = max_vel / limit_vel_ + 1e-4;
if (ratio > limit_ratio_) ratio = limit_ratio_;
/*
在这段代码中，j = i + 2 处开始调整，是因为速度的计算涉及到两个相邻的控制点，即 P.row(i + 1) 和 P.row(i + 2)。
为了调整速度，需要改变这两个控制点之间的时间间隔，即 u_(i + p_ + 1) - u_(i + 1)。因此，j 从 i + 2 开始，
以便调整第 i + 1 和 i + 2 之间的时间节点。
*/
double time_ori = u_(i + p_ + 1) - u_(i + 1);
double time_new = ratio * time_ori;
double delta_t = time_new - time_ori;
double t_inc = delta_t / double(p_);
for (int j = i + 2; j <= i + p_ + 1; ++j) {
u_(j) += double(j - i - 1) * t_inc;
if (j <= 5 && j >= 1) {
// cout << "vel j: " << j << endl;
}
}
for (int j = i + p_ + 2; j < u_.rows(); ++j) {
u_(j) += delta_t;
}
}
}
/* acc feasibility */
for (int i = 0; i < P.rows() - 2; ++i) {
Eigen::VectorXd acc = p_ * (p_ - 1) *
((P.row(i + 2) - P.row(i + 1)) / (u_(i + p_ + 2) - u_(i + 2)) -
(P.row(i + 1) - P.row(i)) / (u_(i + p_ + 1) - u_(i + 1))) /
(u_(i + p_ + 1) - u_(i + 2));
if (fabs(acc(0)) > limit_acc_ + 1e-4 || fabs(acc(1)) > limit_acc_ + 1e-4 ||
fabs(acc(2)) > limit_acc_ + 1e-4) {
fea = false;
if (show) cout << "[Realloc]: Infeasible acc " << i << " :" << acc.transpose() << endl;
max_acc = -1.0;
for (int j = 0; j < dimension; ++j) {
max_acc = max(max_acc, fabs(acc(j)));
}
double ratio = sqrt(max_acc / limit_acc_) + 1e-4;
if (ratio > limit_ratio_) ratio = limit_ratio_;
// cout << "ratio: " << ratio << endl;
double time_ori = u_(i + p_ + 1) - u_(i + 2);
double time_new = ratio * time_ori;
double delta_t = time_new - time_ori;
double t_inc = delta_t / double(p_ - 1);
if (i == 1 || i == 2) {
// cout << "acc i: " << i << endl;
for (int j = 2; j <= 5; ++j) {
u_(j) += double(j - 1) * t_inc;
}
for (int j = 6; j < u_.rows(); ++j) {
u_(j) += 4.0 * t_inc;
}
} else {
for (int j = i + 3; j <= i + p_ + 1; ++j) {
u_(j) += double(j - i - 2) * t_inc;
if (j <= 5 && j >= 1) {
// cout << "acc j: " << j << endl;
}
}
for (int j = i + p_ + 2; j < u_.rows(); ++j) {
u_(j) += delta_t;
}
}
}
}
return fea;
}6.时间重分配
对于速度、加速度分配不合理的曲线的时间节点进行重分配，注意速度与加速度不合理时不仅仅要修改当前时间节点，因为与多节点有关都要修改。调整时间比例因子通过计算超过的比例系数获得。
/*
函数参数：ts：轨迹执行时间、point_set：原轨迹点集合、start_end_derivative：起点与终点的高阶约束
输出：更新ctrl_pts控制点矩阵的值
函数功能：将给定点集和起始/终止导数转换为 B-样条曲线的控制点矩阵，通过对原始轨迹的拟合得到B样条轨迹的控制点
*/
void NonUniformBspline::parameterizeToBspline(const double& ts, const vector<Eigen::Vector3d>& point_set,
const vector<Eigen::Vector3d>& start_end_derivative,
Eigen::MatrixXd& ctrl_pts) {
//判断输入的时间是否合理
if (ts <= 0) {
cout << "[B-spline]:time step error." << endl;
return;
}
//判断输入的轨迹点是否合理
if (point_set.size() < 2) {
cout << "[B-spline]:point set have only " << point_set.size() << " points." << endl;
return;
}
//起点与终点的导数限制（即一阶和二阶导数，有此信息才可以确保得到一个具有足够信息的线性方程组，从而可以求解出 B-样条曲线的控制点。）
if (start_end_derivative.size() != 4) {
cout << "[B-spline]:derivatives error." << endl;
}
//记录原曲线路经点个数
int K = point_set.size();
// write A
//该矩阵用来构建线性方程组， B-样条曲线参数化过程中求解控制点
//一阶 B-样条基函数的系数向量、一阶 B-样条基函数的导数系数向量、二阶 B-样条基函数的系数向量
//取该数值的原因是B样条曲线矩阵表示论文中提出的，以满足 B-样条曲线的一些良好性质。
Eigen::Vector3d prow(3), vrow(3), arow(3);
prow << 1, 4, 1;
vrow << -1, 0, 1;
arow << 1, -2, 1;
//K+4是因为添加了四行导数约束信息
Eigen::MatrixXd A = Eigen::MatrixXd::Zero(K + 4, K + 2);
for (int i = 0; i < K; ++i) A.block(i, i, 1, 3) = (1 / 6.0) * prow.transpose();
A.block(K, 0, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 1, K - 1, 1, 3) = (1 / 2.0 / ts) * vrow.transpose();
A.block(K + 2, 0, 1, 3) = (1 / ts / ts) * arow.transpose();
A.block(K + 3, K - 1, 1, 3) = (1 / ts / ts) * arow.transpose();
// cout << "A:\n" << A << endl;
// A.block(0, 0, K, K + 2) = (1 / 6.0) * A.block(0, 0, K, K + 2);
// A.block(K, 0, 2, K + 2) = (1 / 2.0 / ts) * A.block(K, 0, 2, K + 2);
// A.row(K + 4) = (1 / ts / ts) * A.row(K + 4);
// A.row(K + 5) = (1 / ts / ts) * A.row(K + 5);
// write b
Eigen::VectorXd bx(K + 4), by(K + 4), bz(K + 4);
for (int i = 0; i < K; ++i) {
bx(i) = point_set[i](0);
by(i) = point_set[i](1);
bz(i) = point_set[i](2);
}
for (int i = 0; i < 4; ++i) {
bx(K + i) = start_end_derivative[i](0);
by(K + i) = start_end_derivative[i](1);
bz(K + i) = start_end_derivative[i](2);
}
// solve Ax = b
//求解线性方程
Eigen::VectorXd px = A.colPivHouseholderQr().solve(bx);
Eigen::VectorXd py = A.colPivHouseholderQr().solve(by);
Eigen::VectorXd pz = A.colPivHouseholderQr().solve(bz);
// convert to control pts
//控制点赋值
ctrl_pts.resize(K + 2, 3);
ctrl_pts.col(0) = px;
ctrl_pts.col(1) = py;
ctrl_pts.col(2) = pz;
// cout << "[B-spline]: parameterization ok." << endl;
}通过以上步骤，我们通过离散的路径点求得B样条控制点，此时为均匀B样条并通过速度、加速度约束条件调整不合理的时间节点，此时曲线也变成了非均匀B样条。但是，为了获得更加光滑且安全的路径，还需要对路径进行优化，接下来将基于控制点构建路径优化模型，并对约束条件建模，将此问题建模为一个带约束的优化问题，通过优化器得到最优轨迹。
三、B样条优化
由上一步得到了初始化的B样条轨迹,次部分为了得到更加光滑安全的轨迹,此处通过优化控制点优化其生成的B样条轨迹.
1.代价函数
a.平滑代价函数
采用jerk最小化,代码中采用的是差分的形式，对应于论文中的下式子：
/*计算光滑代价惩罚函数*/
/*输入：三维点集合（控制点）、更新cost、梯度信息gradient*/
void BsplineOptimizer::calcSmoothnessCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
//初始化代价为0
cost = 0.0;
//初始化一个0向量并且用0向量初始化梯度矩阵
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
//初始化临时变量
Eigen::Vector3d jerk, temp_j;
//遍历控制点求jerk
for (int i = 0; i < q.size() - order_; i++) {
/* evaluate jerk */
//采用三阶差分的形式求取jerk
jerk = q[i + 3] - 3 * q[i + 2] + 3 * q[i + 1] - q[i];
//累加所有带价值
cost += jerk.squaredNorm();
//*2是为了调整效果
temp_j = 2.0 * jerk;
/* jerk gradient */
//由于上述采用的计算形式是差分，所以直接求导就是得到系数，这里设计得很巧妙因为是累加
gradient[i + 0] += -temp_j;
gradient[i + 1] += 3.0 * temp_j;
gradient[i + 2] += -3.0 * temp_j;
gradient[i + 3] += temp_j;
}
}b.障碍物碰撞惩罚函数：
障碍物碰撞距离获取直接通过地图函数调用实现，对应于文中以下公式（跟代码不是统一方法）：
/*计算障碍物距离代价惩罚函数*/
void BsplineOptimizer::calcDistanceCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
//初始化代价为0
cost = 0.0;
//初始化一个0向量并且用0向量初始化梯度矩阵
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
//初始化距离值
double dist;
//定义变量存储梯度向量
Eigen::Vector3d dist_grad, g_zero(0, 0, 0);
//位运算检查是否包含ENDPOINT标志，函数根据是否需要考虑路径的终点来调整计算
//如果包含 ENDPOINT，则处理整个路径点向量；如果不包含，就排除掉最后 order_ 个点，可能是为了避免在计算中出现索引越界的问题。
int end_idx = (cost_function_ & ENDPOINT) ? q.size() : q.size() - order_;
for (int i = order_; i < end_idx; i++) {
//通过调用evaluateEDTWithGrad计算距离dist与距离梯度dist_grad
edt_environment_->evaluateEDTWithGrad(q[i], -1.0, dist, dist_grad);
//果梯度的范数大于阈值，则对梯度进行归一化。
if (dist_grad.norm() > 1e-4) dist_grad.normalize();
//当路径点距离环境较近时，代价增加，通过梯度的计算，可以在后续的优化过程中调整路径点，以增加与环境的距离，从而减少代价。
if (dist < dist0_) {
cost += pow(dist - dist0_, 2);
gradient[i] += 2.0 * (dist - dist0_) * dist_grad;
}
}
}c.动力学可行性惩罚
对每一个超过速度、加速度限制的控制范围进行惩罚，对应于文中以下公式：
/*计算控制点速度、加速度可行性函数*/
void BsplineOptimizer::calcFeasibilityCost(const vector<Eigen::Vector3d>& q, double& cost,
vector<Eigen::Vector3d>& gradient) {
//初始化代价为0
cost = 0.0;
//初始化一个0向量并且用0向量初始化梯度矩阵
Eigen::Vector3d zero(0, 0, 0);
std::fill(gradient.begin(), gradient.end(), zero);
//速度、加速度平方，ts时间倒数平方与四次方
/* abbreviation */
double ts, vm2, am2, ts_inv2, ts_inv4;
vm2 = max_vel_ * max_vel_;
am2 = max_acc_ * max_acc_;
ts = bspline_interval_;
ts_inv2 = 1 / ts / ts;
ts_inv4 = ts_inv2 * ts_inv2;
/* velocity feasibility */
for (int i = 0; i < q.size() - 1; i++) {
//位置差分，位置差/步长等于速度
Eigen::Vector3d vi = q[i + 1] - q[i];
for (int j = 0; j < 3; j++) {
//vm2是最大的速度，由于速度是的一阶导数，因此在从位置差分转换为加速度时要/t^2
double vd = vi(j) * vi(j) * ts_inv2 - vm2;
if (vd > 0.0) {
//方超过最大速度时累加惩罚项
cost += pow(vd, 2);
//4为比例系数
double temp_v = 4.0 * vd * ts_inv2;
gradient[i + 0](j) += -temp_v * vi(j);
gradient[i + 1](j) += temp_v * vi(j);
}
}
}
/* acceleration feasibility */
for (int i = 0; i < q.size() - 2; i++) {
//采用差分形式计算加速度差
Eigen::Vector3d ai = q[i + 2] - 2 * q[i + 1] + q[i];
for (int j = 0; j < 3; j++) {
//每个方向上加速度大小的平方/t^4，由于加速度是位置的二阶导数，因此在从位置差分转换为加速度时要/t^4
double ad = ai(j) * ai(j) * ts_inv4 - am2;
if (ad > 0.0) {
cost += pow(ad, 2);
//计算梯度的向量采用累加
double temp_a = 4.0 * ad * ts_inv4;
gradient[i + 0](j) += temp_a * ai(j);
gradient[i + 1](j) += -2 * temp_a * ai(j);
gradient[i + 2](j) += temp_a * ai(j);
}
}
}
}2.计算总代价函数和
对于以上三者求和得到总代价函数，其比例系数在上述函数已经单独乘了，此处只需要直接相加，其中还定义了其他代价函数。对应于论文中的下式：
/*优化项结合函数，输入为控制点、引用更新梯度向量、联合函数*/
void BsplineOptimizer::combineCost(const std::vector<double>& x, std::vector<double>& grad,
double& f_combine) {
/* convert the NLopt format vector to control points. */
/*NLopt 格式的向量转换到控制点表示，以支持1D至3D的B样条优化*/
// This solver can support 1D-3D B-spline optimization, but we use Vector3d to store each control point
// For 1D case, the second and third elements are zero, and similar for the 2D case.
for (int i = 0; i < order_; i++) {
for (int j = 0; j < dim_; ++j) {
g_q_[i][j] = control_points_(i, j);
}
//如果 dim_ 小于3，则剩余的维度设置为0。这是为了处理1D和2D情况。
for (int j = dim_; j < 3; ++j) {
g_q_[i][j] = 0.0;
}
}
//处理由NLopt传入的变量 x，将其转换为控制点并存储在 g_q_ 中。同样，如果 dim_ 小于3，则剩余的维度设置为0，得到矩阵g_q_
for (int i = 0; i < variable_num_ / dim_; i++) {
for (int j = 0; j < dim_; ++j) {
g_q_[i + order_][j] = x[dim_ * i + j];
}
for (int j = dim_; j < 3; ++j) {
g_q_[i + order_][j] = 0.0;
}
}
//考虑终点，如果 cost_function_ 不包含 ENDPOINT 标志，则将 control_points_ 中的最后 order_ 个控制点复制到 g_q_ 的相应位置
if (!(cost_function_ & ENDPOINT)) {
for (int i = 0; i < order_; i++) {
for (int j = 0; j < dim_; ++j) {
g_q_[order_ + variable_num_ / dim_ + i][j] =
control_points_(control_points_.rows() - order_ + i, j);
}
for (int j = dim_; j < 3; ++j) {
g_q_[order_ + variable_num_ / dim_ + i][j] = 0.0;
}
}
}
//定义总代价函数
f_combine = 0.0;
grad.resize(variable_num_);
fill(grad.begin(), grad.end(), 0.0);
if (cost_function_ & SMOOTHNESS) {
//此处思路真的很清晰，标志位cost_function_设计的很巧妙
/* evaluate costs and their gradient */
//每个代价变量
double f_smoothness, f_distance, f_feasibility, f_endpoint, f_guide, f_waypoints;
f_smoothness = f_distance = f_feasibility = f_endpoint = f_guide = f_waypoints = 0.0;
if (cost_function_ & SMOOTHNESS) {
calcSmoothnessCost(g_q_, f_smoothness, g_smoothness_);
f_combine += lambda1_ * f_smoothness;
for (int i = 0; i < variable_num_ / dim_; i++)
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda1_ * g_smoothness_[i + order_](j);
}
if (cost_function_ & DISTANCE) {
calcDistanceCost(g_q_, f_distance, g_distance_);
f_combine += lambda2_ * f_distance;
for (int i = 0; i < variable_num_ / dim_; i++)
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda2_ * g_distance_[i + order_](j);
}
if (cost_function_ & FEASIBILITY) {
calcFeasibilityCost(g_q_, f_feasibility, g_feasibility_);
f_combine += lambda3_ * f_feasibility;
for (int i = 0; i < variable_num_ / dim_; i++)
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda3_ * g_feasibility_[i + order_](j);
}
if (cost_function_ & ENDPOINT) {
calcEndpointCost(g_q_, f_endpoint, g_endpoint_);
f_combine += lambda4_ * f_endpoint;
for (int i = 0; i < variable_num_ / dim_; i++)
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda4_ * g_endpoint_[i + order_](j);
}
if (cost_function_ & GUIDE) {
calcGuideCost(g_q_, f_guide, g_guide_);
f_combine += lambda5_ * f_guide;
for (int i = 0; i < variable_num_ / dim_; i++)
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda5_ * g_guide_[i + order_](j);
}
if (cost_function_ & WAYPOINTS) {
calcWaypointsCost(g_q_, f_waypoints, g_waypoints_);
f_combine += lambda7_ * f_waypoints;
for (int i = 0; i < variable_num_ / dim_; i++)
for (int j = 0; j < dim_; j++) grad[dim_ * i + j] += lambda7_ * g_waypoints_[i + order_](j);
}
/* print cost */
// if ((cost_function_ & WAYPOINTS) && iter_num_ % 10 == 0) {
// cout << iter_num_ << ", total: " << f_combine << ", acc: " << lambda8_ * f_view
// << ", waypt: " << lambda7_ * f_waypoints << endl;
// }
// if (optimization_phase_ == SECOND_PHASE) {
// << ", smooth: " << lambda1_ * f_smoothness
// << " , dist:" << lambda2_ * f_distance
// << ", fea: " << lambda3_ * f_feasibility << endl;
// << ", end: " << lambda4_ * f_endpoint
// << ", guide: " << lambda5_ * f_guide
// }
}
}3.通过Nlopt进行优化：
Eigen::MatrixXd BsplineOptimizer::BsplineOptimizeTraj(const Eigen::MatrixXd& points, const double& ts,
const int& cost_function, int max_num_id,
int max_time_id) {
//初始化控制点与维度
setControlPoints(points);
//初始化节点向量
setBsplineInterval(ts);
//初始化目代价函数
setCostFunction(cost_function);
//初始化最大控制点与时间id
setTerminateCond(max_num_id, max_time_id);
//执行优化
optimize();
//返回优化后的控制点
return this->control_points_;
}
/*优化函数，目标最小化代价输出的控制点*/
void BsplineOptimizer::optimize() {
/* initialize solver */
//初始化迭代次数和最小代价、设置控制点的数量
iter_num_ = 0;
min_cost_ = std::numeric_limits<double>::max();
const int pt_num = control_points_.rows();
//初始化存储代价值向量
g_q_.resize(pt_num);
g_smoothness_.resize(pt_num);
g_distance_.resize(pt_num);
g_feasibility_.resize(pt_num);
g_endpoint_.resize(pt_num);
g_waypoints_.resize(pt_num);
g_guide_.resize(pt_num);
//判断是否有终点硬约束
if (cost_function_ & ENDPOINT) {
variable_num_ = dim_ * (pt_num - order_);
// end position used for hard constraint
end_pt_ = (1 / 6.0) *
(control_points_.row(pt_num - 3) + 4 * control_points_.row(pt_num - 2) +
control_points_.row(pt_num - 1));
} else {
variable_num_ = max(0, dim_ * (pt_num - 2 * order_)) ;
}
/* do optimization using NLopt slover */
//创建 NLopt 优化器，选择合适的算法。
nlopt::opt opt(nlopt::algorithm(isQuadratic() ? algorithm1_ : algorithm2_), variable_num_);
opt.set_min_objective(BsplineOptimizer::costFunction, this);
//设置最大迭代次数、最长运行时间和相对容差。
opt.set_maxeval(max_iteration_num_[max_num_id_]);
opt.set_maxtime(max_iteration_time_[max_time_id_]);
opt.set_xtol_rel(1e-5);
vector<double> q(variable_num_);
for (int i = order_; i < pt_num; ++i) {
if (!(cost_function_ & ENDPOINT) && i >= pt_num - order_) continue;
for (int j = 0; j < dim_; j++) {
q[dim_ * (i - order_) + j] = control_points_(i, j);
}
}
if (dim_ != 1) {
vector<double> lb(variable_num_), ub(variable_num_);
const double bound = 10.0;
for (int i = 0; i < variable_num_; ++i) {
lb[i] = q[i] - bound;
ub[i] = q[i] + bound;
}
opt.set_lower_bounds(lb);
opt.set_upper_bounds(ub);
}
try {
// cout << fixed << setprecision(7);
// vec_time_.clear();
// vec_cost_.clear();
// time_start_ = ros::Time::now();
//执行优化
double final_cost;
nlopt::result result = opt.optimize(q, final_cost);
/* retrieve the optimization result */
// cout << "Min cost:" << min_cost_ << endl;
} catch (std::exception& e) {
ROS_WARN("[Optimization]: nlopt exception");
cout << e.what() << endl;
}
//遍历向量
for (int i = order_; i < control_points_.rows(); ++i) {
if (!(cost_function_ & ENDPOINT) && i >= pt_num - order_) continue;
for (int j = 0; j < dim_; j++) {
control_points_(i, j) = best_variable_[dim_ * (i - order_) + j];
}
}
if (!(cost_function_ & GUIDE)) ROS_INFO_STREAM("iter num: " << iter_num_);
}
综上，通过最小化代价，得到一组最优的控制点。
版权声明：本文为博主作者：小陈最爱学习了原创文章，版权归属原作者，如果侵权，请联系我们删除！
原文链接：https://blog.csdn.net/weixin_43793717/article/details/134360566
