CN112697138A - 一种基于因子图优化的仿生偏振同步定位与构图的方法 - Google Patents

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

Info

Publication number
CN112697138A
CN112697138A CN202011419966.0A CN202011419966A CN112697138A CN 112697138 A CN112697138 A CN 112697138A CN 202011419966 A CN202011419966 A CN 202011419966A CN 112697138 A CN112697138 A CN 112697138A
Authority
CN
China
Prior art keywords
carrier
navigation system
factor
graph
optimization
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202011419966.0A
Other languages
English (en)
Other versions
CN112697138B (zh
Inventor
杜涛
曾云豪
石书尧
张洁
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
North China University of Technology
Original Assignee
North China University of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by North China University of Technology filed Critical North China University of Technology
Priority to CN202011419966.0A priority Critical patent/CN112697138B/zh
Publication of CN112697138A publication Critical patent/CN112697138A/zh
Application granted granted Critical
Publication of CN112697138B publication Critical patent/CN112697138B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/14—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by recording the course traversed by the object
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

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

Description

一种基于因子图优化的仿生偏振同步定位与构图的方法
技术领域
本发明属于自主导航技术的范畴,涉及一种基于因子图优化的仿生偏振同步定位与构图(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时刻的组合导航系统的状态节点表示如下：
Figure BDA0002821781380000021
其中：xk为k时刻的组合导航系统的状态节点，
Figure BDA0002821781380000031
表示k时刻载体世界坐标系下的三维位置信息，
Figure BDA0002821781380000032
分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值，T表示向量的转置；
Figure BDA0002821781380000033
表示k时刻载体世界坐标系下的三维速度，
Figure BDA0002821781380000034
分别表示k时刻载体世界坐标系下指向x,y,z三个轴方向的速度；
Figure BDA0002821781380000035
表示载体姿态，载体姿态以欧拉角的形式表示，
Figure BDA0002821781380000036
分别为滚转角、俯仰角和偏航角。
优选地，选取所述雷达里程计测量的位置信息作为测量值，建立步骤2所述雷达里程计的量测模型如下：
zl,k＝hl(xk)+vl,k
Figure BDA0002821781380000037
其中：
Figure BDA0002821781380000038
表示k时刻雷达里程计量测值，其
Figure BDA0002821781380000039
为雷达里程计量测的x,y,z三个轴方向的坐标值；hl(xk)为雷达里程计量测函数，xk为k时刻的组合导航系统的状态节点，
Figure BDA00028217813800000310
分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值；vl,k表示雷达里程计量测的高斯噪声。
优选地，建立步骤2所述偏振光相机量测模型如下：
zp,k＝hp(xk)+vp,k
Figure BDA00028217813800000311
其中：
Figure BDA00028217813800000312
为偏振光相机测量值，
Figure BDA00028217813800000313
为偏振光相机量测的载体纵轴与太阳子午线的夹角作为量测值数据；hp(xk)为偏振光相机测量函数，xk为k时刻的组合导航系统的状态节点；
Figure BDA00028217813800000314
为载体偏航角，太阳方位角As可以通过查表计算，所述太阳方位角
Figure BDA0002821781380000041
其
Figure BDA0002821781380000042
为太阳高度角，
Figure BDA0002821781380000043
为太阳赤纬，
Figure BDA0002821781380000044
为观测点的纬度，
Figure BDA0002821781380000045
为太阳时角；vp,k表示偏振光相机量测的高斯噪声。
优选地，基于惯性导航系统测量的载体姿态、速度、位置作为测量值数据建立步骤2所述的基于惯性导航系统的运动模型如下：
Figure BDA0002821781380000046
Figure BDA0002821781380000047
其中
Figure BDA0002821781380000048
为惯性导航系统的量测值，其中
Figure BDA0002821781380000049
为惯性导航系统量测的x,y,z三个轴方向的坐标值；hi(xk-1,uk-1)为惯性导航系统的量测函数,
Figure BDA00028217813800000410
为k-1时刻载体世界坐标系下的位置、速度、姿态；
Figure BDA00028217813800000411
为k-1时刻的控制输入；其中
Figure BDA00028217813800000412
为世界坐标系下惯性导航系统补偿零偏后的加速度和角速度，
Figure BDA00028217813800000413
为载体坐标系下惯性导航系统补偿的加速度和角速度，Δt为惯性导航系统采样间隔，
Figure BDA00028217813800000414
为载体坐标系到世界坐标系的旋转矩阵：
Figure BDA00028217813800000415
Figure BDA00028217813800000416
为标定后的加计和陀螺零偏常值,vi,k-1表示惯性导航系统量测的高斯噪声。
优选地，步骤3所述组合导航系统初始化是根据载体坐标系到世界坐标系的旋转矩阵
Figure BDA00028217813800000417
将载体坐标系下的雷达扫描点云转换到全局坐标系下，并建立以起始位置为原点的全局地图。
优选地，获得步骤4所述当前时刻载体状态节点位置约束的步骤如下：根据获取当前时刻时间戳和雷达里程计量测数据，将雷达里程计量测数据中的扫描点注册到地图中，通过雷达里程计量测模型和雷达里程计量测数据建立当前时刻载体状态节点位置约束。
优选地，获得步骤4所述载体相邻状态节点位姿变换约束的步骤如下：对当前时刻时间戳和上一时刻时间戳之间所有的惯性导航量测数据通过惯性导航系统运动模型进行积分，利用积分得到载体的姿态、速度、位置数据建立载体相邻状态节点位姿变换约束。
优选地，获得步骤4载体状态节点航向约束的步骤如下：寻找当前时刻时间戳的偏振光相机量测数据，通过偏振光相机量测模型和偏振光相机量测数据建立载体状态节点航向约束。
优选地，获得步骤5所述误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值的具体步骤如下：
步骤(5a)：构建载体(k-m)～k时刻m为正整数的状态节点误差函数：
步骤(5b)：根据状态节点位置约束，建立雷达里程计量测的误差函数为：
Figure BDA0002821781380000051
其中el,k为雷达里程计量测误差，Σl,k为雷达里程计节点协方差；
步骤(5c)：根据状态节点航向约束，建立偏振光相机量测的误差函数为：
Figure BDA0002821781380000052
其中ep,k为偏振光相机量测误差，Σp,k为偏振光相机节点协方差；
步骤(5d)：根据相邻状态节点位姿变换约束，建立惯性导航系统量测的误差函数为：
Figure BDA0002821781380000053
其中ei,k为惯性导航系统量测误差，Σi,k为惯性导航系统运动模型节点协方差；
步骤(5e)：建立因子图优化问题,表示如下：
Figure BDA0002821781380000061
其中
Figure BDA0002821781380000062
Figure BDA0002821781380000063
为X的最优估计；
步骤(5f)：使用高斯-牛顿法对因子图优化问题进行求解，获得误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值。
优选地，对所述误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值更新是对的状态节点的更新，并将对应的更新后的雷达里程计的扫描点注册到全局地图中，再对全局地图进行更新，并对全局地图中重复的雷达里程计扫描点进行剔除。
本发明的有益效果：本发明一种基于因子图优化的仿生偏振同步定位与构图的方法，对如何确定自身位置及感知外界环境的问题，SLAM系统旨在通过系统模型，结合相应的滤波方法或优化方法完成定位与周围环境的绘制，是通过建立惯性导航系统模型和雷达里程计量测模型、偏振光相机的量测模型，利用因子图优化算法实现位置的确定和周围环境地图的构建，有效的利用了偏振信息、惯导信息和雷达里程计信息及多源信息的融合匹配互补、不受其他外界干扰的特性，因子图优化充分考虑量测和状态的历史信息的特点，环境适应性更好、抗干扰性更强、定位与构图精度更高。无需依赖先验知识，环境适应性更好、鲁棒性更强、定位与构图精度更高。本发明使用了偏振导航机制是一种非常有效的导航手段，具有无源、无辐射、隐蔽性好等特点,能够为复杂环境下的导航任务提供新的解决途径。引入偏振信息，使用图优化方法在SLAM上可以提高自身位置的和构建周围环境地图的精确度，提升了系统鲁棒性、自主性。
附图说明
图1是本发明一种基于因子图优化的仿生偏振同时定位与构图(SLAM)的方法流程图。
图2是本发明因子图优化结构图。
图2中符号含义说明：
Figure BDA0002821781380000071
状态节点，
Figure BDA0002821781380000072
偏振光相机节点，
■惯性导航系统节点，●雷达里程计节点。
具体实施方式
下面结合附图详细说明本发明，其作为本说明书的一部分，通过实施来说明本发明的原理，本发明的其他方面、特征及其优点通过该详细说明将会变得一目了然。
本发明涉及一种基于图优化的仿生偏振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时刻组合导航系统状态如下：
Figure BDA0002821781380000081
其中：xk为k时刻的组合导航系统的状态节点，
Figure BDA0002821781380000082
表示k时刻载体世界坐标系下的三维位置信息，
Figure BDA0002821781380000083
分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值，T表示向量的转置；
Figure BDA0002821781380000091
表示k时刻载体世界坐标系下的三维速度，
Figure BDA0002821781380000092
分别表示k时刻载体世界坐标系下指向x,y,z三个轴方向的速度；
Figure BDA0002821781380000093
表示载体姿态，载体姿态以欧拉角的形式表示，
Figure BDA0002821781380000094
分别为滚转角、俯仰角和偏航角。
优选实施例，本发明选取所述雷达里程计测量的位置信息作为测量值，建立步骤2所述雷达里程计的量测模型如下：
zl,k＝hl(xk)+vl,k (2)
Figure BDA0002821781380000095
其中：
Figure BDA0002821781380000096
表示k时刻雷达里程计量测值，其
Figure BDA0002821781380000097
为雷达里程计量测的x,y,z三个轴方向的坐标值；hl(xk)为雷达里程计量测函数，xk为k时刻的组合导航系统的状态节点，
Figure BDA0002821781380000098
分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值；vl,k表示雷达里程计量测的高斯噪声。
优选实施例，建立步骤2所述偏振光相机量测模型如下：
zp,k＝hp(xk)+vp,k (4)
Figure BDA0002821781380000099
其中：
Figure BDA00028217813800000910
为偏振光相机测量值，
Figure BDA00028217813800000911
为偏振光相机量测的载体纵轴与太阳子午线的夹角作为量测值数据；hp(xk)为偏振光相机测量函数，xk为k时刻的组合导航系统的状态节点；
Figure BDA00028217813800000912
为载体偏航角，太阳方位角As可以通过查表计算，所述太阳方位角
Figure BDA00028217813800000913
其
Figure BDA00028217813800000914
为太阳高度角，
Figure BDA00028217813800000915
为太阳赤纬，
Figure BDA00028217813800000916
为观测点的纬度，
Figure BDA00028217813800000917
为太阳时角；vp,k表示偏振光相机量测的高斯噪声。
优选实施例，本发明基于惯性导航系统测量的载体姿态、速度、位置作为测量值数据建立步骤2所述的基于惯性导航系统的运动模型如下：
Figure BDA0002821781380000101
Figure BDA0002821781380000102
其中
Figure BDA0002821781380000103
为惯性导航系统的量测值，其中
Figure BDA0002821781380000104
为惯性导航系统量测的x,y,z三个轴方向的坐标值；hi(xk-1,uk-1)为惯性导航系统的量测函数,
Figure BDA0002821781380000105
为k-1时刻载体世界坐标系下的位置、速度、姿态；
Figure BDA0002821781380000106
为k-1时刻的控制输入；其中
Figure BDA0002821781380000107
为世界坐标系下惯性导航系统补偿零偏后的加速度和角速度，
Figure BDA0002821781380000108
为载体坐标系下惯性导航系统补偿的加速度和角速度，Δt为惯性导航系统采样间隔，
Figure BDA0002821781380000109
为载体坐标系到世界坐标系的旋转矩阵：
Figure BDA00028217813800001010
Figure BDA00028217813800001011
为标定后的加计和陀螺零偏常值,vi,k-1表示惯性导航系统量测的高斯噪声。
优选实施例，本发明步骤3所述组合导航系统初始化是根据载体坐标系到世界坐标系的旋转矩阵
Figure BDA00028217813800001012
将载体坐标系下的雷达扫描点云转换到全局坐标系下，并建立以起始位置为原点的全局地图。
优选实施例，本发明获得步骤4所述当前时刻载体状态节点位置约束的步骤如下：根据获取当前时刻时间戳和雷达里程计量测数据，将雷达里程计量测数据中的扫描点注册到地图中，通过雷达里程计量测模型和雷达里程计量测数据建立当前时刻载体状态节点位置约束。
优选实施例，本发明获得步骤4所述载体相邻状态节点位姿变换约束的步骤如下：对当前时刻时间戳和上一时刻时间戳之间所有的惯性导航量测数据通过惯性导航系统运动模型进行积分，利用积分得到载体的姿态、速度、位置数据建立载体相邻状态节点位姿变换约束。
优选实施例，本发明获得步骤4载体状态节点航向约束的步骤如下：寻找当前时刻时间戳的偏振光相机量测数据，通过偏振光相机量测模型和偏振光相机量测数据建立载体状态节点航向约束。
优选实施例，获得步骤5所述误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值的具体步骤如下：
步骤(5a)：构建载体(k-m)～k时刻m为正整数的状态节点误差函数：
步骤(5b)：根据状态节点位置约束，建立雷达里程计量测的误差函数为：
Figure BDA0002821781380000111
其中el,k为雷达里程计量测误差，Σl,k为雷达里程计节点协方差；
步骤(5c)：根据状态节点航向约束，建立偏振光相机量测的误差函数为：
Figure BDA0002821781380000112
其中ep,k为偏振光相机量测误差，∑p,k为偏振光相机节点协方差；
步骤(5d)：根据相邻状态节点位姿变换约束，建立惯性导航系统量测的误差函数为：
Figure BDA0002821781380000113
其中ei,k为惯性导航系统量测误差，∑i,k为惯性导航系统运动模型节点协方差；
步骤(5e):建立因子图优化问题,表示如下：
Figure BDA0002821781380000114
其中X＝[xk-m…xk]T，
Figure BDA0002821781380000121
为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与惯性导航系统节点的连接关系。
对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种基于因子图优化的仿生偏振同步定位与构图的方法，其特征在于，所述方法包括以下步骤：
步骤1：选取载体姿态、速度、位置作为组合导航系统状态；
步骤2：建立雷达里程计量测模型、建立偏振光相机量测模型和建立惯性导航系统运动模型；
步骤3：对由雷达里程计、偏振光相机和惯性导航系统组成的组合导航系统初始化；
步骤4：获得当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束；
步骤5：根据当前时刻载体状态节点位置约束、载体相邻状态节点位姿变换约束和载体状态节点航向约束建立组合导航系统状态和量测节点的误差函数，获得待优化因子图，并对待优化因子图进行优化，获得误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值；
步骤6：对误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值和地图更新，同步实现载体的定位和构图。
2.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：通过步骤1所述的组合导航系统状态构建k时刻的组合导航系统的状态节点表示如下：
Figure FDA0002821781370000011
其中：
xk为k时刻的组合导航系统的状态节点，
Figure FDA0002821781370000012
表示k时刻载体世界坐标系下的三维位置信息，
Figure FDA0002821781370000013
分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值，T表示向量的转置；
Figure FDA0002821781370000021
表示k时刻载体世界坐标系下的三维速度，
Figure FDA0002821781370000022
分别表示k时刻载体世界坐标系下指向x,y,z三个轴方向的速度；
Figure FDA0002821781370000023
表示载体姿态，载体姿态以欧拉角的形式表示，
Figure FDA0002821781370000024
分别为滚转角、俯仰角和偏航角。
3.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：选取所述雷达里程计测量的位置信息作为测量值，建立步骤2所述雷达里程计的量测模型如下：
zl,k＝hl(xk)+vl,k
Figure FDA0002821781370000025
其中：
Figure FDA0002821781370000026
表示k时刻雷达里程计量测值，其
Figure FDA0002821781370000027
为雷达里程计量测的x,y,z三个轴方向的坐标值；
hl(xk)为雷达里程计量测函数，xk为k时刻的组合导航系统的状态节点，
Figure FDA0002821781370000028
分别表示k时刻载体世界坐标系下x,y,z三个轴方向的坐标值；vl,k表示雷达里程计量测的高斯噪声。
4.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：建立步骤2所述偏振光相机量测模型如下：
zp,k＝hp(xk)+vp,k
Figure FDA0002821781370000029
其中：
Figure FDA00028217813700000210
为偏振光相机测量值，
Figure FDA00028217813700000211
为偏振光相机量测的载体纵轴与太阳子午线的夹角作为量测值数据；
hp(xk)为偏振光相机测量函数，xk为k时刻的组合导航系统的状态节点；
Figure FDA0002821781370000031
为载体偏航角，太阳方位角As可以通过查表计算，所述太阳方位角
Figure FDA0002821781370000032
其
Figure FDA0002821781370000033
为太阳高度角，
Figure FDA0002821781370000034
为太阳赤纬，
Figure FDA0002821781370000035
为观测点的纬度，
Figure FDA0002821781370000036
为太阳时角；vp,k表示偏振光相机量测的高斯噪声。
5.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位与构图的方法，其特征在于：基于惯性导航系统测量的载体姿态、速度、位置作为测量值数据建立步骤2所述的基于惯性导航系统的运动模型如下：
Figure FDA0002821781370000037
Figure FDA0002821781370000038
其中
Figure FDA0002821781370000039
为惯性导航系统的量测值，其中
Figure FDA00028217813700000310
为惯性导航系统量测的x,y,z三个轴方向的坐标值；hi(xk-1,uk-1)为惯性导航系统的量测函数,
Figure FDA00028217813700000311
为k-1时刻载体世界坐标系下的位置、速度、姿态；
Figure FDA00028217813700000312
为k-1时刻的控制输入；其中
Figure FDA00028217813700000313
为世界坐标系下惯性导航系统补偿零偏后的加速度和角速度，
Figure FDA00028217813700000314
为载体坐标系下惯性导航系统补偿的加速度和角速度，Δt为惯性导航系统采样间隔，
Figure FDA00028217813700000315
为载体坐标系到世界坐标系的旋转矩阵：
Figure FDA00028217813700000316
Figure FDA00028217813700000317
为标定后的加计和陀螺零偏常值,vi,k-1表示惯性导航系统量测的高斯噪声。
6.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位与构图的方法，其特征在于：步骤3所述组合导航系统初始化是根据载体坐标系到世界坐标系的旋转矩阵
Figure FDA0002821781370000041
将载体坐标系下的雷达扫描点云转换到全局坐标系下，并建立以起始位置为原点的全局地图。
7.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：获得步骤4所述当前时刻载体状态节点位置约束的步骤如下：根据获取当前时刻时间戳和雷达里程计量测数据，将雷达里程计量测数据中的扫描点注册到地图中，通过雷达里程计量测模型和雷达里程计量测数据建立当前时刻载体状态节点位置约束。
8.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：获得步骤4所述载体相邻状态节点位姿变换约束的步骤如下：对当前时刻时间戳和上一时刻时间戳之间所有的惯性导航量测数据通过惯性导航系统运动模型进行积分，利用积分得到载体的姿态、速度、位置数据建立载体相邻状态节点位姿变换约束。
9.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法，其特征在于：获得步骤4载体状态节点航向约束的步骤如下：寻找当前时刻时间戳的偏振光相机量测数据，通过偏振光相机量测模型和偏振光相机量测数据建立载体状态节点航向约束。
10.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位与构图的方法,其特征在于：获得步骤5所述误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值的具体步骤如下：
步骤(5a)：构建载体(k-m)～k时刻m为正整数的状态节点误差函数：
步骤(5b)：根据状态节点位置约束，建立雷达里程计量测的误差函数为：
Figure FDA0002821781370000042
其中el,k为雷达里程计量测误差，∑l,k为雷达里程计节点协方差；
步骤(5c)：根据状态节点航向约束，建立偏振光相机量测的误差函数为：
Figure FDA0002821781370000043
其中ep,k为偏振光相机量测误差，∑p,k为偏振光相机节点协方差；
步骤(5d)：根据相邻状态节点位姿变换约束，建立惯性导航系统量测的误差函数为：
Figure FDA0002821781370000051
其中ei,k为惯性导航系统量测误差，∑i,k为惯性导航系统运动模型节点协方差；
步骤(5e)：建立因子图优化问题，表示如下：
Figure FDA0002821781370000052
其中X＝[xk-m…xk]T，
Figure FDA0002821781370000053
为X的最优估计；
步骤(5f)：使用高斯-牛顿法对因子图优化问题进行求解，获得误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值；
对所述误差函数最小的世界坐标系下载体位置、速度、姿态的最优估计值更新是对的状态节点的更新，并将对应的更新后的雷达里程计的扫描点注册到全局地图中，再对全局地图进行更新，并对全局地图中重复的雷达里程计扫描点进行剔除。
CN202011419966.0A 2020-12-07 2020-12-07 一种基于因子图优化的仿生偏振同步定位与构图的方法 Active CN112697138B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011419966.0A CN112697138B (zh) 2020-12-07 2020-12-07 一种基于因子图优化的仿生偏振同步定位与构图的方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011419966.0A CN112697138B (zh) 2020-12-07 2020-12-07 一种基于因子图优化的仿生偏振同步定位与构图的方法

Publications (2)

Publication Number Publication Date
CN112697138A true CN112697138A (zh) 2021-04-23
CN112697138B CN112697138B (zh) 2022-09-27

Family

ID=75506253

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011419966.0A Active CN112697138B (zh) 2020-12-07 2020-12-07 一种基于因子图优化的仿生偏振同步定位与构图的方法

Country Status (1)

Country Link
CN (1) CN112697138B (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113739795A (zh) * 2021-06-03 2021-12-03 东北电力大学 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法
CN114459474A (zh) * 2022-02-16 2022-05-10 北方工业大学 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN114485648A (zh) * 2022-02-08 2022-05-13 北京理工大学 一种基于仿生复眼惯性系统的导航定位方法
CN114545472A (zh) * 2022-01-26 2022-05-27 中国人民解放军国防科技大学 一种gnss/ins组合系统的导航方法和装置
CN114562992A (zh) * 2022-02-09 2022-05-31 南京航空航天大学 一种基于因子图及场景约束的多径环境组合导航方法
CN115574816A (zh) * 2022-11-24 2023-01-06 东南大学 仿生视觉多源信息智能感知无人平台
CN116337048A (zh) * 2023-05-30 2023-06-27 中国人民解放军国防科技大学 一种无人平台的偏振视觉惯性同步建图与定位方法和装置

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2322901A2 (de) * 2009-11-16 2011-05-18 Riegl Laser Measurement Systems GmbH Verfahren zum Verbessern von Positions- und Lagemessdaten
CN104864874A (zh) * 2015-06-19 2015-08-26 北京理工大学 一种低成本单陀螺航位推算导航方法及系统
CN106197408A (zh) * 2016-06-23 2016-12-07 南京航空航天大学 一种基于因子图的多源导航信息融合方法
CN108362288A (zh) * 2018-02-08 2018-08-03 北方工业大学 一种基于无迹卡尔曼滤波的偏振光slam方法
CN108387236A (zh) * 2018-02-08 2018-08-10 北方工业大学 一种基于扩展卡尔曼滤波的偏振光slam方法
CN111207773A (zh) * 2020-01-16 2020-05-29 大连理工大学 一种用于仿生偏振光导航的姿态无约束优化求解方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2322901A2 (de) * 2009-11-16 2011-05-18 Riegl Laser Measurement Systems GmbH Verfahren zum Verbessern von Positions- und Lagemessdaten
CN104864874A (zh) * 2015-06-19 2015-08-26 北京理工大学 一种低成本单陀螺航位推算导航方法及系统
CN106197408A (zh) * 2016-06-23 2016-12-07 南京航空航天大学 一种基于因子图的多源导航信息融合方法
CN108362288A (zh) * 2018-02-08 2018-08-03 北方工业大学 一种基于无迹卡尔曼滤波的偏振光slam方法
CN108387236A (zh) * 2018-02-08 2018-08-10 北方工业大学 一种基于扩展卡尔曼滤波的偏振光slam方法
CN111207773A (zh) * 2020-01-16 2020-05-29 大连理工大学 一种用于仿生偏振光导航的姿态无约束优化求解方法

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113739795A (zh) * 2021-06-03 2021-12-03 东北电力大学 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法
CN113739795B (zh) * 2021-06-03 2023-10-20 东北电力大学 一种基于偏振光/惯性/视觉组合导航的水下同步定位与建图方法
CN114545472A (zh) * 2022-01-26 2022-05-27 中国人民解放军国防科技大学 一种gnss/ins组合系统的导航方法和装置
CN114485648A (zh) * 2022-02-08 2022-05-13 北京理工大学 一种基于仿生复眼惯性系统的导航定位方法
CN114485648B (zh) * 2022-02-08 2024-02-02 北京理工大学 一种基于仿生复眼惯性系统的导航定位方法
CN114562992A (zh) * 2022-02-09 2022-05-31 南京航空航天大学 一种基于因子图及场景约束的多径环境组合导航方法
CN114459474A (zh) * 2022-02-16 2022-05-10 北方工业大学 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN114459474B (zh) * 2022-02-16 2023-11-24 北方工业大学 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN115574816A (zh) * 2022-11-24 2023-01-06 东南大学 仿生视觉多源信息智能感知无人平台
CN115574816B (zh) * 2022-11-24 2023-03-14 东南大学 仿生视觉多源信息智能感知无人平台
CN116337048A (zh) * 2023-05-30 2023-06-27 中国人民解放军国防科技大学 一种无人平台的偏振视觉惯性同步建图与定位方法和装置
CN116337048B (zh) * 2023-05-30 2023-07-21 中国人民解放军国防科技大学 一种无人平台的偏振视觉惯性同步建图与定位方法和装置

Also Published As

Publication number Publication date
CN112697138B (zh) 2022-09-27

Similar Documents

Publication Publication Date Title
CN112697138B (zh) 一种基于因子图优化的仿生偏振同步定位与构图的方法
CN110243358B (zh) 多源融合的无人车室内外定位方法及系统
CN109991636B (zh) 基于gps、imu以及双目视觉的地图构建方法及系统
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN110057354B (zh) 一种基于磁偏角修正的地磁匹配导航方法
CN108731670A (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN110954102B (zh) 用于机器人定位的磁力计辅助惯性导航系统及方法
CN109506660B (zh) 一种用于仿生导航的姿态最优化解算方法
CN106767767A (zh) 一种微纳多模星敏感器系统及其数据融合方法
CN114018242B (zh) 一种基于偏振/太阳/惯性信息智能匹配的自主定姿方法
CN115574816B (zh) 仿生视觉多源信息智能感知无人平台
CN108195376A (zh) 小型无人机自主导航定位方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN104697520A (zh) 一体化无陀螺捷联惯导系统与gps系统组合导航方法
CN113834483A (zh) 一种基于可观测度的惯性/偏振/地磁容错导航方法
CN111220151B (zh) 载体系下考虑温度模型的惯性和里程计组合导航方法
CN113503892A (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
Wang et al. Land vehicle navigation using odometry/INS/vision integrated system
CN113008229B (zh) 一种基于低成本车载传感器的分布式自主组合导航方法
CN114459474B (zh) 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法
CN116839591A (zh) 一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法
Zhang et al. An improved inertial preintegration model in factor graph optimization for high accuracy positioning of intelligent vehicles
CN116067369A (zh) 一种基于距离和角度的行人协同导航方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant