CN115388884A - 一种智能体位姿估计器联合初始化方法 - Google Patents

一种智能体位姿估计器联合初始化方法 Download PDF

Info

Publication number
CN115388884A
CN115388884A CN202210985024.1A CN202210985024A CN115388884A CN 115388884 A CN115388884 A CN 115388884A CN 202210985024 A CN202210985024 A CN 202210985024A CN 115388884 A CN115388884 A CN 115388884A
Authority
CN
China
Prior art keywords
coordinate system
imu
intelligent body
intelligent
gnss
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
CN202210985024.1A
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 CN202210985024.1A priority Critical patent/CN115388884A/zh
Publication of CN115388884A publication Critical patent/CN115388884A/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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/53—Determining attitude

Landscapes

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

Abstract

本发明公开了一种智能体位姿估计器联合初始化方法，其步骤包括：首先，构建智能体状态向量，所述状态向量包含智能体当前朝向、位置、速度、陀螺仪和加速度计偏置、GNSS接收机偏置和偏置率、视觉特征深度、里程计坐标系与世界坐标系的偏置信息，并对智能体机载传感器测量数据进行预处理；然后，利用PnP算法求解纯视觉条件下的智能体位姿，并将视觉与IMU的联合初始化构建为最小二乘优化问题，通过带入视觉与IMU先验信息即可完成视觉与IMU的联合初始化；最后，利用SPP算法求解智能体在地心地固坐标系下的粗略位置，并构建GNSS伪距、多普勒频移和接收机时钟偏置的概率因子图模型，即可获得智能体位姿估计器的所有初值。

Description

