CN115435779A - 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 - Google Patents
一种基于gnss/imu/光流信息融合的智能体位姿估计方法 Download PDFInfo
- Publication number
- CN115435779A CN115435779A CN202210985155.XA CN202210985155A CN115435779A CN 115435779 A CN115435779 A CN 115435779A CN 202210985155 A CN202210985155 A CN 202210985155A CN 115435779 A CN115435779 A CN 115435779A
- Authority
- CN
- China
- Prior art keywords
- gnss
- imu
- factor
- constraint
- bias
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 65
- 230000003287 optical effect Effects 0.000 title claims abstract description 60
- 230000004927 fusion Effects 0.000 title claims abstract description 25
- 239000013598 vector Substances 0.000 claims abstract description 55
- 230000000007 visual effect Effects 0.000 claims abstract description 32
- 230000008569 process Effects 0.000 claims abstract description 22
- 238000000605 extraction Methods 0.000 claims abstract description 14
- 238000001914 filtration Methods 0.000 claims abstract description 12
- 238000007781 pre-processing Methods 0.000 claims abstract description 12
- 238000005457 optimization Methods 0.000 claims abstract description 11
- 230000010354 integration Effects 0.000 claims abstract description 6
- 239000003795 chemical substances by application Substances 0.000 claims description 100
- 238000005259 measurement Methods 0.000 claims description 95
- 239000011159 matrix material Substances 0.000 claims description 33
- 230000009466 transformation Effects 0.000 claims description 23
- 238000006073 displacement reaction Methods 0.000 claims description 18
- 238000011161 development Methods 0.000 claims description 11
- 238000010276 construction Methods 0.000 claims description 8
- 230000000694 effects Effects 0.000 claims description 5
- 239000005433 ionosphere Substances 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 239000005436 troposphere Substances 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 abstract description 3
- 230000001133 acceleration Effects 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 238000003384 imaging method Methods 0.000 description 2
- HPTJABJPZMULFH-UHFFFAOYSA-N 12-[(Cyclohexylcarbamoyl)amino]dodecanoic acid Chemical compound OC(=O)CCCCCCCCCCCNC(=O)NC1CCCCC1 HPTJABJPZMULFH-UHFFFAOYSA-N 0.000 description 1
- 230000005653 Brownian motion process Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000005537 brownian motion Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
- 238000013519 translation 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
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
- G06T7/269—Analysis of motion using gradient-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10016—Video; Image sequence
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10024—Color image
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Multimedia (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其步骤为:首先,分别对传智能体机载感器原始数据进行预处理,包括IMU预积分、视觉特征提取及跟踪、GNSS信号滤波;然后,分别构建视觉、IMU、GNSS代价函数及智能体状态向量,并对GNSS/IMU/光流信息进行联合初始化,获取智能体位姿估计器的所有初值;最后,将智能体位姿求解过程构建为状态估计问题,并设计相应的概率因子图模型及边缘化策略,以图优化的方式求解智能体位姿。与现有的机器人位姿估计方法相比,本发明具有更高的计算效率及自适应性,能够准确、鲁棒、无漂地获取智能体位姿信息。
Description
技术领域
本发明涉及智能体位姿估计技术,特别是涉及一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法。
背景技术
智能体是一种能够凭借传感器感知其周围环境,并借助于执行器作用于该环境的智能机器人。目前,智能体的自主导航能力已经成为衡量其智能化水平的重要指标。而解决智能体自主导航问题的难点是如何实时、准确地获取智能体当前的位姿。相比于基于单一传感器的智能体位姿估计器,多传感器融合位姿估计技术能够充分利用不同种类传感器之间的互补特性,以获得更为准确且鲁棒的智能体位姿估计结果。立体相机和惯性测量单元(IMU)能够为智能体提供机体坐标系下厘米级精度的位姿,但局部坐标系下的位姿会随着智能体的运动而漂移。全球导航卫星系统(GNSS)为能够为智能体提供无漂移的位置信息,已广泛应用于各种移动机器人导航任务。但基于GNSS的智能体导航方法难以在室内使用,且易受噪声和多径效应的影响,导致其只能达到米级的定位精度。为了充分利用不同传感器之间的互补特性,基于GNSS/IMU/光流信息融合的智能体位姿估计算法能够利用导航卫星、加速度计、陀螺仪、立体相机各自的优势,以获取准确、无漂的智能体位姿估计。
然而,基于GNSS/IMU/光流信息融合的位姿估计方法将会面临如下问题:首先,不同传感器的输出频率并不相同(GNSS接收机约为10HZ;相机约为30HZ;IMU约为250HZ),如何同步并对齐这些来自不同传感器的原始测量数据将会是一个难题;其次,视觉/惯性里程计通常可以达到厘米级的定位精度,而GNSS的定位精度比视觉/惯性里程计要低两个数量级;最后,当位姿估计系统的某个传感器突然失效,然后又重新正常工作时,位姿估计器怎样才能快速恢复到之前的状态。因此,如何利用GNSS/IMU/光流信息快速、准确、无漂地获取智能体位姿已成为智能机器人导航领域亟需解决的难题。
发明内容
发明目的:本发明的目的是提供一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法。
技术方案:本发明的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,包括以下步骤:
S1、分别对智能体原始测量数据进行预处理,包括对相机的RGB输出进行Shi-Tomasi角点特征提取,并对随后的RGB视频帧执行KLT光流跟踪;在机器人坐标系下对惯性测量单元(IMU)的原始测量数据进行预积分;对捕获到的GNSS原始信号进行滤波;
S2、位姿估计器GNSS/IMU/光流信息联合初始化:构建智能体状态向量χ,利用PnP算法和BA算法求解智能体的初始位置和初始朝向,同时利用SPP算法粗略求解智能体的经纬度;将智能体的初始位置和经纬度对齐,获得里程计坐标系与世界坐标系之间的变换矩阵,完成GNSS/IMU/光流信息联合初始化;
S3、构建视觉约束因子:将相机投影模型构建为针孔相机模型,建立空间特征点l从视频帧i到视频帧j的重投影函数,将视觉误差因子项构建为特征点重投影残差;
S4、构建IMU约束因子:IMU约束因子包括陀螺仪误差因子约束项和加速度计误差因子约束项,其中,将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差;
S5、构建GNSS约束因子:GNSS约束因子包括GNSS伪距约束因子、GNSS多普勒频移约束因子和接收机时钟偏置约束因子;其中,GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差,GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差,接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差;
S6、以概率因子图优化的方式求解智能体位姿信息。
进一步的,步骤S2具体为:
S201、构建智能体状态向量χ,所述状态向量包含智能体当前的朝向速度位置陀螺仪偏置和加速度计偏置GNSS接收机的时钟偏置δt和偏置率δt′、视觉特征点深度d1,d2,...,dm和里程计坐标系与世界坐标系之间的Z轴偏置ψ,智能体状态向量χ表示为:
进一步的,步骤S3中将视觉误差因子项构建为特征点重投影残差的具体方法为:把空间特征点l从视频帧i的像素坐标系投影到世界坐标系,再从世界坐标系投影到视频帧j的像素坐标系,然后求解特征点l在视频帧j中的测量位置与实际位置的残差。
进一步的,步骤S4具体为:
S401、将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,分别用智能体在两个连续视频帧内真实的旋转角度与陀螺仪预积分项的逆做四元数乘法,即获取陀螺仪的测量残差;
S402、将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差;速度偏置残差由智能体在两个连续视频帧内的真实速度偏置与加速度计的速度预积分项做差获得;位移偏置残差由智能体在两个连续视频帧内的真实位移偏置与加速度计位移预积分项做差获得;
S403、陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差获得;
S404、联立步骤S401、步骤S402和步骤S403即完成IMU约束因子的构建;
构建的IMU约束因子为:
其中,χI表示智能体状态向量χ中与IMU相关的子向量,为IMU朝向的预积分项、为IMU速度的预积分项、为IMU位置的预积分项,为IMU朝向预积分项的测量值、为IMU速度预积分项的测量值、为IMU位置预积分项的测量值,δbω和δba分别为陀螺仪偏置和加速度计偏置的残差,代表tk时刻世界坐标系到机器人坐标系之间的旋转,分别为世界坐标系下tk+1时刻智能体的朝向、速度和位置,为tk时刻世界坐标系与机器人坐标系之间的变换矩阵,符号代表四元数乘法,和分别为tk+1时刻的陀螺仪偏置和加速度计偏置,和分别为tk时刻的陀螺仪偏置和加速度计偏置。
进一步的,步骤S5具体为:
S501、建立GNSS伪距测量模型,利用伪距测量模型求解真实GNSS原始信号中的伪距信息,则GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差,表示为:
其中,表示卫星与接收机之间的观测方程,χpr表示智能体状态向量χ中与GNSS伪距相关的子向量,和分别为接收机和卫星在地心惯性坐标系下的位置、c为光在真空中的传播速度、和分别为接收机和卫星的时钟偏置,△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、代表伪距的测量值;
S502、建立GNSS多普勒频移测量模型,利用多普勒频移测量模型求解真实的GNSS多普勒频移信息,则GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差,表示为:
其中,表示卫星与接收机之间的观测方程,χdp表示智能体状态向量χ中与GNSS多普勒频移相关的子向量,wl为载波波长,为卫星与接收机的方向向量,和分别为地心惯性坐标系下接收机和卫星的速度、c为光在真空中的传播速度、和分别为接收机和卫星的时钟偏置率、为多普勒频移测量值;
S503、建立GNSS接收机时钟偏置测量模型,则接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差,表示为:
S504、联立步骤S501、步骤S502和步骤S503即完成GNSS约束因子的构建。
进一步的,步骤S6具体为:
S601、将智能体位姿求解过程视为状态估计问题,则智能体位姿求解过程变为状态向量的最大后验估计;
S602、构建GNSS/IMU/光流信息紧耦合的位姿估计器因子图模型,表达式为:
其中,χ为智能体状态向量,P(χ|z)为智能体状态向量的最大后验概率,z为总的智能体状态测量模型,ep为系统先验信息,Hp代表智能体先验位姿矩阵,n为滑动窗内智能体状态向量的个数,符号E表示视觉约束因子EV、IMU约束因子EI、GNSS约束因子Epr、Edp、Eτ之和,zk为智能体的第k次观测信息;
S603、设计观测数据边缘化策略,剔除滑动窗内冗余的光流、IMU、GNSS信息;
S604、求解GNSS/IMU/光流信息紧耦合的位姿估计器因子图模型,得到智能体状态向量。
进一步的,该方法采用的硬件设备的搭建过程为:
(1)采用NVIDIA Jetson Xavier NX作为智能体机载嵌入式计算平台,并安装ROSMelodic机器人操作系统及位姿估计器相关运行环境;
(3)采用Ublox NEO-M8T模块作为GNSS信号接收机,并以TTL电平标准与NVIDIAJetson Xavier NX的UART端口连接。
进一步的,机载相机、IMU、GNSS接收机之间的硬件连接及同步过程为:
Intel RealSense D435i立体相机和BMI055惯性测量单元通过USB3.0与NVIDIAJetson Xavier NX开发板连接,Ublox NEO-M8T模块连接到NVIDIA Jetson Xavier NX开发板的UART接口,各传感器之间通过GNSS的PPS信号进行硬件同步。
进一步的,由NVIDIA Jetson Xavier NX的GPU模块完成视觉特征提取及光流跟踪,由机载CPU对惯性测量单元的原始数据和GNSS原始信号分别进行预积分和滤波处理。
本发明的一种基于GNSS/IMU/光流信息融合的智能体位姿估计系统,包括:
数据预处理模块,用于对智能体原始测量数据进行预处理,包括对相机的RGB输出进行Shi-Tomasi角点特征提取,并对随后的RGB视频帧执行KLT光流跟踪;在机器人坐标系下对惯性测量单元IMU的原始测量数据进行预积分;对捕获到的GNSS原始信号进行滤波;
联合初始化模块,用于对GNSS/IMU/光流信息联合初始化;
视觉约束因子构建模块,用于将相机投影模型构建为针孔相机模型,建立空间特征点l从视频帧i到视频帧j的重投影函数,将视觉误差因子项构建为特征点重投影残差;
IMU约束因子构建模块,用于将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差;
GNSS约束因子构建模块,用于将GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差,GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差,接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差;
智能体位姿信息求解模块,用于以概率因子图优化的方式求解智能体位姿信息。
经由上述智能体位姿估计方法可知,本发明的一种基于GNSS/IMU/光流信息紧耦合的智能体位姿估计方法。首先,分别对智能体机载传感器数据进行预处理,包括IMU预积分、角点特征提取及跟踪、GNSS信号滤波等过程;然后,分别构建视觉、IMU、GNSS因子图约束,并对GNSS/IMU/光流信息进行联合初始化;最后,将智能体位姿求解过程构建为状态估计问题,并设计相应的概率因子图模型,以图优化的方式求解智能体位姿。
有益效果:与现有技术相比,本发明的基于GNSS/IMU/光流信息融合的智能体位姿估计方法具有更高的计算效率及自适应性,充分利用NVIDIA Jetson Xavier NX机载计算平台中的统一计算设备架构(CUDA)进行数据并行处理,能够达到200HZ以上的智能体位姿更新速率,极大地提高智能体位姿估计器的实时性。实验结果表明,本发明提出方法的位姿估计误差比VINS-Fusion降低了11.7%,计算速度比VINS-Fusion提高了1.5倍。得益于GNSS/IMU/光流信息的紧耦合,当遭遇某个机载传感器数据异常时(如:从户外进入室内、相机被遮挡等),联合位姿估计器仍然能够利用处在正常工作的传感器进行智能体位姿求解,待状态异常的传感器恢复正常时重新进行联合初始化,以获得准确、鲁棒、无漂的智能体位姿信息,而现有的其它同类型位姿估计方法无法达到与本发明相同的鲁棒性。
附图说明
图1为基于GNSS/IMU/光流信息融合的智能体位姿估计方法流程示意图;
图2为智能体位姿估计器硬件设备连接示意图;
图3为本发明坐标系变换关系示意图;
图4为地心地固坐标系、局部世界坐标系、里程计坐标系之间的关系图解。
具体实施方式
下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
针对传统多传感器融合位姿估计器鲁棒性差、轨迹估计不平滑等问题,本发明提供了一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法。首先,分别对智能体机载传感器原始数据进行预处理,包括视觉特征提取及跟踪、IMU预积分、GNSS原始信号滤波等过程;然后,分别构建智能体状态向量及视觉、IMU、GNSS代价函数,并对GNSS/IMU/光流信息进行联合初始化,获取智能体位姿估计器的所有初值;最后,将智能体位姿求解过程构建为状态估计问题,并设计相应的概率因子图模型及边缘化策略,以图优化的方式求解智能体位姿,最终实现快速、准确、鲁棒、无漂的智能体位姿估计。
智能体位姿估计器硬件设备连接示意图如图1所示,智能体位姿估计器硬件设备部署为:
选择NVIDIA Jetson Xavier NX开发板作为智能体机载嵌入式计算平台,并安装ROS Melodic机器人操作系统及C++与Python相关开发环境。其中,角点特征提取与光流跟踪使用OpenCV3.4.1的GPU版本,矩阵操作使用Eigen 3.3,因子图优化过程使用Ceres1.14来求解非线性最小二乘问题。
选择Intel Realsense D435i作为机载立体相机,用于捕获视觉与IMU测量信息,并通过USB3.0连接到NVIDIA Jetson Xavier NX开发板。分别标定Intel Realsense D435i相机的RGB镜头和两个双目镜头,获取每个相机的内参矩阵K,然后联合标定每个镜头与相机内置BMI055惯性测量单元,获取相机到IMU的变换矩阵
选择Ublox NEO-M8T作为GNSS信号接收机主芯片,并以TTL电平标准与NVIDIAJetson Xavier NX的UART接口连接,PPS信号连接到NVIDIA Jetson Xavier NX开发板的GPIO接口。
机载传感器之间的硬件连接及同步过程为:
Intel RealSense D435i立体相机和BMI055惯性测量单元通过USB3.0与NVIDIAJetson Xavier NX开发板连接,Ublox NEO-M8T模块连接到NVIDIA Jetson Xavier NX开发板的UART接口,立体相机、惯性测量单元、GNSS接收机之间通过GNSS的PPS信号进行硬件同步。
智能体位姿估计器包括集成在智能体上的立体相机、IMU和GNSS接收机,立体相机用于采集智能体视觉信息;IMU包括陀螺仪和加速度计,陀螺仪用于测量智能体角速度信息,加速度计用于测量智能体加速度和地球重力加速度;GNSS接收机用于测量智能体伪距、多普勒频移和时钟偏置信息。
如图2所示,本发明的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,包括以下步骤:
S1、对立体相机、IMU、GNSS接收机原始测量数据进行预处理,包括:
S101、对Intel Realsense D435i相机的RGB输出进行Shi-Tomasi角点特征提取,并对随后的RGB视频帧执行KLT光流跟踪;
S102、构建IMU测量模型,并在机器人坐标系下对BMI055惯性测量单元的原始测量数据进行预积分;
S103、对Ublox NEO-M8T捕获到的GNSS原始信号进行滤波,剔除信号不稳定和仰角较低的卫星。
其中,步骤S101中的视觉特征提取(即角点特征提取)及光流跟踪过程,可由NVIDIA Jetson Xavier NX的GPU模块完成,步骤S102及步骤S103则由机载CPU进行处理。
具体的:
提取Intel Realsense D435i相机RGB输出图像的Shi-Tomasi角点特征,并将相邻特征之间的间隔约束在25-100个像素。随后以第一帧图像中提取到的Shi-Tomasi角点特征为基础,对后续图像帧执行KLT稀疏光流跟踪,获取角点特征的帧间相对关系。当跟踪的特征点少于200个时,重新提取Shi-Tomasi角点特征。所述Shi-Tomasi角点特征提取及KLT光流跟踪过程由NVIDIA Jetson Xavier NX开发板的GPU模块进行求解。
BMI055惯性测量单元的原始测量数据包含陀螺仪数据与加速度计数据两部分,其测量值分别受到陀螺仪偏置和加速度偏置的影响,陀螺仪和加速度计的原始测量值可由如下公式构建:
其中,和分别表示以当前机器人坐标系为参考系,t时刻陀螺仪和加速度计的测量值,和表示t时刻陀螺仪和加速度计的真实数值,和分别为陀螺仪偏置和加速度计偏置,和分别为陀螺仪噪声和加速度计噪声,为世界坐标系与机器人坐标系之间的旋转矩阵,gw为重力加速度。所述陀螺仪和加速度计噪声为高斯白噪声,所述陀螺仪偏置和加速度计偏置服从布朗运动,其导数服从高斯分布。
假设智能体在连续两个视频帧内的运动时间为tk和tk+1,将参考坐标系由局部世界坐标系(w)转换为机器人坐标系(r),智能体的朝向(o)、速度(v)和位置(p)可以由下式表示:
其中,分别为世界坐标系(w)下tk+1时刻智能体的朝向、速度和位置,分别为tk时刻智能体的朝向、速度和位置,代表tk时刻世界坐标系到机器人坐标系之间的旋转,为tk时刻世界坐标系与机器人坐标系之间的变换矩阵,符号代表四元数乘法。IMU预积分项可表示为:
其中,为IMU朝向的预积分项、为IMU速度的预积分项、为IMU位置的预积分项,为从t时刻到tk时刻在机器人坐标系下的智能体朝向变化值,为从t时刻到tk时刻智能体本身的变换矩阵,和分别为陀螺仪和加速度计的测量值。
则IMU预积分项的一阶雅可比近似可由如下公式表示:
其中,为IMU朝向预积分项的测量值、为IMU速度预积分项的测量值、为IMU位置预积分项的测量值,和分别表示陀螺仪偏置和加速度计偏置的初始值,分别表示与雅可比矩阵相对应的子矩阵。当陀螺仪或加速度计偏置发生变化时,可以利用上述一阶雅可比近似来替代IMU预积分项,而无需重新积分。
最后,对Ublox NEO-M8T接收机捕获到的GNSS原始信号进行滤波,剔除信号不稳定和仰角较低的卫星,已减少信号传播过程中多径效应带来的影响。
S2、GNSS/IMU/光流信息联合初始化;包括:
S201、构建智能体状态向量χ,所述状态向量包含智能体当前的朝向、速度、位置、陀螺仪偏置、加速度计偏置、GNSS接收机的时钟偏置及偏置率、视觉特征点深度、里程计坐标系与世界坐标系之间的Z轴偏置;
S202、利用PnP算法和BA算法求解智能体的初始位置和初始朝向,同时利用SPP算法粗略求解智能体的经纬度;
具体的:
首先,将智能体状态向量χ构建为如下型式:
智能体状态向量包含智能体在局部世界坐标系下的朝向速度位置陀螺仪偏置和加速度计偏置GNSS接收机的时钟偏置δt和偏置率δt′、视觉特征点深度d1,d2,...,dm和里程计坐标系与世界坐标系之间的Z轴偏置ψ,式中n为滑动窗大小。
利用PnP算法计算两个相邻图像帧的相对运动关系,同时利用BA算法优化滑动窗内所有角点特征的重投影误差,获得智能体的初始位置和初始朝向对GNSS原始信号中的伪距信息执行单点定位(SPP)算法,计算智能体在地心地固(ECEF)坐标系下的经纬度。然后将智能体的初始位置和经纬度对齐,从而获得里程计坐标系与世界坐标系之间的变换矩阵完成GNSS/IMU/光流信息联合初始化。初始化完成后即可获得智能体的初始位置、朝向、里程计坐标系与世界坐标系之间的变换矩阵。
S3、构建视觉约束因子,包括:
S301、将Intel Realsense D435i的相机模型构建为针孔相机模型;
S302、建立空间特征点l从视频帧i到视频帧j的重投影函数;
S303、将视觉误差因子项构建为特征点重投影残差,即把空间特征点l从视频帧i的像素坐标系投影到世界坐标系,再从世界坐标系投影到视频帧j的像素坐标系,然后求解空间特征点l在视频帧j中的测量位置与实际位置的残差。
具体的:
将Intel RealSense D435i立体相机的成像模型构建为针孔相机模型,可假设图像中的空间特征点l在世界坐标系下的齐次坐标为:
其中,Xl、Yl、Zl分别为特征点l的坐标值,那么特征点l在视频帧i的像素平面内的齐次坐标可表示为:
内参矩阵K的元素已经由相机标定过程获得,fx和fy为尺度缩放量,cx和cy为平移量,空间特征点l从视频帧i到视频帧j的重投影函数可表示为:
其中,表示空间特征点l在视频帧j中的测量位置,表示空间特征点l在视频帧i中的实际位置,为j时刻世界坐标系与机器人坐标系之间的变换矩阵、为i时刻机器人坐标系与世界坐标系之间的变换矩阵、为相机坐标系与世界坐标系之间的变换矩阵,为相机坐标系与像素坐标系的变换矩阵;
其中,χV表示智能体状态向量χ中与视觉信息相关的子向量。
S4、构建IMU约束因子,包括:
S401、将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,分别用智能体在两个连续视频帧内真实的旋转角度与陀螺仪预积分项的逆做四元数乘法,即可获取陀螺仪的测量残差;
S402、将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差;速度偏置残差由智能体在两个连续视频帧内的真实速度偏置与加速度计的速度预积分项做差获得;位移偏置残差由智能体在两个连续视频帧内的真实位移偏置与加速度计位移预积分项做差获得;
S403、陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差获得;
S404、联立所述步骤S401、步骤S402和步骤S403即可完成IMU约束因子的构建。
具体的:
将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,同时将加速度计误差因子约束项分别构建为速度残差与位移残差,陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差获得。则IMU约束因子可构建为如下型式:
其中,χI表示智能体状态向量χ中与IMU相关的子向量,为IMU朝向的预积分项、为IMU速度的预积分项、为IMU位置的预积分项,为IMU朝向预积分项的测量值、为IMU速度预积分项的测量值、为IMU位置预积分项的测量值,IMU预积分项的测量值可由步骤S1获得,δbω和δba分别为陀螺仪偏置和加速度计偏置的残差,代表tk时刻世界坐标系到机器人坐标系之间的旋转,分别为世界坐标系(w)下tk+1时刻智能体的朝向、速度和位置,代表tk时刻世界坐标系到机器人坐标系之间的旋转,为tk时刻世界坐标系与机器人坐标系之间的变换矩阵,符号代表四元数乘法,和分别为tk+1时刻的陀螺仪偏置和加速度计偏置,和分别为tk时刻的陀螺仪偏置和加速度计偏置。
S5、构建GNSS约束因子,包括:
S501:建立GNSS伪距测量模型,利用伪距测量模型求解真实的GNSS伪距信息,则GNSS伪距约束因子可构建为真实的伪距与接收机测量伪距的残差;
S502:建立GNSS多普勒频移测量模型,利用多普勒频移测量模型求解真实的GNSS多普勒频移信息,则GNSS多普勒频移约束因子可构建为真实的载波多普勒频移与多普勒频移测量值的残差;
S503:建立GNSS接收机时钟偏置测量模型,则接收机时钟偏置约束因子可构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差;
S504:联立所述步骤S501、步骤S502和步骤S503即可完成GNSS约束因子的构建。
具体的:
智能体概率因子图模型中的GNSS约束因子项由伪距约束因子、多普勒频移约束因子和接收机时钟偏置约束因子共同组成。其中,智能体机载Ublox NEO-M8T接收机与导航卫星的伪距测量模型可表示为:
上述式中
其中,和为分别为Ublox NEO-M8T接收机和导航卫星在地心惯性(ECI)坐标系中的位置,和分别为地心地固坐标系下的位置,为GNSS伪距的测量值、c为光在真空中的传播速度、δtr和δts分别为接收机和卫星的时钟偏置、△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、npr为伪距信号的噪声、R代表由地球自转引起的坐标系误差、ωearth为地球自转角速度、代表卫星到接收机的信号传播时间。
则GNSS伪距约束因子可构建为真实的伪距与接收机测量伪距的残差:
智能体机载Ublox NEO-M8T接收机与导航卫星的多普勒频移测量模型可表示为:
上述式中
其中,为接收机和卫星的多普勒频移测量值,wl为载波波长、为卫星与接收机的方向向量、和分别为地心惯性坐标系下接收机和卫星的速度、和分别为地心地固坐标系下接收机和卫星的速度,R代表由地球自转引起的坐标系误差、ωearth为地球自转角速度、δtr′和δts′分别为接收机和卫星的时钟偏置率,代表卫星到接收机的信号传播时间,ndp为多普勒噪声。
则GNSS多普勒频移约束因子可构建为真实的载波多普勒频移与多普勒频移测量值的残差:
现将tk到tk+1内接收机时钟偏置约束因子构建为如下形式:
其中,为接收机时钟偏置观测模型、χτ表示智能体状态向量χ中与接收机时钟偏置相关的子向量、和分别为tk和tk-1时刻的接收机时钟偏置、为tk-1时刻的接收机时钟偏置率。本发明详细的坐标系变换关系如图3、图4所示。
联立所述伪距约束因子、GNSS多普勒频移约束因子和接收机时钟偏置约束因子,即可构成智能体概率因子图模型中的GNSS约束因子项。
S6、求解智能体位姿信息,包括:
S601:将智能体位姿求解过程视为状态估计问题,则智能体位姿求解过程变为状态向量的最大后验估计;
S602:构建GNSS/IMU/光流信息紧耦合的智能体位姿估计器因子图模型;
S603:设计观测数据边缘化策略,剔除滑动窗内冗余的光流、IMU、GNSS信息;
S604:以因子图优化的方式求解智能体状态向量。
具体的:
将智能体位姿求解过程视为状态估计问题,则智能体的最优状态即为状态向量的最大后验估计。假设智能体机载相机、IMU、GNSS接收机的测量信号相互独立,且测量噪声符合均值为零的高斯分布,则最大后验估计问题可等同于误差之和最小化,那么GNSS/IMU/光流信息紧耦合的智能体位姿估计器因子图模型可表示为:
其中,P(χ|z)为智能体状态向量的最大后验概率,z为总的智能体状态测量模型,ep为系统先验信息,Hp代表智能体先验位姿矩阵,n为滑动窗内智能体状态向量的个数,符号E代表由步骤S3中的视觉约束因子EV、步骤S4中的IMU约束因子EI、和步骤S5中的GNSS约束因子Epr、Edp、Eτ之和,zk为智能体的第k次观测信息。
最后,以概率因子图优化的方式求解智能体状态向量χ,即求解公式(21)即可获得完整的智能体位姿信息。
一种基于GNSS/IMU/光流信息融合的智能体位姿估计系统,包括:
数据预处理模块,用于对智能体原始测量数据进行预处理,包括对相机的RGB输出进行Shi-Tomasi角点特征提取,并对随后的RGB视频帧执行KLT光流跟踪;在机器人坐标系下对惯性测量单元IMU的原始测量数据进行预积分;对捕获到的GNSS原始信号进行滤波;
联合初始化模块,用于对GNSS/IMU/光流信息联合初始化;
视觉约束因子构建模块,用于将相机投影模型构建为针孔相机模型,建立空间特征点l从视频帧i到视频帧j的重投影函数,将视觉误差因子项构建为特征点重投影残差;
IMU约束因子构建模块,用于将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差;
GNSS约束因子构建模块,用于将GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差,GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差,接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差;
智能体位姿信息求解模块,用于以概率因子图优化的方式求解智能体位姿信息。
Claims (10)
1.一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,包括以下步骤:
S1、分别对智能体原始测量数据进行预处理,包括对相机的RGB输出进行Shi-Tomasi角点特征提取,并对随后的RGB视频帧执行KLT光流跟踪;在机器人坐标系下对惯性测量单元(IMU)的原始测量数据进行预积分;对捕获到的GNSS原始信号进行滤波;
S2、位姿估计器GNSS/IMU/光流信息联合初始化:构建智能体状态向量χ,利用PnP算法和BA算法求解智能体的初始位置和初始朝向,同时利用SPP算法粗略求解智能体的经纬度;将智能体的初始位置和经纬度对齐,获得里程计坐标系与世界坐标系之间的变换矩阵,完成GNSS/IMU/光流信息联合初始化;
S3、构建视觉约束因子:将相机投影模型构建为针孔相机模型,建立空间特征点l从视频帧i到视频帧j的重投影函数,将视觉误差因子项构建为特征点重投影残差;
S4、构建IMU约束因子:IMU约束因子包括陀螺仪误差因子约束项和加速度计误差因子约束项,其中,将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差;
S5、构建GNSS约束因子:GNSS约束因子包括GNSS伪距约束因子、GNSS多普勒频移约束因子和接收机时钟偏置约束因子;其中,GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差,GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差,接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差;
S6、以概率因子图优化的方式求解智能体位姿信息。
2.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,步骤S2具体为:
S201、构建智能体状态向量χ,所述状态向量包含智能体当前的朝向速度位置陀螺仪偏置和加速度计偏置GNSS接收机的时钟偏置δt和偏置率δt′、视觉特征点深度d1,d2,...,dm和里程计坐标系与世界坐标系之间的Z轴偏置ψ,智能体状态向量χ表示为:
3.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,步骤S3中将视觉误差因子项构建为特征点重投影残差的具体方法为:把空间特征点l从视频帧i的像素坐标系投影到世界坐标系,再从世界坐标系投影到视频帧j的像素坐标系,然后求解特征点l在视频帧j中的测量位置与实际位置的残差。
4.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,步骤S4具体为:
S401、将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,分别用智能体在两个连续视频帧内真实的旋转角度与陀螺仪预积分项的逆做四元数乘法,即获取陀螺仪的测量残差;
S402、将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差;速度偏置残差由智能体在两个连续视频帧内的真实速度偏置与加速度计的速度预积分项做差获得;位移偏置残差由智能体在两个连续视频帧内的真实位移偏置与加速度计位移预积分项做差获得;
S403、陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差获得;
S404、联立步骤S401、步骤S402和步骤S403即完成IMU约束因子的构建;
构建的IMU约束因子为:
5.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,步骤S5具体为:
S501、建立GNSS伪距测量模型,利用伪距测量模型求解真实GNSS原始信号中的伪距信息,则GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差,表示为:
其中,表示卫星与接收机之间的观测方程,χpr表示智能体状态向量χ中与GNSS伪距相关的子向量,和分别为接收机和卫星在地心惯性坐标系下的位置、c为光在真空中的传播速度、和分别为接收机和卫星的时钟偏置,△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、代表伪距的测量值;
S502、建立GNSS多普勒频移测量模型,利用多普勒频移测量模型求解真实的GNSS多普勒频移信息,则GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差,表示为:
其中,表示卫星与接收机之间的观测方程,χdp表示智能体状态向量χ中与GNSS多普勒频移相关的子向量,wl为载波波长,为卫星与接收机的方向向量,和分别为地心惯性坐标系下接收机和卫星的速度、c为光在真空中的传播速度、和分别为接收机和卫星的时钟偏置率、为多普勒频移测量值;
S503、建立GNSS接收机时钟偏置测量模型,则接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差,表示为:
S504、联立步骤S501、步骤S502和步骤S503即完成GNSS约束因子的构建。
6.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,步骤S6具体为:
S601、将智能体位姿求解过程视为状态估计问题,则智能体位姿求解过程变为状态向量的最大后验估计;
S602、构建GNSS/IMU/光流信息紧耦合的位姿估计器因子图模型,表达式为:
其中,χ为智能体状态向量,P(χ|z)为智能体状态向量的最大后验概率,z为总的智能体状态测量模型,ep为系统先验信息,Hp代表智能体先验位姿矩阵,n为滑动窗内智能体状态向量的个数,符号E表示视觉约束因子EV、IMU约束因子EI、GNSS约束因子Epr、Edp、Eτ之和,zk为智能体的第k次观测信息;
S603、设计观测数据边缘化策略,剔除滑动窗内冗余的光流、IMU、GNSS信息;
S604、求解GNSS/IMU/光流信息紧耦合的位姿估计器因子图模型,得到智能体状态向量。
7.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,该方法采用的硬件设备的搭建过程为:
(1)采用NVIDIA Jetson Xavier NX作为智能体机载嵌入式计算平台,并安装ROSMelodic机器人操作系统及位姿估计器相关运行环境;
(3)采用Ublox NEO-M8T模块作为GNSS信号接收机,并以TTL电平标准与NVIDIA JetsonXavier NX的UART端口连接。
8.根据权利要求7所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,机载相机、IMU、GNSS接收机之间的硬件连接及同步过程为:
Intel RealSense D435i立体相机和BMI055惯性测量单元通过USB3.0与NVIDIAJetson Xavier NX开发板连接,Ublox NEO-M8T模块连接到NVIDIA Jetson Xavier NX开发板的UART接口,各传感器之间通过GNSS的PPS信号进行硬件同步。
9.根据权利要求7所述的一种基于GNSS/IMU/光流信息融合的智能体位姿估计方法,其特征在于,由NVIDIA Jetson Xavier NX的GPU模块完成视觉特征提取及光流跟踪,由机载CPU对惯性测量单元的原始数据和GNSS原始信号分别进行预积分和滤波处理。
10.一种基于GNSS/IMU/光流信息融合的智能体位姿估计系统,其特征在于,包括:
数据预处理模块,用于对智能体原始测量数据进行预处理,包括对相机的RGB输出进行Shi-Tomasi角点特征提取,并对随后的RGB视频帧执行KLT光流跟踪;在机器人坐标系下对惯性测量单元IMU的原始测量数据进行预积分;对捕获到的GNSS原始信号进行滤波;
联合初始化模块,用于对GNSS/IMU/光流信息联合初始化;
视觉约束因子构建模块,用于将相机投影模型构建为针孔相机模型,建立空间特征点l从视频帧i到视频帧j的重投影函数,将视觉误差因子项构建为特征点重投影残差;
IMU约束因子构建模块,用于将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差,将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差;
GNSS约束因子构建模块,用于将GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差,GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差,接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差;
智能体位姿信息求解模块,用于以概率因子图优化的方式求解智能体位姿信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210985155.XA CN115435779A (zh) | 2022-08-17 | 2022-08-17 | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210985155.XA CN115435779A (zh) | 2022-08-17 | 2022-08-17 | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115435779A true CN115435779A (zh) | 2022-12-06 |
Family
ID=84242648
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210985155.XA Pending CN115435779A (zh) | 2022-08-17 | 2022-08-17 | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115435779A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116381760A (zh) * | 2023-06-05 | 2023-07-04 | 之江实验室 | Gnss rtk/ins紧耦合定位方法、装置及介质 |
CN117128951A (zh) * | 2023-10-27 | 2023-11-28 | 中国科学院国家授时中心 | 适用于自动驾驶农机的多传感器融合导航定位系统及方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109916394A (zh) * | 2019-04-04 | 2019-06-21 | 山东智翼航空科技有限公司 | 一种融合光流位置和速度信息的组合导航算法 |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
CN113203411A (zh) * | 2021-04-23 | 2021-08-03 | 南京理工大学 | 一种弹性嵌入式gnss/惯性组合导航系统及方法 |
CN113790726A (zh) * | 2021-09-07 | 2021-12-14 | 中国科学院合肥物质科学研究院 | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 |
US20220076032A1 (en) * | 2020-09-08 | 2022-03-10 | Nvidia Corporation | Adaptive object tracking algorithm for autonomous machine applications |
-
2022
- 2022-08-17 CN CN202210985155.XA patent/CN115435779A/zh active Pending
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
CN109916394A (zh) * | 2019-04-04 | 2019-06-21 | 山东智翼航空科技有限公司 | 一种融合光流位置和速度信息的组合导航算法 |
US20220076032A1 (en) * | 2020-09-08 | 2022-03-10 | Nvidia Corporation | Adaptive object tracking algorithm for autonomous machine applications |
CN113203411A (zh) * | 2021-04-23 | 2021-08-03 | 南京理工大学 | 一种弹性嵌入式gnss/惯性组合导航系统及方法 |
CN113790726A (zh) * | 2021-09-07 | 2021-12-14 | 中国科学院合肥物质科学研究院 | 一种融合相机、轮速计和单uwb信息的机器人室内定位方法 |
Non-Patent Citations (2)
Title |
---|
蔡英凤 等: "基于多传感器融合的紧耦合SLAM系统", 《汽车工程》, 31 March 2022 (2022-03-31), pages 350 - 361 * |
高军强 等: "基于因子图的车载INS/GNSS/OD组合导航算法", 《系统工程与电子技术》, 30 November 2018 (2018-11-30), pages 2547 - 2553 * |
Cited By (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116381760A (zh) * | 2023-06-05 | 2023-07-04 | 之江实验室 | Gnss rtk/ins紧耦合定位方法、装置及介质 |
CN116381760B (zh) * | 2023-06-05 | 2023-08-15 | 之江实验室 | Gnss rtk/ins紧耦合定位方法、装置及介质 |
CN117128951A (zh) * | 2023-10-27 | 2023-11-28 | 中国科学院国家授时中心 | 适用于自动驾驶农机的多传感器融合导航定位系统及方法 |
CN117128951B (zh) * | 2023-10-27 | 2024-02-02 | 中国科学院国家授时中心 | 适用于自动驾驶农机的多传感器融合导航定位系统及方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US20240230335A1 (en) | Vision-Aided Inertial Navigation System for Ground Vehicle Localization | |
Schall et al. | Global pose estimation using multi-sensor fusion for outdoor augmented reality | |
Xiong et al. | G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving | |
CN113203418B (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统 | |
Sun et al. | An adaptive weighting strategy for multisensor integrated navigation in urban areas | |
CN115435779A (zh) | 一种基于gnss/imu/光流信息融合的智能体位姿估计方法 | |
WO2013037034A1 (en) | Method and apparatus for navigation with nonlinear models | |
Li et al. | Multi-GNSS PPP/INS/Vision/LiDAR tightly integrated system for precise navigation in urban environments | |
CN115388884A (zh) | 一种智能体位姿估计器联合初始化方法 | |
CN113503872A (zh) | 一种基于相机与消费级imu融合的低速无人车定位方法 | |
CN115218889A (zh) | 一种基于点线特征融合的多传感器室内定位方法 | |
Wen et al. | 3D vision aided GNSS real-time kinematic positioning for autonomous systems in urban canyons | |
Wang et al. | GIVE: A tightly coupled RTK-inertial–visual state estimator for robust and precise positioning | |
Ćwian et al. | GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments | |
Afia et al. | A low-cost gnss/imu/visual monoslam/wss integration based on federated kalman filtering for navigation in urban environments | |
CN115523920B (zh) | 一种基于视觉惯性gnss紧耦合的无缝定位方法 | |
US11860287B2 (en) | System and method for detecting outliers in GNSS observations | |
CN114646993A (zh) | 一种基于gnss、视觉以及imu进行精确定位的数据融合算法 | |
CN116105729A (zh) | 一种针对野外洞穴森林环境侦察的多传感器融合定位方法 | |
CN115930948A (zh) | 一种果园机器人融合定位方法 | |
Hou et al. | A Loosely-Coupled GNSS-Visual-Inertial Fusion for State Estimation Based On Optimation | |
CN105874352B (zh) | 使用旋转半径确定设备与船只之间的错位的方法和装置 | |
Song et al. | R2-GVIO: A Robust, Real-Time GNSS-Visual-Inertial State Estimator in Urban Challenging Environments | |
Shen et al. | Accurate and Capable GNSS-Inertial-Visual Vehicle Navigation via Tightly Coupled Multiple Homogeneous Sensors | |
CN115344033B (zh) | 一种基于单目相机/imu/dvl紧耦合的无人船导航与定位方法 |
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 |