基于满足动力学约束下的路径规划(State Lattice Planning,Kinodynamic-RRT*)

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

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=\frac{1}{T}\begin{array}{c}{\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 )=\frac{1}{T}{j}^2+{\lambda }^T{f}_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)前向可达集