利用合成数据的视觉惯性里程计

开放脚本

本例显示了如何使用惯性测量单元（IMU）和单目摄像机估计地面车辆的姿势（位置和方向）。在本例中，您：

创建包含车辆地面真实轨迹的驾驶场景。
使用IMU和视觉里程计模型生成测量值。
融合这些测量值以估计车辆的姿势，然后显示结果。

通过熔断来自单眼相机和IMU的姿势估计，通过融合视觉径管姿势估计姿势估计姿势。IMU返回一个精确的姿势估计，对于小的时间间隔，而是由于集成惯性传感器测量而导致的大漂移。单眼摄像机通过较大的时间间隔返回准确的姿势估计，但是遭受比例歧义。鉴于这些互补强度和缺点，这些传感器使用视觉惯性内径测量的融合是合适的选择。该方法可以用于GPS读数不可用的场景中，例如在城市峡谷中。

使用轨迹创建一个驾驶场景

创建一个驱动器Cenario.（自动驾驶工具箱）对象，该对象包含：

车辆行驶的道路
道路两旁的建筑物
车辆的地面真理姿势
车辆的估计姿势

车辆的地面真理姿势显示为纯蓝色长方体。估计的姿势被显示为透明蓝色长方体。请注意，估计的姿势不会出现在初始可视化中，因为地面真相和估计姿势重叠。

使用航路点轨迹系统对象™. 请注意航路点轨迹用于代替驱动器Cenario /轨迹由于需要车辆的加速度。轨迹以指定的采样率使用一组航点，到达时间和速度以指定的采样率生成。

