(三)路径规划算法---基于Kinodynamic的路径规划简单介绍

基于Kinodynamic的路径规划简单介绍

Kinodynamic=Kinematic+Dynamic，其中

Kinematic：运动学约束，例如避障

Dynamic：动力学约束，例如加速度(acceleration)、力(force)

在介绍Kinodynamic之前，先简单介绍现实中常见的机器人模型

1. 机器人模型

1.1 Unicycle Model

Unicycle Model中文译为自行车模型，模型表示如下图。
$$\left(\begin{array}{c}\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right)=\left(\begin{array}{c}\cos (\theta )\\ \sin (\theta )\\ 0\end{array}\right)v+\left(\begin{array}{c}0\\ 0\\ 1\end{array}\right)\omega$$

模型推导这里不在赘述，详情参见：http://planning.cs.uiuc.edu/node659.html

1.2 Differential Drive Model

Differential Drive Model中文名为差速模型，模型表示如下图
$$\begin{gathered} \left(\begin{array}{c}l\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right)=\left(\begin{array}{c}\frac{r}{2}({\omega }_l+{\omega }_r)\cos (\theta )\\ \frac{r}{2}({\omega }_l+{\omega }_r)\sin (\theta )\\ \frac{r}{L}({\omega }_r-{\omega }_l)\end{array}\right) \\ v=\frac{r}{2}({\omega }_l+{\omega }_r)\cos (\theta ) \\ \omega =\frac{r}{L}({\omega }_r-{\omega }_l) \end{gathered}$$

模型推导这里不在赘述，详情参见：http://planning.cs.uiuc.edu/node659.html

1.3 Car Model

Car Model就是我们经常说的阿克曼模型，模型表示如下图
$$\left(\begin{array}{c}l\dot{x}\\ \dot{y}\\ \dot{\theta }\end{array}\right)=\left(\begin{array}{c}v\cos (\theta )\\ v\sin (\theta )\\ \frac{v}{l}\tan (\varphi )\end{array}\right)$$

Simple Car Model：$\left|v\right|\le v_{\max },\left|\varphi \right|\le {\varphi }_{\max }\le \frac{\pi }{2}$

Reeds Shepp Car： v ∈ { − v max ⁡ , v max ⁡ } , ∣ ϕ ∣ ≤ ϕ max ⁡ ≤ π 2 v \in \{ - {v_{\max }},{v_{\max }}\} ,\left| \phi \right| \le {\phi _{\max }} \le \frac{\pi }{2} v∈{−vmax​,vmax​},∣ϕ∣≤ϕmax​≤2π​

Dubbin Car：$v=v_{\max },\left|\varphi \right|\le {\varphi }_{\max }\le \frac{\pi }{2}$

模型推导这里不在赘述，详情参见：http://planning.cs.uiuc.edu/node658.html

https://blog.csdn.net/u013914471/article/details/82968608

---

2. State Lattice Planning

假设机器人并不能只表示一个质点，而需要充分考虑机器人的动力学和运动学约束。因此我们需要一个可行的运动连接的图，这种图也被称为Lattice Graph。建立这种图有两种方法，可分为

Forward direction：在控制空间(control space)中进行采样，将控制空间离散，驱动机器人向前运动,以得到一个一个的可行的运动连接。

例如：假设机器人在某一个栅格节点，我们可以选择机器人的控制量，所谓控制量就比如车的油门、方向盘等。在以往的Astar、JPS算法中，控制量简单化了，即机器人类似为质点，可以随意运动，控制量为4方向或者8方向运动。

Reverse direction：在状态空间(state space)中进行采样，将状态空间进行离散，找一条从当前状态到某一状态的可行的运动连接。

例如：在前一章的RRT算法中，将机器人简单考虑成质点，不考虑机器人的动力学约束，此时机器人的状态就是$(x,y)$，我们不段采样环境中的$(x,y)$,用于反算寻找路径。

2.1 控制空间采样

控制空间属于前向积分，通过选择一个输入$u$（比如油门、方向盘）等，保持一段时间$T$后，所处的状态就是机器人下次可能出现的状态。这种方式缺乏任务导向，规划效率并不高(并不是速度不快)。

