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

Открыть 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 и исследуйте, как это влияет на оценку одометрии. Этот шум добавляет случайные отклонения к измеренным тактам от энкодера колеса.

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 и исследуйте, как это влияет на оценку одометрии. Установка значения, больше нуля, моделирует проскальзывание колеса. Это проскальзывание приводит к оценке одометрии на большее пройденное расстояние. Отрицательное значение коэффициента скольжения нарушает скольжение.

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

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