CN114690230A - 一种基于视觉惯性slam的自动驾驶车辆导航方法 - Google Patents

一种基于视觉惯性slam的自动驾驶车辆导航方法 Download PDF

Info

Publication number
CN114690230A
CN114690230A CN202210480382.7A CN202210480382A CN114690230A CN 114690230 A CN114690230 A CN 114690230A CN 202210480382 A CN202210480382 A CN 202210480382A CN 114690230 A CN114690230 A CN 114690230A
Authority
CN
China
Prior art keywords
imu
representing
camera
dynamic target
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
CN202210480382.7A
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.)
Guilin University of Electronic Technology
Original Assignee
Guilin University of Electronic Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guilin University of Electronic Technology filed Critical Guilin University of Electronic Technology
Priority to CN202210480382.7A priority Critical patent/CN114690230A/zh
Publication of CN114690230A publication Critical patent/CN114690230A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • 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
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C3/00—Measuring distances in line of sight; Optical rangefinders
    • G01C3/02—Details

Landscapes

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

Abstract

本发明公开了一种基于视觉惯性SLAM的自动驾驶车辆导航方法，具体是一种视觉惯性SLAM结合目标检测网络的导航方法。目的是移除道路场景下动态目标的影响，解决自动驾驶车辆在动态环境下导航精度下降的问题。该方法是由如下步骤实现的：步骤一：车载传感器采集图像、加速度和角速度，之后对齐数据；步骤二：检测动态目标，移除动态目标对位姿预估的影响；步骤三：IMU预积分，每次优化更新后不需重新进行积分，减小计算量；步骤四：初始化计算出各个参数；步骤五：紧耦合优化计算出边缘化先验信息、IMU测量残差和视觉观测残差。本发明适用于视觉惯性导航***。

Description

