使用合成数据的视觉-惯性里程测量
这个例子展示了如何使用惯性测量单元(IMU)和单目摄像机来估计地面车辆的姿态(位置和方向)。在这个例子中，你:
创建一个包含车辆地面真实轨迹的驾驶场景。
使用IMU和可视化里程计模型来生成测量值。
融合这些测量来估计车辆的姿态，然后显示结果。
视觉惯性测程通过融合来自单目相机的视觉测程和来自IMU的姿态估计来估计姿态。IMU可以在较短的时间间隔内获得准确的姿态估计，但由于集成了惯性传感器的测量，会产生较大的漂移。单目相机在更大的时间间隔内返回准确的姿态估计，但受到尺度模糊的影响。考虑到这些互补的优点和缺点，使用视觉-惯性里程计融合这些传感器是一个合适的选择。这种方法可用于GPS读数不可用的场景，例如在城市峡谷中。
用轨迹创建一个驾驶场景
创建一个drivingScenario(自动驾驶工具箱)对象，该对象包含:
车辆行驶的道路
道路两旁的建筑物
车辆的真实姿态
车辆的估计姿态
地面真实姿态的车辆显示为一个坚实的蓝色长方体。估计的姿态显示为一个透明的蓝色长方体。请注意，估计的姿态不会出现在初始可视化中，因为地面真相和估计的姿态重叠。
生成地面车辆的基线轨迹waypointTrajectory系统对象™。注意waypointTrajectory用来代替drivingScenario /轨迹因为车辆需要加速。使用一组路径点、到达时间和速度，以指定的采样率生成轨迹。
用实际情况和估计数据创建驾驶场景。%车辆姿势。scene = drivingScenario;groundTruthVehicle =车辆(场景，“PlotColor”， [0 0.4470 0.7410]);estVehicle =车辆(场景，“PlotColor”， [0 0.4470 0.7410]);生成基线轨迹。sampleRate = 100;wayPoints = [0 0 0;2000 00;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 = insfilterErrorState with properties: IMUSampleRate: 100 Hz ReferenceLocation: [0 00] [deg deg m] State: [17x1 double] StateCovariance: [16x16 double] Process Noise Variances 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)²AccelerometerBiasNoise: [0.0001 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 = imsensor (“SampleRate”sampleRate,“ReferenceFrame”，“ENU表示”）;%加速度计imu.Accelerometer.MeasurementRange = 19.6;% m / s ^ 2imu.Accelerometer.Resolution = 0.0024;% m / s ^ 2 / LSBimu. accelerometer . noisendensity = 0.01;% (m / s ^ 2) /√(赫兹)%陀螺仪imu.Gyroscope.MeasurementRange = deg2rad(250);% rad /秒imu.Gyroscope.Resolution = deg2rad(0.0625);% rad / s / LSBimu.Gyroscope.NoiseDensity = deg2rad(0.0573);% (rad / s) /√(赫兹)imu.Gyroscope.ConstantBias = deg2rad(2);% rad /秒
设置模拟
指定运行模拟的时间量，并初始化在模拟循环期间记录的变量。
运行模拟60秒。numsecondstosimulation = 60;numIMUSamples = numSecondsToSimulate * sampleRate;定义视觉里程计采样率。imuSamplesPerCamera = 4;numCameraSamples = ceil(numIMUSamples / imuSamplesPerCamera);为绘制结果预分配数据数组。[pos, orient, vel, acc, angvel，.．.posVO orientVO,.．.posEst, orient, velEst.．.= helppreallocatedata (numIMUSamples, numCameraSamples);设置视觉测程融合的测量噪声参数。RposVO = 0.1;RorientVO = 0.1;
运行模拟循环
以IMU采样率运行模拟。每个IMU样本被用来预测滤波器的状态向前一个时间步。一旦有了新的可视里程表读数，就可以用它来修正当前的过滤器状态。
在滤波器估计中有一些漂移，可以用额外的传感器(如GPS)或额外的约束(如道路边界图)进一步修正。
cameraIdx = 1;为i = 1:numIMUSamples生成地面真值轨迹值。(pos(我:),东方(我:),或者(我,:),acc(我,:),angvel(我,:)]= traj ();生成加速度计和陀螺仪测量从地面真相%轨迹值。[accelMeas, gyroMeas] = imu (acc(我,:),angvel(我:),东方(我));的基础上，预测滤波状态向前一个时间步%加速度计和陀螺仪测量。predict(filt, accelMeas, gyroMeas);如果(1 == mod(i, imuSamplesPerCamera)) && useVO根据地面实况生成一个视觉测程姿势估计。%值和可视化里程表模型。[posVO(cameraIdx，:)， orientVO(cameraIdx，:)， paramsVO] =.．.helperVisualOdometryModel(pos(i，:)， orient(i，:)， 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)，.．.posEst(:，1)， posEst(:，2)， posEst(:，3)，.．.“线宽”， 3)传说(“地面实况”，“视觉里程计(VO)”，.．.“视觉惯性测程(VIO)”，“位置”，“东北”）其他的Plot3 (pos(:，1)， pos(:，2)， pos(:，3)，“-”。，.．.posEst(:，1)， posEst(:，2)， posEst(:，3)，.．.“线宽”， 3)传说(“地面实况”，“IMU姿态估计”）结束查看(- 90,90)车辆位置的)包含(“X (m)”) ylabel (“Y (m)”网格)在
图中显示，目测程估计在估计弹道形状方面是相对准确的。IMU和目视里程测量的融合消除了目视里程测量的比例因子不确定性和IMU测量的漂移。
金宝app支持功能
helperVisualOdometryModel
根据地面真实值输入和参数结构计算视觉里程测量。为了模拟单目相机后续帧间缩放的不确定性，将常数缩放因子结合随机漂移应用于真实位置。
函数[posVO, orientVO, paramsVO].．.= helperVisualOdometryModel(pos, orient, paramsVO)提取模型参数。scaleVO = paramsVO.scale;sigmaN = paramsVO.sigmaN;tau = paramsVO.tau;sigmaB = paramsVO.sigmaB;sigmaA =√((2/tau) + 1/(tau*tau))*sigmaB;b = paramsVO.driftBias;%计算漂移。B = (1 - 1/)*b + randn(1,3)*sigma;漂移= randn(1,3)*sigmaN + b;paramsVO。driftBias = b;计算视觉里程测量。posVO = scaleVO*pos +漂移;orientVO =东方;结束
helperInitialize
设置融合滤波器的初始状态和协方差值。
函数helperInitialize (filt traj)对象中检索初始位置、方向和速度%轨迹对象，并重置内部状态。[pos, orient, vel] = traj();重置(traj);设置初始状态值。filt.State(1:4) = compact(orient(1)).';filt.State(5:7) = pos(1，:).';filt.State(8:10) = vel(1，:).';设置陀螺仪偏差和视觉里程计比例因子协方差为%大值对应低置信度。filt.StateCovariance(10:12,10:12) = 1e6;filt.StateCovariance(end) = 2e2;结束
helperPreallocateData
预分配数据以记录模拟结果。
函数[pos, orient, vel, acc, angvel，.．.posVO orientVO,.．.posEst, orient, velEst.．.= helppreallocatedata (numIMUSamples, numCameraSamples)指定基本真值。pos = 0 (numIMUSamples, 3);东方=四元数。0 (numIMUSamples, 1);vel = 0 (numimussamples, 3);acc = 0 (numIMUSamples, 3);angvel = 0 (numIMUSamples, 3);%视觉里程表输出。posVO = 0 (numCameraSamples, 3);orientVO =四元数。0 (numCameraSamples, 1);过滤输出。posEst = 0 (numimussamples, 3);orientEst =四元数。0 (numIMUSamples, 1);velEst = 0 (numimussamples, 3);结束
helperUpdatePose
更新车辆的姿态。
函数helpupdatepose (veh, pos, vel, orient) veh。位置= pos;阿明费。速度= vel;Rpy =欧拉(东方，“ZYX股票”，“帧”）;阿明费。偏航= rpy(1);阿明费。Pitch = rpy(2);阿明费。Roll = rpy(3);结束
参考文献
苍井空,J。误差状态卡尔曼滤波器的四元数运动学ArXiv电子打印，ArXiv:1711.02508v1 [cs。RO] 3 2017年11月。
蒋若，R.， R. Klette和S. Wang。视觉测程中无界远距离漂移的建模2010第四届环太平洋图像与视频技术研讨会。2010年11月，第121-126页。
您也可以从以下列表中选择一个网站: