一种基于因子图优化的仿生偏振同步定位与构图的方法技术

本发明专利技术一种基于因子图优化的仿生偏振同步定位与构图的方法，包括选取组合导航系统状态，建立雷达里程计及偏振光相机量测模型和惯性导航系统运动模型并对组合导航系统初始化，根据当前时刻载体的状态节点位置约束、相邻状态节点位姿变换约束和状态节点航向约束，建立组合导航系统状态和量测节点误差函数获得待优化因子图，用高斯

【技术实现步骤摘要】
一种基于因子图优化的仿生偏振同步定位与构图的方法


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

技术介绍

[0002]SLAM是的Simultaneous Localization and Mapping缩写，意为“同时定位与建图”。它是指运动体根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。目前，SLAM技术主要被运用于航空航天、智能交通、物联网、VR和AR等多领域。
[0003]SLAM研究侧重于使用滤波器理论，最小化运动体位姿和地图的路标点的估计误差协方差，一般采用惯性导航系统的输出作为预测过程的输入，激光传感器的输出作为更新过程的输入。扩展卡尔曼滤波算法是目前大多数SLAM采用的滤波算法，缺点是滤波算法未完全融合历史信息，对离群值的抵抗性差、线性化过程中存在误差，使其精度下降、计算量会随着地图扩大而成倍增加。近年来图优化的发展非常迅速，基于图优化的SLAM，通常以关键帧为基础，建立多个节点和节点之间的相对变换关系，并不断地进行关键节点的维护，保证图的容量，在保证精度的同时，降低了计算量，且充分考虑历史信息。但是基于图优化的SLAM方案的定位精度和构图精确度也都依赖于传感器精度，当传感器受到干扰时(如卫星导航系统被屏蔽或受多径效应干扰时),SLAM系统的累积误差增大，精度降低。
[0004]近年来发现仿生偏振光具备较好的自主特性，有望用于SLAM系统中。1871年英国著名物理学家瑞利提出了瑞利散射定律，揭示了光线散射特性，随后基于瑞利散射定律获得了全空域大气偏振分布模式。大气偏振分布模式相对稳定，其中蕴涵着丰富的导航信息，自然界中很多生物都能够利用天空偏振光进行导航或辅助导航。

技术实现思路