一种基于视觉惯性SLAM的自动驾驶车辆导航方法
技术领域
本发明涉及自动驾驶车辆导航技术领域，尤其涉及一种基于视觉惯性SLAM 的自动驾驶车辆导航方法。
背景技术
将视觉惯性SLAM应用于自动驾驶车辆面临着许多问题和挑战，仍有一些关键技术问题亟待解决。比如视觉惯性SLAM的性能在很大程度上依赖于周围的视觉特征是静态的假设，使得***在动态环境中的导航精度急剧下降。为了提高自动驾驶车辆在动态环境下的定位和状态估计精度，本发明提出了一种结合动态目标检测的视觉惯性导航***，为视觉惯性SLAM应用于自动驾驶车辆提供了理论指导和解决方案。
为了解决视觉惯性SLAM在动态环境下的位姿预估精度下降、轨迹漂移甚至导航失败等问题，国内外出现了许多优秀的解决方案。现有的较好的解决方法是结合神经网络-语义分割来剔除动态特征点，但由于计算量较大，未能达到实时性要求。另一个解决方法是利用随机抽样一致算法来剔除异常值，但当场景中的异常值过多时会导致漂移甚至失败。最后一个解决方法是利用深度相机来检测运动物体，然后把物体上的特征点剔除，但限定的相机同时也限制了应用的场景和机载的传感器。由于实时性、计算量、传感器等限制，以上的解决方案仍不能使视觉惯性SLAM很好地应用于自动驾驶车辆。
发明内容
本发明针对现有技术的不足，提出了一种基于视觉惯性SLAM的自动驾驶车辆导航方法，提高了自动驾驶车辆导航的精度。
本发明采用下述技术方案实现：
基于视觉惯性SLAM的自动驾驶车辆导航方法，该方法采用下述步骤实现：
步骤一，车载双目相机采集前方道路的灰度图像，IMU传感器采集车辆的加速度和角速度，主控进行数据的对齐；
步骤二，训练动态目标检测模型，检测出道路场景下的动态目标，提取目标边界框，移除边界框内的特征点；
步骤三，消除IMU的噪声和偏差，对IMU数据进行预积分，计算出IMU数据的观测值以及残差的协方差矩阵和雅各比矩阵，之后修正偏差；
步骤四，初始化计算出绝对尺度、陀螺仪偏置、加速度偏置、重力加速度和每个IMU时刻的速度；
步骤五，初始化之后，执行一个基于滑动窗口的紧耦合优化，得到精度更高鲁棒性更好的状态估计值。
本发明的有效效果：一、本发明对采集的数据进行时间校准，提高位姿预估的精度；本发明对IMU数据进行预积分，减小了积分时所消耗的计算量。二、本发明基于YOLOv5网络训练所提出的动态目标检测模型来检测道路环境中的动态目标。相比于YOLOv5自带的模型，本模型只检测道路场景下的汽车、卡车、摩托车和行人等动态目标，缩减了模型的大小并且加快了识别动态目标的速度。本模型利用BD100K数据集训练得出，经过大量的调试和参数优化，我们的动态目标检测模型比YOLOv5自带的模型减小了大约70％的模型体量，目标检测速度平均提高了63％。三、本发明训练出的动态目标检测模型能检测道路环境下的动态目标，利用模型提取出目标边界框后，前端的特征点追踪模块移除目标边界框内的特征点，从而达到消除动态干扰的目的，提高自动驾驶车辆导航的精度。
本发明的效果通过如下实验得到验证：
本提出的方法与VINS-Fusion进行比较，VINS-Fusion具有闭环、重装载和高精度地图复用等功能。本发明已经在公共KITTI数据集和现实世界中评估了所提出的方法。为了使估计的轨迹与地面实况对齐，本发明使用了Horn’s的方法。本发明通过绝对位姿误差和绝对轨迹误差来评估导航的准确性。绝对位姿误差和绝对轨迹误差由EVO工具计算。本发明采用绝对轨迹均方根误差进行定量评估。绝对轨迹误差比较估计和地面实况轨迹的平移分量之间的绝对距离。在时间步i处的绝对轨迹误差计算公式如下：
Figure BDA0003627372500000021
其中，T表示对齐两个轨迹的变换；G表示真实地面；E表示预估的轨迹。
对于N个姿势的序列，绝对轨迹误差的绝对轨迹均方根误差的计算公式如下：
Figure BDA0003627372500000031
考虑到***的不确定性，所有值都是通过五次实验获得的，并显示了估计轨迹准确性的中值结果。本发明使用KITTI数据集来评估所提出的方法，该数据集适用于评估视觉惯性SLAM在自动驾驶车辆中的性能。在这个数据集中，为序列 00-10提供了真实地面。为了提供准确的地面实况，对激光雷达、摄像头和GPS 进行校准和同步。KITTI数据集是从现实世界中的各种场景数据中收集的，例如农村、高速公路和城市。本发明主要考虑序列00和序列05-07，因为在这些城市序列中有移动的车辆和行人。
在现实世界的实验中，本发明利用自制传感器套件来演示所提出的方法。本发明在户外高动态环境中驾驶汽车，并使用四种不同的组合进行状态估计。为了对该实验进行误差评估，本发明通过RTK-GPS传感器设置了控制点，控制点作为真实地面点。为了获得绝对误差，本发明将估计的轨迹与控制点对齐。
在KITTI数据集和真实世界上的实验表明，本发明提出的方法在大规模动态城市环境中，与VINS-Fusion相比，将定位精度提高了约40％。对比实验结果表明，本方法在动态环境中提高了准确性和鲁棒性。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明中步骤一到步骤五的流程示意图。
图2是实验中本发明在KITTI数据集中的运动轨迹和稠密点云。
图3是实验中本发明在现实世界中的实验设备。
图4是本发明和传统方法绝对轨迹误差的绝对轨迹均方根误差对比图。
具体实施方式
下面结合具体实施例对本发明作详细的说明。
基于视觉惯性SLAM的自动驾驶车辆导航方法，该方法采用下述步骤实现，具体见图1的流程：
步骤一，车载双目相机采集前方道路的灰度图像，IMU传感器采集车辆的加速度和角速度，主控进行数据的对齐。
步骤二，训练动态目标检测模型，检测出道路场景下的动态目标，提取目标边界框，移除边界框内的特征点。
步骤三，消除IMU的噪声和偏差，对IMU数据进行预积分，计算出IMU数据的观测值以及残差的协方差矩阵和雅各比矩阵，之后修正偏差。
步骤四，初始化计算出绝对尺度、陀螺仪偏置、加速度偏置、重力加速度和每个IMU时刻的速度。
步骤五，初始化之后，执行一个基于滑动窗口的紧耦合优化，得到精度更高鲁棒性更好的状态估计值。
所述步骤一中，具体步骤包括：
(1)车载双目相机以10Hz的频率采集左右目的灰度图像；
(2)车载IMU以100Hz的频率采集车体的三轴加速度和角速度；
(3)主控进行数据的对齐；由于触发延迟、传输延迟和不同步的时钟，生成的时间戳不等于实际采样时间，导致摄像机与IMU之间的时间失调。对齐数据的计算公式为：
tIMU＝tcam+td；
其中，tIMU和tcam分别表示IMU和相机的时间戳；td表示一个未知常数的时间偏移量。
所述步骤二中，具体步骤包括：
(1)训练动态目标检测模型。自训练的神经网络模型可以更准确、更快速地检测特定的动态目标。训练图像被传递到神经网络以创建定制训练的检测器。本发明准备了一个BDD100K训练集来训练动态目标检测模型。动态目标检测模型使用YOLOv5网络进行训练。与其他神经网络一样，YOLO网络也需要标记大量图像并将其发送到模型中，以训练神经网络的参数。动态目标检测模型检测到的动态目标包括人、汽车、摩托车、公共汽车和卡车。
动态目标检测模型的准确性和运行速度对整体结果有决定性的影响。本发明做了很多工作来提高动态目标检测模型的准确性。在实际应用中，将动态目标检测模型添加到视觉惯性SLAM中会增加算法的计算量，但不会影响***的实时性。
(2)提取边界框。动态目标检测模型提供动态目标的类和两维边界框。动态目标检测模型需要三个步骤来获得动态目标的边界框。首先，整个图像被分成大小相等的网格单元，并且网络同时在所有组合的单元上运行。其次，网络预测输出特征图中每个单元的边界框。最后，为了得到预测边界框相对于原图的实际坐标值，算法将(bx,by,bw,bh)除以特征图大小，再乘以原图的大小。预测边界框的计算公式为：
Figure BDA0003627372500000051
其中，(bx,by)表示每个网格单元在预测中心的坐标；(bw,bh)表示边界框的宽度和高度；(cx,cy)表示特征图中网格单元的左上坐标；pw和ph分别表示预测的边界框的宽度和高度。
所述步骤三中，具体步骤包括：
(1)消除IMU的噪声和偏置。测量值受到加速度偏置、陀螺仪偏置和噪声的影响，消除噪声和偏置的计算公式如下：
Figure BDA0003627372500000052
其中，
Figure BDA0003627372500000053
和
Figure BDA0003627372500000054
分别表示加速度计和陀螺仪的随机游走噪声；at表示加速度计测量的理想值；ωt表示陀螺仪测量的理想值；
Figure BDA0003627372500000055
表示IMU坐标系到相机坐标系的旋转矩阵；gw表示重力矢量。加速度噪声na和陀螺仪噪声nw为高斯白噪声，表示为：
Figure BDA0003627372500000056
(2)IMU测量值预积分；对于两个连续帧bk和bk+1，需要将时间间隔[tk,tk+1] 内的惯性测量值积分到局部帧bk中，计算公式如下：
Figure BDA0003627372500000061
其中，
Figure BDA0003627372500000062
表示位置预积分；
Figure BDA0003627372500000063
表示速度预积分；
Figure BDA0003627372500000064
表示旋转预积分；
Figure BDA0003627372500000065
(3)偏差修正。如果预估的偏差变化很小，将通过对偏差的一阶近似来调整
Figure BDA0003627372500000066
和
Figure BDA0003627372500000067
偏差修正的计算公式如下：
Figure BDA0003627372500000068
所述步骤四中，具体步骤包括：
(1)将姿态从相机帧转换到IMU帧中。计算公式如下：
Figure BDA0003627372500000069
其中，s为待求解的比例参数；
Figure BDA00036273725000000610
和
Figure BDA00036273725000000611
表示相机与IMU间的外参。
(2)校准陀螺仪偏置；通过线性化IMU预积分关于陀螺仪的偏置并最小化代价函数，来得到陀螺仪偏置的初始校准。代价函数的计算公式为：
Figure BDA00036273725000000612
其中，
Figure BDA00036273725000000613
表示窗口中所有帧的索引。
(3)初始化速度、重力向量和度量比例。速度、重力向量和度量比例的向量如下公式所示：
Figure BDA0003627372500000071
其中，
Figure BDA0003627372500000072
表示第k张图像中IMU的速度；g表示重力矢量。
窗口中两个连续帧bk和bk+1的转换公式为：
Figure BDA0003627372500000073
得到以下线性模型：
Figure BDA0003627372500000074
其中，
Figure BDA0003627372500000075
Δtk为两个连续帧间的时间间隔。
通过解决线性最小二乘问题，得到窗口中每一帧的速度，计算公式如下：
Figure BDA0003627372500000076
所述步骤五中，具体步骤包括：
(1)代价函数；对于相机和IMU，需要预估的整个状态定义为：
Figure BDA0003627372500000077
其中，p表示预估的位置；R表示预估的方向；xcam表示相机相关的状态向量； ximu表示IMU相关的状态向量；λ表示特征的深度；v表示速度向量；ba表示加速度偏置；bg表示陀螺仪偏置。
定义要预估的状态后进行状态预估，状态估计是一个最大似然预估问题，定义为：
Figure BDA0003627372500000078
其中，S表示测量值集；χ＝{x0,x1,…,xn}，
Figure BDA0003627372500000079
Figure BDA00036273725000000710
和
Figure BDA00036273725000000711
分别为位置矢量和方向四元数；
Figure BDA00036273725000000712
表示测量值的不确定度为高斯分布h(·)。
上式的负对数似然函数为：
Figure BDA0003627372500000081
其中，
Figure BDA0003627372500000082
Figure BDA0003627372500000083
表示高斯分布的信息矩阵
Figure BDA0003627372500000084
和
Figure BDA0003627372500000085
h(·)表示传感器模型。
之后将状态预估转换为一个非线性最小二乘问题。求解非线性最小二乘问题需要求出相机和IMU传感器的因子。
(2)相机和IMU传感器因子。相机因子由每一帧图像的特征组成。图像t 中观测的残差计算公式为：
Figure BDA0003627372500000086
其中，πc和
Figure BDA0003627372500000087
分别为相机模型的投影和反向投影函数；T表示4x4的齐次变换矩阵；
Figure BDA0003627372500000088
表示离线校准的IMU中心到相机中心的转换矩阵。
IMU残差计算公式为：
Figure BDA0003627372500000089
其中，α表示位置；β表示速度；γ表示旋转；
Figure BDA00036273725000000810
表示专门用于非线性旋转的负值操作；g表示等于9.81的重力向量。
(3)优化。通过(2)得到相机因子和IMU因子后，对非线性最小二乘问题进行优化。代价函数的计算公式为：
Figure BDA00036273725000000811
其中，J表示每个因子相对于当前状态
Figure BDA00036273725000000812
的雅可比矩阵。
在经过非线性近似后，上式得到了δχ的封闭解。求解的推导计算公式如下：
Figure BDA00036273725000000813
(4)边缘化。为了减小计算量，在不丢失有用信息的前提下引入边缘化。通过上式总结所有边缘化因素得到新的H和b在重新排列状态的顺序后，得到了以下的关系：
Figure BDA0003627372500000091
其中，
Figure BDA0003627372500000095
表示被边缘化的状态集合；Xr表示剩余状态集。
边缘化的计算公式如下：
Figure BDA0003627372500000092
通过上式得到剩余状态的新先验Hp和bp。在获得有关当前状态的先验信息后，使用贝叶斯规则将后验计算为可能性和先验的乘积：
Figure BDA0003627372500000093
然后状态估计变为最大后验问题，最大后验问题的计算公式为：
Figure BDA0003627372500000094
图2是通过本发明所提出的方法所构建的稀疏地图和运动轨迹。本发明在现实世界中采集数据集的装置如图3所示，图3中包含双目深度相机(MYNTEYE-S1030)、惯性测量单元(IMU)和GPS传感器(GPS-U-Blox)。在得到数据集后进行算法的精度验证，精度对比结果如图4所示，图4中的传统算法为 VINS-Fusion。
以上所揭露的仅为本发明一种较佳实施例而已，当然不能以此来限定本发明之权利范围，本领域普通技术人员可以理解实现上述实施例的全部或部分流程，并依本发明权利要求所作的等同变化，仍属于发明所涵盖的范围。

