路规算法详细解读（一）—— FAST-Planner重要部分代码解读
于最近的研究需要，需要对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 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() / v_max;//最短时间限制
ts.push_back(t_bar);
double cost = 100000000;
double t_d = t_bar;
for (auto t : ts)
{
if (t
这部分对应论文的：
庞特里亚金极大值原理的基本思想是，如果在某个时间点，存在一个最优轨迹或最优控制策略， 本论文中的启发函数利用庞特里亚金原理解决两点边值问题，得到最优解后用最优解的控制代价作为启发函数。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 KinodynamicAstar::quartic(double a, double b, double c, double d, double e)
{
vector dts;
double a3 = b / a;
double a2 = c / a;
double a1 = d / a;
double a0 = e / a;
vector 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)
{
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的情况利用了求根公式，判别式time = time_start;
cur_node->time_idx = timeToIndex(time_start);
expanded_nodes_.insert(cur_node->index, cur_node->time_idx, cur_node);
// cout time_idx）。
如果 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)) index(1) - end_index(1)) index(2) - end_index(2)) 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 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 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
免责声明：我们致力于保护作者版权，注重分享，被刊用文章因无法核实真实出处，未能及时与作者取得联系，或有版权异议的，请联系管理员，我们会立即处理!
部分文章是来自自研大数据AI进行生成,内容摘自(百度百科,百度知道,头条百科,中国民法典,刑法,牛津词典,新华词典,汉语词典,国家院校,科普平台)等数据,内容仅供学习参考,不准确地方联系删除处理!
图片声明：本站部分配图来自人工智能系统AI生成,觅知网授权图片,PxHere摄影无版权图库和百度，360，搜狗等多加搜索引擎自动关键词搜索配图，如有侵权的图片，请第一时间联系我们，邮箱：ciyunidc@ciyunshuju.com。本站只作为美观性配图使用,无任何非法侵犯第三方意图,一切解释权归图片著作权方,本站不承担任何责任。如有恶意碰瓷者,必当奉陪到底严惩不贷!