[0005]主要解决的技术问题：本专利技术的目的是如何引入偏振光信息，将使用图优化方法应用到SLAM中以解决同时定位与构图中存在的自身位置确定困难、环境适应性差、抗干扰性低、定位与构图不够精确的问题。
[0006]为达成本专利技术要解决的技术问题，本专利技术提供一种基于因子图优化的仿生偏振同步定位与构图的方法，其技术方案包括以下步骤：
[0007]步骤1：选取载体姿态、速度、位置作为组合导航系统状态；
[0008]步骤2：建立雷达里程计量测模型、建立偏振光相机量测模型和建立惯性导航系统运动模型；
[0009]步骤3：对由雷达里程计、偏振光相机和惯性导航系统组成的组合导航系统初始化；
[0010]步骤4：获得当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束；
[0011]步骤5：根据当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和
载体状态节点航向约束建立组合导航系统状态和量测节点的误差函数，获得待优化因子图，并对待优化因子图进行优化，获得误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值；
[0012]步骤6：对误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值和地图更新，同步实现载体的定位和构图。
[0013]优选地，通过步骤1所述的组合导航系统状态构建k时刻的组合导航系统的状态节点表示如下：
[0014][0015]其中：x
k
为k时刻的组合导航系统的状态节点，表示k时刻载体世界坐标系下的三维位置信息，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值，T表示向量的转置；表示k时刻载体世界坐标系下的三维速度，分别表示k时刻载体世界坐标系下指向x,y,z三个轴方向的速度；表示载体姿态，载体姿态以欧拉角的形式表示，分别为滚转角、俯仰角和偏航角。
[0016]优选地，选取所述雷达里程计测量的位置信息作为测量值，建立步骤2所述雷达里程计的量测模型如下：
[0017]z
l,k
＝h
l
(x
k
)+v
l,k
[0018][0019]其中：表示k时刻雷达里程计量测值，其为雷达里程计量测的x,y,z三个轴方向的坐标值；h
l
(x
k
)为雷达里程计量测函数，x
k
为k时刻的组合导航系统的状态节点，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值；v
l,k
表示雷达里程计量测的高斯噪声。
[0020]优选地，建立步骤2所述偏振光相机量测模型如下：
[0021]z
p,k
＝h
p
(x
k
)+v
p,k
[0022][0023]其中：为偏振光相机测量值，为偏振光相机量测的载体纵轴与太阳子午线的夹角作为量测值数据；h
p
(x
k
)为偏振光相机测量函数，x
k
为k时刻的组合导航系统的状态节点；为载体偏航角，太阳方位角A
s
可以通过查表计算，所述太阳方位角其为太阳高度角，为太阳
赤纬，为观测点的纬度，为太阳时角；v
p,k
表示偏振光相机量测的高斯噪声。
[0024]优选地，基于惯性导航系统测量的载体姿态、速度、位置作为测量值数据建立步骤2所述的基于惯性导航系统的运动模型如下：
[0025][0026][0027]其中为惯性导航系统的量测值，其中为惯性导航系统量测的x,y,z三个轴方向的坐标值；h
i
(x
k
‑1,u
k
‑1)为惯性导航系统的量测函数,为k
‑
1时刻载体世界坐标系下的位置、速度、姿态；为k
‑
1时刻的控制输入；其中为世界坐标系下惯性导航系统补偿零偏后的加速度和角速度，为载体坐标系下惯性导航系统补偿的加速度和角速度，Δt为惯性导航系统采样间隔，为载体坐标系到世界坐标系的旋转矩阵：
[0028][0029]为标定后的加计和陀螺零偏常值,v
i,k
‑1表示惯性导航系统量测的高斯噪声。
[0030]优选地，步骤3所述组合导航系统初始化是根据载体坐标系到世界坐标系的旋转矩阵将载体坐标系下的雷达扫描点云转换到全局坐标系下，并建立以起始位置为原点的全局地图。
[0031]优选地，获得步骤4所述当前时刻载体状态节点位置约束的步骤如下：根据获取当前时刻时间戳和雷达里程计量测数据，将雷达里程计量测数据中的扫描点注册到地图中，通过雷达里程计量测模型和雷达里程计量测数据建立当前时刻载体状态节点位置约束。
[0032]优选地，获得步骤4所述载体相邻状态节点位姿变换约束的步骤如下：对当前时刻时间戳和上一时刻时间戳之间所有的惯性导航量测数据通过惯性导航系统运动模型进行积分，利用积分得到载体的姿态、速度、位置数据建立载体相邻状态节点位姿变换约束。
[0033]优选地，获得步骤4载体状态节点航向约束的步骤如下：寻找当前时刻时间戳的偏本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于因子图优化的仿生偏振同步定位与构图的方法，其特征在于，所述方法包括以下步骤：步骤1：选取载体姿态、速度、位置作为组合导航系统状态；步骤2：建立雷达里程计量测模型、建立偏振光相机量测模型和建立惯性导航系统运动模型；步骤3：对由雷达里程计、偏振光相机和惯性导航系统组成的组合导航系统初始化；步骤4：获得当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束；步骤5：根据当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束建立组合导航系统状态和量测节点的误差函数，获得待优化因子图，并对待优化因子图进行优化，获得误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值；步骤6：对误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值和地图更新，同步实现载体的定位和构图。2.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：通过步骤1所述的组合导航系统状态构建k时刻的组合导航系统的状态节点表示如下：其中：x
k
为k时刻的组合导航系统的状态节点，表示k时刻载体世界坐标系下的三维位置信息，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值，T表示向量的转置；表示k时刻载体世界坐标系下的三维速度，分别表示k时刻载体世界坐标系下指向x,y,z三个轴方向的速度；表示载体姿态，载体姿态以欧拉角的形式表示，分别为滚转角、俯仰角和偏航角。3.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：选取所述雷达里程计测量的位置信息作为测量值，建立步骤2所述雷达里程计的量测模型如下：z
l,k
＝h
l
(x
k
)+v
l,k
其中：表示k时刻雷达里程计量测值，其为雷达里程计
量测的x,y,z三个轴方向的坐标值；h
l
(x
k
)为雷达里程计量测函数，x
k
为k时刻的组合导航系统的状态节点，分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值；v
l,k
表示雷达里程计量测的高斯噪声。4.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：建立步骤2所述偏振光相机量测模型如下：z
p,k
＝h
p
(x
k
)+v
p,k
其中：为偏振光相机测量值，为偏振光相机量测的载体纵轴与太阳子午线的夹角作为量测值数据；h
p
(x
k
)为偏振光相机测量函数，x
k
为k时刻的组合导航系统的状态节点；为载体偏航角，太阳方位角A
s
可以通过查表计算，所述太阳方位角其为太阳高度角，为太阳赤纬，为观测点的纬度，为太阳时角；v
p,k
表示偏振光相机量测的高斯噪声。5.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位与构图的方法，其特征在于：基于惯性导航系统测量的载体姿态、速度、位置作为测量值数据建立步骤2所述的基于惯性导航系统的运动模型如下：的基于惯性导航系统的运动模型如下：其中为惯性导航系统的量测值，其中为惯性导航系统量测的x,y,z三个轴方向的坐标值；h
i
(x
k

【专利技术属性】
技术研发人员：杜涛，曾云豪，石书尧，张洁，
申请(专利权)人：北方工业大学，
类型：发明
国别省市：