Claims (6)

1.一种基于视觉惯性SLAM的自动驾驶车辆导航方法，其特征在于，包括步骤：
步骤一，车载双目相机采集前方道路的灰度图像，IMU传感器采集车辆的加速度和角速度，主控进行数据的对齐；
步骤二，训练动态目标检测模型，检测出道路场景下的动态目标，提取目标边界框，移除边界框内的特征点；
步骤三，消除IMU的噪声和偏差，对IMU数据进行预积分，计算出IMU数据的观测值以及残差的协方差矩阵和雅各比矩阵，之后修正偏差；
步骤四，初始化计算出绝对尺度、陀螺仪偏置、加速度偏置、重力加速度和每个IMU时刻的速度；
步骤五，初始化之后，执行一个基于滑动窗口的紧耦合优化，得到精度更高鲁棒性更好的状态估计值。
2.根据权利要求1所述基于视觉惯性SLAM的自动驾驶车辆导航方法，其特征在于：所述步骤一中，具体步骤包括：
(1)车载双目相机以10Hz的频率采集左右目的灰度图像；
(2)车载IMU以100Hz的频率采集车体的三轴加速度和角速度；
(3)主控进行数据的对齐；由于触发延迟、传输延迟和不同步的时钟，生成的时间戳不等于实际采样时间，导致摄像机与IMU之间的时间失调；对齐数据的计算公式为：
tIMU＝tcam+td；
其中，tIMU和tcam分别表示IMU和相机的时间戳；td表示一个未知常数的时间偏移量。
3.根据权利要求1所述基于视觉惯性SLAM的自动驾驶车辆导航方法，其特征在于：所述步骤二中，具体步骤包括：
(1)训练动态目标检测模型；自训练的神经网络模型可以更准确、更快速地检测特定的动态目标；训练图像被传递到神经网络以创建定制训练的检测器；本发明准备了一个BDD100K训练集来训练动态目标检测模型；动态目标检测模型使用YOLOv5网络进行训练；与其他神经网络一样，YOLO网络也需要标记大量图像并将其发送到模型中，以训练神经网络的参数；动态目标检测模型检测到的动态目标包括人、汽车、摩托车、公共汽车和卡车；
动态目标检测模型的准确性和运行速度对整体结果有决定性的影响；本发明做了很多工作来提高动态目标检测模型的准确性；在实际应用中，将动态目标检测模型添加到视觉惯性SLAM中会增加算法的计算量，但不会影响***的实时性；
(2)提取边界框；动态目标检测模型提供动态目标的类和两维边界框；动态目标检测模型需要三个步骤来获得动态目标的边界框；首先，整个图像被分成大小相等的网格单元，并且网络同时在所有组合的单元上运行；其次，网络预测输出特征图中每个单元的边界框；最后，为了得到预测边界框相对于原图的实际坐标值，算法将(bx,by,bw,bh)除以特征图大小，再乘以原图的大小；预测边界框的计算公式为：
Figure FDA0003627372490000021
其中，(bx,by)表示每个网格单元在预测中心的坐标；(bw,bh)表示边界框的宽度和高度；(cx,cy)表示特征图中网格单元的左上坐标；pw和ph分别表示预测的边界框的宽度和高度。
4.根据权利要求1所述基于视觉惯性SLAM的自动驾驶车辆导航方法，其特征在于：所述步骤三中，具体步骤包括：
(1)消除IMU的噪声和偏置；测量值受到加速度偏置、陀螺仪偏置和噪声的影响，消除噪声和偏置的计算公式如下：
Figure FDA0003627372490000022
其中，
Figure FDA0003627372490000023
和
Figure FDA0003627372490000024
分别表示加速度计和陀螺仪的随机游走噪声；at表示加速度计测量的理想值；ωt表示陀螺仪测量的理想值；
Figure FDA0003627372490000025
表示IMU坐标系到相机坐标系的旋转矩阵；gw表示重力矢量；加速度噪声na和陀螺仪噪声nw为高斯白噪声，表示为：
Figure FDA0003627372490000026
(2)IMU测量值预积分；对于两个连续帧bk和bk+1，需要将时间间隔[tk,tk+1]内的惯性测量值积分到局部帧bk中，计算公式如下：
Figure FDA0003627372490000031
其中，
Figure FDA0003627372490000032
表示位置预积分；
Figure FDA0003627372490000033
表示速度预积分；
Figure FDA0003627372490000034
表示旋转预积分；
Figure FDA0003627372490000035
(3)偏差修正；如果预估的偏差变化很小，将通过对偏差的一阶近似来调整
Figure FDA0003627372490000036
和
Figure FDA0003627372490000037
计算公式如下：
Figure FDA0003627372490000038
5.根据权利要求1所述基于视觉惯性SLAM的自动驾驶车辆导航方法，其特征在于：所述步骤四中，具体步骤包括：
(1)将姿态从相机帧转换到IMU帧中；计算公式如下：
Figure FDA0003627372490000039
其中，s为待求解的比例参数；
Figure FDA00036273724900000310
和
Figure FDA00036273724900000311
表示相机与IMU间的外参；
(2)校准陀螺仪偏置；通过线性化IMU预积分关于陀螺仪的偏置并最小化代价函数，来得到陀螺仪偏置的初始校准；代价函数的计算公式为：
Figure FDA00036273724900000312
其中，B表示窗口中所有帧的索引；
(3)初始化速度、重力向量和度量比例；速度、重力向量和度量比例的向量如下公式所示：
Figure FDA0003627372490000041
其中，
Figure FDA0003627372490000042
表示第k张图像中IMU的速度；g表示重力矢量；
窗口中两个连续帧bk和bk+1的转换公式为：
Figure FDA0003627372490000043
得到以下线性模型：
Figure FDA0003627372490000044
其中，
Figure FDA0003627372490000045
Δtk为两个连续帧间的时间间隔；
通过解决线性最小二乘问题，得到窗口中每一帧的速度，计算公式如下：
Figure FDA0003627372490000046
6.根据权利要求1所述基于视觉惯性SLAM的自动驾驶车辆导航方法，其特征在于：所述步骤五中，具体步骤包括：
(1)代价函数；对于相机和IMU，需要预估的整个状态定义为：
Figure FDA0003627372490000047
其中，p表示预估的位置；R表示预估的方向；xcam表示相机相关的状态向量；ximu表示IMU相关的状态向量；λ表示特征的深度；v表示速度向量；ba表示加速度偏置；bg表示陀螺仪偏置；
定义要预估的状态后进行状态预估，状态估计是一个最大似然预估问题，定义为：
Figure FDA0003627372490000048
其中，S表示测量值集；χ＝{x0,x1,…,xn}，
Figure FDA0003627372490000049
Figure FDA00036273724900000410
和
Figure FDA00036273724900000411
分别为位置矢量和方向四元数；
Figure FDA0003627372490000051
表示测量值的不确定度为高斯分布h(·)；
上式的负对数似然函数为：
Figure FDA0003627372490000052
其中，
Figure FDA0003627372490000053
Figure FDA0003627372490000054
表示高斯分布的信息矩阵
Figure FDA0003627372490000055
和
Figure FDA0003627372490000056
h(·)表示传感器模型；
之后将状态预估转换为一个非线性最小二乘问题；求解非线性最小二乘问题需要求出相机和IMU传感器的因子；
(2)相机和IMU传感器因子；相机因子由每一帧图像的特征组成；图像t中观测的残差计算公式为：
Figure FDA0003627372490000057
其中，πc和
Figure FDA0003627372490000058
分别为相机模型的投影和反向投影函数；T表示4x4的齐次变换矩阵；
Figure FDA0003627372490000059
表示离线校准的IMU中心到相机中心的转换矩阵；
IMU残差计算公式为：
Figure FDA00036273724900000510
其中，α表示位置；β表示速度；γ表示旋转；
Figure FDA00036273724900000511
表示专门用于非线性旋转的负值操作；g表示等于9.81的重力向量；
(3)优化；通过(2)得到相机因子和IMU因子后，对非线性最小二乘问题进行优化；代价函数的计算公式为：
Figure FDA00036273724900000512
其中，J表示每个因子相对于当前状态
Figure FDA00036273724900000513
的雅可比矩阵；
在经过非线性近似后，上式得到了δχ的封闭解；求解的推导计算公式如下：
Figure FDA0003627372490000061
(4)边缘化；为了减小计算量，在不丢失有用信息的前提下引入边缘化；通过上式总结所有边缘化因素得到新的H和b在重新排列状态的顺序后，得到了以下的关系：
Figure FDA0003627372490000062
其中，χm表示被边缘化的状态集合；χr表示剩余状态集；
边缘化的计算公式如下：
Figure FDA0003627372490000063
通过上式得到剩余状态的新先验Hp和bp；在获得有关当前状态的先验信息后，使用贝叶斯规则将后验计算为可能性和先验的乘积：p(χ∣z)∝p(z∣χ)p(χ)；然后状态估计变为最大后验问题，最大后验问题的计算公式为：
Figure FDA0003627372490000064
CN202210480382.7A 2022-05-05 2022-05-05 一种基于视觉惯性slam的自动驾驶车辆导航方法 Pending CN114690230A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210480382.7A CN114690230A (zh) 2022-05-05 2022-05-05 一种基于视觉惯性slam的自动驾驶车辆导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210480382.7A CN114690230A (zh) 2022-05-05 2022-05-05 一种基于视觉惯性slam的自动驾驶车辆导航方法

