CN115435779A - 一种基于gnss/imu/光流信息融合的智能***姿估计方法 - Google Patents

一种基于gnss/imu/光流信息融合的智能***姿估计方法 Download PDF

Info

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
Application number
CN202210985155.XA
Other languages
English (en)
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.)
Nanjing University of Aeronautics and Astronautics
Original Assignee
Nanjing University of Aeronautics and Astronautics
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 Nanjing University of Aeronautics and Astronautics filed Critical Nanjing University of Aeronautics and Astronautics
Priority to CN202210985155.XA priority Critical patent/CN115435779A/zh
Publication of CN115435779A publication Critical patent/CN115435779A/zh
Pending legal-status Critical Current

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
    • 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/光流信息融合的智能***姿估计方法
技术领域
本发明涉及智能***姿估计技术，特别是涉及一种基于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、构建智能体状态向量χ，所述状态向量包含智能体当前的朝向
Figure BDA0003801793930000021
速度
Figure BDA0003801793930000022
位置
Figure BDA0003801793930000023
陀螺仪偏置
Figure BDA0003801793930000024
和加速度计偏置
Figure BDA0003801793930000025
GNSS接收机的时钟偏置δt和偏置率δt′、视觉特征点深度d1,d2,...,dm和里程计坐标系与世界坐标系之间的Z轴偏置ψ，智能体状态向量χ表示为：
Figure BDA0003801793930000026
S202、利用PnP算法计算两个相邻图像帧的相对运动关系，同时利用BA算法优化滑动窗内所有角点特征的重投影误差，获得智能体的初始位置
Figure BDA0003801793930000031
和初始朝向
Figure BDA0003801793930000032
同时利用SPP算法粗略求解智能体的经纬度；
S203、将智能体的初始位置和经纬度对齐，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure BDA0003801793930000033
完成GNSS/IMU/光流信息联合初始化。
进一步的，步骤S3中将视觉误差因子项构建为特征点重投影残差的具体方法为：把空间特征点l从视频帧i的像素坐标系投影到世界坐标系，再从世界坐标系投影到视频帧j的像素坐标系，然后求解特征点l在视频帧j中的测量位置与实际位置的残差。
进一步的，步骤S4具体为：
S401、将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差，分别用智能体在两个连续视频帧内真实的旋转角度与陀螺仪预积分项的逆做四元数乘法，即获取陀螺仪的测量残差；
S402、将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差；速度偏置残差由智能体在两个连续视频帧内的真实速度偏置与加速度计的速度预积分项做差获得；位移偏置残差由智能体在两个连续视频帧内的真实位移偏置与加速度计位移预积分项做差获得；
S403、陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差获得；
S404、联立步骤S401、步骤S402和步骤S403即完成IMU约束因子的构建；
构建的IMU约束因子为：
Figure BDA0003801793930000041
其中，χI表示智能体状态向量χ中与IMU相关的子向量，
Figure BDA0003801793930000042
为IMU朝向的预积分项、
Figure BDA0003801793930000043
为IMU速度的预积分项、
Figure BDA0003801793930000044
为IMU位置的预积分项，
Figure BDA0003801793930000045
为IMU朝向预积分项的测量值、
Figure BDA0003801793930000046
为IMU速度预积分项的测量值、
Figure BDA0003801793930000047
为IMU位置预积分项的测量值，δbω和δba分别为陀螺仪偏置和加速度计偏置的残差，
Figure BDA0003801793930000048
代表tk时刻世界坐标系到机器人坐标系之间的旋转，
Figure BDA0003801793930000049
分别为世界坐标系下tk+1时刻智能体的朝向、速度和位置，
Figure BDA00038017939300000410
为tk时刻世界坐标系与机器人坐标系之间的变换矩阵，符号
Figure BDA00038017939300000411
代表四元数乘法，
Figure BDA00038017939300000412
和
Figure BDA00038017939300000413
分别为tk+1时刻的陀螺仪偏置和加速度计偏置，
Figure BDA00038017939300000414
和
Figure BDA00038017939300000415
分别为tk时刻的陀螺仪偏置和加速度计偏置。
进一步的，步骤S5具体为：
S501、建立GNSS伪距测量模型，利用伪距测量模型求解真实GNSS原始信号中的伪距信息，则GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差，表示为：
Figure BDA00038017939300000416
其中，
Figure BDA00038017939300000417
表示卫星与接收机之间的观测方程，χpr表示智能体状态向量χ中与GNSS伪距相关的子向量，
Figure BDA00038017939300000418
和
Figure BDA00038017939300000419
分别为接收机和卫星在地心惯性坐标系下的位置、c为光在真空中的传播速度、
Figure BDA00038017939300000420
和
Figure BDA00038017939300000421
分别为接收机和卫星的时钟偏置，△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、
Figure BDA0003801793930000051
代表伪距的测量值；
S502、建立GNSS多普勒频移测量模型，利用多普勒频移测量模型求解真实的GNSS多普勒频移信息，则GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差，表示为：
Figure BDA0003801793930000052
其中，
Figure BDA0003801793930000053
表示卫星与接收机之间的观测方程，χdp表示智能体状态向量χ中与GNSS多普勒频移相关的子向量，wl为载波波长，
Figure BDA0003801793930000054
为卫星与接收机的方向向量，
Figure BDA0003801793930000055
和
Figure BDA0003801793930000056
分别为地心惯性坐标系下接收机和卫星的速度、c为光在真空中的传播速度、
Figure BDA0003801793930000057
和
Figure BDA0003801793930000058
分别为接收机和卫星的时钟偏置率、
Figure BDA0003801793930000059
为多普勒频移测量值；
S503、建立GNSS接收机时钟偏置测量模型，则接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差，表示为：
Figure BDA00038017939300000510
其中，
Figure BDA00038017939300000511
为接收机时钟偏置观测模型，χτ表示智能体状态向量χ中与接收机时钟偏置相关的子向量，
Figure BDA00038017939300000512
和
Figure BDA00038017939300000513
分别为tk和tk-1时刻的接收机时钟偏置，
Figure BDA00038017939300000514
为tk-1时刻的接收机时钟偏置率；
S504、联立步骤S501、步骤S502和步骤S503即完成GNSS约束因子的构建。
进一步的，步骤S6具体为：
S601、将智能***姿求解过程视为状态估计问题，则智能***姿求解过程变为状态向量的最大后验估计；
S602、构建GNSS/IMU/光流信息紧耦合的位姿估计器因子图模型，表达式为：
Figure BDA00038017939300000515
其中，χ为智能体状态向量，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机器人操作***及位姿估计器相关运行环境；
(2)采用Intel RealSense D435i作为机载立体相机，分别标定该相机的3个镜头和内置BMI055惯性测量单元，获取相机的内参矩阵K及相机与内置BMI055的变换矩阵
Figure BDA0003801793930000061
(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的变换矩阵
Figure BDA0003801793930000081
选择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惯性测量单元的原始测量数据包含陀螺仪数据与加速度计数据两部分，其测量值分别受到陀螺仪偏置和加速度偏置的影响，陀螺仪和加速度计的原始测量值可由如下公式构建：
Figure BDA0003801793930000091
其中，
Figure BDA0003801793930000092
和
Figure BDA0003801793930000093
分别表示以当前机器人坐标系为参考系，t时刻陀螺仪和加速度计的测量值，
Figure BDA0003801793930000101
和
Figure BDA0003801793930000102
表示t时刻陀螺仪和加速度计的真实数值，
Figure BDA0003801793930000103
和
Figure BDA0003801793930000104
分别为陀螺仪偏置和加速度计偏置，
Figure BDA0003801793930000105
和
Figure BDA0003801793930000106
分别为陀螺仪噪声和加速度计噪声，
Figure BDA0003801793930000107
为世界坐标系与机器人坐标系之间的旋转矩阵，gw为重力加速度。所述陀螺仪和加速度计噪声为高斯白噪声，所述陀螺仪偏置和加速度计偏置服从布朗运动，其导数服从高斯分布。
假设智能体在连续两个视频帧内的运动时间为tk和tk+1，将参考坐标系由局部世界坐标系(w)转换为机器人坐标系(r)，智能体的朝向(o)、速度(v)和位置(p)可以由下式表示：
Figure BDA0003801793930000108
其中，
Figure BDA0003801793930000109
分别为世界坐标系(w)下tk+1时刻智能体的朝向、速度和位置，
Figure BDA00038017939300001010
分别为tk时刻智能体的朝向、速度和位置，
Figure BDA00038017939300001011
代表tk时刻世界坐标系到机器人坐标系之间的旋转，
Figure BDA00038017939300001012
为tk时刻世界坐标系与机器人坐标系之间的变换矩阵，符号
Figure BDA00038017939300001013
代表四元数乘法。IMU预积分项可表示为：
Figure BDA00038017939300001014
其中，
Figure BDA00038017939300001015
为IMU朝向的预积分项、
Figure BDA00038017939300001016
为IMU速度的预积分项、
Figure BDA00038017939300001017
为IMU位置的预积分项，
Figure BDA00038017939300001018
为从t时刻到tk时刻在机器人坐标系下的智能体朝向变化值，
Figure BDA00038017939300001019
为从t时刻到tk时刻智能体本身的变换矩阵，
Figure BDA00038017939300001020
和
Figure BDA00038017939300001021
分别为陀螺仪和加速度计的测量值。
则IMU预积分项的一阶雅可比近似可由如下公式表示：
Figure BDA0003801793930000111
其中，
Figure BDA0003801793930000112
为IMU朝向预积分项的测量值、
Figure BDA0003801793930000113
为IMU速度预积分项的测量值、
Figure BDA0003801793930000114
为IMU位置预积分项的测量值，
Figure BDA0003801793930000119
和
Figure BDA00038017939300001110
分别表示陀螺仪偏置和加速度计偏置的初始值，
Figure BDA0003801793930000115
分别表示与雅可比矩阵
Figure BDA0003801793930000116
相对应的子矩阵。当陀螺仪或加速度计偏置发生变化时，可以利用上述一阶雅可比近似来替代IMU预积分项，而无需重新积分。
最后，对Ublox NEO-M8T接收机捕获到的GNSS原始信号进行滤波，剔除信号不稳定和仰角较低的卫星，已减少信号传播过程中多径效应带来的影响。
S2、GNSS/IMU/光流信息联合初始化；包括：
S201、构建智能体状态向量χ，所述状态向量包含智能体当前的朝向、速度、位置、陀螺仪偏置、加速度计偏置、GNSS接收机的时钟偏置及偏置率、视觉特征点深度、里程计坐标系与世界坐标系之间的Z轴偏置；
S202、利用PnP算法和BA算法求解智能体的初始位置和初始朝向，同时利用SPP算法粗略求解智能体的经纬度；
S203、将智能体的初始位置和经纬度对齐，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure BDA0003801793930000117
具体的：
首先，将智能体状态向量χ构建为如下型式：
Figure BDA0003801793930000118
智能体状态向量包含智能体在局部世界坐标系下的朝向
Figure BDA0003801793930000121
速度
Figure BDA0003801793930000122
位置
Figure BDA0003801793930000123
陀螺仪偏置
Figure BDA0003801793930000124
和加速度计偏置
Figure BDA0003801793930000125
GNSS接收机的时钟偏置δt和偏置率δt′、视觉特征点深度d1,d2,...,dm和里程计坐标系与世界坐标系之间的Z轴偏置ψ，式中n为滑动窗大小。
利用PnP算法计算两个相邻图像帧的相对运动关系，同时利用BA算法优化滑动窗内所有角点特征的重投影误差，获得智能体的初始位置
Figure BDA0003801793930000126
和初始朝向
Figure BDA0003801793930000127
对GNSS原始信号中的伪距信息执行单点定位(SPP)算法，计算智能体在地心地固(ECEF)坐标系下的经纬度。然后将智能体的初始位置和经纬度对齐，从而获得里程计坐标系与世界坐标系之间的变换矩阵
Figure BDA0003801793930000128
完成GNSS/IMU/光流信息联合初始化。初始化完成后即可获得智能体的初始位置、朝向、里程计坐标系与世界坐标系之间的变换矩阵。
S3、构建视觉约束因子，包括：
S301、将Intel Realsense D435i的相机模型构建为针孔相机模型；
S302、建立空间特征点l从视频帧i到视频帧j的重投影函数；
S303、将视觉误差因子项构建为特征点重投影残差，即把空间特征点l从视频帧i的像素坐标系投影到世界坐标系，再从世界坐标系投影到视频帧j的像素坐标系，然后求解空间特征点l在视频帧j中的测量位置与实际位置的残差。
具体的：
将Intel RealSense D435i立体相机的成像模型构建为针孔相机模型，可假设图像中的空间特征点l在世界坐标系下的齐次坐标为：
Figure BDA0003801793930000129
其中，Xl、Yl、Zl分别为特征点l的坐标值，那么特征点l在视频帧i的像素平面内的齐次坐标可表示为：
Figure BDA00038017939300001210
其中，
Figure BDA00038017939300001211
分别为视频帧i的像素平面内的X、Y坐标，则Intel RealSense D435i立体相机的投影模型可表示为：
Figure BDA0003801793930000131
其中，
Figure BDA0003801793930000132
为机器人坐标系与相机坐标系之间的变换矩阵，
Figure BDA0003801793930000133
为i时刻世界坐标系与机器人坐标系之间的变换矩阵，nc为相机成像噪声，K为相机内参矩阵：
Figure BDA0003801793930000134
内参矩阵K的元素已经由相机标定过程获得，fx和fy为尺度缩放量，cx和cy为平移量，空间特征点l从视频帧i到视频帧j的重投影函数可表示为：
Figure BDA0003801793930000135
其中，
Figure BDA0003801793930000136
表示空间特征点l在视频帧j中的测量位置，
Figure BDA0003801793930000137
表示空间特征点l在视频帧i中的实际位置，
Figure BDA0003801793930000138
为j时刻世界坐标系与机器人坐标系之间的变换矩阵、
Figure BDA0003801793930000139
为i时刻机器人坐标系与世界坐标系之间的变换矩阵、
Figure BDA00038017939300001310
为相机坐标系与世界坐标系之间的变换矩阵，
Figure BDA00038017939300001311
为相机坐标系与像素坐标系的变换矩阵；
Figure BDA00038017939300001312
其中
Figure BDA00038017939300001313
分别为特征点l在视频帧i的像素平面内的X、Y坐标，
Figure BDA00038017939300001314
为空间特征点l相对于相机ci的深度。
则视觉约束因子可表示为空间特征点l在视频帧j中的实际位置
Figure BDA00038017939300001315
与测量位置
Figure BDA00038017939300001316
的残差：
Figure BDA00038017939300001317
其中，χV表示智能体状态向量χ中与视觉信息相关的子向量。
S4、构建IMU约束因子，包括：
S401、将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差，分别用智能体在两个连续视频帧内真实的旋转角度与陀螺仪预积分项的逆做四元数乘法，即可获取陀螺仪的测量残差；
S402、将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差；速度偏置残差由智能体在两个连续视频帧内的真实速度偏置与加速度计的速度预积分项做差获得；位移偏置残差由智能体在两个连续视频帧内的真实位移偏置与加速度计位移预积分项做差获得；
S403、陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差获得；
S404、联立所述步骤S401、步骤S402和步骤S403即可完成IMU约束因子的构建。
具体的：
将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差，同时将加速度计误差因子约束项分别构建为速度残差与位移残差，陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差获得。则IMU约束因子可构建为如下型式：
Figure BDA0003801793930000141
其中，χI表示智能体状态向量χ中与IMU相关的子向量，
Figure BDA0003801793930000142
为IMU朝向的预积分项、
Figure BDA0003801793930000143
为IMU速度的预积分项、
Figure BDA0003801793930000144
为IMU位置的预积分项，
Figure BDA0003801793930000145
为IMU朝向预积分项的测量值、
Figure BDA0003801793930000146
为IMU速度预积分项的测量值、
Figure BDA0003801793930000147
为IMU位置预积分项的测量值，IMU预积分项的测量值可由步骤S1获得，δbω和δba分别为陀螺仪偏置和加速度计偏置的残差，
Figure BDA0003801793930000148
代表tk时刻世界坐标系到机器人坐标系之间的旋转，
Figure BDA0003801793930000149
分别为世界坐标系(w)下tk+1时刻智能体的朝向、速度和位置，
Figure BDA00038017939300001410
代表tk时刻世界坐标系到机器人坐标系之间的旋转，
Figure BDA0003801793930000151
为tk时刻世界坐标系与机器人坐标系之间的变换矩阵，符号
Figure BDA0003801793930000152
代表四元数乘法，
Figure BDA0003801793930000153
和
Figure BDA00038017939300001514
分别为tk+1时刻的陀螺仪偏置和加速度计偏置，
Figure BDA0003801793930000154
和
Figure BDA0003801793930000155
分别为tk时刻的陀螺仪偏置和加速度计偏置。
S5、构建GNSS约束因子，包括：
S501：建立GNSS伪距测量模型，利用伪距测量模型求解真实的GNSS伪距信息，则GNSS伪距约束因子可构建为真实的伪距与接收机测量伪距的残差；
S502：建立GNSS多普勒频移测量模型，利用多普勒频移测量模型求解真实的GNSS多普勒频移信息，则GNSS多普勒频移约束因子可构建为真实的载波多普勒频移与多普勒频移测量值的残差；
S503：建立GNSS接收机时钟偏置测量模型，则接收机时钟偏置约束因子可构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差；
S504：联立所述步骤S501、步骤S502和步骤S503即可完成GNSS约束因子的构建。
具体的：
智能体概率因子图模型中的GNSS约束因子项由伪距约束因子、多普勒频移约束因子和接收机时钟偏置约束因子共同组成。其中，智能体机载Ublox NEO-M8T接收机与导航卫星的伪距测量模型可表示为：
Figure BDA0003801793930000156
上述式中
Figure BDA0003801793930000157
其中，
Figure BDA0003801793930000158
和
Figure BDA0003801793930000159
为分别为Ublox NEO-M8T接收机和导航卫星在地心惯性(ECI)坐标系中的位置，
Figure BDA00038017939300001510
和
Figure BDA00038017939300001511
分别为地心地固坐标系下的位置，
Figure BDA00038017939300001512
为GNSS伪距的测量值、c为光在真空中的传播速度、δtr和δts分别为接收机和卫星的时钟偏置、△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、npr为伪距信号的噪声、R代表由地球自转引起的坐标系误差、ωearth为地球自转角速度、
Figure BDA00038017939300001513
代表卫星到接收机的信号传播时间。
则GNSS伪距约束因子可构建为真实的伪距与接收机测量伪距的残差：
Figure BDA0003801793930000161
其中，
Figure BDA0003801793930000162
表示卫星与接收机之间的观测方程，χpr表示智能体状态向量χ中与GNSS伪距相关的子向量，
Figure BDA0003801793930000163
和
Figure BDA0003801793930000164
分别为接收机和卫星的时钟偏置，
Figure BDA0003801793930000165
代表伪距的测量值。
智能体机载Ublox NEO-M8T接收机与导航卫星的多普勒频移测量模型可表示为：
Figure BDA0003801793930000166
上述式中
Figure BDA0003801793930000167
其中，
Figure BDA0003801793930000168
为接收机和卫星的多普勒频移测量值，wl为载波波长、
Figure BDA0003801793930000169
为卫星与接收机的方向向量、
Figure BDA00038017939300001610
和
Figure BDA00038017939300001611
分别为地心惯性坐标系下接收机和卫星的速度、
Figure BDA00038017939300001612
和
Figure BDA00038017939300001613
分别为地心地固坐标系下接收机和卫星的速度，R代表由地球自转引起的坐标系误差、ωearth为地球自转角速度、δtr′和δts′分别为接收机和卫星的时钟偏置率，
Figure BDA00038017939300001614
代表卫星到接收机的信号传播时间，ndp为多普勒噪声。
则GNSS多普勒频移约束因子可构建为真实的载波多普勒频移与多普勒频移测量值的残差：
Figure BDA00038017939300001615
其中，
Figure BDA00038017939300001616
表示卫星与接收机之间的观测方程，χdp表示智能体状态向量χ中与GNSS多普勒频移相关的子向量、
Figure BDA00038017939300001617
为卫星与接收机的方向向量、
Figure BDA00038017939300001618
为多普勒频移测量值。
现将tk到tk+1内接收机时钟偏置约束因子构建为如下形式：
Figure BDA00038017939300001619
其中，
Figure BDA00038017939300001620
为接收机时钟偏置观测模型、χτ表示智能体状态向量χ中与接收机时钟偏置相关的子向量、
Figure BDA0003801793930000171
和
Figure BDA0003801793930000172
分别为tk和tk-1时刻的接收机时钟偏置、
Figure BDA0003801793930000173
为tk-1时刻的接收机时钟偏置率。本发明详细的坐标系变换关系如图3、图4所示。
联立所述伪距约束因子、GNSS多普勒频移约束因子和接收机时钟偏置约束因子，即可构成智能体概率因子图模型中的GNSS约束因子项。
S6、求解智能***姿信息，包括：
S601：将智能***姿求解过程视为状态估计问题，则智能***姿求解过程变为状态向量的最大后验估计；
S602：构建GNSS/IMU/光流信息紧耦合的智能***姿估计器因子图模型；
S603：设计观测数据边缘化策略，剔除滑动窗内冗余的光流、IMU、GNSS信息；
S604：以因子图优化的方式求解智能体状态向量。
具体的：
将智能***姿求解过程视为状态估计问题，则智能体的最优状态即为状态向量的最大后验估计。假设智能体机载相机、IMU、GNSS接收机的测量信号相互独立，且测量噪声符合均值为零的高斯分布，则最大后验估计问题可等同于误差之和最小化，那么GNSS/IMU/光流信息紧耦合的智能***姿估计器因子图模型可表示为：
Figure BDA0003801793930000174
其中，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、构建智能体状态向量χ，所述状态向量包含智能体当前的朝向
Figure FDA0003801793920000011
速度
Figure FDA0003801793920000012
位置
Figure FDA0003801793920000013
陀螺仪偏置
Figure FDA0003801793920000014
和加速度计偏置
Figure FDA0003801793920000015
GNSS接收机的时钟偏置δt和偏置率δt′、视觉特征点深度d1,d2,...,dm和里程计坐标系与世界坐标系之间的Z轴偏置ψ，智能体状态向量χ表示为：
Figure FDA0003801793920000021
S202、利用PnP算法计算两个相邻图像帧的相对运动关系，同时利用BA算法优化滑动窗内所有角点特征的重投影误差，获得智能体的初始位置
Figure FDA0003801793920000022
和初始朝向
Figure FDA0003801793920000023
同时利用SPP算法粗略求解智能体的经纬度；
S203、将智能体的初始位置和经纬度对齐，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure FDA0003801793920000024
完成GNSS/IMU/光流信息联合初始化。
3.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能***姿估计方法，其特征在于，步骤S3中将视觉误差因子项构建为特征点重投影残差的具体方法为：把空间特征点l从视频帧i的像素坐标系投影到世界坐标系，再从世界坐标系投影到视频帧j的像素坐标系，然后求解特征点l在视频帧j中的测量位置与实际位置的残差。
4.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能***姿估计方法，其特征在于，步骤S4具体为：
S401、将陀螺仪误差因子约束项构建为基于四元数外积的旋转残差，分别用智能体在两个连续视频帧内真实的旋转角度与陀螺仪预积分项的逆做四元数乘法，即获取陀螺仪的测量残差；
S402、将加速度计误差因子约束项分别构建为速度偏置残差与位移偏置残差；速度偏置残差由智能体在两个连续视频帧内的真实速度偏置与加速度计的速度预积分项做差获得；位移偏置残差由智能体在两个连续视频帧内的真实位移偏置与加速度计位移预积分项做差获得；
S403、陀螺仪偏置与加速度计偏置误差因子项分别由两个连续视频帧之间的偏置做差获得；
S404、联立步骤S401、步骤S402和步骤S403即完成IMU约束因子的构建；
构建的IMU约束因子为：
Figure FDA0003801793920000031
其中，χI表示智能体状态向量χ中与IMU相关的子向量，
Figure FDA0003801793920000032
为IMU朝向的预积分项、
Figure FDA0003801793920000033
为IMU速度的预积分项、
Figure FDA0003801793920000034
为IMU位置的预积分项，
Figure FDA0003801793920000035
为IMU朝向预积分项的测量值、
Figure FDA0003801793920000036
为IMU速度预积分项的测量值、
Figure FDA0003801793920000037
为IMU位置预积分项的测量值，δbω和δba分别为陀螺仪偏置和加速度计偏置的残差，
Figure FDA0003801793920000038
代表tk时刻世界坐标系到机器人坐标系之间的旋转，
Figure FDA0003801793920000039
Figure FDA00038017939200000310
分别为世界坐标系下tk+1时刻智能体的朝向、速度和位置，
Figure FDA00038017939200000311
为tk时刻世界坐标系与机器人坐标系之间的变换矩阵，符号
Figure FDA00038017939200000312
代表四元数乘法，
Figure FDA00038017939200000313
和
Figure FDA00038017939200000314
分别为tk+1时刻的陀螺仪偏置和加速度计偏置，
Figure FDA00038017939200000315
和
Figure FDA00038017939200000316
分别为tk时刻的陀螺仪偏置和加速度计偏置。
5.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能***姿估计方法，其特征在于，步骤S5具体为：
S501、建立GNSS伪距测量模型，利用伪距测量模型求解真实GNSS原始信号中的伪距信息，则GNSS伪距约束因子构建为真实的伪距与接收机测量伪距的残差，表示为：
Figure FDA00038017939200000317
其中，
Figure FDA00038017939200000318
表示卫星与接收机之间的观测方程，χpr表示智能体状态向量χ中与GNSS伪距相关的子向量，
Figure FDA00038017939200000319
和
Figure FDA00038017939200000320
分别为接收机和卫星在地心惯性坐标系下的位置、c为光在真空中的传播速度、
Figure FDA00038017939200000321
和
Figure FDA00038017939200000322
分别为接收机和卫星的时钟偏置，△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、
Figure FDA0003801793920000041
代表伪距的测量值；
S502、建立GNSS多普勒频移测量模型，利用多普勒频移测量模型求解真实的GNSS多普勒频移信息，则GNSS多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差，表示为：
Figure FDA0003801793920000042
其中，
Figure FDA0003801793920000043
表示卫星与接收机之间的观测方程，χdp表示智能体状态向量χ中与GNSS多普勒频移相关的子向量，wl为载波波长，
Figure FDA0003801793920000044
为卫星与接收机的方向向量，
Figure FDA0003801793920000045
和
Figure FDA0003801793920000046
分别为地心惯性坐标系下接收机和卫星的速度、c为光在真空中的传播速度、
Figure FDA0003801793920000047
和
Figure FDA0003801793920000048
分别为接收机和卫星的时钟偏置率、
Figure FDA0003801793920000049
为多普勒频移测量值；
S503、建立GNSS接收机时钟偏置测量模型，则接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差，表示为：
Figure FDA00038017939200000410
其中，
Figure FDA00038017939200000411
为接收机时钟偏置观测模型，χτ表示智能体状态向量χ中与接收机时钟偏置相关的子向量，
Figure FDA00038017939200000412
和
Figure FDA00038017939200000413
分别为tk和tk-1时刻的接收机时钟偏置，
Figure FDA00038017939200000414
为tk-1时刻的接收机时钟偏置率；
S504、联立步骤S501、步骤S502和步骤S503即完成GNSS约束因子的构建。
6.根据权利要求1所述的一种基于GNSS/IMU/光流信息融合的智能***姿估计方法，其特征在于，步骤S6具体为：
S601、将智能***姿求解过程视为状态估计问题，则智能***姿求解过程变为状态向量的最大后验估计；
S602、构建GNSS/IMU/光流信息紧耦合的位姿估计器因子图模型，表达式为：
Figure FDA00038017939200000415
其中，χ为智能体状态向量，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机器人操作***及位姿估计器相关运行环境；
(2)采用Intel RealSense D435i作为机载立体相机，分别标定该相机的3个镜头和内置BMI055惯性测量单元，获取相机的内参矩阵K及相机与内置BMI055的变换矩阵
Figure FDA0003801793920000051
(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多普勒频移约束因子构建为真实的载波多普勒频移与多普勒频移测量值的残差，接收机时钟偏置约束因子构建为某时刻真实的接收机时钟偏置与该时刻的时钟偏置测量值的残差；
智能***姿信息求解模块，用于以概率因子图优化的方式求解智能***姿信息。
CN202210985155.XA 2022-08-17 2022-08-17 一种基于gnss/imu/光流信息融合的智能***姿估计方法 Pending CN115435779A (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (5)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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
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视觉融合定位方法及***
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能***姿估计方法
WO2013037034A1 (en) Method and apparatus for navigation with nonlinear models
Sun et al. An adaptive weighting strategy for multisensor integrated navigation in urban areas
CN115388884A (zh) 一种智能***姿估计器联合初始化方法
Li et al. Multi-GNSS PPP/INS/Vision/LiDAR tightly integrated system for precise navigation in urban environments
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
Wen et al. 3D LiDAR aided GNSS real-time kinematic positioning
Wen et al. 3D vision aided GNSS real-time kinematic positioning for autonomous systems in urban canyons
Ćwian et al. GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments
Wang et al. GIVE: A tightly coupled RTK-inertial–visual state estimator for robust and precise positioning
US20240094415A1 (en) System and method for detecting outliers in gnss observations
CN113503872A (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
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紧耦合的无缝定位方法
CN116105729A (zh) 一种针对野外洞穴森林环境侦察的多传感器融合定位方法
CN114646993A (zh) 一种基于gnss、视觉以及imu进行精确定位的数据融合算法
CN115930948A (zh) 一种果园机器人融合定位方法
Hou et al. A Loosely-Coupled GNSS-Visual-Inertial Fusion for State Estimation Based On Optimation
CN105874352B (zh) 使用旋转半径确定设备与船只之间的错位的方法和装置
Le et al. The Performance Analysis of Multi-Sensor Integration Using Smartphone’s Sensors for Seamless Urban and Indoor Navigation
CN117128951B (zh) 适用于自动驾驶农机的多传感器融合导航定位***及方法
Li et al. A filter-based integration of GNSS, INS, and stereo vision in tight mode with optimal smoothing

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