一种智能体位姿估计器联合初始化方法
技术领域
本发明涉及多传感器融合位姿估计器技术，特别是涉及一种智能体位姿估计器联合初始化方法。
背景技术
相比于基于单一传感器的智能体位姿估计器，多传感器融合位姿估计技术能够充分利用不同种类传感器之间的互补特性，以获得更为准确且鲁棒的智能体位姿估计结果。立体相机和惯性测量单元(IMU)能够为智能体提供机器人坐标系下厘米级精度的位姿，但局部坐标系下的位姿会随着智能体的运动而漂移。全球导航卫星系统(GNSS)为能够为智能体提供无漂移的位置信息，已广泛应用于各种移动机器人导航任务。但基于 GNSS的智能体导航方法难以在室内使用，且易受噪声和多径效应的影响，导致其只能达到米级的定位精度。基于GNSS/IMU/视觉信息紧耦合的智能体位姿估计算法能够充分利用导航卫星、加速度计、陀螺仪、机载相机各自的优势，以获取准确、无漂、平滑的智能体位姿估计。
尽管基于GNSS/IMU/视觉信息紧耦合的智能体位姿估计算法具有较高的性能优势，但是由于紧耦合算法的非线性性质，位姿估计器的性能严重依赖于系统初始值。误差较大的系统初值将会导致智能体位姿估计器发散，甚至完全错误的位姿估计结果。在基于局部位姿传感器与全局位姿传感器融合的导航系统中，由于局部坐标系与全局坐标系之间的变换矩阵是一个动态变化的变量，变换矩阵必需随着智能体的运动而实时修正。这导致了基于GNSS/IMU/视觉信息紧耦合的智能体位姿估计器必需采取在线初始化的方式进行联合初始化。因此，如何快速、准确、鲁棒地获取智能体位姿估计器的初值已成为智能机器人导航领域亟需解决的难题。
发明内容
发明目的：本发明的目的是提供一种智能体位姿估计器联合初始化方法，以提高位姿估计器初值估计精度，且能够实时修正系统初值。
技术方案：本发明的一种智能体位姿估计器联合初始化方法，智能体位姿估计器包括集成在智能体上的相机、IMU和GNSS接收机，其联合初始化方法包括以下步骤：
S1、对IMU原始测量数据进行预处理：构建机器人坐标系下的IMU测量模型，获得IMU原始测量数据，在机器人坐标系下对IMU原始测量数据进行预积分，并对IMU 预积分项进行修正；
S2、对视觉信息进行预处理：提取第一帧图像的Shi-Tomasi角点特征，并以此为基础对后续图像帧进行角点特征跟踪，获取角点特征的帧间相对关系，计算两个相邻图像帧的相对运动关系，并优化滑动窗内所有角点特征的重投影误差，从而获得智能体的初始位姿；
S3、视觉与IMU联合初始化：将步骤S2获得的每个图像帧对应的智能体初始位姿带入陀螺仪偏置代价函数求得陀螺仪偏置，构建智能体状态向量χ，将智能体观测模型构建为最小二乘问题，并将智能体状态向量χ带入智能体观测模型，求解机器人坐标系下滑动窗内每个图像帧当前的智能体朝向、速度、位置和重力加速度信息，得到机器人坐标系r与里程计坐标系o之间的旋转矩阵
Figure RE-GDA0003810542080000021
利用旋转矩阵
Figure RE-GDA0003810542080000022
将智能体状态向量χ中的速度和位置信息投影到里程计坐标系，获得IMU/视觉联合轨迹；
S4、利用GNSS原始信号中的伪距信息粗略求解智能体的位置：根据GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型，计算智能体在地心地固坐标系下的智能体经纬度，同时获取地心地固坐标系与世界坐标系之间的旋转矩阵
Figure RE-GDA0003810542080000023
S5、对齐智能体里程计坐标系与世界坐标系，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure RE-GDA0003810542080000024
S6、GNSS/IMU/视觉信息联合初始化：获取智能体GNSS轨迹，在地心地固坐标系下，将IMU/视觉联合轨迹与智能体GNSS轨迹进行对齐，完成智能体位姿估计器的联合初始化。
进一步的，步骤S1中IMU测量模型包括陀螺仪测量模型与加速度计测量模型，IMU原始测量数据包括陀螺仪偏置、陀螺仪噪声、地球重力加速度、加速度计偏置和加速度计噪声，陀螺仪测量模型测量陀螺仪偏置与陀螺仪噪声，加速度计测量模型测量地球重力加速度、加速度计偏置和加速度计噪声；
以机载相机的视频输出间隔作为积分时间差，在机器人坐标系下对IMU原始测量数据进行预积分，tk时刻到tk+1时刻IMU预积分项表示为：
Figure RE-GDA0003810542080000031
所述公式中
Figure RE-GDA0003810542080000032
为IMU朝向的预积分项、
Figure RE-GDA0003810542080000033
为IMU速度的预积分项、
Figure RE-GDA0003810542080000034
为IMU 位置的预积分项；
Figure RE-GDA0003810542080000035
为IMU朝向预积分项的测量值、
Figure RE-GDA0003810542080000036
为IMU速度预积分项的测量值、
Figure RE-GDA0003810542080000037
为IMU位置预积分项的测量值；△bωtk和△batk分别表示陀螺仪偏置和加速度计偏置的初始值，
Figure RE-GDA0003810542080000038
分别代表与雅可比矩阵
Figure RE-GDA0003810542080000039
相对应的子矩阵。当陀螺仪或加速度计偏置发生变化时，可以利用上述一阶雅可比近似来替代IMU预积分项，而无需重新积分。
进一步的，步骤S2中将角点特征分布约束为相邻特征之间的像素间隔大于等于预先设定的阈值，对后续图像帧的跟踪采用KLT稀疏光流跟踪；
设置滑动窗的阈值D，利用PnP算法计算两个相邻图像帧的相对运动关系，并用 BA算法优化滑动窗内所有角点特征的重投影误差，获得智能体的初始位姿，包括智能体的初始位置和初始朝向。
进一步的，步骤S2中跟踪的角点特征少于设定的阈值时，重新执行步骤S2对视觉信息进行预处理。
进一步的，步骤S3中智能体状态向量χ包括智能体当前的朝向、速度、位置、陀螺仪偏置和加速度计偏置、GNSS接收机偏置和偏置率、视觉特征深度和里程计坐标系与世界坐标系的Z轴偏航角偏置，表示为：
Figure RE-GDA00038105420800000310
其中，n代表滑动窗的大小，
Figure RE-GDA00038105420800000311
δt、δt′、ψ分别为智能体朝向、速度、位置、陀螺仪偏置、加速度计偏置、GNSS接收机偏置和偏置率、里程计坐标系与世界坐标系的Z轴偏航角偏置，d1,d2,...,dm为视觉特征深度；
机器人坐标系下滑动窗内每个图像帧当前的状态信息包括智能体的朝向、速度、位置和重力加速度。
进一步的，步骤S4中先对GNSS原始信号进行滤波，剔除仰角过低的卫星信号，再根据GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型，距离模型表达式为：
Figure RE-GDA0003810542080000041
其中，
Figure RE-GDA0003810542080000042
和
Figure RE-GDA0003810542080000043
分别为GNSS接收机和导航卫星在地心惯性坐标系中的位置，
Figure RE-GDA0003810542080000044
为GNSS原始信号中的伪距信息的测量值、c为光在真空中的传播速度、δtr和δts分别为 GNSS接收机和导航卫星的时钟偏置、△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、npr为伪距信息的噪声；
根据距离模型求解GNSS接收机与导航卫星的距离，进而在没有视觉与IMU先验信息的条件下，利用单点定位算法获得智能体在地心地固坐标系下的经纬度。
进一步的，步骤S5具体为：
构建GNSS原始信号中多普勒频移的概率因子图模型，
Figure RE-GDA0003810542080000045
上述式中，
Figure RE-GDA0003810542080000046
其中，
Figure RE-GDA0003810542080000047
为观测方程、χdp为智能体状态向量中与多普勒频移有关的子向量、wl为载波波长、
Figure RE-GDA0003810542080000048
为卫星与接收机的方向向量、
Figure RE-GDA0003810542080000049
和
Figure RE-GDA00038105420800000410
分别为地心惯性坐标系下接收机和卫星的速度、
Figure RE-GDA00038105420800000411
和
Figure RE-GDA00038105420800000412
分别为地心地固坐标系下接收机和卫星的速度，R代表由地球自转引起的坐标系误差、
Figure RE-GDA00038105420800000413
和
Figure RE-GDA00038105420800000414
分别为接收机和卫星的时钟偏置率、
Figure RE-GDA00038105420800000415
为多普勒频移测量值、ωearth为地球自转角速度、
Figure RE-GDA00038105420800000416
代表卫星到接收机的信号传播时间。
将步骤S3获得的智能体速度
Figure RE-GDA0003810542080000051
和步骤S4粗略求解的智能体经纬度带入GNSS原始信号中多普勒频移的概率因子图模型，求解里程计坐标系与世界坐标系之间的Z轴偏航角偏置ψ和GNSS接收机时钟偏置漂移率δt′；对齐里程计坐标系o与世界坐标系w，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure RE-GDA0003810542080000052
进一步的，步骤S6具体为：
构建GNSS伪距信息和接收机时钟偏置的概率因子图模型，
Figure RE-GDA0003810542080000053
上述式中：
Figure RE-GDA0003810542080000054
其中，
Figure RE-GDA0003810542080000055
和
Figure RE-GDA0003810542080000056
分别为接收机和卫星在地心惯性坐标系下的位置、
Figure RE-GDA0003810542080000057
和
Figure RE-GDA0003810542080000058
分别为接收机和卫星在地心地固坐标系下的位置、c为光在真空中的传播速度、
Figure RE-GDA0003810542080000059
和
Figure RE-GDA00038105420800000510
分别为接收机和卫星的时钟偏置、△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、
Figure RE-GDA00038105420800000511
为伪距信息测量值。
将步骤S3得到的智能体朝向、速度、位置和重力加速度信息带入GNSS伪距信息和接收机时钟偏置的概率因子图模型，求解精确的接收机时钟偏置和智能体位置，获得智能体GNSS轨迹；
在地心地固坐标系下，将步骤S3获得的IMU/视觉联合轨迹与智能体GNSS轨迹进行对齐，完成智能体位姿估计器的初始化。
本发明的一种智能体位姿估计器联合初始化系统，包括：
数据预处理模块，用于在机器人坐标系下对IMU原始测量数据进行预积分，并对IMU预积分项进行修正；并对相机采集的视频数据提取第一帧图像的Shi-Tomasi角点特征，并以此为基础对后续图像帧进行跟踪，获取角点特征的帧间相对关系，计算两个相邻图像帧的相对运动关系，优化滑动窗内所有角点特征的重投影误差，获得相机的初始位姿；
视觉与IMU联合初始化模块，将每个图像帧对应的相机初始位姿带入陀螺仪偏置代价函数求得陀螺仪偏置，构建智能体状态向量χ，将智能体观测模型构建为最小二乘问题，并将智能体状态向量带入最小二乘问题，求解机器人坐标系下滑动窗内每个图像帧当前的智能体状态信息，得到机器人坐标系r与里程计坐标系o之间的旋转矩阵
Figure RE-GDA0003810542080000061
利用旋转矩阵
Figure RE-GDA0003810542080000062
将智能体状态向量χ中的智能体朝向、速度、位置和重力加速度信息投影到里程计坐标系，获得IMU/视觉联合轨迹；
智能体位置粗略求解模块，利用GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型，计算智能体在地心地固坐标系下的智能体经纬度，同时获取地心地固坐标系与世界坐标系之间的旋转矩阵
Figure RE-GDA0003810542080000063
坐标系对齐模块，用于对齐智能体里程计坐标系与世界坐标系，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure RE-GDA0003810542080000064
GNSS/IMU/视觉信息联合初始化模块，获取智能体GNSS轨迹，在地心地固坐标系下，将IMU/视觉联合轨迹与智能体GNSS轨迹进行对齐，完成智能体位姿估计器的联合初始化。
本发明的一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现如上述一种智能体位姿估计器联合初始化方法的步骤。
经由上述位姿估计器初始化方案可知，本发明公开一种基于非线性优化的 GNSS/IMU/视觉紧耦合智能体位姿估计器联合初始化方法。首先，利用运动恢复结构算法(SFM)求解相机的运动轨迹，同时对IMU数据进行预积分处理，从而初始化IMU/ 视觉紧耦合智能体位姿估计器；然后，通过单点定位算法(SPP)求解智能体在地心地固坐标系下的经纬度，在将IMU/视觉位姿估计器作为先验信息的条件下，以非线性优化的方式求解智能体里程计坐标系与世界坐标系之间的变换矩阵
Figure RE-GDA0003810542080000065
最后再以概率因子图优化的方式修正智能体在全局坐标系下的精确位姿，最终实现智能体位姿估计器的联合初始化。
有益效果：与现有技术相比，本发明的技术效果为：本发明将GNSS、IMU和视觉信息融合，具有更强的鲁棒性，能够有效应对传感器接收信号中的噪声和多径效应。在没有GNSS信号的情况下也能利用IMU与视觉信息完成位姿估计器的临时初始化，使得智能体位姿估计器能够在短时间内提供局部位姿信息。当接收机捕获到GNSS信号时，再对先前的初始化结果进行修正，从而获得准确、平滑、无漂的智能体位姿信息。
附图说明
图1是本发明方法流程图；
图2是本发明坐标系变换关系示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
针对传统多传感器融合位姿估计器初始化方法初值估计精度较低，且无法实时修正系统初值的问题，本发明提出了一种基于非线性优化的GNSS/IMU/视觉紧耦合智能体位姿估计器联合初始化方法。在系统初始化阶段，首先利用运动恢复结构算法(SFM) 求解相机的运动轨迹，同时对IMU数据进行预积分处理，进而初始化IMU/视觉紧耦合智能体里程计；然后通过单点定位算法(SPP)求解智能体在地心地固坐标系下的粗略位置，在把IMU/视觉里程计作为先验信息的条件下，以非线性优化的方式求解里程计坐标系与世界坐标系之间的变换矩阵；最后再以概率因子图优化的方式修正智能体在世界坐标系下的精确位姿，最终实现智能体位姿估计器的联合初始化。
智能体位姿估计器包括集成在智能体上的相机、IMU和GNSS接收机，相机用于采集智能体视觉信息；IMU包括陀螺仪和加速度计，陀螺仪用于测量智能体角速度信息，加速度计用于测量智能体加速度和地球重力加速度；GNSS接收机用于测量智能体伪距、多普勒频移和时钟偏置信息。
如图1所示，本发明的一种智能体位姿估计器联合初始化方法，具体步骤如下：
S1、对IMU原始测量数据进行预处理，包括：
S101、构建机器人坐标系下的IMU测量模型，所述IMU测量模型包含陀螺仪测量模型与加速度计测量模型，所述陀螺仪测量模型包含陀螺仪偏置与陀螺仪噪声，所述加速度计测量模型包含地球重力加速度、加速度计偏置和加速度计噪声；
S102、以机载相机的视频输出间隔作为积分时间差，在机器人坐标系下对IMU原始测量数据进行预积分；
S103、求解IMU预积分项的协方差矩阵和一阶雅可比矩阵，当IMU偏置(包括陀螺仪偏置和加速度计偏置)发生变化时，根据IMU偏置项的一阶雅克比近似来修正IMU 预积分项。
具体的：
IMU原始测量数据包含加速度计测量数据与陀螺仪测量数据两部分，其测量值分别受到加速度偏置和陀螺仪偏置的影响，陀螺仪和加速度计的原始测量值可由如下公式构建：
Figure RE-GDA0003810542080000081
其中，
Figure RE-GDA0003810542080000082
和
Figure RE-GDA0003810542080000083
分别表示以当前机器人坐标系(r)为参考系，t时刻陀螺仪和加速度计的原始测量值，
Figure RE-GDA0003810542080000084
和
Figure RE-GDA0003810542080000085
表示t时刻陀螺仪和加速度计的真实数值，
Figure RE-GDA0003810542080000086
和
Figure RE-GDA0003810542080000087
分别为陀螺仪偏置和加速度计偏置，
Figure RE-GDA0003810542080000088
和
Figure RE-GDA0003810542080000089
分别为陀螺仪噪声和加速度计噪声，
Figure RE-GDA00038105420800000810
为世界坐标系与机器人坐标系之间的旋转矩阵，gw为重力加速度。所述陀螺仪和加速度计噪声为高斯白噪声，所述陀螺仪偏置和加速度计偏置服从布朗运动，其导数服从高斯分布。
假设智能体在连续两个视频帧内的运动时间为tk和tk+1，则在世界坐标系(w)下tk+1时刻智能体的朝向
Figure RE-GDA00038105420800000811
速度
Figure RE-GDA00038105420800000812
和位置
Figure RE-GDA00038105420800000813
可以由下式表示：
Figure RE-GDA00038105420800000814
其中，
Figure RE-GDA00038105420800000815
分别为世界坐标系(w)下tk时刻智能体的朝向、速度和位置，
Figure RE-GDA00038105420800000816
为智能体朝向角度变化值，
Figure RE-GDA00038105420800000817
为地球重力加速度，
Figure RE-GDA00038105420800000818
为机器人坐标系与世界坐标系之间的变换矩阵，符号
Figure RE-GDA00038105420800000819
代表四元数乘法。
Figure RE-GDA00038105420800000820
将参考坐标系由世界坐标系(w)转换为机器人坐标系(r)，则上述公式(3)可改写为：
Figure RE-GDA0003810542080000091
其中，
Figure RE-GDA0003810542080000092
代表tk时刻世界坐标系到机器人坐标系之间的旋转，
Figure RE-GDA0003810542080000093
为tk时刻世界坐标系与机器人坐标系之间的变换矩阵，IMU预积分项如下式所述：
Figure RE-GDA0003810542080000094
其中，
Figure RE-GDA0003810542080000095
为IMU朝向的预积分项、
Figure RE-GDA0003810542080000096
为IMU速度的预积分项、
Figure RE-GDA0003810542080000097
为IMU位置的预积分项，
Figure RE-GDA0003810542080000098
为从t时刻到tk时刻在机器人坐标系下的智能体朝向变化值，
Figure RE-GDA0003810542080000099
为从t时刻到tk时刻智能体自身的变换矩阵。考虑到IMU预积分项的离散情况，
Figure RE-GDA00038105420800000910
为单位四元数，表示没有旋转，
Figure RE-GDA00038105420800000911
和
Figure RE-GDA00038105420800000912
等于0。由于
Figure RE-GDA00038105420800000913
和
Figure RE-GDA00038105420800000914
的均值为0，可以忽略不计，则tk到tk+1时刻IMU预积分项的估计值可由下式表示：
Figure RE-GDA00038105420800000915
其中，i表示从时间tk到tk+1之间的任意离散时刻，△t代表从时刻i到i+1之间的时间间隔，
Figure RE-GDA00038105420800000916
为从时刻tk到i之间智能体的变换矩阵，
Figure RE-GDA00038105420800000917
为时刻tk到i+1之间IMU朝向预积分项的测量值、
Figure RE-GDA00038105420800000918
为时刻tk到i之间IMU朝向预积分项的测量值、
Figure RE-GDA00038105420800000919
为时刻 tk到i+1之间IMU速度预积分项的测量值、
Figure RE-GDA00038105420800000920
为时刻tk到i之间IMU速度预积分项的测量值、
Figure RE-GDA0003810542080000101
为时刻tk到i+1之间IMU位置预积分项的测量值、
Figure RE-GDA0003810542080000102
为时刻tk到i之间 IMU位置预积分项的测量值、
Figure RE-GDA0003810542080000103
和
Figure RE-GDA0003810542080000104
分别表示i时刻陀螺仪和加速度计的测量值、
Figure RE-GDA0003810542080000105
和
Figure RE-GDA0003810542080000106
分别为i时刻陀螺仪偏置和加速度计偏置。
则IMU预积分项的一阶雅可比近似可由如下公式表示：
Figure RE-GDA0003810542080000107
所述公式中
Figure RE-GDA0003810542080000108
为IMU朝向预积分项的测量值、
Figure RE-GDA0003810542080000109
为IMU速度预积分项的测量值、
Figure RE-GDA00038105420800001010
为IMU位置预积分项的测量值，
Figure RE-GDA00038105420800001011
和
Figure RE-GDA00038105420800001012
分别表示陀螺仪偏置和加速度计偏置的初始值，
Figure RE-GDA00038105420800001013
分别代表与雅可比矩阵
Figure RE-GDA00038105420800001014
相对应的子矩阵。当陀螺仪或加速度计偏置发生变化时，可以利用上述一阶雅可比近似来替代IMU预积分项，而无需重新积分。
S2、对视觉信息进行预处理，包括：
S201、提取第一帧图像的Shi-Tomasi角点特征，并将相邻角点特征之间的像素间隔范围设置为25-100个像素；
S202、以第一帧图像中提取到的Shi-Tomasi角点特征为基础，对后续图像帧执行KLT稀疏光流跟踪，获取角点特征的帧间相对关系；
若跟踪的角点特征少于提前设定的阈值时，重新执行步骤S201；
S203、设置滑动窗的阈值，利用PnP算法计算两个相邻图像帧的相对运动关系；然后用BA算法优化滑动窗内所有角点特征的重投影误差，从而获得智能体的初始位姿，即智能体的初始位置和初始朝向。
具体的：
首先，提取第一帧图像的Shi-Tomasi角点特征，并将角点特征分布约束为相邻角点特征之间的间隔范围设置为25-100个像素，使得特征分布更均匀，有利于后续的PnP 算法求解。然后，以第一帧图像中提取到的Shi-Tomasi角点特征为基础，对后续图像帧执行KLT稀疏光流跟踪，获取角点特征的帧间相对关系。当跟踪的角点特征少于200 个时，重新提取Shi-Tomasi角点特征。最后，将滑动窗的阈值设置为5-15，利用PnP 算法计算两个相邻图像帧的相对运动关系，同时利用BA算法优化滑动窗内所有角点特征的重投影误差，获得智能体的初始位置
Figure RE-GDA0003810542080000111
和初始朝向
Figure RE-GDA0003810542080000112
从而完成位智能体姿估计器的视觉初始化部分。
S3、视觉与IMU联合初始化，包括：
S301、根据所述步骤S2可初步得出每个图像帧对应的智能体初始位置和初始朝向，将其带入陀螺仪偏置代价函数求得陀螺仪偏置；
S302、构建智能体状态向量χ，所述状态向量包含智能体当前的朝向、速度、位置、陀螺仪偏置和加速度计偏置、GNSS接收机偏置和偏置率、视觉特征深度和里程计坐标系与世界坐标系的Z轴偏航角偏置；
S303、将视觉与IMU的联合初始化过程构建为最小二乘问题，并带入步骤S302中所述的状态向量，求解机器人坐标系下滑动窗内每个图像帧当前的朝向、速度、位置和重力加速度；
S304、所述步骤S303完成后便可得到机器人坐标系(r)与里程计坐标系(o)之间的旋转矩阵
Figure RE-GDA0003810542080000113
利用此旋转矩阵即可将智能体状态向量中的其他变量投影到里程计坐标系，即完成视觉与IMU的联合初始化。
具体的：
首先将智能体状态向量χ构建为如下形式：
Figure RE-GDA0003810542080000114
其中，n代表滑动窗的大小，智能体状态向量包含智能体在世界坐标系(w)下的朝向
Figure RE-GDA0003810542080000115
速度
Figure RE-GDA0003810542080000116
位置
Figure RE-GDA0003810542080000117
陀螺仪偏置
Figure RE-GDA0003810542080000118
和加速度计偏置
Figure RE-GDA0003810542080000119
GNSS接收机偏置δt 和偏置率δt′、视觉特征深度d1,d2,...,dm和里程计坐标系与世界坐标系的Z轴偏航角偏置ψ。
由所述步骤S2可得出滑动窗内任意两个连续帧对应的智能体初始位置
Figure RE-GDA0003810542080000121
和初始朝向
Figure RE-GDA0003810542080000122
将此初始位姿信息作为先验来估计陀螺仪偏置的初值，则可把陀螺仪偏置的初始值△bω构建为如下优化问题：
Figure RE-GDA0003810542080000123
所述公式中L为滑动窗内所有视频帧，
Figure RE-GDA0003810542080000124
为t0到tk时间段内由视觉信息得到的智能体朝向变化，
Figure RE-GDA0003810542080000125
为t0到tk+1时间段内由视觉信息得到的智能体朝向变化，通过求解上述公式即可估计出陀螺仪偏置的初始值。
当获取陀螺仪偏置初始值后，构建智能体状态向量χ的子向量χVI为如下形式：
Figure RE-GDA0003810542080000126
子向量χVI包含智能体朝向、速度、位置、陀螺仪和加速度计偏置、视觉特征深度。然后将智能体坐标系作为参考系，将视觉与IMU的联合初始化过程构建成如下最小二乘问题：
Figure RE-GDA0003810542080000127
其中，
Figure RE-GDA0003810542080000128
为智能体的线性观测，
Figure RE-GDA0003810542080000129
矩阵是由相机获得的先验位姿信息。
把步骤S2中由机载相机获得的智能体初始位置和初始朝向代入上述公式，即可得出智能体状态子向量χVI。
最后将参考系重新转换为世界坐标系，即可获得视觉/IMU联合位置和朝向，从而完成视觉与IMU的联合初始化。
S4：利用SPP算法粗略求解智能体的位置，包括：
S401、对GNSS原始信号进行滤波，剔除仰角过低的卫星信号，减少多径效应的影响；
S402、根据GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型；
S403、在没有视觉与IMU先验信息的条件下，对GNSS原始信号中的伪距信息执行单点定位(SPP)算法，计算智能体在地心地固(ECEF)坐标系下的经纬度(即智能体粗略位置)，同时获取地心地固坐标系与世界坐标系之间的旋转矩阵
Figure RE-GDA0003810542080000131
具体的：
首先，对GNSS原始信号进行滤波，剔除仰角低于30度卫星，已减少信号传播过程中多径效应带来的影响，并将电离层延迟和对流层延迟分别构建为克罗布歇模型和萨斯塔莫宁模型。
然后，以如下形式构建导航卫星与GNSS接收机的距离模型：
Figure RE-GDA0003810542080000132
其中，
Figure RE-GDA0003810542080000133
和
Figure RE-GDA0003810542080000134
为分别为GNSS接收机和导航卫星在地心惯性(ECI)坐标系中的位置，
Figure RE-GDA0003810542080000135
为GNSS伪距信息的测量值、c为光在真空中的传播速度、δtr和δts分别为接收机和卫星的时钟偏置、△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、npr为伪距信息的噪声。
最后，根据上述公式求解GNSS接收机与导航卫星的距离，进而在没有视觉与IMU先验信息的条件下，利用单点定位(SPP)算法获得智能体在地心地固(ECEF)坐标系下的经纬度(即智能体粗略位置)，粗略求解出智能体在地心地固坐标系(e)下的位置。
当获得智能体的经纬度后，即可得到地心地固坐标系与世界坐标系的旋转矩阵：
Figure RE-GDA0003810542080000136
其中，λ代表经度、
Figure RE-GDA0003810542080000137
代表维度。
S5：求解世界坐标系(w)与智能体里程计坐标系(o)之间的变换矩阵，包括：
S501、构建GNSS多普勒频移的概率因子图模型(如式(15)所示)，因子图内的因子包含智能体里程计坐标系(o)与世界坐标系(w)之间的Z轴角度偏置ψ和时钟偏置漂移率δt′；
S502、将所述步骤S3中获得的智能体速度和所述步骤S4中获得的智能体经纬度带入所述步骤S501中的因子图模型，求解里程计坐标系与世界坐标系之间的Z轴偏航角偏置ψ和接收机时钟偏置漂移率δt′；
S503、对齐里程计坐标系(o)与世界坐标系(w)，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure RE-GDA0003810542080000141
具体的：
本发明将世界坐标系与智能体里程计坐标系的原点和Z轴对齐，即两个坐标系的原点和Z轴完全重合，两个坐标系仅仅相差一个绕Z轴的偏航角偏置ψ。
首先，将智能体状态向量χ中与多普勒频移有关的子向量构建为：
Figure RE-GDA0003810542080000142
则GNSS原始信号中的多普勒频移误差Edp可构建为如下形式：
Figure RE-GDA0003810542080000143
上述式中，
Figure RE-GDA0003810542080000144
其中，
Figure RE-GDA0003810542080000145
为观测方程、χdp为智能体状态向量中与多普勒频移有关的子向量、wl为载波波长、
Figure RE-GDA0003810542080000146
为卫星与接收机的方向向量、
Figure RE-GDA0003810542080000147
和
Figure RE-GDA0003810542080000148
分别为地心惯性坐标系下接收机和卫星的速度、
Figure RE-GDA0003810542080000149
和
Figure RE-GDA00038105420800001410
分别为地心地固坐标系下接收机和卫星的速度，R代表由地球自转引起的坐标系误差、c为光在真空中的传播速度、
Figure RE-GDA00038105420800001411
和
Figure RE-GDA00038105420800001412
分别为接收机和卫星的时钟偏置率、
Figure RE-GDA00038105420800001413
为多普勒频移测量值、ωearth为地球自转角速度、
Figure RE-GDA00038105420800001414
代表卫星到接收机的信号传播时间。
里程计坐标系与世界坐标系之间的Z轴初始角度偏差ψ和接收机时钟偏置漂移率可构建为如下优化问题：
Figure RE-GDA00038105420800001415
其中，n为滑动窗内智能体状态向量的数量、m为捕获到的卫星数量。将步骤S3 和步骤S4获得的智能体状态信息作为先验带入公式(17)，即可求解出ψ和δt′。
S6：GNSS/IMU/视觉信息联合初始化，包括：
S601、构建GNSS伪距信息和接收机时钟偏置的概率因子图模型(如公式(19)所示)，因子图内的因子包含接收机时钟偏置和所述步骤S4获得的智能体经纬度；
S602、将所述步骤S3中获得的视觉/IMU联合智能体朝向和位置带入所述步骤S601中的因子图模型，求解接收机时钟偏置和智能体精确位置；
S603、在地心地固坐标系下，将所述步骤S3完成的IMU/视觉联合智能体位置与步骤S602中得到的智能体精确位置进行对齐，完成智能体位姿估计器的初始化。
具体的：
首先，将智能体状态向量χ中与伪距信息有关的子向量构建为：
Figure RE-GDA0003810542080000151
则GNSS原始信号中的伪距误差可构建为如下形式：
Figure RE-GDA0003810542080000152
上述式中：
Figure RE-GDA0003810542080000153
其中，
Figure RE-GDA0003810542080000154
和
Figure RE-GDA0003810542080000155
分别为接收机和卫星在地心惯性坐标系下的位置、
Figure RE-GDA0003810542080000156
和
Figure RE-GDA0003810542080000157
分别为地心地固坐标系下的位置、c为光在真空中的传播速度、
Figure RE-GDA0003810542080000158
和
Figure RE-GDA0003810542080000159
分别为接收机和卫星的时钟偏置、△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、
Figure RE-GDA00038105420800001510
为伪距信息测量值。
然后，将tk到tk+1内接收机时钟偏置误差构建为如下形式：
Figure RE-GDA00038105420800001511
其中
Figure RE-GDA00038105420800001512
为tk-1时刻的接收机时钟偏置率。则智能体的精确位置可构建为如下最小二乘优化问题：
Figure RE-GDA00038105420800001513
将步骤S3和步骤S5求得的先验信息带入公式(22)即可获得智能体的精确位姿。
待步骤S6完成后，即可获得智能体位姿估计器的所有初值。
本发明的一种智能体位姿估计器联合初始化系统，包括：
数据预处理模块，用于在机器人坐标系下对IMU原始测量数据进行预积分，并对IMU预积分项进行修正；并对相机采集的视频数据提取第一帧图像的Shi-Tomasi角点特征，并以此为基础对后续图像帧进行跟踪，获取角点特征的帧间相对关系，计算两个相邻图像帧的相对运动关系，优化滑动窗内所有角点特征的重投影误差，获得相机的初始位姿；
视觉与IMU联合初始化模块，将每个图像帧对应的相机初始位姿带入陀螺仪偏置代价函数求得陀螺仪偏置，构建智能体状态向量χ，将智能体观测模型构建为最小二乘问题，并将智能体状态向量带入最小二乘问题，求解机器人坐标系下滑动窗内每个图像帧当前的智能体状态信息，得到机器人坐标系r与里程计坐标系o之间的旋转矩阵
Figure RE-GDA0003810542080000161
利用旋转矩阵
Figure RE-GDA0003810542080000162
将智能体状态向量χ中的智能体朝向、速度、位置和重力加速度信息投影到里程计坐标系，获得IMU/视觉联合轨迹；
智能体位置粗略求解模块，利用GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型，计算智能体在地心地固坐标系下的智能体经纬度，同时获取地心地固坐标系与世界坐标系之间的旋转矩阵
Figure RE-GDA0003810542080000163
坐标系对齐模块，用于对齐智能体里程计坐标系与世界坐标系，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure RE-GDA0003810542080000164
GNSS/IMU/视觉信息联合初始化模块，获取智能体GNSS轨迹，在地心地固坐标系下，将IMU/视觉联合轨迹与智能体GNSS轨迹进行对齐，完成智能体位姿估计器的联合初始化。
本发明的一种存储介质，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现所述一种智能体位姿估计器联合初始化方法的步骤，并能达到上述方法的技术效果。
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。

