一种基于先验地图的智能车辆定位方法与流程

本发明涉及智能车辆定位技术领域，特别涉及一种基于先验地图的智能车辆定位方法。

背景技术

智能车辆自主驾驶或辅助驾驶的前提之一就是要准确知道车辆的位姿，当前智能车辆自主驾驶或辅助驾驶中，定位方法一般包括：基于gps定位、基于gps与里程计融合进行定位以及同步定位和地图构件技术。

其中，对于基于gps技术进行定位，其容易受到外界环境的干扰，特别高楼林立的城市环境对gps信号的干扰很大，会导致定位结果产生很大误差。

对于gps和里程计融合的定位方法，由于车辆里程计在车辆行驶过程中本身会累积较大的误差，因此得到的定位结果不准确。

对于同步定位与地图构建技术，其使用雷达数据对里程计数据进行校正，得到智能车辆局部的位姿估计，再结合gps定位，通过坐标转换和卡尔曼滤波，将二者进行融合，得到智能车辆的一个较为准确的位姿。但是受到先有计算机的运算能力的限制，需舍弃掉一部分的观测信息，导致地图精度下降，进而影响定位精度。

技术实现要素：

本发明的目的在于提供一种基于先验地图的智能车辆定位方法，以提高智能车辆的定位精度。

为实现以上目的，本发明采用一种基于先验地图的智能车辆定位方法，智能车辆在预先设定的运行区域内移动，该方法包括如下步骤：

通过智能车辆的里程计、车载雷达，分别获取智能车辆的位姿信息、观测信息，所述位姿信息包括智能车辆在整个运行区域的位姿信息以及在局部运行区域的位姿信息，所述观测信息包括覆盖整个运行区域的观测信息以及覆盖局部运行区域的观测信息；

利用slam算法对所述智能车辆的位姿信息、观测信息进行处理，得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆slam坐标系下的位姿信息；

获取智能车辆在gps地理坐标系下的位姿信息，并将gps地理坐标系下的位姿信息转换至所述slam坐标系下，得到智能车辆在slam坐标系下的gps位姿信息；

利用卡尔曼滤波技术，将智能车辆slam坐标系下的位姿信息和智能车辆在slam坐标系下的gps位姿信息进行融合，得到智能车辆的初步位姿估计；

利用所述全局地图和局部地图进行匹配，并利用匹配结果对所述初步位姿估计进行优化，得到智能车辆的最终位姿。

优选地，所述通过智能车辆的里程计、车载雷达，分别获取智能车辆的位姿信息、观测信息，包括：

构建里程计坐标系以及里程计坐标系下的智能车辆位姿模型，并结合智能车辆自带的里程计，获得智能车辆的位姿信息；

利用所述车载雷达获取并记录智能车辆的观测信息

优选地，所述利用slam算法对所述智能车辆的位姿信息、观测信息进行处理，得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆slam坐标系下的位姿信息，包括：

建立与所述里程计坐标系同原点同轴的所述slam坐标系；

在所述slam坐标系下，将所述整个运行区域的位姿信息作为slam算法的控制输入信息，将所述覆盖整个运行区域的观测信息作为slam算法的观测信息，得到整个运行区域的全局地图；

在所述slam坐标系下，将所述局部运行区域的位姿信息作为slam算法的控制输入信息，将所述覆盖局部运行区域的观测信息作为slam算法的观测信息，得到局部运行区域的局部地图和智能车辆slam坐标系下的位姿信息。

优选地，还包括：

将所述全局地图中相对于同一个观测节点的两个位姿节点之间的差值作为全局误差项，利用高斯牛顿法对所述全局误差项进行迭代优化；

将优化完成后全局误差项的位姿节点、观测节点添加至所述全局地图中，并在所述全局地图中将相应的位姿节点、观测节点及节点之间边剔除以得到优化后的全局地图；

将所述局部地图中相对于同一个观测节点的两个位姿节点之间的差值作为局部误差项，利用高斯牛顿法对所述局部误差项进行迭代优化；

将优化完成后局部误差项的位姿节点、观测节点添加至所述局部地图中，并在所述局部地图中将相应的位姿节点、观测节点及节点之间的边剔除以得到优化后的局部地图。

优选地，该方法还包括：

在离线状态下，采用高斯牛顿法对所述优化后的全局地图进行离线优化，得到所述离线优化的全局地图。

