自动驾驶（四）---------GPS前世今生

自动驾驶越来越离不开高精地图和高精GPS

本文把已知的GPS总结如下：

1. GPS原理   2.差分GPS原理   3.组合惯导原理   4.opendrive格式解析  5.地图匹配算法   6.导航全局规划（A*算法）

1. GPS原理

    GPS的全称是全球定位系统(the Global Positioning System)。它属于美国政府，并由洛杉矶的联合项目办公室(JPO, Joint Program Office)管理。

    三维空间，已知三个点的坐标位置，并且知道这三个点到未知点的距离[r1,r2,r3r1,r2,r3]，那么就可以有三个方程，用来解三个未知数[(x,y,z)(x,y,z)]，

                                               

     按理说更多的卫星都满足这个方程，是唯一解，但是信号在传输个过程中受到电离层、墙面反射、多径效应等干扰，求出来的就不是精确解了，我们只能用多个卫星信号求模糊解。

2.差分GPS原理

      GPS信号有很多误差，如果知道这些误差，不就可以求出唯一解（正确的GPS位置），基准站正是为此准备的，

      基准站提前计算出每个卫星到本地的误差，并把这个误差发给附近的GPS设备，用它来矫正GPS和每个卫星的误差，从而计算出当前的GPS位置，当然，这些误差也不是相同的，离基准站越近越准。一般用于100KM之内。

      载波相位差分技术又称为RTK技术，是另一种差分技术。

3.组合惯导原理

      GPS/INS组合导航系统由全球卫星导航系统(GPS)、惯性导航系统(INS)和信息处理系统三部分构成。

      惯性导航系统 一般是3轴加速度传感器，采用加速度传感器和角度做状态估计，GPS信号做观测，

      利用kalman filter 计算实时GPS位置。

       近年来还有融合北斗系统的组合惯导。

4.opendrive格式解析

       OpenDrive地图文件格式为xml，详细的介绍可以参考
        http://www.opendrive.org/docs/OpenDRIVEFormatSpecRev1.4H.pdf

       该xml文件中中包含了很多地图信息，例如Road、Junction等，下图是xml文件的主要结构，

在OpenDRIVE坐标采用轨迹坐标系：

reference line是路网结构中一个很重要的概念，绘制地图的时候先是画reference line，reference line包含xy位置坐标、路的形状属性，然后在reference line基础上再去画其他其他元素。

算了 不粘贴了：https://blog.csdn.net/lewif/article/details/78575840

5.地图匹配算法

     所谓地图匹配算法，是指通过当前的GPS坐标，和高精地图信息，分析出车辆所在的位置，你可能好奇知道坐标还要分析什么？

     1.gps有误差，2，地图有误差。很难准确得到结果。

     百度地图做法如下：

     路网中附近有N条link，分析当前车辆在哪条link上，具体在哪条LINK上由以下部分决定：

       1 gps点到每一link的距离。

       2 gps方向与每一link夹角cos值。

       3 gps历史link和每一link的拓扑关系：当前link为1，相连link为0.5，不相连为0

       4 gps轨迹和每一link的形状相似度

      以上所有link权重相加，最终得到权重最大的link

      特别的：

         1. 百度地图训练了3组参数（以上4部分的权重），分别对应城市 乡村 复杂立交等不同场景。

         2. 加入错误检测机制，当匹配的link距离超过阈值，则剔除拓扑关系和轨迹相似度两部分，重新计算新的link

6.导航全局规划（A*算法）

     A*算法一般用于导航路劲规划中。

A*算法的思想是这样的：

1. 每个节点记录三个数据，G：离开点的步数、H：和终点的距离估算、F=G+H
   同时还应该有一个P，记录其父节点，一遍能够找到路径

2. 两个集合，接壤集合，和已走结合
   同时还有一个集合是没有走的，但是这个集合并不会显式的写出来。

3. 从起点开始计算与起点接壤的节点的GHF值，然后放入接壤集合
   这里计算完了，需要将起始节点放入已走节点集合，否则会重复计算两次。
   同时需要处理p的值

4. 从接壤集合中取出F值最小的节点，计算与该节点接壤的节点GHF值，放入接壤结合，然后将该节点放入已走节点
   这里，如果一个节点已经计算过了，或者在已走节点中，那就不要算了。
   如果一个节点已经计算过，那么表明该节点以前的F值比再次计算出来的F值要小。

5. 重复步骤4，直到到达终点

    启发函数一般推荐如下：

• 如果图形中只允许朝上下左右四个方向移动，则可以使用曼哈顿距离（Manhattan distance）。
• 如果图形中允许朝八个方向移动，则可以使用对角距离。
• 如果图形中允许朝任何方向移动，则可以使用欧几里得距离（Euclidean distance）。

A* c++代码：https://blog.csdn.net/A_L_A_N/article/details/81392212