Этот пример показывает, как оценить положение (положение и ориентация) наземного транспортного средства с помощью инерционного модуля измерения (IMU) и монокулярной камеры. В этом примере, вас:
Создайте ведущий сценарий, содержащий наземную траекторию истины автомобиля.
Используйте IMU и визуальную модель одометрии, чтобы сгенерировать измерения.
Плавьте эти измерения, чтобы оценить положение автомобиля и затем отобразить результаты.
Визуально-инерционная одометрия оценивает положение путем плавления визуальной оценки положения одометрии от монокулярной камеры и оценки положения от IMU. IMU возвращает точную оценку положения для маленьких временных интервалов, но страдает от большого дрейфа из-за интеграции инерционных измерений датчика. Монокулярная камера возвращает точную оценку положения по большему временному интервалу, но страдает от неоднозначности шкалы. Учитывая эти дополнительные достоинства и недостатки, сплав этих датчиков с помощью визуально-инерционной одометрии является подходящим выбором. Этот метод может использоваться в сценариях, где показания GPS недоступны, такой как в городском каньоне.
Создайте объект drivingScenario, который содержит:
Дорога автомобиль перемещается на
Создания, окружающие любую сторону дороги
Наземное положение истины автомобиля
Предполагаемое положение автомобиля
Наземное положение истины автомобиля показывается чисто синим кубоидом. Предполагаемое положение показывается прозрачным синим кубоидом. Обратите внимание на то, что предполагаемое положение не появляется в начальной визуализации, потому что наземная истина и оценила перекрытие положений.
Сгенерируйте базовую траекторию для наземного транспортного средства с помощью Системы waypointTrajectory object™. Обратите внимание на то, что waypointTrajectory используется вместо drivingScenario/trajectory, поскольку ускорение автомобиля необходимо. Траектория сгенерирована на заданном уровне выборки с помощью набора waypoints, времена прибытия и скорости.
% Create the driving scenario with both the ground truth and estimated % vehicle poses. scene = drivingScenario; groundTruthVehicle = vehicle(scene); estVehicle = vehicle(scene); % 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 и визуальные измерения одометрии. Этот пример использует слабо связанный метод, чтобы плавить измерения. В то время как результаты не так точны как сильно связанный метод, объем обработки необходимого значительно меньше, и результаты соответствуют. Фильтр сплава использует Фильтр Калмана состояния ошибки, чтобы отследить ориентацию (как кватернион), положение, скорость и смещения датчика.
Объект insfilter имеет следующие функции, чтобы обработать данные о датчике: predict и fusemvo.
Функция predict берет акселерометр и измерения гироскопа от IMU как входные параметры. Вызовите функцию predict каждый раз, когда акселерометр и гироскоп выбираются. Эта функция предсказывает состояние вперед одним временным шагом на основе акселерометра и измерений гироскопа, и обновляет ковариацию состояния ошибки фильтра.
Функция fusemvo берет визуальные оценки положения одометрии, как введено. Эта функция обновляет состояния ошибки на основе визуальных оценок положения одометрии путем вычисления усиления Кальмана, которое взвешивает различные входные параметры согласно их неуверенности. Как с функцией predict, эта функция также обновляет ковариацию состояния ошибки, на этот раз принимая усиление Кальмана во внимание. Состояние затем обновляется с помощью нового состояния ошибки, и состояние ошибки сбрасывается.
filt = insfilter('errorstate') filt.IMUSampleRate = sampleRate; % Set the initial state and error state covariance. helperInitialize(filt, traj);
filt =
ErrorStateIMUGPSFuser 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. Модель датчика содержит свойства смоделировать и детерминированные и стохастические источники шума. Набор значений свойств здесь типичен для недорогих датчиков MEMS.
% Set the RNG seed to default to obtain the same results for subsequent % runs. rng('default') imu = imuSensor('SampleRate', sampleRate); % 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
Вычислите визуальное измерение одометрии из наземного входа истины и 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
Тратта, J. "Кинематика кватерниона для Фильтра Калмана Состояния ошибки". Электронная печать ArXiv, arXiv:1711.02508v1 [cs.RO] 3 ноября 2017.
R. Цзян, R., Р. Клетт и С. Ван. "Моделирование Неограниченного Дрейфа Дальнего в Визуальной Одометрии". 2 010 Четвертых Симпозиумов Тихоокеанского региона по Изображению и Видео Технологии. Ноябрь 2010, стр 121-126.