IT评测·应用市场-qidao123.com技术社区

标题: FastPlanner论文解读（一）——前端路径搜索 [打印本页]

---

作者: 河曲智叟    时间: 2025-4-3 05:25
标题: FastPlanner论文解读（一）——前端路径搜索
本系列将更新FastPlanner论文内容对应代码的解读，其github的工程位置如下：
GitCode - 环球开辟者的开源社区,开源代码托管平台
一、择要

        在本文中，提出了一种稳健高效的四旋翼运动规划体系，主要架构如下：
1.kinodynamic 路径搜索方法在离散控制空间中找到一个安全的、kinodynamic 可行且时间最短初始轨迹。
2.我们通过 B 样条优化来改善轨迹的平滑度和间隙，该优化结合了来自欧几里得距离场 （EDF） 的梯度信息和动态约束，有效地利用了 B 样条的凸包特性。
3.将最终轨迹表示为非均匀 B 样条，采用迭代时间调解方法来包管动态可行和非保守轨迹。

前端采用Hybrid A*算法开端搜索出一条路径，进入后端使用软约束优化得到优化好的B样条曲线，再将其表示为非均匀B样条的方式，调解控制点和时间隔断。
二、前端

1.算法架构

        前端的kinodynamic path searching使用hybrid A*算法，在地图中搜索出一条安全的、动力学的可行轨迹，算法流程如下：

2.图搜索算法

        首先我们阐明图搜索算法的思路：
图搜索算法本质上是在维护一个容器，里面装着未来大概访问的节点，开始为容器里面节点数目为0，然后放入第一个节点举行循环，在循环中：
1.把容器中最符合条件的节点找到并访问弹出
2.将这个节点对周围的节点举行扩展
3.将扩展的新节点放入容器中
        同时，为了防止节点被反复循环，还需要别的一个容器装载被访问的节点，后面搜索时不能再举行访问。
        在这个架构下我们分析经典的基于搜索的算法：
Dijkstra算法每次弹出容器中距离源节点代价最小的节点（距离源节点的代价我们使用g来表示）。
A*算法容器的代价函数记作f，其由两个部分组成，第一个部分是g，即从源节点到当前节点累计代价，第二个部分是h，h代表启发式Heuristic代价，f=g+h。对于h的衡量有多种选择，如L2距离、曼哈顿距离等。
        在原本的A*算法下，hybrid A*不同于启发式函数和动力学可行kinodynamic。在这里采用离散输入量的办法，输入量为加速率a，则位置p可以表示为：

        在a的范围内举行离散采样，根据初始位置和速率可以算出该采样输入a作用下转移的新状态，由此拓展出新的节点来，而且是动力学可行的。
3.代码架构

对于这里的算法结构
首先阐明此中的变量寄义：
P为开集，即优先级队列，C为闭集，即已经举行扩展后的节点容器，nc为优先级队列中队首元素，nodes为拓展出的周围节点，primitives是离散输入作用的一段位移，gc为代价g，fc为总的代价f，整个流程为：
这里对应fast planner中的代码为：path_searching/src/kinodynamic_astar.cpp