Claims (10)

1.一种智能体位姿估计器联合初始化方法，其特征在于，智能体位姿估计器包括集成在智能体上的相机、IMU和GNSS接收机，其联合初始化方法包括以下步骤：
S1、对IMU原始测量数据进行预处理：构建机器人坐标系下的IMU测量模型，获得IMU原始测量数据，在机器人坐标系下对IMU原始测量数据进行预积分，并对IMU预积分项进行修正；
S2、对视觉信息进行预处理：提取第一帧图像的Shi-Tomasi角点特征，并以此为基础对后续图像帧进行角点特征跟踪，获取角点特征的帧间相对关系，计算两个相邻图像帧的相对运动关系，并优化滑动窗内所有角点特征的重投影误差，从而获得智能体的初始位姿；
S3、视觉与IMU联合初始化：将步骤S2获得的每个图像帧对应的智能体初始位姿带入陀螺仪偏置代价函数求得陀螺仪偏置，构建智能体状态向量χ，将智能体观测模型构建为最小二乘问题，并将智能体状态向量χ带入智能体观测模型，求解机器人坐标系下滑动窗内每个图像帧当前的智能体朝向、速度、位置和重力加速度信息，得到机器人坐标系r与里程计坐标系o之间的旋转矩阵
Figure FDA0003801748040000011
利用旋转矩阵
Figure FDA0003801748040000012
将智能体状态向量χ中的速度和位置信息投影到里程计坐标系，获得IMU/视觉联合轨迹；
S4、利用GNSS原始信号中的伪距信息粗略求解智能体的位置：根据GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型，计算智能体在地心地固坐标系下的智能体经纬度，同时获取地心地固坐标系与世界坐标系之间的旋转矩阵
Figure FDA0003801748040000013
S5、对齐智能体里程计坐标系与世界坐标系，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure FDA0003801748040000014
S6、GNSS/IMU/视觉信息联合初始化：获取智能体GNSS轨迹，在地心地固坐标系下，将IMU/视觉联合轨迹与智能体GNSS轨迹进行对齐，完成智能体位姿估计器的联合初始化。
2.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S1中IMU测量模型包括陀螺仪测量模型与加速度计测量模型，IMU原始测量数据包括陀螺仪偏置、陀螺仪噪声、地球重力加速度、加速度计偏置和加速度计噪声，陀螺仪测量模型测量陀螺仪偏置与陀螺仪噪声，加速度计测量模型测量地球重力加速度、加速度计偏置和加速度计噪声；
以机载相机的视频输出间隔作为积分时间差，在机器人坐标系下对IMU原始测量数据进行预积分，tk时刻到tk+1时刻IMU预积分项表示为：
Figure FDA0003801748040000021
所述公式中
Figure FDA0003801748040000022
为IMU朝向的预积分项、
Figure FDA0003801748040000023
为IMU速度的预积分项、
Figure FDA0003801748040000024
为IMU位置的预积分项；
Figure FDA0003801748040000025
为IMU朝向预积分项的测量值、
Figure FDA0003801748040000026
为IMU速度预积分项的测量值、
Figure FDA0003801748040000027
为IMU位置预积分项的测量值；
Figure FDA0003801748040000028
和
Figure FDA0003801748040000029
分别表示陀螺仪偏置和加速度计偏置的初始值，
Figure FDA00038017480400000210
分别代表与雅可比矩阵
Figure FDA00038017480400000211
相对应的子矩阵。当陀螺仪或加速度计偏置发生变化时，可以利用上述一阶雅可比近似来替代IMU预积分项，而无需重新积分。
3.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S2中将角点特征分布约束为相邻特征之间的像素间隔大于等于预先设定的阈值，对后续图像帧的跟踪采用KLT稀疏光流跟踪；
设置滑动窗的阈值D，利用PnP算法计算两个相邻图像帧的相对运动关系，并用BA算法优化滑动窗内所有角点特征的重投影误差，获得智能体的初始位姿，包括智能体的初始位置和初始朝向。
4.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S2中跟踪的角点特征少于设定的阈值时，重新执行步骤S2对视觉信息进行预处理。
5.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S3中智能体状态向量χ包括智能体当前的朝向、速度、位置、陀螺仪偏置和加速度计偏置、GNSS接收机偏置和偏置率、视觉特征深度和里程计坐标系与世界坐标系的Z轴偏航角偏置，表示为：
Figure FDA0003801748040000031
其中，n代表滑动窗的大小，
Figure FDA0003801748040000032
δt、δt′、ψ分别为智能体朝向、速度、位置、陀螺仪偏置、加速度计偏置、GNSS接收机偏置和偏置率、里程计坐标系与世界坐标系的Z轴偏航角偏置，d1,d2,...,dm为视觉特征深度；
机器人坐标系下滑动窗内每个图像帧当前的状态信息包括智能体的朝向、速度、位置和重力加速度。
6.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S4中先对GNSS原始信号进行滤波，剔除仰角过低的卫星信号，再根据GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型，距离模型表达式为：
Figure FDA0003801748040000033
其中，
Figure FDA0003801748040000034
和
Figure FDA0003801748040000035
分别为GNSS接收机和导航卫星在地心惯性坐标系中的位置，
Figure FDA0003801748040000036
为GNSS原始信号中的伪距信息的测量值、c为光在真空中的传播速度、δtr和δts分别为GNSS接收机和导航卫星的时钟偏置、△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、npr为伪距信息的噪声；
根据距离模型求解GNSS接收机与导航卫星的距离，进而在没有视觉与IMU先验信息的条件下，利用单点定位算法获得智能体在地心地固坐标系下的经纬度。
7.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S5具体为：
构建GNSS原始信号中多普勒频移的概率因子图模型，
Figure FDA0003801748040000037
上述式中，
Figure FDA0003801748040000038
其中，
Figure FDA0003801748040000039
为观测方程、χdp为智能体状态向量中与多普勒频移有关的子向量、wl为载波波长、
Figure FDA0003801748040000041
为卫星与接收机的方向向量、
Figure FDA0003801748040000042
和
Figure FDA0003801748040000043
分别为地心惯性坐标系下接收机和卫星的速度、
Figure FDA0003801748040000044
和
Figure FDA0003801748040000045
分别为地心地固坐标系下接收机和卫星的速度，R代表由地球自转引起的坐标系误差、
Figure FDA0003801748040000046
和
Figure FDA0003801748040000047
分别为接收机和卫星的时钟偏置率、
Figure FDA0003801748040000048
为多普勒频移测量值、ωearth为地球自转角速度、
Figure FDA0003801748040000049
代表卫星到接收机的信号传播时间。
将步骤S3获得的智能体速度
Figure FDA00038017480400000410
和步骤S4粗略求解的智能体经纬度带入GNSS原始信号中多普勒频移的概率因子图模型，求解里程计坐标系与世界坐标系之间的Z轴偏航角偏置ψ和GNSS接收机时钟偏置漂移率δt′；对齐里程计坐标系o与世界坐标系w，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure FDA00038017480400000411
8.根据权利要求1所述的一种智能体位姿估计器联合初始化方法，其特征在于，步骤S6具体为：
构建GNSS伪距信息和接收机时钟偏置的概率因子图模型，
Figure FDA00038017480400000412
上述式中：
Figure FDA00038017480400000413
其中，
Figure FDA00038017480400000414
和
Figure FDA00038017480400000415
分别为接收机和卫星在地心惯性坐标系下的位置、
Figure FDA00038017480400000416
和
Figure FDA00038017480400000417
分别为接收机和卫星在地心地固坐标系下的位置、c为光在真空中的传播速度、
Figure FDA00038017480400000418
和
Figure FDA00038017480400000419
分别为接收机和卫星的时钟偏置、△ttro和△tion分别为大气中对流层和电离层的延迟、△tmul为由多经效应引起的延迟、
Figure FDA00038017480400000420
为伪距信息测量值。
将步骤S3得到的智能体朝向、速度、位置和重力加速度信息带入GNSS伪距信息和接收机时钟偏置的概率因子图模型，求解精确的接收机时钟偏置和智能体位置，获得智能体GNSS轨迹；
在地心地固坐标系下，将步骤S3获得的IMU/视觉联合轨迹与智能体GNSS轨迹进行对齐，完成智能体位姿估计器的初始化。
9.一种智能体位姿估计器联合初始化系统，其特征在于，包括：
数据预处理模块，用于在机器人坐标系下对IMU原始测量数据进行预积分，并对IMU预积分项进行修正；并对相机采集的视频数据提取第一帧图像的Shi-Tomasi角点特征，并以此为基础对后续图像帧进行跟踪，获取角点特征的帧间相对关系，计算两个相邻图像帧的相对运动关系，优化滑动窗内所有角点特征的重投影误差，获得相机的初始位姿；
视觉与IMU联合初始化模块，将每个图像帧对应的相机初始位姿带入陀螺仪偏置代价函数求得陀螺仪偏置，构建智能体状态向量χ，将智能体观测模型构建为最小二乘问题，并将智能体状态向量带入最小二乘问题，求解机器人坐标系下滑动窗内每个图像帧当前的智能体状态信息，得到机器人坐标系r与里程计坐标系o之间的旋转矩阵
Figure FDA0003801748040000051
利用旋转矩阵
Figure FDA0003801748040000052
将智能体状态向量χ中的智能体朝向、速度、位置和重力加速度信息投影到里程计坐标系，获得IMU/视觉联合轨迹；
智能体位置粗略求解模块，利用GNSS原始信号中的伪距信息构建导航卫星与GNSS接收机的距离模型，计算智能体在地心地固坐标系下的智能体经纬度，同时获取地心地固坐标系与世界坐标系之间的旋转矩阵
Figure FDA0003801748040000053
坐标系对齐模块，用于对齐智能体里程计坐标系与世界坐标系，获得里程计坐标系与世界坐标系之间的变换矩阵
Figure FDA0003801748040000054
GNSS/IMU/视觉信息联合初始化模块，获取智能体GNSS轨迹，在地心地固坐标系下，将IMU/视觉联合轨迹与智能体GNSS轨迹进行对齐，完成智能体位姿估计器的联合初始化。
10.一种存储介质，其特征在于，所述存储介质上存储有计算机程序，所述计算机程序被至少一个处理器执行时实现如权利要求1-8任一项所述一种智能体位姿估计器联合初始化方法的步骤。
CN202210985024.1A 2022-08-17 2022-08-17 一种智能体位姿估计器联合初始化方法 Pending CN115388884A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210985024.1A CN115388884A (zh) 2022-08-17 2022-08-17 一种智能体位姿估计器联合初始化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210985024.1A CN115388884A (zh) 2022-08-17 2022-08-17 一种智能体位姿估计器联合初始化方法

