智能驾驶规划控制理论学习05-车辆运动学规划案例分析

案例一——Hybrid A*（基于正向运动学）

1、基本思想

2、实现流程

3、启发函数设计

4、分析扩张（Analytic Expansions）

5、分级规划（Hierarchical planning）

案例二——State Lattice Planning（基于逆向运动学）

1、基本思想

2、Conformal Lattice

3、目标状态实例

4、生成路径集合

5、代价值评估与路径选择

案例比较

案例一——Hybrid A*（基于正向运动学）

1、基本思想

Hybrid A*是A*算法的一种扩展，本质也是一种图搜索算法，Hybrid A*有以下特点：

通过在控制空间中实时构件状态图

整体思想是在控制空间中采样利用前向积分的方式生成多条运动基元作为状态图的边。以自行车模型为例，输入量有两个，分别是运动速度v和前轮转角φ，为了简化过程可以假设速度恒定，此时只需要考虑φ一个变量，可以假设φ∈[-umax, umax]，对其做等距的离散采样（上图采样了三份，实际应用当中会分成跟多份），取得的三个控制量分别为-umax，0和umax，然后对这三个控制量进行前向仿真/积分（给定初始状态量，给定时间△t通过积分得到新的状态）；

在网格地图中修剪一些节点，减少图的大小（剪枝）

若在采样时状态空间取的足够密（采样等距距离足够小），那么会导致图中的节点和边过多，极大地消耗计算资源，因此为了保证整体算法的实时性，Hybrid A*采用剪枝的操作，将状态空间进行离散得到网格，若多个state落入同一个网格中，那么就选择最优的一个state(cost最小)保留；

使用与A*算法相同的方法在状态图上搜索路径

A*算法与Hybrid A*算法的比较：

A*将代价值与网格中心联系，并且只访问与网格中心相对应的状态；
Hybrid A*为每一个网格赋予了连续的状态（通过在模型的控制空间中采样并积分得到），每一个网格对应的分数是其关联的连续状态下的代价值。

2、实现流程

下面用伪代码的方式对Hybrid A*算法的实现流程进行简要描述：

Algorithm Astar(G, start):
    let open_list be priority queue;
    g[start] := 0;
    f[start] := g[start] + h[start];
    open_list.push(start,f[start]);
    while (open_list is not empty):
        current := open_list.pop();
        mark current as visited
        if current is the goal:
            return current;
        // 通过从控制空间采样和前向仿真来扩展邻居节点
        for all unvisited neighbours next of current in Graph G:
            next_cost := g[current] + cost(current,next);
            if next is not in open_list:
                open_list.push(next,next_cost + h[next])    
            // 合并在离散空间中占用相同单元的连续坐标状态
            else:
                if g[next] > next_cost:
                    g[next] := next_cost;
                    f[next] := next_cost + h[next];

对于Hybrid A*算法，g(n)不单单表示路径距离上的代价值，还包括一些其他方面的代价，比如方向改变（包括前后和左右）的惩罚项；

而对于启发式函数h(n)的设置，可以用经典的欧几里得距离或和曼哈顿距离，也可以用一些其它的方式使整体的搜索效率更高，这方面会在下面讲解；

3、启发函数设计

在Practical search techniques in path planning for autonomous driving. 论文中作者给出了两种启发函数的设计，分别是：

Non-holonomic-without-obstacles

这种启发式函数的估计是基于Reeds Sheep Path的方法，此方法考虑了车辆本身的特性，却忽略了环境；

Holonomic-with-obstacles

这种启发式函数的估计是基于目标节点和当前正在扩展的顶点之间的最短距离，此方法忽略了车辆本身的特性，只考虑了障碍物。

原论文中给出了两个特定场景下使用不同的启发式函数取得的效果，如下图所示：

(a)使用最简单的欧几里得距离作为启发式函数

(b)使用non-holonomic-without-obstacles作为启发式函数

(c)使用non-holonomic-without-obstacles作为启发式函数

(d)non-holonomic-without-obstacles和Holonomic-with-obstacles组合使用

通过(a)图和(b)图的比较，可以得到相比与简单使用欧几里得距离，使用non-holonomic-without-obstacles作为启发式函数使树节点缩小了接近十倍，效率有显著的提升；

(c)(d)图中是一个更为复杂的场景，(c)图表示在复杂场景下单纯使用non-holonomic-without-obstacles作为启发式函数会导致树的规模很大，因为没有考虑环境障碍物的因素，需要把环境探索完全才能得到可行的路径；

而将non-holonomic-without-obstacles和Holonomic-with-obstacles组合进行使用就能得到(d)图所示的路径，有前面的学习可知通过一次完整的迪杰斯特拉算法可以实现图中某个节点到其他所有节点啊之间的距离，利用这个性质，在做Hybrid A*算法前，先从终点反向的进行一次迪杰斯特拉算法对整个图进行遍历我们可以得到图中任意节点到终点的最短路劲（考虑障碍物），将节点距离记录在表格中（前期的离线收集过程）。然后在Hybrid A*算法实时的搜索时直接根据当前位置从表格中获取对应的离终点考虑障碍物的最短路径。

4、分析扩张（Analytic Expansions）

论文指出离散搜索（前向积分）永远无法达到精确的连续目标状态（整体的精度还取决于A*网格的分辨率），因此文章中使用Reeds Shepp Path解析地连接当前节点和目标节点，利用这种方法可以大大提高障碍物在稀疏环境下的搜索速度。

5、分级规划（Hierarchical planning）

在一个复杂的状态空间下想要一次性找到两点之间的最优轨迹是比较困难的，分级规划的思想就是利用分布的思路，先利用某种方式（上图是利用Hybrid A*）获得一条初解（全局最优解），再基于初解做局部的平滑得到局部最优解。

由Hybrid A*得到的路径通常是次优的，主要有两个原因：一是在剪枝的过程中，每一个栅格内都只保留一个state，这种操作会导致在某种层面上不是最优的；二是在控制空间采样时，不管是对前轮转角（方向盘转角）或者是转角变化率进行采样，都无法实现严格意义上的连续，最终的结果可能并不是很平滑。因此需要进一步改善。

论文中通过采用共轭梯度下降法对Hybrid A*路径进行光滑处理。