基于满足动力学约束、采样、搜索的路径规划算法介绍

---

总结课程《深蓝学院移动机器人路径规划》

Kinodynamic : Kinematic + Dynamic

1.State Lattice Planning

汽车质点模型不满足动力学约束，需要满足两点之间是可行的运动连接。 1.离散机器人控制空间

2.离散机器人状态空间 考虑动力学约束后，建立一个机器人模型，由状态和输入描述。$\dot{S}=f(s,u)$ 当给系统不同的输入，持续一定的时间T。（离散机器人控制空间） （没有任务导向性，效率相对低。） 当选择两个状态，找到两个状态之间的输入控制（u，T）。（离散机器人状态空间） （导向性，难以实现）

---

Sample in control space 当确定末状态后，解算中间状态。状态转移方程：

---

2.1 Boundary Value Problem

2.2 Optimal Boundary Value Problem (OBVP)

从一个状态到另一个状态，使得其$J_k$最小值。 $J_{\sum}=\sum_{k=1}^3{J_k},J_k={1 \over T}\begin{array}{cc} \int_0^T{ j_k(t)^2 dt }\end{array}$状态：$S_k=(p_k,v_k,a_k)$ ，输入$u_k=j_k$ 系统模型：$\dot{S_k}=f_s(s_k,u_k)=(v_k,a_k,j_k)$

利用极小值原理： 引入协态costate：$\lambda=(\lambda_1,\lambda_2,\lambda_3)$，由系统变量决定。 定义Hamiltonian function：$H(s,u,\lambda)={1\over T}j^2+\lambda^Tf_s(s,u)$得到最优的状态量$s$，最优输入$u$。

3.Planning in Frenet-serret Frame

4.混合A* （Hybrid A*）

在每个栅格中保证只存在一个节点。 启发函数的设计：

5.基于运动学约束的RRT*

1.(Sample)在整个状态空间采样，包括位置和速度。 2.(Near)利用optimal control将新找到的节点和邻近节点进行OBVP连接。 3.(ChooseParent)以最优控制cost指标为半径的球，范围内的节点。 4.(find near nodes efficiently)后向可达集 5.(Rewire)前向可达集

---

总结课程《深蓝学院移动机器人路径规划》

1.概率路图 Probabilistic Road Map

分为学习阶段，查询阶段。 通过采样的方式代替整个2D栅格图，通过一个图结构简化地图。 学习阶段：用一定的采样方式撒点，把落在障碍物中的点剔除，完成采样。 采样后通过线段连接采样点，连接时需要考虑距离限制以及障碍物限制。 查询阶段：构建完成一张图后，可以通过D或者A*去查找路径。

概率完备，高效。

2.RRT(Rapidly-exploring Random Tree)

相对于PRM，更针对的去寻找从起点到终点的路径，但是其并不是一条最佳的路径，也不高效。

3.RRT*

1.选择一条从起始点到$X_{new}$最短(代价最小)的路径。 2.剪枝：如果其他节点从$X_{new}$节点到达起始点更短的话，则需要被修改。(例如$X_{2}$父节点修改为$X_{new}$)

4.Kinodynamic-RRT*

用曲线代替直线连接点$X_{near}-X_{new}$。此时原本被舍弃的线段，可能因为变成曲线而不会经过障碍物。

5.Anytime-RRT*

在机器人运动的过程中，RRT*在不断更新，即起始点不断更新为机器人当前位置点。 能够更好的适应变化的环境。

6.Informed RRT*

把采样的过程限制在一个椭圆中，该椭圆由已经产生的路径决定。 以起始点、终止点作为椭圆的焦点，以生成轨迹的长度作为椭圆上点到达俩焦点的距离之和(常数)。

7.Cross-entropy motion planning

在路径点附近多高斯采样，生成多条轨迹，通过求均值可得到新的多高斯采样，生成新的路径。

---

总结课程《深蓝学院移动机器人路径规划》

深度优先遍历：栈 广度优先遍历：队列

1.Dijkstra

Dijkstra = 广度优先遍历 + 权重

在Dijkstra 算法中，相当于在每条路径上添加了权重。在每次弹出扩展点的时候，需要计算代价函数g(n)最小的节点。如果通过该节点所扩展得到的邻居节点并不在容器中，则需要直接添加进去。如果已经存在在容器中，则需要对比从该节点出发所得到的代价是否比其原有代价更小。

代价函数：f(n)=g(n)

2. A*

A*= Dijkstra + 启发式

Dijkstra算法仅仅考虑了从起始点出发的累计代价g(n)，当考虑从路标点到目标点之间的代价h(n)，即类似贪心算法，则为A*算法。 此时与Dijkstra算法考虑的代价则不同，其他则一样。

代价函数：f(n)=g(n)+h(n)

如何保证A*算法的最优质： 估计的启发式函数cost < 真实cost ， 即对于任何一个节点，h(n)<h*(n)。例如使用欧式距离作为启发式函数！

3.Weighted A*

如果h(n)>=h*(n)，可以理解为A*算法在往贪心算法演变，即尽可能的往目标点移动。

代价函数：f(n)=g(n)+$\varepsilon$h(n) 当$\varepsilon>1$，则朝着目标点更近的方向规划 当$\varepsilon=0$， Dijkstra 算法 当$\varepsilon=1$， A*算法 并且存在：当前cost <= $\varepsilon$最优cost

4.最佳启发函数

启发式函数代价需要小于真实的代价，那么当其相等的时候，就是最佳的启发式函数。例如在一个二维的栅格地图中： 如果用两者的最小距离，即对角的启发式函数，此时所遍历的节点明显减少，三维同理。 dx=abs(node.x −goal.x) $$$$dy=abs(node.y −goal.y) $$$$h=(dx+dy)+(√2−2)∗min(dx,dy)

5.打破平衡 Tie Breaker

在一次规划中，存在很多代价一样但结果不一样的最优路径。因此会沿着多条路径同时进行扩展。 当打破对称性之后(右图)，可以明显看到所便利的节点减少。 打破对称性的方法，对相等的cost进行极小的放大。 h = h × 1.0 + p$$$$p <{minimum cost of one step \over expected maximum path cos} 虽然此时启发式函数稍微大于真实的代价，但是并不影响实际的规划效果。

6.Jump Point Search

系统性的消灭对称性问题！ JSP = A* + 消灭对称性问题

---

Look Ahead Rule 如果从该节点出发，到达其父节点可到达的节点的代价，大于等于从其父节点出发的代价，则该目标节点不需要考虑。 即上图中白色节点，则需要被考虑。红色节点因为存在障碍物需要被强制考虑。

Jumping Rules 直线跳跃优先级大于对角线跳跃