Publications (1)

Publication Number Publication Date
CN114690230A true CN114690230A (zh) 2022-07-01

Family

ID=82145617

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210480382.7A Pending CN114690230A (zh) 2022-05-05 2022-05-05 一种基于视觉惯性slam的自动驾驶车辆导航方法

Country Status (1)

Country Link
CN (1) CN114690230A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115688610A (zh) * 2022-12-27 2023-02-03 泉州装备制造研究所 一种无线电磁六维定位方法、***、存储介质及电子设备

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115688610A (zh) * 2022-12-27 2023-02-03 泉州装备制造研究所 一种无线电磁六维定位方法、***、存储介质及电子设备
CN115688610B (zh) * 2022-12-27 2023-08-15 泉州装备制造研究所 一种无线电磁六维定位方法、***、存储介质及电子设备

Similar Documents

Publication Publication Date Title
CN112347840B (zh) 视觉传感器激光雷达融合无人机定位与建图装置和方法
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN112083725B (zh) 一种面向自动驾驶车辆的结构共用多传感器融合定位***
CN102999759B (zh) 一种基于光流的车辆运动状态估计方法
CN114526745B (zh) 一种紧耦合激光雷达和惯性里程计的建图方法及***
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN113052908A (zh) 一种基于多传感器数据融合的移动机器人位姿估计算法
CN113776519B (zh) 一种无光动态开放环境下agv车辆建图与自主导航避障方法
CN114758504B (zh) 一种基于滤波校正的网联车超速预警方法及***
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
CN115479602A (zh) 一种融合事件与距离的视觉惯性里程计方法
CN113920198B (zh) 一种基于语义边缘对齐的由粗到精的多传感器融合定位方法
CN110929402A (zh) 一种基于不确定分析的概率地形估计方法
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
CN114964276A (zh) 一种融合惯导的动态视觉slam方法
CN114754768A (zh) 一种点线融合的视觉惯性导航方法
CN113759364A (zh) 一种基于激光地图的毫米波雷达连续定位方法及装置
CN115218889A (zh) 一种基于点线特征融合的多传感器室内定位方法
CN115540850A (zh) 一种激光雷达与加速度传感器结合的无人车建图方法
CN115453599A (zh) 一种多传感器协同的管道机器人精准定位方法
CN114690230A (zh) 一种基于视觉惯性slam的自动驾驶车辆导航方法
CN114608568A (zh) 一种基于多传感器信息即时融合定位方法
CN112945233A (zh) 一种全局无漂移的自主机器人同时定位与地图构建方法
CN112862818A (zh) 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法
Huang et al. Wheel odometry aided visual-inertial odometry for land vehicle navigation in winter urban environments

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication