【机器人规划】Hybrd A*

一、了解 A*

初始化两个空的列表openlist和closelist，将起点加入openlist中，并设置代价值为0。
while(1)
{
    if(openlist != null)
    {
         从openlist中选取代价值最小的节点n.
         1. 如果节点n为终点，则从终点开始逐步追踪parent节点，一直达到起点，返回找到的结果路径，
            跳出循环，算法结束，break;
         2. 如果节点n不是终点，
            2.1 将节点n从openlist中删除，加入到closelist中.
            2.2 遍历节点n的8个相邻无碰撞节点
                2.2.1 如果相邻节点在closelist或者openlist中，则跳过该节点，计算下一个节点。
                2.2.2 如果相邻节点不在openlist中，则设置该节点父节点为n，通过f(n)计算该节点的
                      代价值，并将该节点放入openlist中。
     }
     else
         break; //不能找到一条最优路径
}

二、Hybrd A* 理论

1. Dubins曲线

2. Simple Car模型

如下图所示，Simple Car模型是一个表达车辆运动的简易模型。Simple Car模型将车辆看做平面上的刚体运动，刚体的原点位于车辆后轮的中心；x轴沿着车辆主轴方向，与车辆运动方向相同；车辆在任意一个时刻的姿态可以表述为(x, y, $\theta$)。车辆的运动速度为s；方向盘的转角为$\varphi$，它与前轮的转角相同；前轮和后轮中心的距离为L；如果方向角的转角固定，车辆会在原地转圈，转圈的半径为$\rho$。

在一个很短的时间$\Delta t$内，可以认为车辆沿着后轮指向的方向前进，当$\Delta t$趋于0时，有：

根据数学定义:

将2) 和3)代入1)中，得到：

显然，$\dot{x}=\cos \theta$和$\dot{y}=\sin \theta$是5)式的一个解，两侧乘以速度s等式仍然满足。因此有:

用$\omega$表示车辆前进的距离，则有：

根据三角几何，有:

将9)式代入8)式，得到：

8)式两侧同除以dt， 并根据$\dot{\omega }=s$，得到：

至此得到了车辆的运动模型(Motion Model)。

然后引入Action变量，假设车辆运动速度s和方向盘转角$\varphi$由Action变量$u_s$和$u_{\varphi }$指定，得到：

三、Reeds-Shepp曲线