Publications (1)

Publication Number Publication Date
CN115388884A true CN115388884A (zh) 2022-11-25

Family

ID=84121423

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210985024.1A Pending CN115388884A (zh) 2022-08-17 2022-08-17 一种智能体位姿估计器联合初始化方法

Country Status (1)

Country Link
CN (1) CN115388884A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115523920A (zh) * 2022-11-30 2022-12-27 西北工业大学 一种基于视觉惯性gnss紧耦合的无缝定位方法
CN117073691A (zh) * 2023-10-12 2023-11-17 中国科学院光电技术研究所 基于优化的视觉惯性紧耦合航天器姿态测量方法
CN117288187A (zh) * 2023-11-23 2023-12-26 北京小米机器人技术有限公司 机器人位姿确定方法、装置、电子设备及存储介质

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115523920A (zh) * 2022-11-30 2022-12-27 西北工业大学 一种基于视觉惯性gnss紧耦合的无缝定位方法
CN115523920B (zh) * 2022-11-30 2023-03-10 西北工业大学 一种基于视觉惯性gnss紧耦合的无缝定位方法
CN117073691A (zh) * 2023-10-12 2023-11-17 中国科学院光电技术研究所 基于优化的视觉惯性紧耦合航天器姿态测量方法
CN117073691B (zh) * 2023-10-12 2024-02-02 中国科学院光电技术研究所 基于优化的视觉惯性紧耦合航天器姿态测量方法
CN117288187A (zh) * 2023-11-23 2023-12-26 北京小米机器人技术有限公司 机器人位姿确定方法、装置、电子设备及存储介质
CN117288187B (zh) * 2023-11-23 2024-02-23 北京小米机器人技术有限公司 机器人位姿确定方法、装置、电子设备及存储介质

