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

打开脚本

这个例子展示了如何使用惯性测量单元(IMU)和单目摄像机估计地面车辆的姿态(位置和方向)。在这个例子中，你:

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

视觉惯性测程法通过融合来自单目摄像机的视觉测程位姿估计和来自IMU的位姿估计来估计位姿。IMU在很小的时间间隔内返回一个精确的姿态估计，但由于集成惯性传感器测量而遭受较大的漂移。单目相机在更长的时间间隔内返回一个准确的姿态估计，但受到尺度模糊的影响。考虑到这些互补的优点和缺点，这些传感器的融合使用视觉-惯性里程计是一个合适的选择。这种方法可用于GPS无法读取的情况下，例如城市峡谷。

用轨迹创建一个驾驶场景

创建一个drivingScenario(自动驾驶工具箱)对象,其中包含:

车辆行驶的道路
路两边的建筑物
车辆的地面真实姿态
车辆的估计姿态

车辆的地面真实姿势显示为一个实心蓝色长方体。估计的姿态显示为一个透明的蓝色长方体。注意，估计的姿势不会出现在初始的可视化中，因为地面真实和估计的姿势重叠。

为地面车辆生成基线轨迹使用waypointTrajectory系统对象™。请注意,waypointTrajectory是用来代替drivingScenario /轨迹因为需要车辆的加速度。轨迹是使用一组航路点、到达时间和速度以指定的采样率生成的。

%创建驾驶场景与地面真相和估计%车辆构成。现场= drivingScenario;groundTruthVehicle =车辆(场景,“PlotColor”， [0 0.4470 0.7410]);estVehicle =车辆(场景,“PlotColor”， [0 0.4470 0.7410]);%生成基线轨迹。sampleRate = 100;wayPoints = [0 0 0;200 0 0;200年50 0;200 230 0;215 245 0;260 245 0;290 240 0;310 258 0;290 275 0; 260 260 0; -20 260 0]; t = [0 20 25 44 46 50 54 56 59 63 90].'; speed = 10; velocities = [ speed 0 0; speed 0 0; 0 speed 0; 0 speed 0; speed 0 0; speed 0 0; speed 0 0; 0 speed 0; -speed 0 0; -speed 0 0; -speed 0 0]; traj = waypointTrajectory(wayPoints,“TimeOfArrival”t．．.“速度”,速度,“SampleRate”, sampleRate);添加一条路和建筑到场景和可视化。helperPopulateScene(场景,groundTruthVehicle);

创建一个融合过滤器

创建过滤器融合IMU和视觉里程测量。本例使用松散耦合的方法来融合度量值。虽然结果不如紧密耦合方法准确，但所需的处理量显著减少，而且结果是足够的。融合滤波器使用误差状态卡尔曼滤波器跟踪方向(作为四元数)、位置、速度和传感器偏差。

的insfilterErrorState对象具有以下功能来处理传感器数据:预测和fusemvo．

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

的fusemvo函数以视觉里程计姿态估计值作为输入。这个函数根据视觉里程计的姿态估计，通过计算一个卡尔曼增益来更新误差状态，该增益根据不同输入的不确定性来衡量。与预测函数，该函数也更新误差状态协方差，这一次考虑了卡尔曼增益。然后使用新的错误状态更新状态，并重置错误状态。