优选地，所述获取智能车辆在gps地理坐标系下的位姿信息，并将gps地理坐标系下的位姿信息转换至所述slam坐标系下，得到智能车辆在slam坐标系下的gps位姿信息，包括：

在slam系统启动时，获取智能车辆在gps坐标系中的位姿信息，gps坐标系中的位姿信息包括原始经度、纬度信息；

采用高斯投影，将所述gps坐标系中的位姿信息转换到平面坐标系中，得到平面坐标系下的位姿信息；

将平面坐标系中的位姿信息转换到所述slam坐标系下，得到智能车辆在slam坐标系下的gps位姿信息。

优选地，所述利用卡尔曼滤波技术，将智能车辆slam坐标系下的位姿信息和智能车辆在slam坐标系下的gps位姿信息进行融合，得到智能车辆的初步位姿估计，包括：

根据状态空间转移方程得到(k-1)时刻后验位姿状态向量的估计值，并通过设定的预测方程组对(k-1)时刻后验位姿状态向量的估计值处理，得到当前时刻k的先验位姿状态估计量；

根据状态空间的观测方程，得到当前时刻k位姿向量的观测值，并通过设定的更新方程组对当前时刻k位姿向量的观测值进行处理，得到当前时刻k位姿状态向量的预测值。

优选地，所述利用所述全局地图和局部地图进行匹配，并利用匹配结果对所述初步位姿估计进行优化，得到智能车辆的最终位姿，包括：

根据所述k时刻的的先验位姿和(k-1)时刻的后验位姿，得到智能车辆在k时刻的里程信息uk、k时刻的观测信息zk；

在粒子滤波器中，对智能车辆在k时刻的里程信息uk、k时刻的观测信息zk进行处理，得到智能车辆的最终位姿。

优选地，所述在粒子滤波器中，对智能车辆在k时刻的里程信息uk、k时刻的观测信息zk进行处理，得到智能车辆的最终位姿，包括：

设置n个粒子，采样计算权重其中，每个粒子代表智能车当前的位姿，表示粒子滤波；

根据智能车辆在k时刻的里程信息uk，推算k时刻智能车位姿的分布，对每个粒子的位姿信息进行采样

计算以当前粒子为车辆中心，车辆中心周边的局部地图与全局地图进行匹配的误差e，并将当前粒子的误差除以所有粒子的总误差得到当前粒子的权重

根据k时刻粒子的位姿信息以及重要性权值计算出当前智能车辆的最终位姿。

优选地，还包括：

对每个粒子权重信息进行采样，并根据采样结果增加权值高的粒子，删除权值低的粒子，对所述当前智能车辆的最终位姿进行优化。

与现有技术相比，本发明存在以下技术效果：本发明首先通过同步定位与地图构建方法(simultaneouslocalizationandmapping，slam)构建信号智能车辆在整个行驶区域的先验全局地图，由于在构建先验全局地图的过程中，可以不考虑实时性，因此可以对整个先验全局地图进行优化，得到更为准确的全局地图信息。同时，通过slam实时构建局部地图以及智能车辆的位姿信息，通过卡尔曼滤波器对智能车辆的gps定位信息以及智能车辆的位姿信息进行融合处理，得到一个较优的位姿，然后将局部地图与全局地图进行匹配，并通过粒子滤波器对智能车辆进行定位，得到智能车辆的准确位姿。由于采用了多个传感器，可以有效的克服gps信号弱，以及车辆里程漂移的问题。通过局部地图与全局地图匹配的方式，可以有效的解决计算机计算能力的限制造成的实时定位与地图构建中精度不高的问题。

附图说明

下面结合附图，对本发明的具体实施方式进行详细描述：

图1是一种基于先验地图的智能车辆定位方法的流程示意图；

图2是一种基于先验地图的智能车辆定位方法的整体流程示意图。

具体实施方式

为了更进一步说明本发明的特征，请参阅以下有关本发明的详细说明与附图。所附图仅供参考与说明之用，并非用来对本发明的保护范围加以限制。

参阅图1至图2所示，本实施例公开了一种基于先验地图的智能车辆定位方法，智能车辆在预先设定的运行区域内移动，该方法包括如下步骤：

s1、通过智能车辆的里程计、车载雷达，分别获取智能车辆的位姿信息、观测信息，所述位姿信息包括智能车辆在整个运行区域的位姿信息以及在局部运行区域的位姿信息，所述观测信息包括覆盖整个运行区域的观测信息以及覆盖局部运行区域的观测信息；

