一种基于先验地图的智能车辆定位方法技术

本发明专利技术公开一种基于先验地图的智能车辆定位方法，包括通过智能车辆的里程计、车载雷达，分别获取智能车辆的位姿信息、观测信息；利用SLAM算法对智能车辆的位姿信息、观测信息进行处理，得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息；获取智能车辆的GPS位姿信息，并将GPS位姿信息转换至SLAM坐标系下，得到智能车辆在SLAM坐标系下的GPS位姿信息；利用卡尔曼滤波技术，将SLAM坐标系下的位姿信息和SLAM坐标系下的GPS位姿信息进行融合，得到智能车辆的初步位姿估计；将全局地图和局部地图进行匹配，利用匹配结果对初步位姿估计进行优化，得到智能车辆的最终位姿。本发明专利技术可极大提高智能车定位结果的精度和准确性。

An intelligent vehicle location method based on prior map

The invention discloses an intelligent vehicle positioning method based on a prior map, which includes acquiring position and attitude information and observation information of intelligent vehicle respectively through odometer and vehicle-borne radar of intelligent vehicle, processing position and attitude information and observation information of intelligent vehicle by SLAM algorithm, and obtaining global map of the whole operation area. Local map of local operation area and position and attitude information of intelligent vehicle in SLAM coordinate system; Obtain GPS position and attitude information of intelligent vehicle, and convert GPS position and attitude information to SLAM coordinate system to get GPS position and attitude information of intelligent vehicle in SLAM coordinate system; Use Kalman filter technology to get position and attitude information of intelligent vehicle in SLAM coordinate system and S GPS position and attitude information in LAM coordinate system is fused to get the initial position and attitude estimation of intelligent vehicle; global map and local map are matched to optimize the initial position and attitude estimation using the matching results, and the final position and attitude of intelligent vehicle are obtained. The invention can greatly improve the accuracy and accuracy of the positioning result of the intelligent vehicle.

【技术实现步骤摘要】
一种基于先验地图的智能车辆定位方法
本专利技术涉及智能车辆定位
，特别涉及一种基于先验地图的智能车辆定位方法。
技术介绍
智能车辆自主驾驶或辅助驾驶的前提之一就是要准确知道车辆的位姿，当前智能车辆自主驾驶或辅助驾驶中，定位方法一般包括：基于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.一种基于先验地图的智能车辆定位方法，智能车辆在预先设定的运行区域内移动，其特征在于，包括：通过智能车辆的里程计、车载雷达，分别获取智能车辆的位姿信息、观测信息，所述位姿信息包括智能车辆在整个运行区域的位姿信息以及在局部运行区域的位姿信息，所述观测信息包括覆盖整个运行区域的观测信息以及覆盖局部运行区域的观测信息；利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理，得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息；获取智能车辆在GPS地理坐标系下的位姿信息，并将GPS地理坐标系下的位姿信息转换至所述SLAM坐标系下，得到智能车辆在SLAM坐标系下的GPS位姿信息；利用卡尔曼滤波技术，将智能车辆SLAM坐标系下的位姿信息和智能车辆在SLAM坐标系下的GPS位姿信息进行融合，得到智能车辆的初步位姿估计；利用所述全局地图和局部地图进行匹配，并利用匹配结果对所述初步位姿估计进行优化，得到智能车辆的最终位姿。

【技术特征摘要】
1.一种基于先验地图的智能车辆定位方法，智能车辆在预先设定的运行区域内移动，其特征在于，包括：通过智能车辆的里程计、车载雷达，分别获取智能车辆的位姿信息、观测信息，所述位姿信息包括智能车辆在整个运行区域的位姿信息以及在局部运行区域的位姿信息，所述观测信息包括覆盖整个运行区域的观测信息以及覆盖局部运行区域的观测信息；利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理，得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息；获取智能车辆在GPS地理坐标系下的位姿信息，并将GPS地理坐标系下的位姿信息转换至所述SLAM坐标系下，得到智能车辆在SLAM坐标系下的GPS位姿信息；利用卡尔曼滤波技术，将智能车辆SLAM坐标系下的位姿信息和智能车辆在SLAM坐标系下的GPS位姿信息进行融合，得到智能车辆的初步位姿估计；利用所述全局地图和局部地图进行匹配，并利用匹配结果对所述初步位姿估计进行优化，得到智能车辆的最终位姿。2.如权利要求1所述的基于先验地图的智能车辆定位方法，其特征在于，所述通过智能车辆的里程计、车载雷达，分别获取智能车辆的位姿信息、观测信息，包括：构建里程计坐标系以及里程计坐标系下的智能车辆位姿模型，并结合智能车辆自带的里程计，获得智能车辆的位姿信息；利用所述车载雷达获取并记录智能车辆的观测信息。3.如权利要求2所述的基于先验地图的智能车辆定位方法，其特征在于，所述利用SLAM算法对所述智能车辆的位姿信息、观测信息进行处理，得到整个运行区域的全局地图、局部运行区域的局部地图以及智能车辆SLAM坐标系下的位姿信息，包括：建立与所述里程计坐标系同原点同轴的所述SLAM坐标系；在所述SLAM坐标系下，将所述整个运行区域的位姿信息作为SLAM算法的控制输入信息，将所述覆盖整个运行区域的观测信息作为SLAM算法的观测信息，得到整个运行区域的全局地图；在所述SLAM坐标系下，将所述局部运行区域的位姿信息作为SLAM算法的控制输入信息，将所述覆盖局部运行区域的观测信息作为SLAM算法的观测信息，得到局部运行区域的局部地图和智能车辆SLAM坐标系下的位姿信息。4.如权利要求3所述的基于先验地图的智能车辆定位方法，其特征在于，还包括：将所述全局地图中相对于同一个观测节点的两个位姿节点之间的差值作为全局误差项，利用高斯牛顿法对所述全局误差项进行迭代优化；将优化完成后全局误差项的位姿节点、观测节点添加至所述全局地图中，并在所述全局地图中将相应的位姿节点、观测节点及节点之间边剔除以得到优化后的全局地图；将所述局部地图中相对于同一个观测节点的两个位姿节点之间的差值作为局部误差项，利用高斯牛顿法对所述局部误差项进行迭代优化；将优化完成后局部误差项的位姿节点、观测节点添加至所述局部地图中，并在所述局部地图中将相应的位姿节点、观测节点及节点之间的边剔...

【专利技术属性】
技术研发人员：祝辉，姜云天，余彪，梁华为，殷若尘，陶翔，黄健，
申请(专利权)人：中国科学院合肥物质科学研究院，
类型：发明
国别省市：安徽,34