%用地面真实情况和估计值创建驾驶场景%车辆姿势。场景=驾驶场景；地面车辆=车辆（场景，'plotcolor', [0 0.4470 0.7410]); estVehicle=车辆（场景，'plotcolor'，[0 0.4470 0.7410]）;%生成基线轨迹。采样器=100；航路点=[0 0 0；200 0 0；200 50 0；200 230 0；215 245 0；260 245 0；290 240 0；310 2580；290 2750；260 260 0；-20 260 0]；t=[0 20 25 44 46 50 54 56 59 63 90]；速度=10；速度=[0速度；0速度；0速度；0速度；0速度；0速度；0速度；0速度；0速度0；0速度；0速度；0速度；0速度；0速度；0速度；0速度0；traj=航路点轨迹（航路点，“到达时间”，t，...“速度”，速度，'采样率'，取样器）；%将道路和建筑物添加到场景并可视化。helperPopulateScene（现场、地面车辆）；

创建融合过滤器

创建过滤器以保险丝IMU和视觉测量测量。此示例使用松散耦合的方法来熔断测量值。虽然结果与紧密耦合的方法不如精确，但所需的加工量明显较低，结果是足够的。融合过滤器使用错误状态卡尔曼滤波器来跟踪方向（作为四元数），位置，速度和传感器偏差。

这个InFilterErrorState对象具有以下功能来处理传感器数据：预测和Fusemvo..

这个预测功能采用IMU作为输入的加速度计和陀螺仪测量。打电话给预测函数每次对加速度计和陀螺仪进行采样。此函数根据加速度计和陀螺仪的测量值预测一个时间步长的状态，并更新滤波器的误差状态协方差。

这个Fusemvo.功能将视觉径管姿势估计为输入。该函数通过计算根据其不确定性来计算称重各种输入的卡尔曼增益来更新错误状态姿势估计。和我们一样预测函数，此功能还更新错误状态协方差，这次将卡尔曼介绍考虑。然后使用新的错误状态更新状态，并且重置错误状态。

filt=INSFILTERRORSTATE('imusamplerate'， 采样率，...“ReferenceFrame”,‘ENU’)%设置初始状态和错误状态协方差。Helperinitialize（Filt，Traj）;

Filt = InsfilterErrorState具有属性：imusamplege：100 Hz参考单元（rad / s）²加速度：[0.0001 0.0001 0.0001]（m /s²）²陀螺仪：[1e-09 1e-09 1e-09]（rad / s）²加速度计百差：[0.0001 0.0001 0.0001]（m /s²）²

指定视觉里程计模型

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

%标志useVO确定是否使用视觉里程计：％umervo = false;使用％仅使用IMU。useVO=真；％IMU和视觉测量术都使用。paramsVO.scale=2；paramsVO.sigmaN=0.139；paramsVO.tau=232；paramsVO.sigmaB=sqrt（1.34）；paramsVO.floftbias=[0]；

指定IMU传感器

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

%将RNG种子设置为默认值，以获得与后续操作相同的结果%跑步。rng(“默认”)imu=imu传感器('采样率'， 采样率，“ReferenceFrame”,‘ENU’);％加速度计imu.Accelerator.MeasurementTrange=19.6；％m / s ^ 2imu.Accelerator.Resolution=0.0024；%m/s^2/LSBimu.Accelerator.NoiseDensity=0.01；%（米/秒^2）/平方米（赫兹）%陀螺仪imu.陀螺仪.测量仪器=deg2rad（250）；%拉德/秒imu陀螺仪分辨率=deg2rad（0.0625）；％rad / s / lsbimu.陀螺仪.噪声度=deg2rad（0.0573）；%（rad/s）/sqrt（Hz）imu.陀螺仪.ConstantBias=deg2rad（2）；%拉德/秒

设置模拟

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

%运行模拟60秒。numsecondstosimulate = 60;numimusamples = numsecondstosimulate * sampleate;%定义视觉里程计采样率。imuSamplesPerCamera=4；numCameraSamples=ceil（numIMUSamples/imuSamplesPerCamera）；％preal分配数据阵列进行绘图结果。[位置、方向、标高、acc、angvel、，...Posvo，Orientvo，...POSEST，Siosest，Velest]...=helperPreallocateData（numIMUSamples、numCameraSamples）；％设定视觉管道融合的测量噪声参数。RposVO=0.1；RorientVO=0.1；

运行模拟循环

以IMU采样率运行模拟。每个IMU样本用于预测过滤器的状态，向前一个时间步。一旦有新的视觉里程计读数可用，则用于校正当前过滤器状态。

滤波器估计值中存在一些漂移，可以使用附加传感器（如GPS）或附加约束（如道路边界地图）进一步校正。

Cameraidx = 1;为了我= 1：numimusamples%生成地面真实轨迹值。[pos（我，:)，东方（我，:)，vel（i，:)，acc（我，:)，agvel（i，i :)] = traj（）;%从地面生成加速计和陀螺仪测量值%轨迹值。[Accelmeas，Gyromeas] = IMU（ACC（I，:)，Angvel（I，:)，东方（I））;%根据预测值向前预测一个时间步的过滤器状态％加速度计和陀螺仪测量。预测（滤波器、加速度计、陀螺仪）；如果（1==mod（i，imuSamplesPerCamera））&&useVO%根据地面真实情况生成视觉里程计姿势估计%数值和视觉里程计模型。[posVO（cameraIdx，：），orientVO（cameraIdx，：），paramsVO]=...helpervisualodel（pos（i，：）、orient（i，：）、paramsVO；%根据视觉里程计数据纠正过滤器状态。fusemvo（filt，posVO（cameraIdx，：），RposVO，...Orientvo（Cameraidx），Roriendvo）;Cameraidx = Cameraidx + 1;结尾[POSEST（我，:)，Sizeest（我，:)，Velest（I，:)] =姿势（菲尔特）;%更新估计的车辆姿势。帮助者更新姿势（estVehicle，posEst（i，：），velEst（i，：），定向测试（i））；%更新地面真实车辆姿势。助手更新姿态（地面车辆，位置（i，：），水平（i，：），方向（i））；%更新驾驶场景可视化。更新平面（场景）;drawn限制;结尾

