Motion Plan之带动力学约束路径搜索

Motion Plan之搜索算法笔记
Motion Plan之基于采样的路径规划算法笔记

为什么要动力学规划：

前面几章介绍的路径规划，我们只是认为机器人是质点，这节课要说的就是，如何在考虑机器人的运动学模型下再去找一个安全可行的路径。考虑了机械运动学模型以后，路径规划方法仍然可以分为像前两课一样归为基于搜索的和基于采样的。KINODYNAMIC是运动学和动力学两部分组成的，机器人不是简单的一个质点，可以随意运动的一个质点；它的高阶模型受到一个微分的约束。Kinodynamic Path Finding是一种在机器人和运动规划中的问题类别，其中必须满足速度、加速度和力/扭矩界限，以及如避免障碍物等运动约束。这是一种解决运动规划问题的方法，它考虑了机器人的动力学约束。这意味着，kinodynamic规划算法输出的引导轨迹不仅要对机器人的环境避免碰撞，而且还要对机器人的连续时间动力学的代表模型可行。
传统规划算法生成的路径无法准确匹配机器人的运动约束,使得规划与控制脱节。例如汽车的非完全约束限制其侧方移动,但传统算法可能生成需要急转的最短路径,很难优化成可行解。 尤其是高速运动场景下,初态末态间的精确运动学建模和控制求解至关重要,这需要求解两点边界值问题。 混合A*等算法试图在规划层面加入约束,但仍面临启发函数设计、离散化、连续性等困难,说明了这是一个 layer 上的系统性问题,而不是简单扩展现有方法就能完美解决。 更准确地对运动学建模,直接在规划层面加入约束,不仅能生成更优路径,也能减轻后续轨迹优化负担,改善整体规划控制系统的协同性。由于系统的微分约束，状态对之间的直线连接通常不是有效的轨迹。找到的路径越平滑，优化它就越容易。 综上,动力学规划为使机器人运动更符合自身约束条件、实现更流畅高效的规划与控制提供了可能。与损失函数设计等其他问题一样,这也是实现复杂机器人功能必需解决的基本问题。
总计一下之所以要在Front end阶段就做KINODYNAMIC path find原因如下：

1. 传统规划方法只考虑避障,不能进行平滑运动
2. 实际机器人受运动学约束和动力学约束影响,不是质点模型
3. 必须在规划层面考虑这些约束,生成符合机器人运动能力的路径
4. 这是实现机器人自动控制和功能的基础
5. 使机器人运动更平滑自然,逼近人的运动
6. 减少后端轨迹优化的负担,提高整体规划和控制的协同性
7. 有利于我们理解和建模机器人系统,为更复杂控制算法打基础

前置知识

所谓的动力学建模其实就是在对移动机器人做刚体形状建模，然后利用数学方程对建模出来的移动机器人刚体做动力学的建模。得到一个可以通过调整控制变量向量来控制移动机器人做状态变化的过程。

四旋翼无人机建模

建模流程
各部分介绍
KINODYNAMIC控制数学形式

位置动力学模型

