CN112697138A - 一种基于因子图优化的仿生偏振同步定位与构图的方法 - Google Patents
一种基于因子图优化的仿生偏振同步定位与构图的方法 Download PDFInfo
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 48
- 238000005457 optimization Methods 0.000 title claims abstract description 46
- 230000010287 polarization Effects 0.000 title claims abstract description 41
- 239000011664 nicotinic acid Substances 0.000 title claims abstract description 31
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 25
- 238000005259 measurement Methods 0.000 claims abstract description 70
- 230000009466 transformation Effects 0.000 claims abstract description 21
- 238000000059 patterning Methods 0.000 claims description 19
- 230000001133 acceleration Effects 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 102000008115 Signaling Lymphocytic Activation Molecule Family Member 1 Human genes 0.000 description 17
- 108010074687 Signaling Lymphocytic Activation Molecule Family Member 1 Proteins 0.000 description 17
- 238000001914 filtration Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000010276 construction Methods 0.000 description 3
- 230000004807 localization Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000000149 argon plasma sintering Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 230000036039 immunity Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000005855 radiation Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
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时刻的组合导航***的状态节点表示如下:
其中: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为惯性导航***采样间隔,为载体坐标系到世界坐标系的旋转矩阵:
优选地,获得步骤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):建立因子图优化问题,表示如下:
步骤(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为惯性导航***采样间隔,为载体坐标系到世界坐标系的旋转矩阵:
优选实施例,本发明获得步骤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):建立因子图优化问题,表示如下:
步骤(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:对误差函数最小的世界坐标系下载***置、速度、姿态的最优估计值和地图更新,同步实现载体的定位和构图。
5.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位与构图的方法,其特征在于:基于惯性导航***测量的载体姿态、速度、位置作为测量值数据建立步骤2所述的基于惯性导航***的运动模型如下:
其中为惯性导航***的量测值,其中为惯性导航***量测的x,y,z三个轴方向的坐标值;hi(xk-1,uk-1)为惯性导航***的量测函数,为k-1时刻载体世界坐标系下的位置、速度、姿态;为k-1时刻的控制输入;其中为世界坐标系下惯性导航***补偿零偏后的加速度和角速度,为载体坐标系下惯性导航***补偿的加速度和角速度,Δt为惯性导航***采样间隔,为载体坐标系到世界坐标系的旋转矩阵:
7.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法,其特征在于:获得步骤4所述当前时刻载体状态节点位置约束的步骤如下:根据获取当前时刻时间戳和雷达里程计量测数据,将雷达里程计量测数据中的扫描点注册到地图中,通过雷达里程计量测模型和雷达里程计量测数据建立当前时刻载体状态节点位置约束。
8.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法,其特征在于:获得步骤4所述载体相邻状态节点位姿变换约束的步骤如下:对当前时刻时间戳和上一时刻时间戳之间所有的惯性导航量测数据通过惯性导航***运动模型进行积分,利用积分得到载体的姿态、速度、位置数据建立载体相邻状态节点位姿变换约束。
9.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位和构图的方法,其特征在于:获得步骤4载体状态节点航向约束的步骤如下:寻找当前时刻时间戳的偏振光相机量测数据,通过偏振光相机量测模型和偏振光相机量测数据建立载体状态节点航向约束。
10.根据权利要求1所述的一种基于因子图优化的仿生偏振同步定位与构图的方法,其特征在于:获得步骤5所述误差函数最小的世界坐标系下载体的位置、速度和姿态的最优估计值的具体步骤如下:
步骤(5a):构建载体(k-m)~k时刻m为正整数的状态节点误差函数:
步骤(5b):根据状态节点位置约束,建立雷达里程计量测的误差函数为:
其中el,k为雷达里程计量测误差,∑l,k为雷达里程计节点协方差;
步骤(5c):根据状态节点航向约束,建立偏振光相机量测的误差函数为:
其中ep,k为偏振光相机量测误差,∑p,k为偏振光相机节点协方差;
步骤(5d):根据相邻状态节点位姿变换约束,建立惯性导航***量测的误差函数为:
其中ei,k为惯性导航***量测误差,∑i,k为惯性导航***运动模型节点协方差;
步骤(5e):建立因子图优化问题,表示如下:
步骤(5f):使用高斯-牛顿法对因子图优化问题进行求解,获得误差函数最小的世界坐标系下载***置、速度、姿态的最优估计值;
对所述误差函数最小的世界坐标系下载***置、速度、姿态的最优估计值更新是对的状态节点的更新,并将对应的更新后的雷达里程计的扫描点注册到全局地图中,再对全局地图进行更新,并对全局地图中重复的雷达里程计扫描点进行剔除。
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)
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)
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 | 大连理工大学 | 一种用于仿生偏振光导航的姿态无约束优化求解方法 |
-
2020
- 2020-12-07 CN CN202011419966.0A patent/CN112697138B/zh active Active
Patent Citations (6)
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)
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) | 多源融合的无人车室内外定位方法及*** | |
CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
Jiancheng et al. | Study on innovation adaptive EKF for in-flight alignment of airborne POS | |
CN110057354B (zh) | 一种基于磁偏角修正的地磁匹配导航方法 | |
CN108827310A (zh) | 一种船用星敏感器辅助陀螺仪在线标定方法 | |
CN110954102B (zh) | 用于机器人定位的磁力计辅助惯性导航***及方法 | |
CN105509739A (zh) | 采用固定区间crts平滑的ins/uwb紧组合导航***及方法 | |
CN109506660B (zh) | 一种用于仿生导航的姿态最优化解算方法 | |
CN114018242B (zh) | 一种基于偏振/太阳/惯性信息智能匹配的自主定姿方法 | |
CN115574816B (zh) | 仿生视觉多源信息智能感知无人平台 | |
CN113834483A (zh) | 一种基于可观测度的惯性/偏振/地磁容错导航方法 | |
CN113252038A (zh) | 基于粒子群算法的航迹规划地形辅助导航方法 | |
Liu et al. | Interacting multiple model UAV navigation algorithm based on a robust cubature Kalman filter | |
CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
CN113503892A (zh) | 一种基于里程计和回溯导航的惯导***动基座初始对准方法 | |
CN116222551A (zh) | 一种融合多种数据的水下导航方法及装置 | |
CN113503872B (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
Wang et al. | Land vehicle navigation using odometry/INS/vision integrated system | |
CN109387198A (zh) | 一种基于序贯检测的惯性/视觉里程计组合导航方法 | |
CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
CN110873577B (zh) | 一种水下快速动基座对准方法及装置 | |
CN114459474B (zh) | 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法 | |
CN116839591A (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 |