1. int KinodynamicAstar::search(Eigen::Vector3d start_pt, Eigen::Vector3d start_v, Eigen::Vector3d start_a,
2.                              Eigen::Vector3d end_pt, Eigen::Vector3d end_v, bool init, bool dynamic, double time_start)//A*搜索
3. {
4.   start_vel_ = start_v;
5.   start_acc_ = start_a;//设置起始点的速度和加速度
7.   PathNodePtr cur_node = path_node_pool_[0];//当前节点nc
8.   cur_node->parent = NULL;//父节点为空
9.   cur_node->state.head(3) = start_pt;//当前节点的位置
10.   cur_node->state.tail(3) = start_v;//当前节点的速度
11.   cur_node->index = posToIndex(start_pt);//将三维位置转化至栅格地图的index
12.   cur_node->g_score = 0.0;//当前节点的gc，即从起点到当前节点的cost，这里起点为0
14.   Eigen::VectorXd end_state(6);
15.   Eigen::Vector3i end_index;
16.   double time_to_goal;
18.   end_state.head(3) = end_pt;
19.   end_state.tail(3) = end_v;
20.   end_index = posToIndex(end_pt);
21.   cur_node->f_score = lambda_heu_ * estimateHeuristic(cur_node->state, end_state, time_to_goal);//计算启发式代价，初始点的fc总代价就是启发式代价，因为gc为0
22.   cur_node->node_state = IN_OPEN_SET;//当前节点的状态，一共两种，OPEN和CLOSED，在CLOSED中的节点是已经访问过的
23.   open_set_.push(cur_node);//OPEN加入当前节点，即优先级队列
24.   use_node_num_ += 1;
26.   if (dynamic)//为节点附加时间维度信息，动态搜索情况
27.   {
28.     time_origin_ = time_start;//记录起始时间
29.     cur_node->time = time_start;
30.     cur_node->time_idx = timeToIndex(time_start);//转换时间索引
31.     expanded_nodes_.insert(cur_node->index, cur_node->time_idx, cur_node);//将 (index, time_idx) 插入到 expanded_nodes_ 中，expanded_nodes_存储已访问节点的集合
32.     // cout << "time start: " << time_start << endl;//使用 (index, time_idx) 作为键值对，确保同一空间位置在不同时间点可以有不同的状态
33.   }
34.   else
35.     expanded_nodes_.insert(cur_node->index, cur_node);//否则将用空间位置索引来记录，静态情况
37.   PathNodePtr neighbor = NULL;
38.   PathNodePtr terminate_node = NULL;
39.   bool init_search = init;
40.   const int tolerance = ceil(1 / resolution_);
42.   while (!open_set_.empty())//主循环，优先级队列不为空
43.   {
44.     cur_node = open_set_.top();//弹出队首元素为当前节点nc
46.     // Terminate?
47.     bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= horizon_;//接近边界
48.     bool near_end = abs(cur_node->index(0) - end_index(0)) <= tolerance &&
49.                     abs(cur_node->index(1) - end_index(1)) <= tolerance &&
50.                     abs(cur_node->index(2) - end_index(2)) <= tolerance;//解决目标节点
52.     if (reach_horizon || near_end)//如果某一个满足
53.     {
54.       terminate_node = cur_node;//当前节点为结束节点
55.       retrievePath(terminate_node);//从结束节点开始向后倒退得到路径
56.       if (near_end)
57.       {
58.         // Check whether shot traj exist
59.         estimateHeuristic(cur_node->state, end_state, time_to_goal);//计算启发式函数，主要得到optimal_time
60.         computeShotTraj(cur_node->state, end_state, time_to_goal);//根据这个optimal_time来计算能否直接生成一条动力学可行的路直接连接到目标点
61.         if (init_search)//如果是初始搜索，会报错，在初始搜索阶段，如果能够直接从起点生成一条连接到目标点的轨迹（shot trajectory），通常是不合理的
62.           ROS_ERROR("Shot in first search loop!");
63.       }
64.     }
65.     if (reach_horizon)//处理各种情况
66.     {
67.       if (is_shot_succ_)
68.       {
69.         std::cout << "reach end" << std::endl;
70.         return REACH_END;
71.       }
72.       else
73.       {
74.         std::cout << "reach horizon" << std::endl;
75.         return REACH_HORIZON;
76.       }
77.     }
79.     if (near_end)
80.     {
81.       if (is_shot_succ_)
82.       {
83.         std::cout << "reach end" << std::endl;
84.         return REACH_END;
85.       }
86.       else if (cur_node->parent != NULL)
87.       {
88.         std::cout << "near end" << std::endl;
89.         return NEAR_END;
90.       }
91.       else
92.       {
93.         std::cout << "no path" << std::endl;
94.         return NO_PATH;
95.       }
96.     }
97.     open_set_.pop();//弹出当前节点
98.     cur_node->node_state = IN_CLOSE_SET;//将当前节点加入CLOSED集合，已访问过的节点集合
99.     iter_num_ += 1;//迭代次数加1
101.     double res = 1 / 2.0, time_res = 1 / 1.0, time_res_init = 1 / 20.0;//输入进行离散化，r取2，则对应5个离散后的输入量，
102.     Eigen::Matrix<double, 6, 1> cur_state = cur_node->state;//当前的状态，位置和速度共6维
103.     Eigen::Matrix<double, 6, 1> pro_state;//扩展后的节点状态
104.     vector<PathNodePtr> tmp_expand_nodes;//临时存储当前节点扩展出的所有子节点
105.     Eigen::Vector3d um;//代表三个维度离散的控制量
106.     double pro_t;//表示扩展出的子节点的时间
107.     vector<Eigen::Vector3d> inputs;
108.     vector<double> durations;
109.     if (init_search)
110.     {
111.       inputs.push_back(start_acc_);
112.       for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_ + 1e-3;
113.            tau += time_res_init * init_max_tau_)
114.         durations.push_back(tau);//如果初次搜索，每次为的时间间隔三1/20，更加精细化的搜索
115.       init_search = false;
116.     }
117.     else
118.     {
119.       for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res)
120.         for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res)
121.           for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res)//对输入进行离散化，每次加max/r，每个离散2r+1个量，一共有(2r+1)3种组合
122.           {
123.             um << ax, ay, az;
124.             inputs.push_back(um);
125.           }
126.       for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_)//对时间进行离散化，控制输入的持续时间集合τ，有多种的时间选择(不只是t=0.5)使得新生成的路径更加多样化，不仅由离散的控制量决定，也可由离散的时间量决定
127.         durations.push_back(tau);//这里由于time_res为1/1.0，所以durations只有一个值max_tau_
128.     }
130.     // cout << "cur state:" << cur_state.head(3).transpose() << endl;
131.     for (int i = 0; i < inputs.size(); ++i)
132.       for (int j = 0; j < durations.size(); ++j)
133.       {
134.         um = inputs[i];
135.         double tau = durations[j];
136.         stateTransit(cur_state, pro_state, um, tau);//调用状态转移函数，控制量um和时间tao作用下，得到新的状态
137.         pro_t = cur_node->time + tau;//新状态的时间为nc当前节点的时间加上tau
139.         Eigen::Vector3d pro_pos = pro_state.head(3);//拓展后节点的位置
141.         // Check if in close set
142.         Eigen::Vector3i pro_id = posToIndex(pro_pos);//拓展后节点的栅格地图的index
143.         int pro_t_id = timeToIndex(pro_t);//拓展后节点的时间index
144.         PathNodePtr pro_node = dynamic ? expanded_nodes_.find(pro_id, pro_t_id) : expanded_nodes_.find(pro_id);//判断是动态搜索还是静态搜索，在expanded_nodes_添加不同的键值对
145.         if (pro_node != NULL && pro_node->node_state == IN_CLOSE_SET)//拓展出来的节点不是空节点，但其位于CLOSED集合内
146.         {
147.           if (init_search)
148.             std::cout << "close" << std::endl;
149.           continue;
150.         }
152.         // Check maximal velocity
153.         Eigen::Vector3d pro_v = pro_state.tail(3);//获取拓展后节点的速度
154.         if (fabs(pro_v(0)) > max_vel_ || fabs(pro_v(1)) > max_vel_ || fabs(pro_v(2)) > max_vel_)//检查新状态的速度是否超出最大速度限制
155.         {
156.           if (init_search)
157.             std::cout << "vel" << std::endl;
158.           continue;
159.         }
161.         // Check not in the same voxel
162.         Eigen::Vector3i diff = pro_id - cur_node->index;// 空间索引差值
163.         int diff_time = pro_t_id - cur_node->time_idx;// 时间索引差值
164.         if (diff.norm() == 0 && ((!dynamic) || diff_time == 0))//diff.norm() == 0在三维空间索引上完全相同，两个节点位于同一个空间体素中；((!dynamic) || diff_time == 0)，静态搜索（!dynamic），不检查时间维度，动态搜索检查时间维度
165.         {
166.           if (init_search)
167.             std::cout << "same" << std::endl;
168.           continue;
169.         }
171.         // Check safety
172.         Eigen::Vector3d pos;
173.         Eigen::Matrix<double, 6, 1> xt;
174.         bool is_occ = false;
175.         for (int k = 1; k <= check_num_; ++k)
176.         {
177.           double dt = tau * double(k) / double(check_num_);//将时间tau分成很多小段
178.           stateTransit(cur_state, xt, um, dt);//每一个小段进行状态转移得到中间过程中的状态
179.           pos = xt.head(3);//获取中间状态的位置
180.           if (edt_environment_->sdf_map_->getInflateOccupancy(pos) == 1 )//检查中间状态是否在障碍物中，1表示在
181.           {
182.             is_occ = true;
183.             break;
184.           }
185.         }
186.         if (is_occ)//路径不安全
187.         {
188.           if (init_search)
189.             std::cout << "safe" << std::endl;
190.           continue;//跳过当前路径扩展
191.         }
193.         double time_to_goal, tmp_g_score, tmp_f_score;
194.         tmp_g_score = (um.squaredNorm() + w_time_) * tau + cur_node->g_score;//拓展后节点的gc值是其父节点的gc值加上从当前节点到拓展节点的cost，cost计算使用ec=(u^2+w_time_)*tau
195.         tmp_f_score = tmp_g_score + lambda_heu_ * estimateHeuristic(pro_state, end_state, time_to_goal);//拓展节点总的cost fc是gc+启发式代价
197.         // Compare nodes expanded from the same parent
198.         bool prune = false;//剪枝的标志位
199.         for (int j = 0; j < tmp_expand_nodes.size(); ++j)//遍历所有拓展节点
200.         {
201.           PathNodePtr expand_node = tmp_expand_nodes[j];
202.           if ((pro_id - expand_node->index).norm() == 0 && ((!dynamic) || pro_t_id == expand_node->time_idx))//检查空间上是否重复或动态模式下时间重复
203.           {
204.             prune = true;//标志位为true，标志需要剪枝
205.             if (tmp_f_score < expand_node->f_score)//判断fc是否比原来的小，如果更小将说明当前的路径更优
206.             {
207.               expand_node->f_score = tmp_f_score;//更新fc
208.               expand_node->g_score = tmp_g_score;//更新gc
209.               expand_node->state = pro_state;//状态量为拓展节点的状态量位置和速度
210.               expand_node->input = um;//节点的输入
211.               expand_node->duration = tau;//节点的持续时间
212.               if (dynamic)
213.                 expand_node->time = cur_node->time + tau;//pro_state
214.             }
215.             break;
216.           }
217.         }
219.         // This node end up in a voxel different from others
220.         if (!prune)//只有在前面的剪枝检查中未被标记为冗余节点（prune == false）时，才进一步处理新扩展的节点
221.         {
222.           if (pro_node == NULL)//拓展节点pro_node，如果新节点 pro_node 尚未存在
223.           {
224.             pro_node = path_node_pool_[use_node_num_];//创建新节点
225.             pro_node->index = pro_id;
226.             pro_node->state = pro_state;
227.             pro_node->f_score = tmp_f_score;
228.             pro_node->g_score = tmp_g_score;
229.             pro_node->input = um;
230.             pro_node->duration = tau;
231.             pro_node->parent = cur_node;
232.             pro_node->node_state = IN_OPEN_SET;
233.             if (dynamic)
234.             {
235.               pro_node->time = cur_node->time + tau;
236.               pro_node->time_idx = timeToIndex(pro_node->time);
237.             }
238.             open_set_.push(pro_node);//将该节点加入open_set中
240.             if (dynamic)//记录一下节点的时间序列键值对
241.               expanded_nodes_.insert(pro_id, pro_node->time, pro_node);
242.             else
243.               expanded_nodes_.insert(pro_id, pro_node);
245.             tmp_expand_nodes.push_back(pro_node);//放在临时存储的节点
247.             use_node_num_ += 1;//内存检查
248.             if (use_node_num_ == allocate_num_)
249.             {
250.               cout << "run out of memory." << endl;
251.               return NO_PATH;
252.             }
253.           }
254.           else if (pro_node->node_state == IN_OPEN_SET)//若节点已经存在在open_set中
255.           {
256.             if (tmp_g_score < pro_node->g_score)//检查fc是否更小，若更优，则更新该节点的信息
257.             {
258.               // pro_node->index = pro_id;
259.               pro_node->state = pro_state;
260.               pro_node->f_score = tmp_f_score;
261.               pro_node->g_score = tmp_g_score;
262.               pro_node->input = um;
263.               pro_node->duration = tau;
264.               pro_node->parent = cur_node;
265.               if (dynamic)
266.                 pro_node->time = cur_node->time + tau;
267.             }
268.           }
269.           else
270.           {
271.             cout << "error type in searching: " << pro_node->node_state << endl;//新节点的状态既不是 NULL，也不是 IN_OPEN_SET
272.           }
273.         }
274.       }
275.     // init_search = false;
276.   }
278.   cout << "open set empty, no path!" << endl;
279.   cout << "use node num: " << use_node_num_ << endl;
280.   cout << "iter num: " << iter_num_ << endl;
281.   return NO_PATH;
282. }

复制代码

4.主要函数

1.核心函数

这里核心函数是三个，Expand()、EdgeCost()和Heuristic()
1.Expand()函数

由节点nc拓展生成小轨迹primitive
轨迹为p(t)，由xyz三个轴的轨迹组成，轨迹由多项式来表示：

由微分平展性可得：

如许得到体系的状态方程为：

得到最后状态方程的解为：

在这里输入量的范围是[−umax, umax]，将其离散化为：
{−umax, − r−1 /r umax, · · · , r−1/r umax, umax}，一共2r+1个离散量，三个维度一共产

种组合，即动作量primitives。
对应的代码是：

1. double res = 1 / 2.0, time_res = 1 / 1.0, time_res_init = 1 / 20.0;//输入进行离散化，r取2，则对应5个离散后的输入量，
2.     Eigen::Matrix<double, 6, 1> cur_state = cur_node->state;//当前的状态，位置和速度共6维
3.     Eigen::Matrix<double, 6, 1> pro_state;//扩展后的节点状态
4.     vector<PathNodePtr> tmp_expand_nodes;//临时存储当前节点扩展出的所有子节点
5.     Eigen::Vector3d um;//代表三个维度离散的控制量
6.     double pro_t;//表示扩展出的子节点的时间
7.     vector<Eigen::Vector3d> inputs;
8.     vector<double> durations;
9.     if (init_search)
10.     {
11.       inputs.push_back(start_acc_);
12.       for (double tau = time_res_init * init_max_tau_; tau <= init_max_tau_ + 1e-3;
13.            tau += time_res_init * init_max_tau_)
14.         durations.push_back(tau);//如果初次搜索，每次为的时间间隔是1/20，更加精细化的搜索
15.       init_search = false;
16.     }
17.     else
18.     {
19.       for (double ax = -max_acc_; ax <= max_acc_ + 1e-3; ax += max_acc_ * res)
20.         for (double ay = -max_acc_; ay <= max_acc_ + 1e-3; ay += max_acc_ * res)
21.           for (double az = -max_acc_; az <= max_acc_ + 1e-3; az += max_acc_ * res)//对输入进行离散化，每次加max/r，每个离散2r+1个量，一共有(2r+1)3种组合
22.           {
23.             um << ax, ay, az;
24.             inputs.push_back(um);
25.           }
26.       for (double tau = time_res * max_tau_; tau <= max_tau_; tau += time_res * max_tau_)//对时间进行离散化，控制输入的持续时间集合τ，有多种的时间选择(不只是t=0.5)使得新生成的路径更加多样化，不仅由离散的控制量决定，也可由离散的时间量决定
27.         durations.push_back(tau);//这里由于time_res为1/1.0，所以durations只有一个值max_tau_
28.     }

