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

权利要求书

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

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

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

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

步骤3：计算激光雷达点云残差、视觉重投影误差、IMU预积分残差和GNSS信息的伪距残差、多普勒频移残差、接收机时钟残差并分别作为因子构建因子图优化模型，求解载体的位置信息；选取关键帧并构建滑动窗口对其中所有的状态量进行优化求解，其中n为滑动窗口中的帧数，m为滑动窗口中所有特征点的数量，分别为载体的位置、速度和姿态，b_a、b_ω为加速度计和陀螺仪的偏置，δt为接收机时钟钟差，为接收机时钟钟差变化率，为相机外参，为激光雷达外参，ρ_k,k∈[0,m]为特征点的逆深度，ψ为载体坐标系和ENU坐标系之间的偏航角；

使用视觉特征点可用性状态切换策略，在视觉特征点匹配小于设定阈值或者初始化IMU的陀螺仪的偏置大于设定阈值时，锁定受视觉特征影响的状态量并在因子图中隔离视觉重投影误差因子，系统退化为激光惯性卫星耦合的定位方法；

通过星历信息对卫星GNSS信息进行筛选，当无可用的GNSS信息时，锁定仅受GNSS信息影响的状态量并在因子图中隔离GNSS信息的伪距残差因子、多普勒频移残差因子和接收机时钟残差因子，此时因子图中剩激光雷达点云残差、视觉重投影误差和IMU残差，系统自动切换到激光视觉惯性定位系统，由此解算出的载体相对位姿联合有GNSS信息时解算出的转换阵，依旧可以不间断得获得ENU坐标系下的绝对位姿，待重新接收到可用得GNSS信息时，再对仅受GNSS信息影响的状态量进行解锁，并加入伪距残差因子、多普勒频移残差因子和接收机时钟残差因子进行联合优化，对偏航角ψ、接收机时钟钟差δt、接收机时钟钟差变化率继续进行优化迭代更新；

步骤4：将步骤3获得的和激光雷达频率相同的位置信息与帧间获取的IMU积分信息统一到同一坐标系下进行加和，获得与IMU信息同频的高频定位信息。

2.根据权利要求1所述的针对野外洞穴森林环境侦察的多传感器融合定位方法，其特征在于所述的视觉重投影误差残差因子：

其中C表示滑窗中至少被观察到两次的特征集，为特征点l在第j帧中图像平面上的特征坐标，为第i帧中特征点在第j帧中的重投影位置，为第i帧中特征点在第j帧中的图像平面上的重投影特征坐标，为世界坐标系和第j帧IMU坐标系之间的转换矩阵，为第j帧IMU坐标系和世界坐标系之间的转换矩阵，为相机坐标系和IMU坐标系之间的转换矩阵，为IMU坐标系和世界坐标系之间的转换矩阵，ρ_l为特征点l的逆深度。