Ошибочные источники энкодера колеса

Скрипт Open Live Script

Исследуйте различные ошибочные источники энкодеров колеса и как они влияют на оценку одометрии колеса. После определения траектории основной истины измените параметры для смещения радиуса колеса, шума положения колеса, уменьшения колеса и ширины дорожки для различных объектов энкодера колеса. Заметьте влияние изменения этих параметров на выходной траектории из моделей датчика энкодера колеса.

Траектория основной истины

Создайте траекторию основной истины, которая будет использоваться при исследовании параметров ошибок. Постройте траекторию.

Fs = 100;
wps = [0 0 0;
    20 0 0;
    20 5 0;
    0 5 0;
    0 0 0];
toa = cumsum([0 10 1.25*pi 10 1.25*pi]).';
vels = [2 0 0;
    2 0 0;
    -2 0 0;
    -2 0 0;
    2 0 0];

traj = waypointTrajectory(wps,...
    'SampleRate',Fs,'ReferenceFrame','ENU', ...
    'TimeOfArrival',toa,'Velocities',vels);

% Fetch pose values.
[pos,orient,vel,acc,angvel] = lookupPose(traj,toa(1):1/Fs:toa(end));
angvelBody = rotateframe(orient,angvel);

% Plot ground truth position.
figure
plot(pos(:,1),pos(:,2))
title('Position (Ground Truth)')
xlabel('X (m)')
ylabel('Y (m)')
axis equal

Сместите в радиусе колеса

Создайте датчик энкодера колеса для модели одноколесного велосипеда как wheelEncoderUnicycle объект. Задайте ненулевой WheelRadiusBias и исследуйте, как это влияет на оценку одометрии. Определение положительного смещения заставляет алгоритм одометрии недооценивать окружность колеса. Это приводит к одометрии, оценивающей, что меньшее расстояние переместилось.

encoder = wheelEncoderUnicycle;
encoder.WheelRadiusBias = 0.05;
odom = wheelEncoderOdometryUnicycle(encoder);

ticks = encoder(vel, angvel, orient);
estPose = odom(ticks, angvelBody(:,3));

% Plot ground truth and estimated positions.
figure
plot(pos(:,1),pos(:,2),estPose(:,1),estPose(:,2))
title('Position (Wheel Radius Bias)')
xlabel('X (m)')
ylabel('Y (m)')
legend('Ground truth','Odometry')
axis equal

Шум в измерении положения колеса

Задайте ненулевой WheelPositionAccuracy и исследуйте, как это влияет на одометрию estimate.This, шум добавляет случайные отклонения в измеренные метки деления от энкодера колеса.

encoder = wheelEncoderUnicycle;
encoder.WheelPositionAccuracy = 0.1;
% Use a local random stream to reproduce results.
encoder.RandomStream = 'mt19937ar with seed';
odom = wheelEncoderOdometryUnicycle(encoder);

ticks = encoder(vel,angvel,orient);
estPose = odom(ticks,angvelBody(:,3));

% Plot ground truth and estimated positions.
figure
plot(pos(:,1),pos(:,2),estPose(:,1),estPose(:,2))
title('Position (Wheel Position Noise)')
xlabel('X (m)')
ylabel('Y (m)')
legend('Ground truth', 'Odometry')
axis equal

Уменьшение колеса и скольжение

Задайте ненулевой SlipRatio и исследуйте, как это влияет на оценку одометрии. Определение значения, больше, чем нуль, симулирует скольжение колеса. Это уменьшение результаты в одометрии, оценивающей большее расстояние, переместилось. Отрицательная величина для отношения промаха inidates скольжение.

encoder = wheelEncoderUnicycle;
encoder.SlipRatio = 0.25;
odom = wheelEncoderOdometryUnicycle(encoder);

ticks = encoder(vel, angvel, orient);
estPose = odom(ticks, angvelBody(:,3));

% Plot ground truth and estimated positions.
figure
plot(pos(:,1),pos(:,2),estPose(:,1),estPose(:,2))
title('Position (Wheel Slippage)')
xlabel('X (m)')
ylabel('Y (m)')
legend('Ground truth', 'Odometry')
axis equal

Сместите в ширине дорожки

Задайте ненулевой TrackWidthBias и исследуйте, как это влияет на оценку одометрии. Определение положительного смещения заставит алгоритм одометрии переоценивать поворачивающийся угол транспортного средства. Эта переоценка превращения результатов в дрейфе, накапливающемся в одометрии, оценивает в поворотах. Для этого сценария, транспортное средство с осью необходимо, так используют wheelEncoderDifferentialDrive объект.

encoder = wheelEncoderDifferentialDrive;
encoder.TrackWidthBias = 0.1;
odom = wheelEncoderOdometryDifferentialDrive(encoder);

ticks = encoder(vel,angvel,orient);
estPose = odom(ticks);

% Plot ground truth and estimated positions.
figure
plot(pos(:,1),pos(:,2), estPose(:,1), estPose(:,2))
title('Position (Track Width Bias)')
xlabel('X (m)')
ylabel('Y (m)')
legend('Ground truth', 'Odometry')
axis equal

Отличающиеся смещения в колесах

Задайте различные ненулевые значения для WheelRadiusBias и исследуйте, как это влияет на оценку одометрии. Определение различных смещений заставляет оценку одометрии дрейфовать в целой траектории. Для этого сценария, два колеса необходимы, так используют wheelEncoderDifferentialDrive объект.

encoder = wheelEncoderDifferentialDrive;
encoder.WheelRadiusBias = [-0.01, 0.001];
odom = wheelEncoderOdometryDifferentialDrive(encoder);

ticks = encoder(vel, angvel, orient);
estPose = odom(ticks);

% Plot ground truth and estimated positions.
figure
plot(pos(:,1),pos(:,2), estPose(:,1), estPose(:,2))
title('Position (Different Wheel Biases)')
xlabel('X (m)')
ylabel('Y (m)')
legend('Ground truth', 'Odometry')
axis equal

Документация