一种基于因子图优化的仿生偏振同步定位与构图的方法
阅读说明：本技术 一种基于因子图优化的仿生偏振同步定位与构图的方法 (Factor graph optimization-based bionic polarization synchronous positioning and composition method ) 是由 杜涛 曾云豪 石书尧 张洁 于 2020-12-07 设计创作，主要内容包括：本发明一种基于因子图优化的仿生偏振同步定位与构图的方法,包括选取组合导航系统状态,建立雷达里程计及偏振光相机量测模型和惯性导航系统运动模型并对组合导航系统初始化,根据当前时刻载体的状态节点位置约束、相邻状态节点位姿变换约束和状态节点航向约束,建立组合导航系统状态和量测节点误差函数获得待优化因子图,用高斯-牛顿法对待优化因子图优化获得误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值；对最优估计值和地图更新同步实现载体的定位和构图。本发明用惯性、偏振和雷达里程计多源信息融合互补,具有不受外界干扰的特征；因子图优化量测和状态的历史信息特点使得环境适应性好、抗干扰性强以及定位与构图精度高。(The invention relates to a bionic polarization synchronous positioning and composition method based on factor graph optimization, which comprises the steps of selecting a state of an integrated navigation system, establishing a radar odometer, a polarized light camera measurement model and an inertial navigation system motion model, initializing the integrated navigation system, establishing an integrated navigation system state and a measurement node error function according to state node position constraint, adjacent state node pose transformation constraint and state node course constraint of a carrier at the current moment to obtain a factor graph to be optimized, and optimizing the factor graph to be optimized by a Gauss-Newton method to obtain optimal estimated values of the position, the speed and the posture of the carrier under a world coordinate system with the minimum error function; and synchronously realizing the positioning and composition of the carrier for the optimal estimation value and the map updating. The invention uses the inertia, polarization and radar odometer multi-source information to be fused and complemented, and has the characteristic of no external interference; the factor graph optimizes the measurement and the historical information characteristics of the state, so that the environment adaptability is good, the anti-interference performance is strong, and the positioning and composition precision is high.)
技术领域
本发明属于自主导航技术的范畴,涉及一种基于因子图优化的仿生偏振同步定位与构图(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时刻的组合导航系统的状态节点表示如下：
其中：xk为k时刻的组合导航系统的状态节点，表示k时刻载体世界坐标系下的三维位置信息，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值，T表示向量的转置；表示k时刻载体世界坐标系下的三维速度，分别表示k时刻载体世界坐标系下指向x,y,z三个轴方向的速度；表示载体姿态，载体姿态以欧拉角的形式表示，分别为滚转角、俯仰角和偏航角。
优选地，选取所述雷达里程计测量的位置信息作为测量值，建立步骤2所述雷达里程计的量测模型如下：
zl,k＝hl(xk)+vl,k
其中：表示k时刻雷达里程计量测值，其为雷达里程计量测的x,y,z三个轴方向的坐标值；hl(xk)为雷达里程计量测函数，xk为k时刻的组合导航系统的状态节点，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值；vl,k表示雷达里程计量测的高斯噪声。
优选地，建立步骤2所述偏振光相机量测模型如下：
zp,k＝hp(xk)+vp,k
其中：为偏振光相机测量值，为偏振光相机量测的载体纵轴与太阳子午线的夹角作为量测值数据；hp(xk)为偏振光相机测量函数，xk为k时刻的组合导航系统的状态节点；为载体偏航角，太阳方位角As可以通过查表计算，所述太阳方位角其为太阳高度角，为太阳赤纬，为观测点的纬度，为太阳时角；vp,k表示偏振光相机量测的高斯噪声。
优选地，基于惯性导航系统测量的载体姿态、速度、位置作为测量值数据建立步骤2所述的基于惯性导航系统的运动模型如下：
其中为惯性导航系统的量测值，其中为惯性导航系统量测的x,y,z三个轴方向的坐标值；hi(xk-1,uk-1)为惯性导航系统的量测函数,为k-1时刻载体世界坐标系下的位置、速度、姿态；为k-1时刻的控制输入；其中为世界坐标系下惯性导航系统补偿零偏后的加速度和角速度，为载体坐标系下惯性导航系统补偿的加速度和角速度，Δt为惯性导航系统采样间隔，为载体坐标系到世界坐标系的旋转矩阵：
为标定后的加计和陀螺零偏常值,vi,k-1表示惯性导航系统量测的高斯噪声。
优选地，步骤3所述组合导航系统初始化是根据载体坐标系到世界坐标系的旋转矩阵将载体坐标系下的雷达扫描点云转换到全局坐标系下，并建立以起始位置为原点的全局地图。
优选地，获得步骤4所述当前时刻载体状态节点位置约束的步骤如下：根据获取当前时刻时间戳和雷达里程计量测数据，将雷达里程计量测数据中的扫描点注册到地图中，通过雷达里程计量测模型和雷达里程计量测数据建立当前时刻载体状态节点位置约束。
优选地，获得步骤4所述载体相邻状态节点位姿变换约束的步骤如下：对当前时刻时间戳和上一时刻时间戳之间所有的惯性导航量测数据通过惯性导航系统运动模型进行积分，利用积分得到载体的姿态、速度、位置数据建立载体相邻状态节点位姿变换约束。
优选地，获得步骤4载体状态节点航向约束的步骤如下：寻找当前时刻时间戳的偏振光相机量测数据，通过偏振光相机量测模型和偏振光相机量测数据建立载体状态节点航向约束。
优选地，获得步骤5所述误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值的具体步骤如下：
步骤(5a)：构建载体(k-m)～k时刻m为正整数的状态节点误差函数：
步骤(5b)：根据状态节点位置约束，建立雷达里程计量测的误差函数为：
其中el,k为雷达里程计量测误差，Σl,k为雷达里程计节点协方差；
步骤(5c)：根据状态节点航向约束，建立偏振光相机量测的误差函数为：
其中ep,k为偏振光相机量测误差，Σp,k为偏振光相机节点协方差；
步骤(5d)：根据相邻状态节点位姿变换约束，建立惯性导航系统量测的误差函数为：
其中ei,k为惯性导航系统量测误差，Σi,k为惯性导航系统运动模型节点协方差；
步骤(5e)：建立因子图优化问题,表示如下：
其中 为X的最优估计；
步骤(5f)：使用高斯-牛顿法对因子图优化问题进行求解，获得误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值。
优选地，对所述误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值更新是对的状态节点的更新，并将对应的更新后的雷达里程计的扫描点注册到全局地图中，再对全局地图进行更新，并对全局地图中重复的雷达里程计扫描点进行剔除。
本发明的有益效果：本发明一种基于因子图优化的仿生偏振同步定位与构图的方法，对如何确定自身位置及感知外界环境的问题，SLAM系统旨在通过系统模型，结合相应的滤波方法或优化方法完成定位与周围环境的绘制，是通过建立惯性导航系统模型和雷达里程计量测模型、偏振光相机的量测模型，利用因子图优化算法实现位置的确定和周围环境地图的构建，有效的利用了偏振信息、惯导信息和雷达里程计信息及多源信息的融合匹配互补、不受其他外界干扰的特性，因子图优化充分考虑量测和状态的历史信息的特点，环境适应性更好、抗干扰性更强、定位与构图精度更高。无需依赖先验知识，环境适应性更好、鲁棒性更强、定位与构图精度更高。本发明使用了偏振导航机制是一种非常有效的导航手段，具有无源、无辐射、隐蔽性好等特点,能够为复杂环境下的导航任务提供新的解决途径。引入偏振信息，使用图优化方法在SLAM上可以提高自身位置的和构建周围环境地图的精确度，提升了系统鲁棒性、自主性。
附图说明
图1是本发明一种基于因子图优化的仿生偏振同时定位与构图(SLAM)的方法流程图。
图2是本发明因子图优化结构图。
图2中符号含义说明：
状态节点，偏振光相机节点，
■惯性导航系统节点，●雷达里程计节点。
具体实施方式
下面结合附图详细说明本发明，其作为本说明书的一部分，通过实施来说明本发明的原理，本发明的其他方面、特征及其优点通过该详细说明将会变得一目了然。
本发明涉及一种基于图优化的仿生偏振SLAM方法，选择世界坐标系下的位置、速度、姿态作为状态向量，建立惯导/偏振相机/雷达里程计组合导航系统状态方程；基于惯性导航系统输出的位置、速度、姿态信息，建立基于惯性导航系统的运动模型，构建惯性导航系统运动模型节点；基于偏振相机输出偏振方位角信息，建立偏振量测方程，构建偏振相机节点；基于雷达里程计输出的位置信息，建立雷达里程计量测方程，构建雷达里程计节点，使用因子图优化算法实现位姿确定和环境地图构建。
请参阅本发明图1示出一种基于因子图优化的仿生偏振SLAM方法，具体实施的技术方案包括以下步骤：
步骤1：开始一种基于因子图优化的仿生偏振SLAM方法，选取载体姿态、速度、位置作为组合导航系统状态；
步骤2：建立雷达里程计量测模型；建立偏振光相机量测模型；建立惯性导航系统运动模型；
步骤3：对由雷达里程计、偏振光相机和惯性导航系统组成的组合导航系统初始化；
步骤4：获得当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束；
步骤5：根据当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束建立组合导航系统状态和量测节点的误差函数，获得到待优化因子图，并对待优化因子图进行优化，获得误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值；
步骤6：对误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值和地图更新，同步实现载体的定位和构图；
步骤7：判断是否有新最优估计值和地图数据输入，当有新最优估计值和地图数据输入，则执行步骤8；当没有新最优估计值和地图数据输入，则执行步骤9；
步骤8：新最优估计值和地图数据输入执行步骤4；
步骤9：没有新最优估计值和地图数据输入结束。
优选实施例，本发明通过步骤1所述的组合导航系统状态构建k时刻的组合导航系统的状态节点的具体步骤如下：
(1)、选取姿态、速度、位置为组合导航系统状态构建状态节点；
(2)、以起始位置为原点的世界坐标系(world系)，即w系，以地理正北方向为世界坐标系的x轴的正方向，以正西方向为世界坐标系的y轴的正方向，根据右手准则确定世界坐标系的z轴正方向；
(3)、建立以载体中心为原点的载体坐标系，即b系，以平行于载体纵轴指向机头方向为载体坐标系x轴的正方向，以平行于载体横轴指向左方为载体坐标系y轴的正方向，根据右手准则确定载体坐标系z轴正方向；
(4)、选取世界坐标系下的位置、速度、姿态作为组合导航系统状态，则构建k时刻组合导航系统状态如下：
其中：xk为k时刻的组合导航系统的状态节点，表示k时刻载体世界坐标系下的三维位置信息，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值，T表示向量的转置；表示k时刻载体世界坐标系下的三维速度，分别表示k时刻载体世界坐标系下指向x,y,z三个轴方向的速度；表示载体姿态，载体姿态以欧拉角的形式表示，分别为滚转角、俯仰角和偏航角。
优选实施例，本发明选取所述雷达里程计测量的位置信息作为测量值，建立步骤2所述雷达里程计的量测模型如下：
zl,k＝hl(xk)+vl,k (2)
其中：表示k时刻雷达里程计量测值，其为雷达里程计量测的x,y,z三个轴方向的坐标值；hl(xk)为雷达里程计量测函数，xk为k时刻的组合导航系统的状态节点，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值；vl,k表示雷达里程计量测的高斯噪声。
优选实施例，建立步骤2所述偏振光相机量测模型如下：
zp,k＝hp(xk)+vp,k (4)
其中：为偏振光相机测量值，为偏振光相机量测的载体纵轴与太阳子午线的夹角作为量测值数据；hp(xk)为偏振光相机测量函数，xk为k时刻的组合导航系统的状态节点；为载体偏航角，太阳方位角As可以通过查表计算，所述太阳方位角其为太阳高度角，为太阳赤纬，为观测点的纬度，为太阳时角；vp,k表示偏振光相机量测的高斯噪声。
优选实施例，本发明基于惯性导航系统测量的载体姿态、速度、位置作为测量值数据建立步骤2所述的基于惯性导航系统的运动模型如下：
其中为惯性导航系统的量测值，其中为惯性导航系统量测的x,y,z三个轴方向的坐标值；hi(xk-1,uk-1)为惯性导航系统的量测函数,为k-1时刻载体世界坐标系下的位置、速度、姿态；为k-1时刻的控制输入；其中为世界坐标系下惯性导航系统补偿零偏后的加速度和角速度，为载体坐标系下惯性导航系统补偿的加速度和角速度，Δt为惯性导航系统采样间隔，为载体坐标系到世界坐标系的旋转矩阵：
为标定后的加计和陀螺零偏常值,vi,k-1表示惯性导航系统量测的高斯噪声。
优选实施例，本发明步骤3所述组合导航系统初始化是根据载体坐标系到世界坐标系的旋转矩阵将载体坐标系下的雷达扫描点云转换到全局坐标系下，并建立以起始位置为原点的全局地图。
优选实施例，本发明获得步骤4所述当前时刻载体状态节点位置约束的步骤如下：根据获取当前时刻时间戳和雷达里程计量测数据，将雷达里程计量测数据中的扫描点注册到地图中，通过雷达里程计量测模型和雷达里程计量测数据建立当前时刻载体状态节点位置约束。
优选实施例，本发明获得步骤4所述载体相邻状态节点位姿变换约束的步骤如下：对当前时刻时间戳和上一时刻时间戳之间所有的惯性导航量测数据通过惯性导航系统运动模型进行积分，利用积分得到载体的姿态、速度、位置数据建立载体相邻状态节点位姿变换约束。
优选实施例，本发明获得步骤4载体状态节点航向约束的步骤如下：寻找当前时刻时间戳的偏振光相机量测数据，通过偏振光相机量测模型和偏振光相机量测数据建立载体状态节点航向约束。
优选实施例，获得步骤5所述误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值的具体步骤如下：
步骤(5a)：构建载体(k-m)～k时刻m为正整数的状态节点误差函数：
步骤(5b)：根据状态节点位置约束，建立雷达里程计量测的误差函数为：
其中el,k为雷达里程计量测误差，Σl,k为雷达里程计节点协方差；
步骤(5c)：根据状态节点航向约束，建立偏振光相机量测的误差函数为：
其中ep,k为偏振光相机量测误差，∑p,k为偏振光相机节点协方差；
步骤(5d)：根据相邻状态节点位姿变换约束，建立惯性导航系统量测的误差函数为：
其中ei,k为惯性导航系统量测误差，∑i,k为惯性导航系统运动模型节点协方差；
步骤(5e):建立因子图优化问题,表示如下：
其中X＝[xk-m…xk]T，为X的最优估计；
步骤(5f)：使用高斯-牛顿法对因子图优化问题进行求解，获得误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值。
优选实施例，本发明对所述误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值更新是对的状态节点的更新，并将对应的更新后的雷达里程计的扫描点注册到全局地图中，再对全局地图进行更新，并对全局地图中重复的雷达里程计扫描点进行剔除。
请参阅图2示出的本发明中因子图优化结构图，下面利用图2对本发明一种基于因子图优化的仿生偏振SLAM方法的因子图优化结构做进一步的描述：第0个时刻的状态节点为x0，x0与第0个时刻的雷达里程计节点通过雷达里程计第0个时刻的量测数据和雷达里程计量测模型、建立当前载体状态节点位置约束，从而建立状态节点x0与雷达里程计节点的连接关系；x0与第0个时刻的偏振光相机节点通过偏振光相机第0个时刻的量测数据和偏振光相机量测模型，建立当前载体状态节点航向约束，从而建立状态节点x0与偏振光相机节点的连接关系；第0个时刻的惯性导航系统节点与x0、x1通过惯性导航0到1时刻之间的量测数据和惯性导航系统运动模型，建立载体x0、x1状态节点之间的位姿变换约束，从而建立状态节点x0、x1与惯性导航系统节点的连接关系；
第1个时刻的状态节点为x1，x1与第1个时刻的雷达里程计节点通过雷达里程计第1个时刻的量测数据和雷达里程计量测模型、建立当前载体状态节点位置约束，从而建立状态节点x1与雷达里程计节点的连接关系；x1与第1个时刻的偏振光相机节点通过偏振光相机第1个时刻的量测数据和偏振光相机量测模型，建立当前载体状态节点航向约束，从而建立状态节点x1与偏振光相机节点的连接关系；第1个时刻的惯性导航系统节点与x1、x2通过惯性导航1到2时刻之间的量测数据和惯性导航系统运动模型，建立载体x1、x2状态节点之间的位姿变换约束，从而建立状态节点x1、x2与惯性导航系统节点的连接关系；
第2个时刻的状态节点为x2，x2与第2个时刻的雷达里程计节点通过雷达里程计第2个时刻的量测数据和雷达里程计量测模型、建立当前载体状态节点位置约束，从而建立状态节点x2与雷达里程计节点的连接关系；x2与第2个时刻的偏振光相机节点通过偏振光相机第2个时刻的量测数据和偏振光相机量测模型，建立当前载体状态节点航向约束，从而建立状态节点x2与偏振光相机节点的连接关系；第2个时刻的惯性导航系统节点与x2、x3通过惯性导航2到3时刻之间的量测数据和惯性导航系统运动模型，建立载体x2、x3状态节点之间的位姿变换约束，从而建立状态节点x2、x3与惯性导航系统节点的连接关系；
第k个时刻的状态节点为xk，xk与第k个时刻的雷达里程计节点通过雷达里程计第k个时刻的量测数据和雷达里程计量测模型、建立当前载体状态节点位置约束，从而建立状态节点xk与雷达里程计节点的连接关系；xk与第k个时刻的偏振光相机节点通过偏振光相机第k个时刻的量测数据和偏振光相机量测模型，建立当前载体状态节点航向约束，从而建立状态节点xk与偏振光相机节点的连接关系；第k-1个时刻的惯性导航系统节点与xk-1、xk通过惯性导航k-1到k时刻之间的量测数据和惯性导航系统运动模型，建立载体xk-1、xk状态节点之间的位姿变换约束，从而建立状态节点xk-1、xk与惯性导航系统节点的连接关系。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。