牛顿第二定律
$m\dot{v}^{e}=G{e}+T^{b} $
式中$\dot{v}$, 与G右上角的e代表这是地面坐标系下的向量，T右上角的b代表这是机体坐标系下的向量。由于拉力是由螺旋桨产生的，与四旋翼飞行器固连，故在机体坐标系下表示。
等式两边同时除以m：
${\dot{v}}^e=g^e+\frac{T^b}{m}$
将机体坐标系下的拉力转换到地面坐标系，左乘旋转矩阵即可：
$ot{v}^{e}=g{e}+R_{b}^{e}\frac{T{b}}{m} \$
展开成矩阵形式，其中g与T都是标量，g为重力加速度，T为四个螺旋桨产生的总升力：
$\left[\begin{array}{c}{\dot{v}}_x\\ {\dot{v}}_y\\ {\dot{v}}_z\end{array}\right]=g\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]+\frac{1}{m}\ast \left[\begin{array}{c}cos\theta cos\psi ;cos\psi sin\theta sin\varphi -sin\psi cos\varphi cos\psi sin\theta cos\varphi +sin\psi sin\varphi \\ cos\theta sin\psi ;sin\psi sin\theta sin\varphi +cos\psi cos\varphi sin\psi sin\theta cos\varphi -cos\psi sin\varphi \\ -sin\theta ;sin\varphi cos\theta cos\varphi cos\theta \end{array}\right]\ast \left[\begin{array}{c}0\\ 0\\ -T\end{array}\right]$
整理得：
$\left{\begin{array}{l} \dot{v}{x}=-\frac{T}{m}(\cos \psi \sin \theta \cos \phi+\sin \psi \sin \phi) \ \dot{v}{y}=-\frac{T}{m}(\sin \psi \sin \theta \cos \phi-\cos \psi \sin \phi) \ \dot{v}_{z}=g-\frac{T}{m} \cos \phi \cos \theta \end{array}\right. \$

姿态动力学模型

由欧拉方程可得
$J{\dot{\omega }}^b+{\omega }^b\times J{\omega }^b=G_a^b+{\tau }^b$
式中， ${\dot{\omega }}^b$ 表示在机体坐标系下的角速度； $G_a^b$表示陀螺力矩； ${\tau }^b$ 表示螺旋桨在机体轴上产生的力矩 。
角速度
$${\mathbf{\omega }}^b=\left[\begin{array}{l}{\omega }_x^b\\ {\omega }_y^b\\ {\omega }_z^b\end{array}\right]=\left[\begin{array}{l}p\\ q\\ r\end{array}\right]$$
陀螺力矩 Ga ：当电机高速旋转的时候，相当于一个陀螺。高速旋转的陀螺是非常稳定的个体，具有保持自身轴向不变的能力
$$G_{\mathbf{a}}=\left[\begin{array}{c}{G}_{a,\varphi }\\ G_{a,\theta }\\ G_{a,\psi }\end{array}\right]=\left[\begin{array}{c}{J}_{1}q\left({\varpi }_1-{\varpi }_2+{\varpi }_3-{\varpi }_4\right)\\ J_{1}p\left(-{\varpi }_1+{\varpi }_2-{\varpi }_3+{\varpi }_4\right)\\ 0\end{array}\right]$$
基于假设1惯性矩阵 J可表示为：
$$\mathbf{J}=\left[\begin{array}{rrr}{I}_{xx}& amp;-I_{xy}& amp;-I_{xz}\\ -I_{xy}& amp;I_{yy}& amp;-I_{yz}\\ -I_{xz}& amp;-I_{yz}& amp;I_{zz}\end{array}\right]=\left[\begin{array}{lll}{I}_{xx}& amp;& amp;\\ & amp;I_{yy}& amp;\\ & amp;& amp;I_{zz}\end{array}\right]$$
可得：
$$\left[\begin{array}{lll}{I}_{xx}& amp;& amp;\\ & amp;I_{yy}& amp;\\ & amp;& amp;I_{zz}\end{array}\right]\left[\begin{array}{l}\dot{p}\\ \dot{q}\\ \dot{r}\end{array}\right]+\left[\begin{array}{ccc}p& amp;q& amp;r\\ I_{xx}p& amp;I_{yy}q& amp;I_{zz}r\\ i& amp;j& amp;k\end{array}\right]=\left[\begin{array}{c}{J}_{1}q\left({\varpi }_1-{\varpi }_2+{\varpi }_3-{\varpi }_4\right)\\ J_{1}p\left(-{\varpi }_1+{\varpi }_2-{\varpi }_3+{\varpi }_4\right)\\ 0\end{array}\right]+\left[\begin{array}{c}{\tau }_x\\ {\tau }_y\\ {\tau }_z\end{array}\right]$$
整理得：
$$\{\begin{array}{l}\dot{p}=\frac{1}{I_{xx}}\left[{\tau }_x+qr\left(I_{yy}-I_{zz}\right)-J_{1}q\Omega \right]\\ \dot{q}=\frac{1}{I_{yy}}\left[{\tau }_y+pr\left(I_{zz}-I_{xx}\right)+J_{1}p\Omega \right]\\ \dot{r}=\frac{1}{I_{zz}}\left[{\tau }_z+pq\left(I_{xx}-I_{yy}\right)\right]\end{array}$$

四旋翼飞行器的运动学模型

运动学模型的输入为速度和角速度，输出为位置和姿态
速度与位置的方程：
$$\{\begin{array}{l}{\dot{\mathbf{p}}}^e={\mathbf{v}}^e\\ {\mathbf{p}}^e={\left[\begin{array}{lll}x& amp;y& amp;z\end{array}\right]}^T\\ {\left[\begin{array}{lll}\dot{x}& amp;\dot{y}& amp;\dot{z}\end{array}\right]}^T={\left[\begin{array}{lll}{v}_x& amp;v_y& amp;v_z\end{array}\right]}^T\end{array}$$
姿态角的变化率与机体的旋转角速度有如下关系:
$$\{\begin{array}{l}\dot{\mathbf{\Theta }}=\mathbf{W}\cdot {\mathbf{\omega }}^b\\ \dot{\mathbf{\Theta }}={\left[\begin{array}{lll}\dot{\varphi }& amp;\dot{\theta }& amp;\dot{\psi }\end{array}\right]}^T\\ {\omega }^b={\left[\begin{array}{lll}p& amp;q& amp;r\end{array}\right]}^T\\ \mathbf{W}=\left[\begin{array}{ccc}1& amp;\tan \theta \sin \varphi & amp;\tan \theta \cos \varphi \\ 0& amp;\cos \varphi & amp;-\sin \varphi \\ 0& amp;\sin \varphi /\cos \theta & amp;\cos \varphi /\cos \theta \end{array}\right]\end{array}$$
整理可得：
$$\left[\begin{array}{l}\dot{\varphi }\\ \dot{\theta }\\ \dot{\psi }\end{array}\right]=\left[\begin{array}{ccc}1& amp;\tan \theta \sin \varphi & amp;\tan \theta \cos \varphi \\ 0& amp;\cos \varphi & amp;-\sin \varphi \\ 0& amp;\sin \varphi /\cos \theta & amp;\cos \varphi /\cos \theta \end{array}\right]\left[\begin{array}{l}p\\ q\\ r\end{array}\right]$$

汽车建模

车辆动力学建模是指通过数学模型描述车辆在不同运动和操控情况下的行为。这种建模是车辆动力学控制的基础，能够帮助设计控制系统以优化车辆性能和稳定性。
车辆动力学模型及其控制的数学形式通常通过一系列微分方程来表示，这些方程描述了车辆在运动过程中的各种物理量随时间的演变。
车辆动力学控制的目标是通过调整操纵输入，使车辆达到期望的运动性能。典型的车辆动力学控制系统包括横向和纵向控制。
车辆动力学模型有很多种，以下是一些常见的模型：

1. 单轨模型：这是一种简化的模型，它将车辆视为一个刚体，只考虑车辆的横向运动。这种模型通常用于研究车辆的稳定性和操控性。
2. 双轨模型：这种模型比单轨模型更复杂一些，它将车辆视为两个并行的刚体（左右两侧），可以分别考虑车辆的横向和纵向运动。这种模型可以更准确地模拟车辆的动态行为。
3. 多体模型：这种模型将车辆分解为多个相互连接的刚体（如车身、悬挂系统、轮胎等），每个刚体都有自己的运动方程。这种模型可以模拟车辆的复杂动态行为，如悬挂系统的运动、轮胎的滑动等。
4. 非线性模型：这种模型考虑了车辆动力学中的非线性因素，如轮胎的非线性摩擦特性、悬挂系统的非线性刚度等。这种模型可以更准确地模拟车辆在极限条件下的行为。
5. 自行车模型（Bicycle Model）

首先要简化汽车运动，其中自行车模型就是简单且有效的简化方式。自行车模型基于如下几个假设：
车辆在垂直方向的运动被忽略掉了，也就是说我们描述的车辆是一个二维平面上的运动物体（可以等价与我们是站在天空中的俯视视角）
车辆自行车模型是一种常用的车辆动力学模型，它将车辆简化为一个自行车，只考虑车辆的横向运动和转向运动。这种模型主要用于研究车辆的稳定性和操控性。
在自行车模型中，车辆的运动状态可以由以下几个变量表示：

• 位置：车辆的位置通常由车辆的横向位置和纵向位置表示。
• 速度：车辆的速度通常由车辆的横向速度和纵向速度表示。
• 航向角：航向角是车辆的前进方向与水平方向的夹角。
• 转向角：转向角是车辆前轮与车辆的夹角。

车辆的动力学行为则可以通过以下的微分方程来描述：

• 位置的变化率：位置的变化率等于速度。
• 速度的变化率：速度的变化率等于加速度。
• 航向角的变化率：航向角的变化率等于车辆的转向速度。
• 转向角的变化率：转向角的变化率等于车辆的转向加速度。

其中 θ是其在 Yaw 方向的偏转角度，它是相对于 x 轴的逆时针方向的角度，v 是 θ 方向的速度，L是车辆的轴距（前后轮胎的距离）, (x,y) 是车辆的坐标。
可以通过以下的微分方程来描述：

1. 位置的变化率：位置的变化率等于速度。在二维平面上，我们可以将其分解为x和y两个方向。假设车辆的速度为v，航向角为θ，那么位置的变化率可以表示为：
   • $\frac{dx}{dt}=v.cos(\theta )$
   • $\frac{dy}{dt}=v.sin(\theta )$
2. 速度的变化率：速度的变化率等于加速度。假设车辆的加速度为a，那么速度的变化率可以表示为：
   • $\frac{dv}{dt}=a$
3. 航向角的变化率：航向角的变化率等于车辆的转向速度。假设车辆的转向速度为ω，那么航向角的变化率可以表示为：
   • $\frac{dt}{d\theta }=\omega$

这些方程描述了车辆在给定控制输入（如油门、刹车和转向）下的运动行为。

简化汽车模型

$\left(\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\theta }\end{array}\right)=\left(\begin{array}{c}vcos\theta \\ vsin\theta \\ \frac{r}{L}tan\varphi \end{array}\right)$

"单轮模型"和"差速驱动模型”

$\left(\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\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$

$\left(\begin{array}{c}\stackrel{.}{x}\\ \stackrel{.}{y}\\ \stackrel{.}{\theta }\end{array}\right)=\left(\begin{array}{c}\frac{r}{2}({\mu }_l+{\mu }_r)cos\theta \\ \frac{r}{2}({\mu }_l+{\mu }_r)sin\theta \\ \frac{r}{L}({\mu }_r-{\mu }_l)\end{array}\right)$

经过动力学建模得到了一个控制方程，前面几章也介绍了如何把移动机器人当成质点得到了一条质点轨迹路径。那么接下来的事情就是如何把动力学建模得到的控制方程融合到质点轨迹路径中。然后把各种约束融合进代价函数，通过求解代价函数最小化得到符合约束的满足动力学控制要求的轨迹。
那么动力学控制方程融入质点轨迹主流有两种方法：控制离散化、状态离散化。
1.控制离散化就是说我们已经知道动力学控制方程了，那么我们能不能把质点轨迹上每个质点出了坐标信息还带上控制向量信息，控制向量是一个连续的向量不好表示，所以为了更方便求解减少求解空间我们就把控制向量离散化变成有限的可选项。这样可以更方便的写出代价函数，也更容易求解得出代动力学约束的可行解。
2.状态离散，意思就是我们把质点轨迹上每个质点带上移动机器人在这个质点的状态信息。然而状态也是一个连续的向量，解空间大不利于求解。所以我们把状态离散成有限的状态表示，这样在求解时候知道每个质点的状态，求解问题就变成在如何求解状态间转移矩阵符合代价函数最小化问题。
下面章节就会具体介绍如何求解带状态约束的路径问题。

正文

State Lattice Planning

状态栅格，顾名思义就在在普通的栅格化地图的基础上，多加了机器人运动学模型的约束，确保机器人能沿着相邻两点之间生成的路径运动，并且能够具有状态连续性（速度连续等）。

如何构建状态栅格

状态栅格图中，每个顶点的连线都是根据机器人的运动学模型生成的，都是切实可行的路径，而普通栅格 (grid)的方法并没有结合模型。在构建状态栅格图（State Lattice Search）之前，需要对被控对象进行运动学分析，即建立对应的运动学模型。运动学模型主要描述了位置、速度、加速度、转向角度等等状态量在时间上的状态转移，以及对所关心的这些状态量的边界条件进行约束。
从控制空间和状态空间中进行采样，对于机器人模型：