使用到了gps传感器、视觉里程计以及惯性导航，本发明只需要用到gps传感器以及激光雷达，并且视觉里程计容易受到光照等因素的影响，鲁棒性不及激光雷达。
上述专利为了实现建图效果，根据不同路段gps的信号状态，选择gps的位姿还是惯导与视觉里程计的融合位姿作为最终的位姿，并没有考虑到在gps信号极差的路段，由于惯性导航误差的不断积累会导致无人车的定位不够jingque。而本发明考虑了gps信号的全部状态，针对园区低速场景，采用多传感器融合的方式，实现了无人车在园区内任意场景下的jingque定位。
本发明依据低速园区无人车运行环境的特点(半开放、路程短、楼宇树木遮挡严重等)，提出了激光雷达slam(simultaneouslocalizationandmapping)与gps(globalpositioningsystem)传感器融合定位方法，解决了无人车在园区任意环境下slam的持续定位问题和激光雷达扫描匹配失败后的快速重新匹配问题。
激光雷达可以生成三维的位置信息，快速确定物体的位置、大小，比摄像头等传感器，探测距离更远、jingque度更高，还有更灵敏的响应速度，还不受环境光照的影响，但是激光雷达最大的缺陷，就是不能在雪雨等极端天气下工作，在这种天气下会导致无人车的“位置晕眩”。gps传感器导航灵活性高，但需要接受卫星信号，会有信号失真及丢失的情况，影响定位精度。单一传感器在无人车的定位过程中都存在不足。
本发明采用gps传感器和激光雷达传感器融合定位的思路，获得gps传感器和激光雷达传感器融合的车辆位姿信息，当无人车在使用激光雷达slam定位算法ndt点云匹配失败时，根据当前的gps信号状态替代点云匹配算法ndt算法的定位预测矩阵，实现无人车slam算法的持续定位，提高无人车在低速园区环境下的定位精度。本发明整体系统框架图如图4所示：
有益效果
本发明提出的多传感器融合的定位方法，在激光雷达slam点云失配时，根据gps当前的状态情况，使用gps传感器与激光雷达的融合位姿来替换激光雷达slam算法中的预测矩阵，可将重新匹配的搜索范围大大缩小，实现重新的快速匹配，实现了园区无人车激光雷达slam算法的持续定位。本方法在园区低速无人驾驶车中得到了实验验证，在匹配精度以及匹配速度上可以取得很好的效果。
本发明基于激光雷达与gps传感器融合实现激光雷达快速重新匹配的方法，为了保证激光雷达与gps传感器坐标一致性。由于激光雷达以及gps传感器与车辆是刚性连接，二者的安装位置已固定，对激光雷达以及gps传感器的安装位置进行标定，使激光雷达坐标系、gps坐标系以及车辆坐标系三者统一。本发明以车辆坐标系为基准，其中车辆坐标系坐标原点为车顶gps传感器前天线位置，x轴方向指向车头正前方，y轴正方向为x轴正方向逆时针旋转90°，z轴正方向指向天空。采用旋转平移矩阵的方法将gps传感器以及激光雷达传感器的坐标原点转换到车辆坐标原点。本发明采用的gps导航装置为双天线导航，一般的双天线导航后天线为定位天线，前天线为定向天线，本发明为了将雷达坐标原点与导航定位点统一故将前天线连接到导航接收机的后天线位置，后天线连接到导航接收机的前天线位置，并且本发明只用到前天线。前天线位置即为车辆坐标系原点。
激光雷达与gps传感器的安装位置关系如图1所示。其中，激光雷达坐标系，x轴正方向为雷达数据输出线缆的相反方向，y轴正方向为x轴正方向逆时针旋转90°，z轴正方向指向天空；假设雷达的安装位置距地面高度为0.5米，距车辆坐标系原点的前向位移为1.5米，水平方向无偏差，那么激光雷达坐标系以及gps坐标系向车辆坐标系的转化过程，以激光雷达为例，设定激光雷达三维坐标系中的一个向量其中点p在xy平面、xz平面、yz平面的投影分别为点m、点p、点n，绕z转旋转θ角，旋转矩阵为绕x转旋转θ角，旋转矩阵为：绕y转旋转θ角，旋转矩阵为：在z、x、y轴的平移矩阵为：
其中，激光雷达、gps传感器与车辆坐标系联合标定算法流程图如图2所示：
ndt(normaldistributionstransform)匹配算法介绍
在slam导航过程中，传统的点云配准算法一般为icp迭代的方法，这种方法一般需要提供一个较好的初值，也就是需要粗配准，同时由于算法本身缺陷，最终迭代结果可能会陷入局部最优，导致配准失败，效果往往不够理想。本发明介绍的是另一种比较好的配准算法，ndt配准算法。所谓ndt就是正态分布变换，作用与icp一样用来估计两个点云之间的刚体变换。用标准最优化技术来确定两个点云间的最优的匹配，因为其在配准过程中不利用对应点的特征计算和匹配，所以时间比其他方法快。这个配准算法耗时稳定，跟初值相关不大，初值误差大时，也能很好的纠正过来。
瑞士KELLER系列产品及部分型号如下：
一，瑞士KELLER压力传感器
① 压敏电阻OEM压力传感器： 7S / 9S系列、7L1系列、8系列、9系列
② 高科技微型压力传感器：2 Mi系列、M5系列、传感器探头
③ 压敏电阻标准压力传感器：11系列、13系列、15系列
④ 经济型压力传感器：1 TAB系列、6 M / S系列、2系列、4系列
二，瑞士KELLER压力变送器
① 工业用压力变送器：PD-39 X系列、41 X系列、22 DT系列、PRD-33 X 系列、33 X / 35 X系列、21 G EOL系列、23 / 25系列、22 M/S系列
② 危险场所用压力变送器：23 Ed/25 Ed, 33 X Ed/35 X Ed、33 X Ei / 35 X Ei / 36 XW Ei、23 SY Ei / 25 Y Ei / 26 Y Ei、PD-39 X Ei系列、41 X Ei系列、46 X Ei系列
③ 被动RFID界面压力变送器：21 D RFID系列
ndt配准算法步骤如下：
将参考点云(referencescan)所占的空间划分成指定大小(cellsize)的网格或体素(voxel)，并计算每个网格的多维正态分布参数，激光雷达的原始点云以及点云在ndt算法下的空间划分效果如图三所示。
(2)初始化变换参数初始化变换参数(赋予零值或者使用里程计数据赋值)。
(3)对于要配准的点云(secondscan)，通过变换t将其转换到参考点云的网格中xi′＝t(xi,p)
根据正态分布参数计算每个转换点的概率密度
ndt配准得分(score)通过对每个网格计算出的概率密度相加得到
根据牛顿优化算法对目标函数-score-行优化，即寻找变换参数p使得score的值最大。
跳转到第3步继续执行，直到达到收敛条件为止。
gps、激光雷达传感器融合位姿
本发明在实验过程中，首先要对gps、激光雷达接收到的数据进行数据预处理，滤波处理以及剔除噪点，无人车的状态可以表示为
其中px,py代表无人车当前的位置，代表无人车的航向角信息。gps传感器可以检测无人车的当前位置，没有速度信息。其测量值z＝(gx,gy)，激光雷达传感器可以检测到距离、航向角、速度信息，其测量值表示为gps与激光雷达位姿融合流程图如图5所示：
首先，对数据进行初始化，在收到传感器返回的测量值时，对无人车的状态进行初始化，初始化默认车辆当前处于原点而且速度值为零，同时加上对时间的更新。
gps传感器处理过程如公式4所示：
在gps的测量值z＝(gx,gy)左乘一个系数矩阵，gps传感器处理之后的结果为无人车此时的位置坐标。
激光雷达传感器处理过程如公式5所示，通过将激光雷达的测量值解算，可以得到当前无人车的位置坐标，以及无人车在x、y轴的速度大小。
将两种传感器处理后的结果进行融合，无人车的位姿表示为
经过gps以及激光雷达传感器数据解算后，得到来自激光雷达和gps传感器解算的无人车位置坐标，以及来自激光雷达表示的无人车航向角信息，当激光雷达slam定位算法失配时，若gps传感器接受信号状态良好，则将gps传感器解析的位置坐标以及激光雷达解析的航向角作为最终的融合位姿对ndt算法的预测矩阵进行替换。若gps传感器接受信号状态差，则将激光雷达传感器解析出来的位置坐标以及航向角作为最终的融合位姿对ndt算法的预测矩阵进行替换。
由于无人车处于持续运动中，故还可以使用卡尔曼滤波算法预测下一位置，利用预测后的信息计算位姿矩阵，达到矫正位姿的作用。两种传感器在预测无人车下一时刻的状态时，使用卡尔曼滤波算法预测下一位置，具体如下
位姿预测状态方程：
其中，x为无人车的状态，f是系统的控制函数，表示系统的改变，在这里，只考虑简单的线性问题，p代表无人车当前的位姿，q代表系统误差。