Визуально-инерционная одометрия с использованием синтетических данных
Этот пример показов, как оценить положение и ориентацию наземного транспортного средства с помощью инерциального измерительного блока (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);
Создайте фильтр слияния
Создайте фильтр, чтобы сплавить измерения БИНС и визуальной одометрии. Этот пример использует свободно связанный метод, чтобы сплавить измерения. Хотя результаты не так точны, как плотно связанный метод, количество требуемой обработки значительно меньше, и результаты являются адекватными. Фильтр слияния использует фильтр Калмана с ошибкой для отслеживания ориентации (как кватернион), положения, скорости и смещений датчика.
The insfilterErrorState объект имеет следующие функции для обработки данных о датчике: predict и fusemvo.
The predict функция принимает в качестве входов измерения акселерометра и гироскопа от БИНС. Вызовите predict функция каждый раз, когда производят выборку акселерометра и гироскопа. Эта функция предсказывает состояние вперед на один временной шаг на основе измерений акселерометра и гироскопа и обновляет ковариацию состояния ошибки фильтра.
The 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²)²
Задайте модель визуальной одометрии
Задайте параметры модели визуальной одометрии. Эти параметры моделируют функцию совпадающую и основанную на отслеживании систему визуальной одометрии с помощью монокулярной камеры. The 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];
Задайте датчик БИНС
Определите модель датчика БИНС, содержащую акселерометр и гироскоп, используя imuSensor Системный объект. Модель датчика содержит свойства для моделирования как детерминированных, так и стохастических источников шума. Набор значений свойств здесь типичен для недорогих MEMS-датчиков.
% 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;
Запуск цикла симуляции
Запустите симуляцию со частотой дискретизации БИНС. Каждая выборка БИНС используется, чтобы предсказать состояние фильтра вперед на один временной шаг. Как только новое чтение визуальной одометрии доступно, оно используется, чтобы исправить текущее состояние фильтра.
В оценках фильтра есть некоторый дрейф, который может быть дополнительно исправлен с помощью дополнительного датчика, такого как 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
График показывает, что оценка визуальной одометрии является относительно точной при оценке формы траектории. Слияние измерений БИНС и визуальной одометрии удаляет неопределенность масштабного фактора из измерений визуальной одометрии и дрейфа из измерений БИНС.
Вспомогательные функции
helperVisualOdometryModel
Вычислите измерения визуальной одометрии из основной истины входа и struct параметров. Чтобы смоделировать неопределенность в масштабировании между последующими системами координат монокулярной камеры, к основной истине положению применяется постоянный коэффициент масштабирования в сочетании со случайным дрейфом.
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 [c.RO] 3 Ноя 2017.
Р. Цзян, Р., Р. Клетт и С. Ван. Моделирование неограниченного дрейфа на большие расстояния в визуальной одометрии 2010 четвёртый Тихоокеанский симпозиум по Изображению и видеотехнологиям. Ноябрь 2010, с. 121-126.