В этом примере показано, как оценить положение (положение и ориентация) наземного транспортного средства с помощью инерциального измерительного блока (IMU) и монокулярной камеры. В этом примере, вас:
Создайте ведущий сценарий, содержащий траекторию основной истины транспортного средства.
Используйте IMU и визуальную модель одометрии, чтобы сгенерировать измерения.
Плавьте эти измерения, чтобы оценить положение транспортного средства и затем отобразить результаты.
Визуально-инерционная одометрия оценивает положение путем плавления визуальной оценки положения одометрии от монокулярной камеры и оценки положения от IMU. IMU возвращает точную оценку положения для маленьких временных интервалов, но страдает от большого дрейфа из-за интеграции инерционных измерений датчика. Монокулярная камера возвращает точную оценку положения по большему временному интервалу, но страдает от неоднозначности шкалы. Учитывая эти дополнительные достоинства и недостатки, сплав этих датчиков с помощью визуально-инерционной одометрии является подходящим выбором. Этот метод может использоваться в сценариях, где показания GPS недоступны, такой как в городском каньоне.
Создайте drivingScenario Объект (Automated Driving Toolbox), который содержит:
Дорога транспортное средство перемещается на
Создания, окружающие любую сторону дороги
Положение основной истины транспортного средства
Предполагаемое положение транспортного средства
Положение основной истины транспортного средства показывается чисто синим кубоидом. Предполагаемое положение показывается прозрачным синим кубоидом. Обратите внимание на то, что предполагаемое положение не появляется в начальной визуализации, потому что основная истина и оценила перекрытие положений.
Сгенерируйте базовую траекторию для наземного транспортного средства с помощью waypointTrajectory Система object™. Обратите внимание на то, что waypointTrajectory используется вместо drivingScenario/trajectory поскольку ускорение транспортного средства необходимо. Траектория сгенерирована на заданной частоте дискретизации с помощью набора waypoints, времена прибытия и скорости.
% 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 Системный объект. Модель датчика содержит свойства смоделировать и детерминированные и стохастические источники шума. Набор значений свойств здесь типичен для недорогих датчиков 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;
Запустите симуляцию на частоте дискретизации 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.