需要说明的是，智能车辆在整个运行区域的位姿信息以及观测信息是不变的，而智能车辆在局部运行区域的位姿信息以及观测信息随着智能车辆的移动实时发生变化。

s2、利用slam算法对所述智能车辆的位姿信息、观测信息进行处理，得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆slam坐标系下的位姿信息；

其中，智能车辆在整个运行区域内的位姿信息和覆盖整个运行区域的观测信息作为先验数据，该先验数据用于构建智能车辆在整个运行区域的先验全局地图，该先验全局地图不考虑实时性。智能车辆在局部运行区域的位姿信息以及覆盖局部运行区域的观测信息用于构建智能车辆在运行区域内行驶时的局部地图，该局部地图在智能车辆的行驶过程中是实时更新的。

s3、获取智能车辆在gps地理坐标系下的位姿信息，并将gps地理坐标系下的位姿信息转换至所述slam坐标系下，得到智能车辆在slam坐标系下的gps位姿信息；

其中，智能车辆在gps地理坐标系下的位姿信息包括智能车辆在gps坐标系中的原始经度、纬度。

s4、利用卡尔曼滤波技术，将智能车辆slam坐标系下的位姿信息和智能车辆在slam坐标系下的gps位姿信息进行融合，得到智能车辆的初步位姿估计；

需要说明的是，智能车辆在slam坐标系下的位姿信息为智能车辆在运行区域内行驶时的实时定位信息，即在局部地图构建的同时所获取的定位信息，智能车辆全局地图构建过程中仅进行地图构件不进行车辆定位。

利用卡尔曼滤波器将智能车辆的两种位姿信息进行融合的过程与现有技术中卡尔曼滤波器对位姿的融合过程相同。

s5、利用所述全局地图和局部地图进行匹配，并利用匹配结果对所述初步位姿估计进行优化，得到智能车辆的最终位姿。

需要说明的是，现有技术中在进行同步定位与地图构建时，使用雷达数据对里程计数据进行校正，由于受到计算机运算能力的限制，在同步定位与地图构建过程中需要舍弃掉一部分的观测信息，导致构建的地图精度下降，影响定位精度。本实施例中将实时更新的局部地图与在先构建的全局地图进行匹配，虽然局部地图在构建过程中也会丢弃掉部分观测信息，但是通过将局部地图与全局地图进行实时匹配，可以有效的弥补局部地图丢失的观测信息，解决了由于计算机运算能力不足导致的构建的地图精度低的问题，从而提高了智能车辆定位的精度。

作为进一步优选的方案，步骤s1：通过智能车辆的里程计、车载雷达，分别获取智能车辆的位姿信息、观测信息，包括：

构建里程计坐标系以及里程计坐标系下的智能车辆位姿模型，并结合智能车辆自带的里程计，获得智能车辆的位姿信息；

利用所述车载雷达获取并记录时刻k智能车辆周边的观测信息。

其中，里程计坐标系构建的过程为：以智能车辆启动时车体中心为原点o，将沿智能车辆前进方向做为x轴、垂直x轴指向车体左侧为y轴方向、垂直x轴和y轴的方向作为z轴。假设智能车辆在z轴上的运动始终为0，则智能车辆的位姿模型为(x,y,θ)，其中，(x，y)表示每个障碍物的位姿信息，θ表示角度信息，然后结合智能车辆自带的里程计即可获取智能车辆的实时位姿信息。

作为进一步优选的方案，步骤s2：利用slam算法对所述智能车辆的位姿信息、观测信息进行处理，得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆slam坐标系下的位姿信息，包括：

建立与所述里程计坐标系同原点同轴的所述slam坐标系；

在所述slam坐标系下，将所述整个运行区域的位姿信息作为slam算法的控制输入信息，将所述覆盖整个运行区域的观测信息作为slam算法的观测信息，得到整个运行区域的全局地图；

在所述slam坐标系下，将所述局部运行区域的位姿信息作为slam算法的控制输入信息，将所述覆盖局部运行区域的观测信息作为slam算法的观测信息，得到局部运行区域的局部地图和智能车辆slam坐标系下的位姿信息。

其中，采用slam算法构建整个运行区域的全局地图的过程为：

(1)信息输入：将整个运行区域的位姿信息作为slam算法的控制输入信息，将所述覆盖整个运行区域的观测信息作为slam算法的观测信息；