复制代码

在这里获得了输入量inputs和连续时间durations，这里除了开始时按照1/20来得到tau，后面的tau都是max_tai_=0.5s，动作量被记录在inputs中。
2.EdgeCost函数

如何盘算每段小轨迹的代价值
每段轨迹的cost盘算函数为：

在这一小段时间τ内，以为离散输入量ud没有发生变革，所以近似其cost为

节点nc对应的gc是从源节点到当前节点的cost和的累计值，因此当前节点的gc为：

3.Heuristic函数

评估一个节点的启发代价值
这里要利用庞特里亚金极小值原理求解：
体系的状态为

，
体系状态方程为

cost为：

构造哈密尔顿函数：

由极值存在的须要条件可得：

得出：

由须要条件可得：

，输入量为a，
因此将H对a求导数可得：

得到：

由a积分得到v和p，再带入T可得：

此时J*是T的函数，求出

得到根Th，则

为启发式代价hc，总代价就为：

此功能在函数estimateHeuristic中实现：这里求根是用的matlab中的特性根法，传入的参数是起点和终点的位置和速率

1. double KinodynamicAstar::estimateHeuristic(Eigen::VectorXd x1, Eigen::VectorXd x2, double& optimal_time)//计算启发式代价，即用庞特里亚金极小值原理求解的最小代价
2. {//x1是起始点的位置和速度向量，共六维，x2是目标点的位置和速度向量，6维
3.   const Vector3d dp = x2.head(3) - x1.head(3);//位置差值，即pug-puc
4.   const Vector3d v0 = x1.segment(3, 3);//起始点的速度
5.   const Vector3d v1 = x2.segment(3, 3);//目标点的速度
7.   double c1 = -36 * dp.dot(dp);//这几个系数是对J(T)函数求导后的多项式系数
8.   double c2 = 24 * (v0 + v1).dot(dp);
9.   double c3 = -4 * (v0.dot(v0) + v0.dot(v1) + v1.dot(v1));
10.   double c4 = 0;
11.   double c5 = w_time_;//即公式中的p(rou)
13.   std::vector<double> ts = quartic(c5, c4, c3, c2, c1);//特征根法求解导数方程的解，得到根T，这里是一个数组ts
15.   double v_max = max_vel_ * 0.5;
16.   double t_bar = (x1.head(3) - x2.head(3)).lpNorm<Infinity>() / v_max;
17.   ts.push_back(t_bar);//估算一个最小值，用dp中最大的一个维度除以最大速度的一半，用于后续进行比较
19.   double cost = 100000000;//记录最小的cost，初始赋值为一个很大的数
20.   double t_d = t_bar;//用于记录最小cost对应的根
22.   for (auto t : ts)//便利所有的解
23.   {
24.     if (t < t_bar)
25.       continue;
26.     double c = -c1 / (3 * t * t * t) - c2 / (2 * t * t) - c3 / t + w_time_ * t;//J(T)函数，计算cost
27.     if (c < cost)
28.     {
29.       cost = c;
30.       t_d = t;
31.     }
32.   }
34.   optimal_time = t_d;//容器optimal_time中记录了最优的时间Th
36.   return 1.0 * (1 + tie_breaker_) * cost;//加入一个微小的偏移量，用于打破相同代价的情况下的平局
37. }