绘制结果

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

数字如果使用图3（位置（：，1），位置（：，2），位置（：，3），' - 。',...posVO（：，1），posVO（：，2），posVO（：，3），...posEst（：，1），posEst（：，2），posEst（：，3），...'行宽'（3）图例(“基本事实”,“视觉里程计（VO）”,...'视觉惯性内径术（VIO）',“位置”,“东北”)别的Plot3（POS（：，1），POS（:,2），POS（：3），' - 。',...posEst（：，1），posEst（：，2），posEst（：，3），...'行宽'（3）图例(“基本事实”,'imu pose estimes')结尾视图（-90，90）标题(“车辆位置”)xlabel(‘X（m）’)伊拉贝尔(‘Y（m）’)网格在…上

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

万博1manbetx辅助功能

HelpervisualodometryModel.

根据地面真值输入和结构参数计算视觉里程测量。为了模拟单目相机后续帧之间缩放的不确定性，对地面真值位置应用了一个结合随机漂移的恒定缩放因子。

功能[posVO、orientVO、paramsVO]...=HelperVisuaLodMethodModel（位置、方向、参数）%提取模型参数。scaleVO=paramsVO.scale；sigmaN=paramsVO.sigmaN；tau=paramsVO.tau；sigmaB=paramsVO.sigmaB；sigmaA=sqrt（（2/tau）+1/（tau*tau））*sigmaB；b=paramsVO.floftbias；%计算漂移。b=（1-1/tau）。*b+randn（1,3）*sigmaA；drift=randn（1,3）*sigmaN+b；paramsVO.driftBias=b；％计算视觉测量测量。posVO=缩放*位置+漂移；方向VO=方向；结尾

Helperinitialize.

设置融合过滤器的初始状态和协方差值。

功能帮助初始化（过滤器，traj）%从中检索初始位置、方向和速度%轨迹对象并重置内部状态。[POS，东方，vel] = traj（）;重置（traj）;%设置初始状态值。过滤状态（1:4）=紧凑（方向（1））。；过滤状态（5:7）=位置（1，：）。；过滤状态（8:10）=水平（1，：）；%将陀螺仪偏差和视觉里程计比例因子协方差设置为与低置信度相对应的大值。过滤状态协方差（10:12,10:12）=1e6；过滤状态协方差（end）=2e2；结尾

Helperpreallocateata

预先分配数据以记录模拟结果。

功能[位置、方向、标高、acc、angvel、，...Posvo，Orientvo，...POSEST，Siosest，Velest]...=helperPreallocateData（numIMUSamples、numCameraSamples）%具体说明基本事实。pos=零（numIMUSamples，3）；orient=四元数。零（numIMUSamples，1）；vel=零（numIMUSamples，3）；acc=零（numIMUSamples，3）；angvel=零（numIMUSamples，3）；%视觉里程计输出。posVO=零（numCamerasSamples，3）；orientVO=四元数。零（numCamerasSamples，1）；%过滤输出。posEst=零（numIMUSamples，3）；orientEst=四元数。零（numIMUSamples，1）；velEst=零（numIMUSamples，3）；结尾

助手更新姿势

更新车辆的姿势。

功能helperUpdatePose（车辆，位置，水平，方向）车辆位置=位置；车辆速度=水平；rpy=eulerd（方向，'Zyx','框架')车辆偏航=rpy（1）；车辆俯仰=rpy（2）；车辆横滚=rpy（3）；结尾

工具书类

误差状态卡尔曼滤波器的四元数运动学〉，ArXiv e-prints，ArXiv:1711.02508v1[cs.RO]2017年11月3日。
江先生、R先生、克莱特先生和王先生。“视觉里程计中无限长程漂移的建模”，2010年第四届环太平洋图像和视频技术研讨会。2010年11月，第121-126页。