利用A*算法，计算出costmap后，则开始应用HybridA*算法。

A*算法中cost函数的计算公式为F = G + H，其中H采用的是欧式距离，实质就是应用勾股定理计算两个栅格中心点的距离；G就是从起点走到当前栅格的距离。

HybridA*算法cost函数计算公式与A*相同，只是H由欧式距离换成了RS曲线的长度，G值从单纯的距离换成了多个cost之和，比如换挡的cost，方向盘转动的cost，这个cost可以根据自身需要来调整，比如期望搜到的路径尽量少换挡，则需要把换挡的cost加大；期望车尽量走直线，则需要将方向盘转动的cost加大。

一、RS曲线简介

在分析HybridA*代码之前，需要先了解RS曲线，RS曲线与dubins曲线类似，都是圆+直线的路径，区别在于RS曲线允许车辆倒车，如下图所示

dubins曲线只允许车辆前进，这个差异是由计算公式导致的，dubins曲线的运动示例如下图所示：

显然，RS曲线更符合车辆泊车的需求，RS曲线脱胎于论文《Optimal paths for a car that goes both forwards and backwards》，并编入了ompl库中。

论文地址：https://pan.baidu.com/s/1oIbb8-kwXfSRA-G0wpws7g      提取码：0wi7 

RS曲线介绍：http://planning.cs.uiuc.edu/node822.html

二、RS曲线的一些问题

RS曲线的讲解有很多，在此只细写我个人在学习RS曲线过程中遇到的问题，RS的介绍可以看上边的地址。

1、RS曲线的本质

RS曲线的本质就是圆+直线，只不过被归纳为了48种情况。

2、RS曲线为什么会有48种，每种是什么意思？

48种只是论文作者经过总结后得到的结果，认为只要两个点间只要存在路，这48种路必然至少有一条路能从A点到达B点。

48种路中，C代表圆，S代表直线，圆有两种可能，L是左转，R是右转，同时因为RS允许倒车，所以每种路“L，S，R”都会有正负之分，细分下可以分成“L+S+L+”这种表达方式的路，但是这样一个个列出来就太过繁杂，因此统一用C和S来表示，这样就把48种曲线分成了九类，并用“|”表示换了档位，即正负切换，从而得到下图

图中，角标β代表C旋转的弧度是β，同理π/2也是代表旋转弧度，意思就是定死了这个圆必须转π/2。

3、是不是A到B点，只要有路，就必然在48种情况中？

不是，A到B点道路可以有千万条，RS只是说，在RS的圆+直线规则下，必然有一种是可以实现的。

4、泊车算法是不是套用RS就好了？

原理上是，但是为了更符合实际，RS可以拆分，比如九类中只选取前四种，毕竟车一般不会转90°，同时前四种更符合车辆的运动特点。

5、什么是归一化，RS的应用流程是什么？

归一化是为了简化计算，方便写代码，不同的车有不同的旋转半径，归一化后可以让RS更方便的套用于不同的车。同时，归一化后，圆的弧长，也代表了圆的弧度，更方便计算。

归一化：就是将圆的半径定位1，即，车辆半径、AB点坐标，全部除以车辆的旋转半径，这样车旋转的圆半径就为1，AB点也同样做了处理，算出路径后，再乘以车辆半径，就得到了世界坐标系下的真实路径长度。

RS应用流程：①以车辆后轴中心为基准，建立车辆坐标系，RS曲线的计算都在车辆坐标系下

                       ②AB点的坐标从世界坐标系转换至车辆坐标系，

                       ③车辆半径，AB点车辆坐标系坐标做归一化处理

                       ④开始计算RS路径

6、什么是对称性？

对称性是RS曲线编程写代码时，为了简化代码而应用的，如果写48种情况的代码，太过复杂，所以利用对称性来降低复杂度。

对称性分两种，符号对称性timeflip，RL的对称性reflect。

符号对称性timeflip：指的是正负的对称，同一种类型L+S+R+，通过对称性可以同时计算L-S-R-的路径。

符号对称性timeflip的原理：比如，代码写了L+S+R+的计算函数“function-L”，那么就不用写L-S-R-的计算，在“function-L”下，通过将目标点A（x，y，phi）变为B（-x，y，-phi）实现L-S-R-类型路径的计算。

以L+S+L+为例，具体原理为，将目标点变更后，相当于把原目标点A（x，y，phi）关于y轴对称一下（注意phi是取负值，不是0→π），得到新目标点B（-x，y，-phi），用L+S+R+的计算函数去命中新目标B，生成一组新的路径t，u，v，这就得到了L-S-R-类型每个关键字的路长，但由于是用L+S+R+生成的t，u，v，值都是正的，所以结果都需要加-号，从而得到L-S-R-的路径（-t，-u，-v）。

用图表示，L+S+R+的计算方式计算到A点的路径如下所示：

那么到A点的L-S-R-路径如何得到？首先将A点坐标对称得到B点，路径如下图所示：

然后对这一组新的（t，u，v）取负，就得到到A点的L-S-R-路径，这里注意，下图的路径是错的：

RL的对称性reflect：指R与L的互换，不涉及符号，即L+S+L+，通过reflect对称，可以得到R+S+R+的路径。

RL的对称性reflect的实现：通过将目标A（x，y，phi）变为B（x，-y，-phi）实现，具体流程为，如L+S+L+，A变换到B，相当于把A目标点关于x轴对称一下，得到新目标点B，仍用L+S+L+的类型去命中新目标，由于目标点已经对称了，用LSL命中B的路径与命中A目标的路径不同，故而生成新的（t，u，v），新的（t，u，v）的值就是命中原目标的RSR的曲线路径，从而得到R+S+R+的路径。

7、RS代码中，CCC的类型用了坐标转换，是为了什么？

以CCC大类的基础类型L+R-L+为例，编写L+R-L+的计算代码后，通过对称性可以得到大多数的类型，但是无法得到L+R+L-的类型，坐标变换就是把车的起始点看成终点，让车从终点用L+R-L+的路径命中起点，得到一组数据（t，u，v），然后再把路径倒过来，（v，u，t）就成了目标点命中终点的路径，从而得到了L-R-L+的路

之后再应用对称性，就可以得到L+R+L-的路径，因此坐标变换就是为了让编写代码更简单，不用写L-R-L+类型的计算函数。