filt = insfilterErrorState (“IMUSampleRate”sampleRate,．．.“ReferenceFrame”，“ENU表示”）设置初始状态和错误状态的协方差。helperInitialize (filt traj);

filt = infiltererrorstate with properties: IMUSampleRate: 100hz ReferenceLocation: [0 00] [deg deg m] State: [17x1 double] StateCovariance: [16x16 double] Process Noise variance GyroscopeNoise: [1e-06 1e-06 1e-06] (rad/s)²AccelerometerNoise: [0.0001 0.0001 0.0001] (m/s²)²GyroscopeBiasNoise:[1e-09 1e-09 1e-09] (rad/s)²加速度偏差:[0.0001 0.0001 0.0001](m/s²)²

指定可视里程计模型

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

%标志useVO决定是否使用视觉里程计:% useVO = false;%只使用IMU。useVO = true;同时使用IMU和视觉测程法。paramsVO。规模= 2; paramsVO.sigmaN = 0.139; paramsVO.tau = 232; paramsVO.sigmaB = sqrt(1.34); paramsVO.driftBias = [0 0 0];

指定IMU传感器

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

%将RNG种子设置为默认，以便后续获得相同的结果%。rng (“默认”) imu = imussensor (“SampleRate”sampleRate,“ReferenceFrame”，“ENU表示”）;%加速度计imu.Accelerometer.MeasurementRange = 19.6;% m / s ^ 2imu.Accelerometer.Resolution = 0.0024;% m / s ^ 2 / LSBimu.Accelerometer.NoiseDensity = 0.01;% (m / s ^ 2) /√(赫兹)%陀螺仪imu.Gyroscope.MeasurementRange =函数(250);% rad /秒imu.Gyroscope.Resolution函数= (0.0625);% rad / s / LSBimu.Gyroscope.NoiseDensity函数= (0.0573);% (rad / s) /√(赫兹)imu.Gyroscope.ConstantBias =函数(2);% rad /秒

设置模拟

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

%运行模拟60秒。numSecondsToSimulate = 60;numimussamples = numSecondsToSimulate * sampleRate;定义目视里程计采样率。imuSamplesPerCamera = 4;numcamerassamples = cell (numimussamples / imuSamplesPerCamera);%预分配数据数组以绘制结果。方位，方向，方向，角度，．．.posVO orientVO,．．.姿势,东方,或者]．．.= helperPreallocateData (numIMUSamples numCameraSamples);%为视觉里程计融合设置测量噪声参数。RposVO = 0.1;RorientVO = 0.1;

运行模拟循环

以IMU采样率运行仿真。每个IMU样本被用来预测一个时间步前滤波器的状态。一旦一个新的视觉里程计读数可用，它被用来纠正当前的过滤器状态。

滤波器估计中存在一些偏差，可以通过GPS等附加传感器或道路边界图等附加约束进一步修正。

cameraIdx = 1;为我= 1:numIMUSamples%生成地面真实轨迹值。(pos(我:),东方(我:),或者(我,:),acc(我,:),angvel(我,:)]= traj ();生成加速度计和陀螺仪测量从地面真实%值轨迹。[accelMeas, gyroMeas] = imu (acc(我,:),angvel(我:),东方(我));预测过滤器状态转发一个时间步加速度计和陀螺仪测量。预测(filt accelMeas gyroMeas);如果(1 == mod(i, imuSamplesPerCamera)) && useVO从地面真相生成一个视觉里程计姿势估计%值和可视化里程计模型。[posVO(cameraIdx，:)， orientVO(cameraIdx，:)， paramsVO] =．．.helperVisualOdometryModel (pos(我:),东方(我,:),paramsVO);%根据视觉测程数据正确的过滤状态。fusemvo (filt posVO (cameraIdx:), RposVO,．．.orientVO (cameraIdx) RorientVO);cameraIdx = cameraIdx + 1;结束(pos(我:),东方(我:),或者(我,:)]=姿势(filt);更新估计的车辆姿态。helperUpdatePose(estVehicle, posEst(i，:)， velEst(i，:)， orientEst(i));更新地面真实车辆姿态。helperUpdatePose(groundTruthVehicle, pos(i，:)， vel(i，:)， orient(i));%更新驾驶场景可视化。updatePlots(现场);drawnowlimitrate;结束

阴谋的结果

绘制地面真实车辆轨迹，视觉测程估计，融合滤波估计。

数字如果useVO plot3(pos(:，1)， pos(:，2)， pos(:，3)，“-”。，．．.posVO (: 1), posVO (:, 2), posVO (:, 3),．．.pos (: 1), pos (:, 2), pos (:, 3),．．.“线宽”3)传说(“地面实况”，“视觉里程计(VO)”，．．.“Visual-Inertial测程法(VIO) '，“位置”，“东北”）其他的plot3 (pos (: 1), pos (:, 2), pos (:, 3),“-”。，．．.pos (: 1), pos (:, 2), pos (:, 3),．．.“线宽”3)传说(“地面实况”，IMU姿态估计的）结束视图(-90、90)标题(车辆位置的)包含(“X (m)”) ylabel (“Y (m)”网格)在

从图中可以看出，目视测程法估计弹道的形状是比较准确的。IMU和视觉里程计测量的融合消除了视觉里程计测量的尺度不确定性和IMU测量的漂移。

金宝app支持功能

helperVisualOdometryModel

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

函数[posVO, orientVO paramsVO]．．.= helpervisualdometrymodel (pos, orient, paramsVO)%提取模型参数。scaleVO = paramsVO.scale;西格曼= paramsVO.sigmaN;τ= paramsVO.tau;sigmaB = paramsVO.sigmaB;sigmaA =√((2/tau) + 1/(tau*tau))*sigmaB;b = paramsVO.driftBias;%计算漂移。B = (1 - 1/)* b + randn(1、3)* sigmaA;(1) = rman (1, 2);paramsVO。driftBias = b;计算目视里程测量值。posVO = scaleVO*pos +漂移;orientVO =东方;结束

helperInitialize

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

函数helperInitialize (filt traj)检索初始位置，方向和速度从%轨迹对象并重置内部状态。[pos, orient, vel] = traj();重置(traj);%设置初始状态值。filt.State(1:4) =紧凑(东方(1))。”;filt.State (7) = pos(1:)。”;filt.State(8) =韦尔(1:)。”;%设置陀螺仪偏差和目视里程计比例因子协方差为%大值对应低置信度。filt.StateCovariance(十12,十12)= 1 e6;filt.StateCovariance(结束)= 2 e2;结束

helperPreallocateData

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

函数方位，方向，方向，角度，．．.posVO orientVO,．．.姿势,东方,或者]．．.= helperPreallocateData (numIMUSamples numCameraSamples)%指定地面真理。pos = 0 (numimussamples, 3);东方=四元数。0 (numIMUSamples, 1);vel = 0 (numimussamples, 3);acc = 0 (numimussamples, 3);角度= 0 (numimussamples, 3);%目视里程输出。posVO = 0 (numcamerasample, 3);orientVO =四元数。0 (numCameraSamples, 1);%滤波器输出。posEst = 0 (numimussamples, 3);东方=四元数。0 (numIMUSamples, 1);velEst = 0 (numimussamples, 3);结束

helperUpdatePose

更新车辆的姿态。

函数helperUpdatePose(veh, pos, vel, orient)位置= pos;阿明费。速度=韦尔;rpy = eulerd(东方,“ZYX股票”，“帧”）;阿明费。偏航= rpy (1);阿明费。距= rpy (2);阿明费。滚= rpy (3);结束

参考文献

苍井空,J。误差状态卡尔曼滤波器的四元数运动学ArXiv e-prints, ArXiv:1711.02508v1 [c .]2017年11月3日。
蒋瑞玲，R. Klette，和S. Wang。“视觉测程中无边界远程漂移的建模”。2010年第四届环太平洋地区图像和视频技术研讨会。2010年11月，页121-126。