Visual-Inertial测程法使用合成数据
这个例子显示了如何估计姿势(位置和姿态)的地面车辆使用的惯性测量单元(IMU)和单眼相机。在本例中,您:
创建一个驾驶场景包含地面车辆的真实轨迹。
使用一个IMU和视觉测距模型生成测量。
融合这些测量来估计车辆的姿势,然后显示结果。
Visual-inertial测程法估计体式通过融合视觉测程法估计的单眼相机和乌兹别克斯坦伊斯兰运动的姿态估计。乌兹别克斯坦伊斯兰运动返回一个准确的姿势估计小的时间间隔,但遭受大漂移由于集成惯性传感器测量。单眼相机返回一个精确的姿势估计在一个更大的时间间隔,但也存在歧义。鉴于这些互补的优势和劣势,这些传感器的融合使用visual-inertial测程法是一个合适的选择。该方法可用于场景GPS数据不可用,如在城市峡谷。
创建一个驾驶场景与轨迹
创建一个drivingScenario(自动驾驶工具箱)对象,其中包含:
路上的汽车旅行
建筑周边道路的两侧
地面真理构成
估计车辆的姿势
地面真理造成车辆的显示为固体蓝色的长方体。估计姿势显示为一个透明的蓝色的长方体。注意,估计姿势没有出现在最初的可视化因为地面真理和估计提出了重叠。
生成基准轨迹的地面车辆使用waypointTrajectory系统对象™。请注意,waypointTrajectory中使用的地方吗drivingScenario /轨迹因为车辆的加速度是必要的。生成的轨迹是在指定采样率使用一组路径点,到达的时间,和速度。
%创建驾驶场景与地面真理和估计%车辆构成。现场= drivingScenario;groundTruthVehicle =车辆(场景,“PlotColor”0.4470 - 0.7410 [0]);estVehicle =车辆(场景,“PlotColor”0.4470 - 0.7410 [0]);%生成基准轨迹。sampleRate = 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,“TimeOfArrival”t…“速度”,速度,“SampleRate”,sampleRate);%添加一个道路和建筑场景和可视化。helperPopulateScene(场景,groundTruthVehicle);
创建一个融合滤波器
创建一个过滤器来融合IMU和视觉里程计测量。这个示例使用松散耦合的方法来融合测量。虽然结果不准确的紧密耦合的方法,所需的处理是大大减少,结果是适当的。融合滤波器使用一个误差状态卡尔曼滤波跟踪定位(作为一个四元数),位置,速度,和传感器偏差。
的insfilterErrorState处理传感器数据对象具有以下功能:预测和fusemvo。
的预测函数接受IMU的加速度计和陀螺仪测量作为输入。调用预测函数每次加速度计和陀螺仪是采样。这个函数预测国家前进到一个时间步基于加速度计和陀螺仪测量,并更新状态误差协方差的过滤器。
的fusemvo函数接受视觉测距姿势估计作为输入。这个函数更新错误状态基于视觉里程计的姿势估计计算卡尔曼增益,重量不同的输入根据他们的不确定性。与预测函数,这个函数也更新状态误差协方差,这次考虑卡尔曼增益。然后状态更新使用新的错误状态和错误状态复位。
filt = insfilterErrorState (“IMUSampleRate”sampleRate,…“ReferenceFrame”,“ENU表示”)%设置初始状态和错误状态协方差。helperInitialize (filt traj);
filt = insfilterErrorState属性:IMUSampleRate: 100 Hz ReferenceLocation:[0 0 0][度度m]状态:[17 x1双]StateCovariance: [16 x16双]过程噪声方差GyroscopeNoise: [1 e-06 e-06 1 e-06] (rad / s)²AccelerometerNoise: [0.0001 0.0001 0.0001] (m / s²)²GyroscopeBiasNoise: [1 e-09 e-09 1 e-09] (rad / s)²AccelerometerBiasNoise: [0.0001 0.0001 0.0001] (m / s²)²
指定的视觉测程法模型
定义视觉测距模型参数。这些参数模型特征匹配和使用单眼相机tracking-based视觉测距系统。的规模参数未知的规模占后续视觉单眼相机的帧。其他参数的漂移模型视觉测程法阅读作为一阶白噪声和高斯-马尔可夫过程。
%国旗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传感器模型使用imuSensor系统对象。传感器模型包含属性模型的确定性和随机噪声来源。属性值设置下面是典型的低成本的MEMS传感器。
%设置RNG种子为后续违约获得相同的结果%。rng (“默认”)imu = imuSensor (“SampleRate”sampleRate,“ReferenceFrame”,“ENU表示”);%加速度计imu.Accelerometer。MeasurementRange = 19.6;% m / s ^ 2imu.Accelerometer。分辨率= 0.0024;% m / s ^ 2 / LSBimu.Accelerometer。NoiseDensity = 0.01;% (m / s ^ 2) /√(赫兹)%陀螺仪imu.Gyroscope。MeasurementRange =函数(250);% rad /秒imu.Gyroscope。解析函数= (0.0625);% rad / s / LSBimu.Gyroscope。NoiseDensity函数= (0.0573);% (rad / s) /√(赫兹)imu.Gyroscope。ConstantBias =函数(2);% rad /秒
建立了模拟
指定运行仿真的时间和初始化变量记录在模拟循环。
% 60秒的模拟运行。numSecondsToSimulate = 60;numIMUSamples = numSecondsToSimulate * sampleRate;%定义视觉里程计采样率。imuSamplesPerCamera = 4;numCameraSamples =装天花板(numIMUSamples / imuSamplesPerCamera);% Preallocate数据数组绘制结果。[pos,东方,或者,acc angvel,…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 = =国防部(我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 pos(我:),或者(我:),东方(我));%更新车辆造成地面真理。helperUpdatePose (groundTruthVehicle pos(我:),或者(我:),东方(我));%更新驾驶场景可视化。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]…= helperVisualOdometryModel (pos、东方paramsVO)%提取模型参数。scaleVO = paramsVO.scale;西格曼= paramsVO.sigmaN;τ= paramsVO.tau;sigmaB = paramsVO.sigmaB;sigmaA =√(2 /τ)+ 1 /(τ)*τ))* sigmaB;b = paramsVO.driftBias;%计算漂移。b =(1 - 1 /τ)。* b + randn (1、3) * sigmaA;漂移= randn(1、3) *西格曼+ b;paramsVO。driftBias = b;%计算视觉里程计测量。posVO = scaleVO * pos +漂移;orientVO =东方;结束
helperInitialize
设置初始状态和协方差值融合滤波器。
函数helperInitialize (filt traj)%检索初始位置、方向和速度的%轨迹对象和重置内部状态。[pos,东方,或者]= 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
Preallocate数据记录仿真结果。
函数[pos,东方,或者,acc angvel,…posVO orientVO,…姿势,东方,或者]…= helperPreallocateData (numIMUSamples numCameraSamples)%指定地面实况。pos = 0 (numIMUSamples, 3);东方=四元数。0 (numIMUSamples, 1);韦尔= 0 (numIMUSamples, 3);acc = 0 (numIMUSamples, 3);angvel = 0 (numIMUSamples, 3);%的视觉里程计输出。posVO = 0 (numCameraSamples, 3);orientVO =四元数。0 (numCameraSamples, 1);%滤波器输出。pos = 0 (numIMUSamples, 3);东方=四元数。0 (numIMUSamples, 1);韦尔= 0 (numIMUSamples, 3);结束
helperUpdatePose
更新车辆的姿势。
函数helperUpdatePose (veh pos、或者东方)阿明费。位置= pos;阿明费。速度=韦尔;rpy = eulerd(东方,“ZYX股票”,“帧”);阿明费。偏航= rpy (1);阿明费。距= rpy (2);阿明费。滚= rpy (3);结束
引用
苍井空,J。“四元数运动学误差状态卡尔曼滤波器”。ArXiv e-prints, arXiv:1711.02508v1 [cs.RO] 3 Nov 2017.
江R, R。,R. Klette, and S. Wang. "Modeling of Unbounded Long-Range Drift in Visual Odometry." 2010 Fourth Pacific-Rim Symposium on Image and Video Technology. Nov. 2010, pp. 121-126.
