[发明专利]一种基于因子图优化的仿生偏振同步定位与构图的方法

说明书

本发明一种基于因子图优化的仿生偏振同步定位与构图的方法，包括选取组合导航系统状态，建立雷达里程计及偏振光相机量测模型和惯性导航系统运动模型并对组合导航系统初始化，根据当前时刻载体的状态节点位置约束、相邻状态节点位姿变换约束和状态节点航向约束，建立组合导航系统状态和量测节点误差函数获得待优化因子图，用高斯‑牛顿法对待优化因子图优化获得误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值；对最优估计值和地图更新同步实现载体的定位和构图。本发明用惯性、偏振和雷达里程计多源信息融合互补，具有不受外界干扰的特征；因子图优化量测和状态的历史信息特点使得环境适应性好、抗干扰性强以及定位与构图精度高。

技术领域

本发明属于自主导航技术的范畴,涉及一种基于因子图优化的仿生偏振同步定位与构图(SLAM)的方法。

背景技术

SLAM是的Simultaneous Localization and Mapping缩写，意为“同时定位与建图”。它是指运动体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。目前，SLAM技术主要被运用于航空航天、智能交通、物联网、VR和AR等多领域。

SLAM研究侧重于使用滤波器理论，最小化运动体位姿和地图的路标点的估计误差协方差，一般采用惯性导航系统的输出作为预测过程的输入，激光传感器的输出作为更新过程的输入。扩展卡尔曼滤波算法是目前大多数SLAM采用的滤波算法，缺点是滤波算法未完全融合历史信息，对离群值的抵抗性差、线性化过程中存在误差，使其精度下降、计算量会随着地图扩大而成倍增加。近年来图优化的发展非常迅速，基于图优化的SLAM，通常以关键帧为基础，建立多个节点和节点之间的相对变换关系，并不断地进行关键节点的维护，保证图的容量，在保证精度的同时，降低了计算量，且充分考虑历史信息。但是基于图优化的SLAM方案的定位精度和构图精确度也都依赖于传感器精度，当传感器受到干扰时(如卫星导航系统被屏蔽或受多径效应干扰时),SLAM系统的累积误差增大，精度降低。

近年来发现仿生偏振光具备较好的自主特性，有望用于SLAM系统中。1871年英国著名物理学家瑞利提出了瑞利散射定律，揭示了光线散射特性，随后基于瑞利散射定律获得了全空域大气偏振分布模式。大气偏振分布模式相对稳定，其中蕴涵着丰富的导航信息，自然界中很多生物都能够利用天空偏振光进行导航或辅助导航。

发明内容

主要解决的技术问题：本发明的目的是如何引入偏振光信息，将使用图优化方法应用到SLAM中以解决同时定位与构图中存在的自身位置确定困难、环境适应性差、抗干扰性低、定位与构图不够精确的问题。

为达成本发明要解决的技术问题，本发明提供一种基于因子图优化的仿生偏振同步定位与构图的方法，其技术方案包括以下步骤：

步骤1：选取载体姿态、速度、位置作为组合导航系统状态；

步骤2：建立雷达里程计量测模型、建立偏振光相机量测模型和建立惯性导航系统运动模型；

步骤3：对由雷达里程计、偏振光相机和惯性导航系统组成的组合导航系统初始化；

步骤4：获得当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束；

步骤5：根据当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束建立组合导航系统状态和量测节点的误差函数，获得待优化因子图，并对待优化因子图进行优化，获得误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值；

步骤6：对误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值和地图更新，同步实现载体的定位和构图。

优选地，通过步骤1所述的组合导航系统状态构建k时刻的组合导航系统的状态节点表示如下：