[发明专利]一种针对野外洞穴森林环境侦察的多传感器融合定位方法

说明书

本发明涉及一种针对野外洞穴森林环境侦察的多传感器融合定位方法，属于目标定位技术领域。通过激光惯性GNSS四传感器紧耦合的方式，将激光雷达点云残差、视觉重投影误差、IMU残差、GNSS的伪距、多普勒频移和时钟钟差以因子的形式加入到优化中，充分利用了激光雷达作用范围大、误差小、不受光线影响的优点，视觉充分利用纹理、像素特征信息，IMU高频的优点以及GNSS提供的绝对位置信息和无累积误差的优点，利用激光雷达信息、视觉、IMU信息和GNSS信息优化计算得到可靠精确的高频位置信息。

技术领域

本发明属于目标定位技术领域，具体涉及一种针对野外洞穴森林环境侦察的多传感器融合定位方法。

背景技术

同时定位与建图(Simultaneous Localization and Mapping,SLAM)技术是智能机器人领域的基础技术，能够准确的定位自身位置和感知周围环境是进行导航与探索的前提。

近年来，SLAM技术被广泛研究，出现了使用相机的视觉SLAM技术和使用激光雷达的激光SLAM技术。其中视觉SLAM技术依赖图像信息，因此在纹理弱、亮度暗、快速运动的情况下极易失效，而激光雷达因其可适应各种环境、可探测范围广、且在黑暗或弱纹理环境下仍然能有效获取点云信息的优点，在定位与建图上更稳定；但即便高性能的激光雷达，输出频率也只有10hz，在长走廊、平坦的空地以及墙角处也会出现定位发散的问题，且随之定位时间推移，会产生逐渐增大的累计误差；GNSS卫星导航系统能得到绝对位置，但是易受到屏蔽与干扰；IMU能输出高频的线加速度和角速度，但仅由IMU积分获得的相对位置误差会快速累积。本发明考虑野外侦察的实际场景，针对任务中常见的山洞、森林等低光照、弱纹理且卫星信号易受干扰的环境中卫星导航信息和视觉信息退化的问题，将激光雷达、视觉信息、GNSS卫星导航信息和IMU信息紧耦合，利用一个因子图进行全局优化，提出一种能够自适应切换系统融合状态，获取无缝高频绝对位置精确定位方法，可被应用于战术侦察、洞穴探测等领域。

发明内容

要解决的技术问题

针对野外侦察中的山洞、森林等低光照、弱纹理且卫星信号易受干扰的环境中卫星导航信息和视觉信息退化条件下，视觉定位与卫星定位极易发散的问题，本发明提出一种针对野外洞穴森林环境侦察的多传感器融合定位方法。

技术方案

一种针对野外洞穴森林环境侦察的多传感器融合定位方法，实现多传感器紧耦合定位系统在卫星导航信息和视觉信息退化条件下，能够自适应切换系统融合状态，获取无缝的高频的绝对位置；其特征在于步骤如下：

步骤1：对激光雷达输入的原始点云信息进行预处理，把点云信息进行点云分割并打上标签，剔除其中不可靠的点云信息，对余下的可靠的点云进行特征提取；对视觉图像信息进行特征点提取，通过激光雷达深度信息辅助视觉特征点跟踪；对IMU信息进行预积分处理；使用星历信息对GNSS信息进行可靠性筛选，得到稳定可靠的传感器信息；

步骤2：进行线性插值把激光雷达帧和视觉图像帧通过时间戳对齐；通过视觉的三维运动重建获得载体的相对于初始位置的初始相对位姿；将前几帧的激光雷达点云合并构成局部点云地图，将一帧的点云与局部点云地图进行匹配，根据匹配关系构建残差函数，求解获得载体的初始相对位姿；将视觉初始化相对位姿和激光雷达初始化位姿进行加权融合得到初始相对位姿；

将获得的初始相对位姿与IMU联合初始化得到IMU加速度计与陀螺仪的偏置以及重力矢量的对齐，利用SPP算法通过至少四颗以上的卫星的伪距信息计算出载体粗略的绝对位置，并将齐与相对位置对齐；将GNSS信息的多普勒频移与激光视觉惯性系统得到的相对速度对齐，计算求解激光视觉惯性系统相对位姿的local局部坐标系、ENU东北天坐标系与GNSS绝对位置所在的ECEF地心地固坐标系之间的转换矩阵；