Similar Documents

Publication Publication Date Title
CN108226980B (zh) 基于惯性测量单元的差分gnss与ins自适应紧耦合导航方法
CA2848217C (en) Method and apparatus for navigation with nonlinear models
US9488480B2 (en) Method and apparatus for improved navigation of a moving platform
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN115388884A (zh) 一种智能体位姿估计器联合初始化方法
US10082583B2 (en) Method and apparatus for real-time positioning and navigation of a moving platform
Xiong et al. G-VIDO: A vehicle dynamics and intermittent GNSS-aided visual-inertial state estimator for autonomous driving
CA2733032C (en) Method and apparatus for improved navigation of a moving platform
CN112146655B (zh) 一种BeiDou/SINS紧组合导航系统弹性模型设计方法
CN113253325B (zh) 惯性卫星序贯紧组合李群滤波方法
CN115435779A (zh) 一种基于gnss/imu/光流信息融合的智能体位姿估计方法
Hide et al. GPS and low cost INS integration for positioning in the urban environment
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
Wen et al. 3D LiDAR aided GNSS real-time kinematic positioning
CN115523920B (zh) 一种基于视觉惯性gnss紧耦合的无缝定位方法
Ćwian et al. GNSS-augmented lidar slam for accurate vehicle localization in large scale urban environments
CN116576849A (zh) 一种基于gmm辅助的车辆融合定位方法及系统
CN112083425A (zh) 一种引入径向速度的sins/lbl紧组合导航方法
CN114674313B (zh) 一种基于ckf算法的gps/bds和sins融合的无人配送车导航定位方法
CN113503872B (zh) 一种基于相机与消费级imu融合的低速无人车定位方法
CN114646993A (zh) 一种基于gnss、视觉以及imu进行精确定位的数据融合算法
Vik et al. A nonlinear observer for integration of gps and inertial navigation systems
CN117760427A (zh) 一种基于环境地标检测的惯导-地图融合定位方法
CN117760461A (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