В этом примере показано, как оценить позу (положение и ориентацию) наземного транспортного средства с помощью инерциального измерительного блока (IMU) и монокулярной камеры. В этом примере выполняется следующее:
Создайте сценарий движения, содержащий траекторию истинного движения транспортного средства на земле.
Используйте модель IMU и визуальную одометрию для создания измерений.
Слейте эти измерения для оценки позы транспортного средства и отображения результатов.
Оценки визуально-инерциальной одометрии позируют путем слияния оценки позы визуальной одометрии из монокулярной камеры и оценки позы из IMU. IMU возвращает точную оценку позы для небольших временных интервалов, но страдает от большого дрейфа из-за интегрирования измерений инерциального датчика. Монокулярная камера возвращает точную оценку позы за больший временной интервал, но страдает неоднозначностью масштаба. Учитывая эти дополнительные сильные и слабые стороны, слияние этих датчиков с использованием визуально-инерционной одометрии является подходящим выбором. Этот метод может использоваться в сценариях, где показания GPS недоступны, например, в городском каньоне.
Создать drivingScenario Объект (Automated Driving Toolbox), содержащий:
Дорога, по которой движется транспортное средство
Здания, окружающие обе стороны дороги
Поза наземной истины транспортного средства
Предполагаемая поза транспортного средства
Поза истинности на земле транспортного средства показана как сплошной синий кубоид. Предполагаемая поза показана как прозрачный синий кубоид. Заметим, что оценочная поза не появляется в начальной визуализации, потому что истинность земли и оценочные позы перекрываются.
Создайте базовую траекторию для наземного транспортного средства с помощью waypointTrajectory(Панель инструментов слияния и отслеживания датчиков) object™ системы. Обратите внимание, что waypointTrajectory используется вместо drivingScenario/trajectory поскольку требуется ускорение транспортного средства. Траектория формируется с заданной частотой дискретизации с использованием набора ППМ, времени прихода и скоростей.
% Create the driving scenario with both the ground truth and estimated % vehicle poses. scene = drivingScenario; groundTruthVehicle = vehicle(scene, 'PlotColor', [0 0.4470 0.7410]); estVehicle = vehicle(scene, 'PlotColor', [0 0.4470 0.7410]); % Generate the baseline trajectory. 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, ... 'Velocities', velocities, 'SampleRate', sampleRate); % Add a road and buildings to scene and visualize. helperPopulateScene(scene, groundTruthVehicle);
Создайте фильтр для слияния измерений IMU и визуальной одометрии. В этом примере используется неплотно связанный метод для сплавления измерений. Хотя результаты не столь точны, как тесно связанный способ, объем требуемой обработки значительно меньше, и результаты адекватны. Фильтр слияния использует фильтр Калмана с ошибочным состоянием для отслеживания ориентации (в качестве кватерниона), положения, скорости и смещений датчика.
insfilterErrorState объект имеет следующие функции для обработки данных датчика: predict и fusemvo.
predict функция принимает измерения акселерометра и гироскопа от IMU в качестве входных данных. Позвоните в predict функция каждый раз при выборке акселерометра и гироскопа. Эта функция предсказывает состояние вперед на один шаг времени на основе измерений акселерометра и гироскопа и обновляет ковариацию состояния ошибки фильтра.
fusemvo функция принимает оценки позы визуальной одометрии в качестве входных данных. Эта функция обновляет состояния ошибок, основываясь на оценках позы визуальной одометрии, вычисляя коэффициент Калмана, который взвешивает различные входные данные в соответствии с их неопределенностью. Как и в случае predict функция, эта функция также обновляет ковариацию состояния ошибки, на этот раз принимая во внимание усиление Калмана. Затем состояние обновляется с использованием нового состояния ошибки, и состояние ошибки сбрасывается.
filt = insfilterErrorState('IMUSampleRate', sampleRate, ... 'ReferenceFrame', 'ENU') % Set the initial state and error state covariance. helperInitialize(filt, traj);
filt =
insfilterErrorState with properties:
IMUSampleRate: 100 Hz
ReferenceLocation: [0 0 0] [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] (m/s²)²
Определите параметры модели визуальной одометрии. Эти параметры моделируют систему визуальной одометрии на основе сопоставления признаков и отслеживания с помощью монокулярной камеры. scale параметр учитывает неизвестную шкалу последующих кадров зрения монокулярной камеры. Другие параметры моделируют дрейф при считывании визуальной одометрии как комбинацию белого шума и процесса Гаусса-Маркова первого порядка.
% The flag useVO determines if visual odometry is used: % useVO = false; % Only IMU is used. useVO = true; % Both IMU and visual odometry are used. paramsVO.scale = 2; paramsVO.sigmaN = 0.139; paramsVO.tau = 232; paramsVO.sigmaB = sqrt(1.34); paramsVO.driftBias = [0 0 0];
Определите модель датчика IMU, содержащую акселерометр и гироскоп, используя imuSensor Системный объект. Модель датчика содержит свойства для моделирования детерминированных и стохастических источников шума. Установленные здесь значения свойств типичны для недорогих МЭМС-датчиков.
% Set the RNG seed to default to obtain the same results for subsequent % runs. rng('default') imu = imuSensor('SampleRate', sampleRate, 'ReferenceFrame', 'ENU'); % Accelerometer imu.Accelerometer.MeasurementRange = 19.6; % m/s^2 imu.Accelerometer.Resolution = 0.0024; % m/s^2/LSB imu.Accelerometer.NoiseDensity = 0.01; % (m/s^2)/sqrt(Hz) % Gyroscope imu.Gyroscope.MeasurementRange = deg2rad(250); % rad/s imu.Gyroscope.Resolution = deg2rad(0.0625); % rad/s/LSB imu.Gyroscope.NoiseDensity = deg2rad(0.0573); % (rad/s)/sqrt(Hz) imu.Gyroscope.ConstantBias = deg2rad(2); % rad/s
Укажите время выполнения моделирования и инициализации переменных, регистрируемых во время цикла моделирования.
% Run the simulation for 60 seconds. numSecondsToSimulate = 60; numIMUSamples = numSecondsToSimulate * sampleRate; % Define the visual odometry sampling rate. imuSamplesPerCamera = 4; numCameraSamples = ceil(numIMUSamples / imuSamplesPerCamera); % Preallocate data arrays for plotting results. [pos, orient, vel, acc, angvel, ... posVO, orientVO, ... posEst, orientEst, velEst] ... = helperPreallocateData(numIMUSamples, numCameraSamples); % Set measurement noise parameters for the visual odometry fusion. RposVO = 0.1; RorientVO = 0.1;
Выполните моделирование с частотой выборки IMU. Каждая выборка IMU используется для прогнозирования состояния фильтра вперед на один шаг времени. Как только новое визуальное одометрическое считывание доступно, оно используется для коррекции текущего состояния фильтра.
Имеется некоторый дрейф в оценках фильтра, который может быть дополнительно скорректирован с помощью дополнительного датчика, такого как GPS, или дополнительного ограничения, такого как дорожная карта границы.
cameraIdx = 1; for i = 1:numIMUSamples % Generate ground truth trajectory values. [pos(i,:), orient(i,:), vel(i,:), acc(i,:), angvel(i,:)] = traj(); % Generate accelerometer and gyroscope measurements from the ground truth % trajectory values. [accelMeas, gyroMeas] = imu(acc(i,:), angvel(i,:), orient(i)); % Predict the filter state forward one time step based on the % accelerometer and gyroscope measurements. predict(filt, accelMeas, gyroMeas); if (1 == mod(i, imuSamplesPerCamera)) && useVO % Generate a visual odometry pose estimate from the ground truth % values and the visual odometry model. [posVO(cameraIdx,:), orientVO(cameraIdx,:), paramsVO] = ... helperVisualOdometryModel(pos(i,:), orient(i,:), paramsVO); % Correct filter state based on visual odometry data. fusemvo(filt, posVO(cameraIdx,:), RposVO, ... orientVO(cameraIdx), RorientVO); cameraIdx = cameraIdx + 1; end [posEst(i,:), orientEst(i,:), velEst(i,:)] = pose(filt); % Update estimated vehicle pose. helperUpdatePose(estVehicle, posEst(i,:), velEst(i,:), orientEst(i)); % Update ground truth vehicle pose. helperUpdatePose(groundTruthVehicle, pos(i,:), vel(i,:), orient(i)); % Update driving scenario visualization. updatePlots(scene); drawnow limitrate; end
Постройте график траектории наземного транспортного средства, оценки визуальной одометрии и оценки фильтра слияния.
figure if useVO plot3(pos(:,1), pos(:,2), pos(:,3), '-.', ... posVO(:,1), posVO(:,2), posVO(:,3), ... posEst(:,1), posEst(:,2), posEst(:,3), ... 'LineWidth', 3) legend('Ground Truth', 'Visual Odometry (VO)', ... 'Visual-Inertial Odometry (VIO)', 'Location', 'northeast') else plot3(pos(:,1), pos(:,2), pos(:,3), '-.', ... posEst(:,1), posEst(:,2), posEst(:,3), ... 'LineWidth', 3) legend('Ground Truth', 'IMU Pose Estimate') end view(-90, 90) title('Vehicle Position') xlabel('X (m)') ylabel('Y (m)') grid on
График показывает, что оценка визуальной одометрии относительно точна в оценке формы траектории. Слияние измерений IMU и визуальной одометрии удаляет неопределенность масштабного коэффициента из измерений визуальной одометрии и дрейф из измерений IMU.
helperVisualOdometryModel
Вычислить измерение визуальной одометрии на основе данных входного сигнала и структуры параметров. Для моделирования неопределенности в масштабировании между последующими кадрами монокулярной камеры к положению истинности земли применяется постоянный коэффициент масштабирования в сочетании со случайным дрейфом.
function [posVO, orientVO, paramsVO] ... = helperVisualOdometryModel(pos, orient, paramsVO) % Extract model parameters. scaleVO = paramsVO.scale; sigmaN = paramsVO.sigmaN; tau = paramsVO.tau; sigmaB = paramsVO.sigmaB; sigmaA = sqrt((2/tau) + 1/(tau*tau))*sigmaB; b = paramsVO.driftBias; % Calculate drift. b = (1 - 1/tau).*b + randn(1,3)*sigmaA; drift = randn(1,3)*sigmaN + b; paramsVO.driftBias = b; % Calculate visual odometry measurements. posVO = scaleVO*pos + drift; orientVO = orient; end
helperInitialize
Задайте начальное состояние и значения ковариации для фильтра слияния.
function helperInitialize(filt, traj) % Retrieve the initial position, orientation, and velocity from the % trajectory object and reset the internal states. [pos, orient, vel] = traj(); reset(traj); % Set the initial state values. filt.State(1:4) = compact(orient(1)).'; filt.State(5:7) = pos(1,:).'; filt.State(8:10) = vel(1,:).'; % Set the gyroscope bias and visual odometry scale factor covariance to % large values corresponding to low confidence. filt.StateCovariance(10:12,10:12) = 1e6; filt.StateCovariance(end) = 2e2; end
helperPreallocateData
Предварительно распределить данные для регистрации результатов моделирования.
function [pos, orient, vel, acc, angvel, ... posVO, orientVO, ... posEst, orientEst, velEst] ... = helperPreallocateData(numIMUSamples, numCameraSamples) % Specify ground truth. pos = zeros(numIMUSamples, 3); orient = quaternion.zeros(numIMUSamples, 1); vel = zeros(numIMUSamples, 3); acc = zeros(numIMUSamples, 3); angvel = zeros(numIMUSamples, 3); % Visual odometry output. posVO = zeros(numCameraSamples, 3); orientVO = quaternion.zeros(numCameraSamples, 1); % Filter output. posEst = zeros(numIMUSamples, 3); orientEst = quaternion.zeros(numIMUSamples, 1); velEst = zeros(numIMUSamples, 3); end
helperUpdatePose
Обновите позу транспортного средства.
function helperUpdatePose(veh, pos, vel, orient) veh.Position = pos; veh.Velocity = vel; rpy = eulerd(orient, 'ZYX', 'frame'); veh.Yaw = rpy(1); veh.Pitch = rpy(2); veh.Roll = rpy(3); end
Сола, Дж. «Кинематика кватерниона для фильтра Калмана с ошибочным состоянием». ArXiv электронные отпечатки, arXiv:1711.02508v1 [cs.RO] 3 ноя 2017.
Р. Цзян, Р., Р. Клетт и С. Ван. 2010 Четвёртый Тихоокеанский симпозиум по технологии изображений и видео. Ноябрь 2010 г., стр. 121-126.