举个例子：

对于一个无人机机器人状态模型
$$\begin{gathered} \dot{s}=f(s,u)=A\bullet s+B\bullet u \\ 状态量：s=\left(\begin{array}{c}x\\ y\\ z\\ \dot{x}\\ \dot{y}\\ \dot{z}\end{array}\right) \\ 输入量：u=\left(\begin{array}{c}\ddot{x}\\ \ddot{y}\\ \ddot{z}\end{array}\right) \end{gathered}$$

其中状态量$s$为无人机三轴的位置和速度信息，输入量$u$为无人机三轴的加速度。

当三轴的速度初始值$v_0=(v_0^x,v_0^y,v_0^z)=(1,0,0{)}^T$时，经过不同的输入量$u=(a_x,a_y,a_z{)}^T$，一段时间$t$后，无人机新的状态$s_1=(x,y,z,\dot{x},\dot{y},\dot{z})=(p_1^x,p_1^y,p_1^z,v_1^x,v_1^y,v_1^z)$为
$$\begin{gathered} p_1^x=p_0^x+v_0^x\times t+\frac{1}{2}\times a_x\times t^2 \\ {p}_1^y=p_0^y+v_0^y\times t+\frac{1}{2}\times a_y\times t^2 \\ {p}_1^z=p_0^z+v_0^z\times t+\frac{1}{2}\times a_z\times t^2 \\ {v}_1^x=v_0^x+a_x\times t \\ {v}_1^y=v_0^y+a_y\times t \\ {v}_1^z=v_0^z+a_z\times t \end{gathered}$$
注意:上述变量中初变量$s_0=(p_0^x,p_0^y,p_0^z,v_0^x,v_0^y,v_0^z)$、时间$t$固定已知，输入变量$u$未知，为了方便显示，将三维将为二维,将输入量$u$(加速度)进行9等分，则生成以下曲线图，不同曲线输入变量$u$不同，同一条曲线在$[0,t]$时间内呈现二次曲线特征。

类似的若输入量为jerk(对加速度进行求导)，且同时进行9等分，则生成以下三次曲线图

通过无人机状态模型，则系数矩阵A和B表示为
$$\begin{gathered} A=\left[\begin{array}{cccccc}0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right] \\ B=\left[\begin{array}{ccc}0& 0& 0\\ 0& 0& 0\\ 0& 0& 0\\ 1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right] \end{gathered}$$
其中系数矩阵A为幂零矩阵(nilpotnet)，它的高次幂会在某次变成零。

通过零状态响应$F(t)$和零输入响应$G(t)$得出
$$s(t)=\underset{F(t)}{\underbrace{e^{At}\times s_0}}+\underset{G(t)}{\underbrace{[{\int }_0^t{e}^{A(t-\sigma )}Bd\sigma ]\times u_m}}$$
将状态转移矩阵$e^{At}$进行泰勒公式展开得：
$$e^{At}=I+\frac{At}{1!}+\frac{(At{)}^2}{2!}+\frac{(At{)}^3}{3!}+...+\frac{(At{)}^k}{k!}+...$$
如果A矩阵是一个普通的矩阵，那么状态转移矩阵求解就变得很困难，好就好在A为幂零矩阵，可以充分利用它的**“高次幂在某次会变为零”**这个性质。

最后给出在状态空间采样的lattice graph的图片

2.2 状态空间采样

状态空间采样属于后项积分，通过反算输入量$u$和时间$T$，以得到一条符合动力学约束，且连接起点和某一点的轨迹。该采样具有任务导向，规划效率高，但是通过两点状态，计算输入量$u$和时间$T$比较困难和复杂。因此有时为了简化计算过程，只计算输入变量$u$，时间$T$固定。

状态空间采样的lattice graph如下图

关于如何仅通过两点状态，来得到最优的输入量$u$和时间$T$,请见下一章的OBVP原理讲解。

注意：
上述图片均来自深蓝学院的路径规划课程，转载请注明出处。