视觉惯性测程使用合成数据

打开脚本

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

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

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

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

视觉惯性测距估计姿态通过融合从单眼相机的视觉里程姿态估计，并从IMU姿态估计。所述IMU返回小的时间间隔的精确姿态的估计，但患有大漂移由于整合惯性传感器测量。单眼相机返回准确的姿态估计在更大的时间间隔，但是从规模歧义缺点。鉴于这些互补的长处和短处，使用视觉惯性测距这些传感器的融合是一个合适的选择。这种方法可以在场景中GPS读数不可用，诸如在城市峡谷中使用。

创建驾驶情形与轨迹

创建一个drivingScenario对象，它包含：

  • 在车辆行驶的道路上

  • 道路两旁的建筑物

  • 车辆的地面实况姿势

  • 估计的车辆姿态

车辆的地面实况姿势显示为蓝色实心长方体。估计姿态被示出为透明的蓝色长方体。请注意，估计姿态不会出现在最初的可视化，因为地面真理，估计姿势重叠。

生成使用地面车辆的基准轨迹waypointTrajectory系统对象™。注意，waypointTrajectory代替使用drivingScenario /轨迹由于所需要的车辆的加速度。该轨迹是在使用一组路点，到达时间，和速度的规定的采样速率产生。

％既创建驾驶情形的地面实况和估计%车辆构成。现场= drivingScenario;groundTruthVehicle =车辆(场景,'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 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,'到达时间'，T，...“速度对”，速度，'采样率',sampleRate);在场景中添加道路和建筑物。helperPopulateScene(场景,groundTruthVehicle);

创建一个融合滤波器

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

该insfilterErrorState对象具有以下功能，以处理传感器数据：预测和fusemvo。

该预测函数从IMU作为输入取加速度计和陀螺仪测量。调用预测每个时间函数的加速度计和陀螺仪进行采样。此功能预测基于加速度计和陀螺仪的测量通过一次一步的状态，并更新过滤器的错误状态协方差。

该fusemvo函数将视觉里程姿态估计作为输入。该功能通过计算卡尔曼增益，根据他们的不确定性是衡量各种输入更新基于视觉里程计的姿态估计错误状态。如同预测功能，该功能也将更新错误状态协方差，此时服用卡尔曼增益考虑在内。那么国家将使用新的错误状态更新和错误状态复位。

FILT = insfilterErrorState（'IMUSampleRate'， 采样率，...'参考范围'，“ENU表示”）设置初始状态和错误状态协方差。helperInitialize（FILT，TRAJ）;
FILT = insfilterErrorState与属性：IMUSampleRate：100赫兹ReferenceLocation：[0 0 0] [度度米]州：[17x1双] StateCovariance：[16×16双]过程噪声方差GyroscopeNoise：1E-06 1E-06 1E-06]（弧度/秒）²AccelerometerNoise：[0.0001 0.0001 0.0001]（米/秒²）²GyroscopeBiasNoise：1E-09 1E-09 1E-09]（弧度/秒）²AccelerometerBiasNoise：[0.0001 0.0001 0.0001]（米/秒²）²

指定可视里程测量模型

定义视觉里程模型参数。这些参数使用单眼相机模型基于跟踪特征匹配和视觉里程计系统。该规模参数占单眼相机的后续视觉帧的未知规模。其它参数建模在视觉测距读数为白噪声和一阶高斯 - 马尔可夫过程的组合的漂移。

％标志useVO确定如果使用视觉里程计：％useVO = FALSE;％只IMU使用。useVO = true;％两种IMU和视觉里程计被使用。paramsVO.scale = 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 = imuSensor('采样率'， 采样率，'参考范围'，“ENU表示”);加速度计％imu.Accelerometer。MeasurementRange = 19.6;％米/秒^ 2imu.Accelerometer。分辨率= 0.0024;％米/秒^ 2 / LSBimu.Accelerometer.NoiseDensity = 0.01;% (m / s ^ 2) /√(赫兹)%陀螺仪imu.Gyroscope.MeasurementRange = deg2rad（250）;% rad /秒imu.Gyroscope。解析函数= (0.0625);％弧度/秒/ LSBimu.Gyroscope。NoiseDensity函数= (0.0573);% (rad / s) /√(赫兹)imu.Gyroscope.ConstantBias = deg2rad（2）;% rad /秒