(2)slam算法进行地图构建：将所述整个运行区域的位姿信息和覆盖整个运行区域的观测信息分别转换在slam坐标系下，然后将所述位姿信息和观测信息作为图的节点，将节点之间的约束信息作为图的边，将节点和边添加至图中；

(3)高斯迭代优化：将添加了全部的节点和边的信息后的地图作为待优化地图，通过高斯迭代的方法对待优化地图进行优化，具体为：

在k时刻下，将里程计当前获取的智能车辆位姿信息作为新的位姿节点添加至待优化的地图中，该位姿节点包含了智能车辆当前的位姿和角度信息(x,y,θ)。

在k时刻下，将车载雷达当前获取的智能车辆的n个观测信息作为新的观测节点0k添加至待优化地图中，其中k∈[1,n]。该观测节点中包含了智能车辆周边每个障碍物的位姿和角度信息(x,y,θ)。

将每个新的位姿节点及其对应的新的观测节点之间在x、y、θ三个方向的差值作为约束节点和节点之间的边添加至待优化的地图中。该节点和节点之间的约束包括同一个观测节点和不同的位姿节点之间的约束，两个位姿节点之间的约束。

将对于同一个观测节点相对于两个位姿节点会有所不同，将两个位姿节点之间的差值作为待优化的误差项，当误差项累计到一定程度的时候，比如已经有了7-8帧的观测数据所包含的所有误差项。采用高斯牛顿法对待优化的地图进行优化处理，以降低整个地图的误差项。

在地图优化完成之后，将已经优化的位姿节点和观测节点加入到全局地图中，并且在待优化的地图中将这些节点和边剔除掉。

当智能车辆完成对整个运行区域的探索后，即可构建出整个运行区域的全局地图。

需要说明的是，局部地图的构建过程与全局地图的构建过程相似，只是slam算法的输入数据不同，构建局部地图输入的是局部运行区域内的位姿信息和覆盖局部运行区域即智能车辆周边的观测信息。将局部运行区域内的位姿信息和覆盖局部运行区域的观测信息分别转换在slam坐标系下，实时的得到局部运行区域中智能车辆在slam坐标系下的位姿信息。

作为进一步优选的方案，本实施例中在构建上述全局地图过程中，将位姿信息、观测信息等添加在第三方提供的地图框架中构建全局地图，同时将位姿信息、观测信息作为节点、以及节点之间的约束条件均存储在数据库中，在全局地图构建完成后，还采用离线的方法对全局地图进行离线优化，具体过程为：

将上述存储在数据库中的所有节点添加至上述全局地图中；

将上述存储在数据库中的节点间约束条件添加至上述全局地图中对应的节点和节点之间；

将添加了数据库中的节点和边的信息的全局地图作为待二次优化地图，并在离线状态下对待二次优化地图进行高斯牛顿迭代优化，得到二次优化后的全局地图。

需要说明的是，在实际应用中，由于系统对实时性的要求导致了记性迭代优化的次数会受到限制，待优化地图的大小也会受到限制。导致两相邻两次迭代优化得到的地图间的约束信息损失，以及整个全局地图的约束信息损失。本实施例中通过在离线状态下对得到的全局地图进行离线优化，降低了地图的约束信息损失量，确保了全局地图构建的准确性和精度。

作为进一步优选的方案，上述步骤s3：获取智能车辆在gps地理坐标系下的位姿信息，并将gps地理坐标系下的位姿信息转换至所述slam坐标系下，得到智能车辆在slam坐标系下的gps位姿信息，包括：

在slam系统启动时，获取智能车辆在gps坐标系中的位姿信息，gps坐标系中的位姿信息包括原始经度、纬度信息；

采用高斯投影，将所述gps坐标系中的位姿信息转换到平面坐标系中，得到平面坐标系下的位姿信息；

将平面坐标系中的位姿信息转换到所述slam坐标系下，得到智能车辆在slam坐标系下的gps位姿信息。

作为进一步优选的方案，步骤s4：利用卡尔曼滤波技术，将智能车辆slam坐标系下的位姿信息和智能车辆在slam坐标系下的gps位姿信息进行融合，得到智能车辆的初步位姿估计，包括：

定义状态空间转移方程和状态空间的观测方程如下：

x(k)＝ax(k-1)+w(k)，

z(k)＝hx(k)+v(k)，

