运动规划算法工作进展记录

算法整理

Search-based path finding

1. Dijkstra algorithm：

2. Astar algorithm：
Astar算法在Dijkstra算法上加入了启发式函数h(n)，在弹出节点时选择f(n)=g(n)+h(n)最小的节点弹出，增强了搜索的目的性。

3. JPS algorithm：
JPS算法，即跳点算法，在Astar的基础上，改变寻找邻居节点的方式：即区别仅在于拓展节点的方式由全部周围节点改为仅拓展起作用的关键节点。

3D JPS算法：

具体参考论文：Planning Dynamically Feasible Trajectories for Quadrotors using Safe Flight Corridors in 3-D Complex Environments, Sikang Liu, RAL 2017
在复杂环境中，JPS比Astar算法性能会有明显提升。

JPS与Astar算法的对比：
对于复杂环境，JPS在绝大多数情况下性能远超Astar，JPS可以快速找到关键节点，省去将稠密的邻居节点全部加入open list的过程，对open list的维护代价会小很多。
但如果地图范围很大，很空旷，JPS性能可能比Astar要差，而且在带有权重的图中，JPS不能使用，而Astar可以。

JPS是一种系统的实现Tie Breaker（消除对称性）的方法

例：使用Astar用时0.3ms，操作次数69。

使用JPS用时0.4ms，操作次数1762，区别很明显。

但是我的仿真结果却发现了问题，JPS总是比Astar花费更多，之后重新梳理代码，发现if语句条件判断有误，修改之后结果如下：

可以发现，一些情况下，Astar比JPS更优(如前三种情况)；一些情况下JPS比Astar更优(如最后一种情况)，具体哪种情况，可能要通过实践确定。

Sample-based path finding

基本思想：在空间中随机洒点，用线段将点连接，构建路图/树来表示整个空间。

1. PRM-概率路图
Learning phase+Query phase

优点：

• 是概率完备的，即如果有解则一定能找到一条路径；
• 相对A*，在高维空间或复杂环境非常高效。

缺点：

• 不满足机器人动力学约束，考虑如何构造曲线；
• 不够有效，两阶段需要浪费很多时间。

改进——Lazy collision-checking

• 障碍检测：撒点但不检测障碍物做连线，直接做路径规划，检测是否可行，如果不可行则删除不可行线段并继续寻找路径，直到找到可行路径。节省大量障碍检测的时间。

2. RRT
Rapidly-exploring Random Tree

• 首先在空间M中通过sample函数采样（随机/偏置）得到采样点Xrand；
• 在树T上找到离Xrand最近的一个点Xnear；
• 从Xnear到Xrand移动一个距离StepSize得到点Xnew；
• Xnew和Xnear连线得到边Ei；
• 判断，如果Ei和Xnew是CollisionFree的，则将Ei和Xnew添加到树T中；
• 至此完成一次树的扩展。

优点：

• 针对性找到起点到终点的路径；

缺点：

• 找到的路径并不是最优的，一旦终点被加入树，则停止生长。
• 并不高效

改进：

• 提高效率——Kd-tree：针对Near()函数改进，如何快速找到最近的节点；
• 提高效率——Bidirectional RRT/RRTconnect：从起点和终点同时构造树，一次采样完成两棵树的生长，对于终点在narrow passage 中的情况很有帮助；
• 针对生成路径不是最优——RRT*
• 针对不满足动力学约束——Kinodynamic-RRT*
• 将移动机器人设置为起点，即start在不断变化——Anytime-RRT*

3. RRT*

当找到Xnew后，判断，如果Xnew是CollisionFree的，则：

• NearC寻找Xnew的附近节点；
• 在近邻节点中选择父节点，依据是由Xnew到start经由哪个节点cost最小就是父节点；
• 将选择的父节点与Xnew连接的边加入树，完成一次树的扩展；
• rewire进行剪枝操作，简化树结构。

总之，RRT*的剪枝操作可以对找到的路径不断优化，这便是RRT*相比RRT最大的改进。

关于我的仿真结果：蓝色为生成的路线，cost在终端输出。

4. Kinodynamic-RRT*
将线段平滑为曲线，以满足机器人动力学约束。针对steer()函数进行更改。

5. Informed RRT*

Informed RRT*将采样范围限制在一个椭圆内，节省搜索时间。
以start和end为椭圆的焦点，以生成路径的长度作为点到两个焦点的距离之和，随着路径优化，椭圆边窄，搜索范围变小。
在受限空间内快速采样，并且受限空间可以快速缩小。

6. Cross-entropy motion planning

• 生成一条路径；
• 在6个节点周围构建高斯圆，在6个圆内采样；
• 新生成了3条路径；
• 将三条轨迹的所有节点求均值，在均值点继续生成多高斯模型并采样；
• 路径不断优化。

Kinodynamic path finding

• 受到高阶模型微分约束——机器人不再是简单的可以随意运动的质点；
• 受力的约束——加速度。

1. lattice graph

OBVP
作业参考：基于ROS的Local Lattice Planner算法代码学习
理论学习参考：Optimal Boundary Value Problem (OBVP) 学习笔记和(三)路径规划算法—OBVP例子。
Boundary value problem(BVP)：给出机器人在起始点与终止点的状态，设计出一条状态转移的轨迹，是stated sampled lattice planning的基础。
Optimal boundary value problem：按照某种原则设计出一条最优轨迹。
设置终止位置和终止状态，属于固定边界值条件，如图中绿色箭头所指：

OBVP规划的结果如下，绿色为最优轨迹，蓝色为非最优轨迹，事实上红色为会发生碰撞的轨迹，在下面情况下并未发生：

终端信息：

本周阅读了两篇论文：

下周计划阅读lattice planning的论文：

社会实践

Trajectory generation

1. minimum snap
第一个后端轨迹生成算法
作业参考：基于ROS的Minimun Snap闭式求解算法代码学习
理论学习参考：
https://gitcode.net/mirrors/symao/minimum_snap_trajectory_generation?utm_source=csdn_github_accelerator


绿色轨迹为前端规划的路径，红色路径为后端轨迹生成符合机器人动力学约束的平滑轨迹。