复制代码

2.其他函数

同时，还有以下几个函数值得注意：
1.AnalyticExpand()

由于离散化的 control input，很难在 goal state 中有一个 primitive end，即我们在nc点通过离散输入的作用，下一个状态刚好到达了goal点。为了赔偿它并加快搜索速率，我们引入了一个分析扩展方案。
主要思路是，我们弹出节点nc，用之前相同的方法算一条从当前点nc到目标点xg的路径，如果这条路径是安全的（没有障碍物碰撞）而且是动力学可行的，那么我们就提前停止搜索。
对应的代码是这一段：

1. // Terminate?
2.     bool reach_horizon = (cur_node->state.head(3) - start_pt).norm() >= horizon_;//接近边界
3.     bool near_end = abs(cur_node->index(0) - end_index(0)) <= tolerance &&
4.                     abs(cur_node->index(1) - end_index(1)) <= tolerance &&
5.                     abs(cur_node->index(2) - end_index(2)) <= tolerance;//解决目标节点
7.     if (reach_horizon || near_end)//如果某一个满足
8.     {
9.       terminate_node = cur_node;//当前节点为结束节点
10.       retrievePath(terminate_node);//从结束节点开始向后倒退得到路径
11.       if (near_end)
12.       {
13.         // Check whether shot traj exist
14.         estimateHeuristic(cur_node->state, end_state, time_to_goal);//计算启发式函数，主要得到optimal_time
15.         computeShotTraj(cur_node->state, end_state, time_to_goal);//根据这个optimal_time来计算能否直接生成一条动力学可行的路直接连接到目标点
16.         if (init_search)//如果是初始搜索，会报错，在初始搜索阶段，如果能够直接从起点生成一条连接到目标点的轨迹（shot trajectory），通常是不合理的
17.           ROS_ERROR("Shot in first search loop!");
18.       }
19.     }
20.     if (reach_horizon)//处理各种情况
21.     {
22.       if (is_shot_succ_)
23.       {
24.         std::cout << "reach end" << std::endl;
25.         return REACH_END;
26.       }
27.       else
28.       {
29.         std::cout << "reach horizon" << std::endl;
30.         return REACH_HORIZON;
31.       }
32.     }
34.     if (near_end)
35.     {
36.       if (is_shot_succ_)
37.       {
38.         std::cout << "reach end" << std::endl;
39.         return REACH_END;
40.       }
41.       else if (cur_node->parent != NULL)
42.       {
43.         std::cout << "near end" << std::endl;
44.         return NEAR_END;
45.       }
46.       else
47.       {
48.         std::cout << "no path" << std::endl;
49.         return NO_PATH;
50.       }
51.     }

