基于Matlab使用惯性测量单元和单眼摄像头估计地面车辆姿态（附源码）

此示例说明如何使用惯性测量单元 （IMU） 和单眼摄像头估计地面车辆的姿态（位置和方向）。在此示例中：

1. 创建包含车辆真实轨迹的驾驶场景。

2. 使用 IMU 和视觉里程计模型生成测量值。

3. 融合这些测量值以估计车辆的姿态，然后显示结果。

视觉惯性测程法通过将单目相机的视觉测程姿态估计值和IMU的姿态估计值融合在一起来估计姿态。IMU 在较小的时间间隔内返回准确的姿态估计值，但由于集成了惯性传感器测量值，因此会出现较大的漂移。单目摄像头在较大的时间间隔内返回准确的姿势估计值，但存在比例模糊性。鉴于这些互补的优点和缺点，使用视觉惯性里程计融合这些传感器是一个合适的选择。此方法可用于 GPS 读数不可用的场景，例如在城市峡谷中。

一、创建具有轨迹的驾驶场景

创建一个包含以下内容的驾驶场景（自动驾驶工具箱）对象：

• 车辆行驶的道路

• 道路两侧周围的建筑物

• 车辆的地面实况姿势

• 车辆的估计姿态

车辆的地面实况姿势显示为纯蓝色长方体。估计的姿势显示为透明的蓝色长方体。请注意，估计姿势不会出现在初始可视化中，因为地面实况和估计姿势重叠。使用航点轨迹系统对象生成地面车辆的基线轨迹。请注意，这是用来代替的因为需要车辆的加速。轨迹是使用一组航点、到达时间和速度以指定的采样率生成的。

 二、创建融合滤镜

创建滤波器以融合 IMU 和视觉里程计测量。此示例使用松散耦合方法来融合测量值。虽然结果不如紧密耦合方法准确，但所需的处理量明显较少，结果也足够。融合滤波器使用误差状态卡尔曼滤波器来跟踪方向（作为四元数）、位置、速度和传感器偏差。

该predictpredict函数将来自IMU的加速度计和陀螺仪测量值作为输入。每次对加速度计和陀螺仪进行采样时调用该函数。此函数根据加速度计和陀螺仪测量值预测一个时间步长前进的状态，并更新滤波器的误差状态协方差。

该fusemvopredict函数将视觉测程法姿势估计值作为输入。该函数通过计算卡尔曼增益来更新基于视觉测程姿态估计的误差状态，该增益根据其不确定性对各种输入进行加权。与函数一样，此函数还更新误差状态协方差，这次考虑了卡尔曼增益。然后使用新的错误状态更新状态，并重置错误状态。

三、指定视觉里程计模型

定义视觉测程模型参数。这些参数使用单眼相机对基于特征匹配和跟踪的视觉里程计系统进行建模。该参数解释了单目相机后续视觉帧的未知比例。其他参数将视觉里程计读数中的漂移建模为白噪声和一阶高斯-马尔可夫过程的组合。

四、指定 IMU 传感器

使用 System 对象定义包含加速度计和陀螺仪的 IMU 传感器模型。传感器模型包含用于对确定性和随机噪声源进行建模的属性。此处设置的属性值是低成本MEMS传感器的典型值。

五、结果

5.1 设置模拟

指定运行模拟和初始化在模拟循环期间记录的变量的时间量。

5.2 运行模拟循环

以 IMU 采样率运行仿真。每个 IMU 样本用于预测滤波器的状态向前预测一个时间步长。一旦新的视觉里程计读数可用，它就会用于校正当前的过滤器状态。 滤波器估计中存在一些漂移，可以使用附加传感器（如 GPS）或附加约束（如道路边界图）进一步校正。

 5.3 绘制结果

绘制真实车辆轨迹、视觉里程计估计值和融合滤波器估计值

该图表明，视觉测程估计在估计轨迹的形状方面相对准确。IMU 和视觉里程计测量的融合消除了视觉里程计测量中的比例因子不确定性和 IMU 测量的漂移。

六、程序

使用Matlab R2022b版本，点击打开。

 打开下面的“VisualInertialOdometryUsingSyntheticDataExample.m”文件，点击运行，就可以看到上述效果。

关注下面公众号，后台回复关键词：惯性测量单元和单眼摄像头估计地面车辆姿态，发送源码链接。