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

打开脚本

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

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

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

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

创建一个驱动器Cenario.对象,其中包含:

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

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

为地面车辆生成基线轨迹使用waypointTrajectory(传感器融合与跟踪工具箱)系统对象™。请注意,waypointTrajectory用于代替驱动器Cenario /轨迹由于需要车辆的加速度。使用一组航点，到达和速度，以指定的采样率生成轨迹。

%创建驾驶场景与地面真相和估计%车辆构成。现场= 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);添加一条路和建筑到场景和可视化。helperPopulateScene(场景,groundTruthVehicle);

创建融合过滤器

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

的insfilterErrorState对象具有以下函数来处理传感器数据：预测和Fusemvo.．

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

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

filt = insfilterErrorState ('imusamplever'， 采样率，．..“ReferenceFrame”,“ENU表示”）设置初始状态和错误状态的协方差。HelperInitialize（Filt，Traj）;

Filt = InsfilterErrorstate具有属性：imusamplege：100 Hz参考单元：[0 0]【DEG DEG M]状态：[17X1 DOUBLE] STATECOVARIANICE：[16X16双]过程噪声差异陀螺仪：[1E-06 1E-06 1E-06]（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 = 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 ('采样率'， 采样率，“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;numimusamples = numsecondstosimulate * sampleate;定义目视里程计采样率。imuSamplesPerCamera = 4;numcamerassamples = cell (numimussamples / imuSamplesPerCamera);％预分配数据阵列以绘制结果。方位，方向，方向，角度，．..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（我，:)，Angvel（I，：）] = traj（）;生成加速度计和陀螺仪测量从地面真实%值轨迹。[Accelmeas，Gyromeas] = IMU（ACC（我，:)，愤怒（我，:)，东方（I））;预测过滤器状态转发一个时间步％加速度计和陀螺仪测量。预测(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;结尾[POSEST（我，:)，安详（我，:)，Velest（I，:)] =姿势（菲尔特）;更新估计的车辆姿态。helperUpdatePose(estVehicle, posEst(i，:)， velEst(i，:)， orientEst(i));更新地面真实车辆姿态。helperUpdatePose(groundTruthVehicle, pos(i，:)， vel(i，:)， orient(i));%更新驾驶场景可视化。更新平面（场景）;drawn限制；结尾

绘制结果

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

数字如果useVO plot3(pos(:，1)， pos(:，2)， pos(:，3)，' - 。',．..posVO (: 1), posVO (:, 2), posVO (:, 3),．..pos (: 1), pos (:, 2), pos (:, 3),．..'行宽'3)传说(“地面实况”,“视觉里程计(VO)”,．..'视觉惯性内径术（VIO）',“位置”,“东北”）别的Plot3（POS（：，1），POS（:,2），POS（:,3），' - 。',．..pos (: 1), pos (:, 2), pos (:, 3),．..'行宽'3)传说(“地面实况”,'imu pose estimes'）结尾视图(-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，东方，vel] = traj（）;重置（traj）;%设置初始状态值。filt.State(1:4) =紧凑(东方(1))。”;filt.State (7) = pos(1:)。”;filt.State(8) =韦尔(1:)。”;%设置陀螺仪偏差和目视里程计比例因子协方差为％大值对应于低置信度。Filt.Statecovariance（10：12,10：12）= 1E6;filt.statecovariance（END）= 2E2;结尾

HelperprealLocatedata.

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

函数方位，方向，方向，角度，．..Posvo，Orientvo，．..POSEST，Siosest，Velest]．..= 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。