复制代码

首先检查是否接近目标点，使用标识位near_end，盘算启发式代价得到最优时间Th，通过这个Th盘算从当前点到终点直接连线的路径，详细是在函数computeShort内完成的。

1. bool KinodynamicAstar::computeShotTraj(Eigen::VectorXd state1, Eigen::VectorXd state2, double time_to_goal)//计算一个从当前节点到目标节点的直接路径
2. {
3.   /* ---------- get coefficient ---------- */
4.   const Vector3d p0 = state1.head(3);
5.   const Vector3d dp = state2.head(3) - p0;
6.   const Vector3d v0 = state1.segment(3, 3);
7.   const Vector3d v1 = state2.segment(3, 3);
8.   const Vector3d dv = v1 - v0;
9.   double t_d = time_to_goal;
10.   MatrixXd coef(3, 4);//三维轨迹的多项式系数存储为矩阵
11.   end_vel_ = v1;
13.   Vector3d a = 1.0 / 6.0 * (-12.0 / (t_d * t_d * t_d) * (dp - v0 * t_d) + 6 / (t_d * t_d) * dv);
14.   Vector3d b = 0.5 * (6.0 / (t_d * t_d) * (dp - v0 * t_d) - 2 / t_d * dv);
15.   Vector3d c = v0;
16.   Vector3d d = p0;
18.   // 1/6 * alpha * t^3 + 1/2 * beta * t^2 + v0  使用三次多项式描述轨迹p(t)
19.   // a*t^3 + b*t^2 + v0*t + p0   a，b，c，d多项是
20.   coef.col(3) = a, coef.col(2) = b, coef.col(1) = c, coef.col(0) = d;//对应每一列是ax，ay，az；bx，by，bz等
22.   Vector3d coord, vel, acc;
23.   VectorXd poly1d, t, polyv, polya;
24.   Vector3i index;
26.   Eigen::MatrixXd Tm(4, 4);
27.   Tm << 0, 1, 0, 0, 0, 0, 2, 0, 0, 0, 0, 3, 0, 0, 0, 0;//用于求导数的
29.   /* ---------- forward checking of trajectory ---------- */
30.   double t_delta = t_d / 10;//td是optimal_time，将其分为一段一段，对每个时间点t，计算对应的轨迹位置、速度、加速度，并进行检查
31.   for (double time = t_delta; time <= t_d; time += t_delta)
32.   {
33.     t = VectorXd::Zero(4);//t是一个多项式，表示[1,t,t2,t3]多项式
34.     for (int j = 0; j < 4; j++)
35.       t(j) = pow(time, j);
37.     for (int dim = 0; dim < 3; dim++)
38.     {
39.       poly1d = coef.row(dim);//从0-2行的系数，对应x，y，z
40.       coord(dim) = poly1d.dot(t);//计算位置
41.       vel(dim) = (Tm * poly1d).dot(t);//计算速度
42.       acc(dim) = (Tm * Tm * poly1d).dot(t);//计算加速度
44.       if (fabs(vel(dim)) > max_vel_ || fabs(acc(dim)) > max_acc_)//检查速度和加速度是否超限
45.       {
46.         // cout << "vel:" << vel(dim) << ", acc:" << acc(dim) << endl;
47.         // return false;
48.       }
49.     }
51.     if (coord(0) < origin_(0) || coord(0) >= map_size_3d_(0) || coord(1) < origin_(1) || coord(1) >= map_size_3d_(1) ||
52.         coord(2) < origin_(2) || coord(2) >= map_size_3d_(2))//检查轨迹是否超出地图边界
53.     {
54.       return false;
55.     }
57.     // if (edt_environment_->evaluateCoarseEDT(coord, -1.0) <= margin_) {
58.     //   return false;
59.     // }
60.     if (edt_environment_->sdf_map_->getInflateOccupancy(coord) == 1)//检查轨迹位置是否碰撞障碍物，碰到障碍物则无法生成
61.     {
62.       return false;
63.     }
64.   }
65.   //轨迹生成结果
66.   coef_shot_ = coef;//系数
67.   t_shot_ = t_d;//最优时间
68.   is_shot_succ_ = true;//能否生成直接连接轨迹标志位
69.   return true;
70. }

