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

Открыть сценарий в реальном времени

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

Траектория истины земли

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

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

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