其中，x(k)为k时刻的位姿状态向量，a(k)为过程矩阵，h为测量矩阵，w(k)为过程噪声矩阵，z(k)表示观测矩阵，其协方差为q，v(k)为测量噪声，其协方差为r，a表示状态转移矩阵。

状态空间转移方程用于通过上一时刻系统状态x(k-1)和状态转移矩阵的乘积加上高斯噪声得到当前时刻系统的状态。

状态空间的观测方程用于通过当前时刻系统状态x(k)和测量矩阵的乘积加上一个观测的高斯噪声得到当前系统的观测量。

卡尔曼滤波的过程如下：

预测方程组：

x(k|k-1)＝ax(k-1|k-1)，

p(k|k-1)＝ap(k-1|k-1)a′+q，

更新方程组：

x(k|k)＝x(k|k-1)+kg(k)(z(k)-hx(k|k-1))，

p(k|k)＝(1-kg(k)h)p(k|k-1)，

其中：

x(k|k-1)是k时刻的预估位姿状态向量；

x(k-1|k-1)是k-1时刻的最优位姿状态；

kg(k)是k时刻的卡尔曼增益；

x(k|k)是k时刻的最优位姿状态估计；

q是系统过程噪声w(k)的协方差；

r是系统测量噪声v(k)的协方差；

p代表协方差矩阵；

p(k|k-1)、p(k-1|k-1)、p(k|k)分别表示k时刻的先验协方差矩阵，k-1时刻的后验协方差矩阵，k时刻的后验协方差矩阵；

a′表示状态转移矩阵a的逆矩阵，h′表示测量矩阵h的逆矩阵。

需要说明的是，本实施例中采用卡尔曼滤波状态估计过程中的经典公式，通过将

需要说明的是，根据状态空间转移方程得到(k-1)时刻后验位姿状态向量的估计值，并通过设定的预测方程组对(k-1)时刻后验位姿状态向量的估计值处理，得到当前时刻k的先验位姿状态估计量，通过对比k时刻的先验位姿和(k-1)时刻的后验位姿，可以得到k时刻的控制信息即里程信息uk，k时刻的观测信息zk。

作为进一步优选的方案，上述步骤s5：在粒子滤波器中，对智能车辆在k时刻的里程信息uk、k时刻的观测信息zk进行处理，得到智能车辆的最终位姿，包括：

设置n个粒子，采样计算权重其中，每个粒子代表智能车当前的位姿，表示粒子滤波，x表示一个粒子的位姿，p表示粒子位姿的推算过程，1≤i≤n；

根据智能车辆在k时刻的里程信息uk，推算k时刻智能车位姿的分布，对每个粒子的位姿信息进行采样

需要说明的是，初始化过程中粒子的位姿是直接赋值的，不需要推算。

计算以当前粒子为车辆中心，全局地图与车辆中心周边的局部地图进行匹配的误差e，并将当前粒子的误差除以所有粒子的总误差得到当前时刻k时粒子i的权重

根据k时刻粒子i的位姿信息以及重要性权值计算出当前时刻k时智能车辆的后验位姿作为最终位姿x(k)，其中

作为进一步优选的方案，在计算出k时刻智能车辆的后验位姿后还包括：

对每个粒子权重信息进行重采样，并根据采样结果增加权值高的粒子，删除权值低的粒子，对所述当前智能车辆的后验位姿即最终位姿进行优化。

需要说明的是，本实施例中通过在粒子滤波器中采用序贯重要性采样算法和采样重要性重采样算法来获得粒子的后验概率，通过对参考分布的采样粒子进行加权来近似得到后验概率分布。

本方案通过使用卡尔曼滤波器对slam坐标和gps坐标进行融合再配合粒子滤波器将局部地图和先验地图进行匹配从而进一步对上述定位结果进行优化，最后可以得到一个更为精确的定位结果。对比别的定位方案，本方案可以更有效的对智能车辆进行定位，特别是可以有效克服由于gps漂移而带来的定位误差。同时由于使用了经过离线优化的先验地图进行匹配，本方案对比别的定位方案会更为鲁棒。

需要说明的是，本方案的总体思路为：通过构建智能车辆在整个行驶区域的先验全局地图，对全局地图进行离线优化得到一个更为准确的地图信息。通过结合多传感器来有效克服gps信号弱以及车辆里程计漂移的问题。通过局部地图与全局地图匹配的方式，解决计算机计算能力的限制造成实时定位与地图构建精度不高的问题。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。