复制代码

传入的参数是起点和终点的状态，包括位置和速率，同时传入轨迹的时间，是之前利用极小值原理盘算出来的最优时间。
系数存放在coef内，三列对应xyz三个轴，4代表多项式系数，对应多项式为：

t从t_delta开始每次增长t_delta，在每个时间点检查速率、加速率是否动力学可行，同时检查位置是否在地图范围内和是否有障碍物，最终返回生成轨迹成功与否。
2.Prune()函数

盘算出根据动作量primitives拓展出来的节点nodes，并举行剪枝操作留下复合要求的节点。
对应代码为：

1. // Check safety
2.         Eigen::Vector3d pos;
3.         Eigen::Matrix<double, 6, 1> xt;
4.         bool is_occ = false;
5.         for (int k = 1; k <= check_num_; ++k)
6.         {
7.           double dt = tau * double(k) / double(check_num_);//将时间tau分成很多小段
8.           stateTransit(cur_state, xt, um, dt);//每一个小段进行状态转移得到中间过程中的状态
9.           pos = xt.head(3);//获取中间状态的位置
10.           if (edt_environment_->sdf_map_->getInflateOccupancy(pos) == 1 )//检查中间状态是否在障碍物中，1表示在
11.           {
12.             is_occ = true;
13.             break;
14.           }
15.         }
16.         if (is_occ)//路径不安全
17.         {
18.           if (init_search)
19.             std::cout << "safe" << std::endl;
20.           continue;//跳过当前路径扩展
21.         }
23.         double time_to_goal, tmp_g_score, tmp_f_score;
24.         tmp_g_score = (um.squaredNorm() + w_time_) * tau + cur_node->g_score;//拓展后节点的gc值是其父节点的gc值加上从当前节点到拓展节点的cost，cost计算使用ec=(u^2+w_time_)*tau
25.         tmp_f_score = tmp_g_score + lambda_heu_ * estimateHeuristic(pro_state, end_state, time_to_goal);//拓展节点总的cost fc是gc+启发式代价
27.         // Compare nodes expanded from the same parent
28.         bool prune = false;//剪枝的标志位
29.         for (int j = 0; j < tmp_expand_nodes.size(); ++j)//遍历所有拓展节点
30.         {
31.           PathNodePtr expand_node = tmp_expand_nodes[j];
32.           if ((pro_id - expand_node->index).norm() == 0 && ((!dynamic) || pro_t_id == expand_node->time_idx))//检查空间上是否重复或动态模式下时间重复
33.           {
34.             prune = true;//标志位为true，标志需要剪枝
35.             if (tmp_f_score < expand_node->f_score)//判断fc是否比原来的小，如果更小将说明当前的路径更优
36.             {
37.               expand_node->f_score = tmp_f_score;//更新fc
38.               expand_node->g_score = tmp_g_score;//更新gc
39.               expand_node->state = pro_state;//状态量为拓展节点的状态量位置和速度
40.               expand_node->input = um;//节点的输入
41.               expand_node->duration = tau;//节点的持续时间
42.               if (dynamic)
43.                 expand_node->time = cur_node->time + tau;//pro_state
44.             }
45.             break;
46.           }
47.         }

复制代码

首先对通过primitives到目标点的路径举行障碍物检查，这里用的是采样的方法，在路径中间一小段时间举行采样，检查其是否位于障碍物内。
对通过安全性检查的节点举行剪枝操作，如果检测到当前扩展的节点与已有节点相同（位置和时间），而且当前路径的总代价更小，则更新节点的 f_score、g_score、state、input 和 duration 等信息并继续。
3.扩展新的节点

接着我们来看如何举行路径扩展 中的新节点创建和更新
对应代码如下：

1. // This node end up in a voxel different from others
2.         if (!prune)//只有在前面的剪枝检查中未被标记为冗余节点（prune == false）时，才进一步处理新扩展的节点
3.         {
4.           if (pro_node == NULL)//拓展节点pro_node，如果新节点 pro_node 尚未存在
5.           {
6.             pro_node = path_node_pool_[use_node_num_];//创建新节点
7.             pro_node->index = pro_id;
8.             pro_node->state = pro_state;
9.             pro_node->f_score = tmp_f_score;
10.             pro_node->g_score = tmp_g_score;
11.             pro_node->input = um;
12.             pro_node->duration = tau;
13.             pro_node->parent = cur_node;
14.             pro_node->node_state = IN_OPEN_SET;
15.             if (dynamic)
16.             {
17.               pro_node->time = cur_node->time + tau;
18.               pro_node->time_idx = timeToIndex(pro_node->time);
19.             }
20.             open_set_.push(pro_node);//将该节点加入open_set中
22.             if (dynamic)//记录一下节点的时间序列键值对
23.               expanded_nodes_.insert(pro_id, pro_node->time, pro_node);
24.             else
25.               expanded_nodes_.insert(pro_id, pro_node);
27.             tmp_expand_nodes.push_back(pro_node);//放在临时存储的节点
29.             use_node_num_ += 1;//内存检查
30.             if (use_node_num_ == allocate_num_)
31.             {
32.               cout << "run out of memory." << endl;
33.               return NO_PATH;
34.             }
35.           }
36.           else if (pro_node->node_state == IN_OPEN_SET)//若节点已经存在在open_set中
37.           {
38.             if (tmp_g_score < pro_node->g_score)//检查fc是否更小，若更优，则更新该节点的信息
39.             {
40.               // pro_node->index = pro_id;
41.               pro_node->state = pro_state;
42.               pro_node->f_score = tmp_f_score;
43.               pro_node->g_score = tmp_g_score;
44.               pro_node->input = um;
45.               pro_node->duration = tau;
46.               pro_node->parent = cur_node;
47.               if (dynamic)
48.                 pro_node->time = cur_node->time + tau;
49.             }
50.           }
51.           else
52.           {
53.             cout << "error type in searching: " << pro_node->node_state << endl;//新节点的状态既不是 NULL，也不是 IN_OPEN_SET
54.           }
55.         }

复制代码

其代码的架构为：

1. if (!prune)
2. {
3.   if (pro_node == NULL)
4.   {
5.     // 创建新节点，表示如果当前扩展的节点还不存在（即是一个新节点），则需要创建一个新的节点。
6.   }
7.   else if (pro_node->node_state == IN_OPEN_SET)
8.   {
9.     // 如果节点已存在并在OPEN集合中，检查路径是否更优
10.   }
11.   else
12.   {
13.     std::cout << "error type in searching: " << pro_node->node_state << std::endl;
14.   }
15. }

复制代码

第一段代码将扩展的节点加入到OPEN集合中
核心在于第二段，从OPEN集合中找到一个fc最优的节点，将当前节点设置为该节点的父节点，并更新其各个代价信息等。
对应这一段功能

总体的代码流程如下：

1. 1. 初始化
2. 输入：起始点位置、速度、加速度，目标点位置、速度。
3. 设置起始节点：
4. 设定起始点的 位置、速度、加速度。
5. 计算起始节点的 代价（g_score） 和 启发式代价（f_score），并加入 开放集（open_set）。
6. 2. 主循环：处理开放集中的节点
7. 判断开放集是否为空：
8. 如果为空，算法结束，返回 NO_PATH。
9. 如果非空，弹出 代价最小的节点 cur_node。
10. 3. 检查终止条件
11. 接近边界：检查当前节点是否接近预定的边界（例如搜索区域的最大范围），若是，设置标志为 reach_horizon。
12. 接近目标节点：检查当前节点是否接近目标节点，若是，设置标志为 near_end。
13. 如果满足终止条件：
14. 从目标节点开始倒退，找到路径并返回 REACH_END。
15. 如果接近边界但未到达目标，返回 REACH_HORIZON。
16. 4. 路径扩展
17. 对于每个可能的 控制输入 和 持续时间，计算新的状态（位置、速度等）。
18. 计算扩展节点的代价：计算从当前节点到扩展节点的代价（g_score 和 f_score）。
19. 5. 剪枝操作
20. 检查扩展的节点是否已经处理过：
21. 如果扩展的节点已经在 CLOSED 集合 中，跳过该节点。
22. 如果扩展的节点已经在 OPEN 集合 中，并且当前路径代价更优，则更新该节点。
23. 6. 安全性检查
24. 将路径分成小段，检查每个小段是否碰到障碍物：
25. 如果路径中的任意小段与障碍物碰撞，则标记该路径为 不安全，跳过该路径扩展。
26. 7. 节点更新
27. 新节点创建：如果扩展的节点是新的，创建该节点并加入 OPEN 集合，同时更新节点的状态和代价。
28. 更新已存在的节点：如果节点已经在 OPEN 集合 中，并且当前路径更优，则更新该节点的信息（状态、代价等）。
29. 8. 内存管理
30. 检查节点是否超出预定的最大数量。如果超出，则返回 NO_PATH，表示内存不足。
31. 9. 返回结果
32. 如果找到路径，返回 REACH_END。
33. 如果没有路径可扩展，返回 NO_PATH。
34. 结束条件：
35. 终止条件检查、路径扩展、剪枝、安全性检查等操作反复进行，直到找到路径或者无法找到路径为止。

复制代码

免责声明：如果侵犯了您的权益，请联系站长，我们会及时删除侵权内容，谢谢合作！更多信息从访问主页：qidao123.com:ToB企服之家，中国第一个企服评测及商务社交产业平台。

---

欢迎光临 IT评测·应用市场-qidao123.com技术社区 (https://dis.qidao123.com/)

Powered by Discuz! X3.4