设置仿真

指定的时间运行模拟和初始化是在模拟循环过程中记录变量的数量。

%运行模拟60秒。numSecondsToSimulate = 60;numIMUSamples = numSecondsToSimulate *采样率;％定义视觉里程计的采样率。imuSamplesPerCamera = 4;numCameraSamples =小区（numIMUSamples / imuSamplesPerCamera）;为绘制结果％预分配数据数组。[pos, orient, vel, acc, angvel，...posVO，orientVO，...posEst，orientEst，velEst]...= helperPreallocateData (numIMUSamples numCameraSamples);％设定测量噪声参数为视觉测距融合。RposVO = 0.1;RorientVO = 0.1;

运行仿真循环

运行在IMU采样率的模拟。每个样品IMU是用来通过一个时间步前向预测滤波器的状态。一旦有新的视觉里程读数是可用的，它是用来纠正当前过滤器的状态。

滤波器估计值中存在一些漂移，可以通过附加的传感器(如GPS)或附加的约束(如道路边界地图)来进一步修正。

cameraIdx = 1;对于I = 1：numIMUSamples%生成地面真值轨迹值。[POS（I，:)，东方（I，:)，VEL（I，:)，ACC（I，:)，angvel（I，:)] = TRAJ（）;产生加速度计和陀螺仪测量从地面真理%值轨迹。[accelMeas，gyroMeas] = IMU（ACC（I，:)，angvel（I，:)，东方（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（I，:)，orientEst（I，:)，velEst（I，:)] =姿态（FILT）;更新估计的车辆姿态。helperUpdatePose（estVehicle，posEst（I，:)，velEst（I，:)，orientEst（I））;％更新地面实况车辆姿态。helperUpdatePose（groundTruthVehicle，POS（i，:)，VEL（I，:)，东方（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）'，...“视觉惯性测程（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]...= helperVisualOdometryModel(pos, orient, paramsVO)％提取模型参数。scaleVO = paramsVO.scale;西格曼= paramsVO.sigmaN;tau蛋白= paramsVO.tau;sigmaB = paramsVO.sigmaB;sigmaA = SQRT（（2 / TAU）+ 1 /（TAU * tau蛋白））* sigmaB;B = paramsVO.driftBias;%计算漂移。b = (1 - 1/)* b + randn (1、3) * sigmaA;漂移= randn(1,3)*sigmaN + b;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（结束）= 2E2;结束

helperPreallocateData

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

函数[pos, orient, vel, acc, angvel，...posVO，orientVO，...posEst，orientEst，velEst]...= helperPreallocateData (numIMUSamples numCameraSamples)指定地面真相。POS =零（numIMUSamples，3）;定向= quaternion.zeros（numIMUSamples，1）;VEL =零（numIMUSamples，3）;ACC =零（numIMUSamples，3）;angvel =零（numIMUSamples，3）;%视觉里程测量输出。posVO =零（numCameraSamples，3）;orientVO = quaternion.zeros（numCameraSamples，1）;％滤波器的输出。posEst = 0 (numIMUSamples, 3);东方=四元数。0 (numIMUSamples, 1);velEst = 0 (numIMUSamples, 3);结束

helperUpdatePose

更新车辆的姿态。

函数帮助更新(veh, pos, vel, orient)位置= pos;阿明费。速度=韦尔;rpy = eulerd(东方,'ZYX'，'帧');阿明费。偏航= rpy (1);阿明费。距= rpy (2);阿明费。滚= rpy (3);结束

参考文献

  • 索拉，J。“四元数运动学的错误状态卡尔曼滤波器。”的arXiv电子印花图案，的arXiv：1711.02508v1 [cs.RO] 2017年11月3日。

  • R. Jiang, R.， R. Klette, S. Wang。无界长程漂移的视觉测程建模。2010 Fourth Pacific-Rim Symposium on Image and Video Technology. Nov. 2010, pp. 121-126.

导航工具的文档
金宝app

尝试MATLAB，Sim金宝appulink和其他产品下载188bet金宝搏

现在就试用