1、轮速计的工作原理

根据安装在双轮差动机器人左右2个驱动轮电机上的光电编码器，来检测车轮在一定时间内移动的距离，从而推算出机器人相对位姿（位置和航向）的变化。

2、轮速计位姿估计模型

给定车辆的初始定位，根据车辆后轴左、右轮的单位移动距离估计车辆的位姿信息。

3、Matlab代码

3.1、main.m

%根据位姿p(x,y,theta)的运动更新画出车辆轨迹图

clear;

clf;

global B;

B = 1.6; %车辆轮距

H = 0.3; %航向长度

p0 = [0; 0; 0]; %车辆初始位姿

% 运动更新

%% 1 左右轮移动距离随机生成

dist_lr = randi(2, 800, 2) / 5; %左右轮移动距离, m

dist_lr(1, :) = 0; %第一行为初始状态

%% 2 绕圆圈左右轮移动距离

% r = 10; %后轴中心绕圈半径

% r1 = r – B/2; %内轮绕圈半径

% r2 = r + B/2; %外轮绕圈半径

% theta_temp = 1; %车辆航向每周期的变化,度

% n_temp = 360 / theta_temp;

% d1 = theta_temp / 180 * pi * r1; %左轮每个周期移动的距离

% d2 = theta_temp / 180 * pi * r2; %右轮每个周期移动的距离

% dist_lr = ones(n_temp, 2) * [d1 0; 0 d2];

%% 3 画图

figure(1)

hold on

axis equal

grid on

n = length(dist_lr);

for i = 1 : n

pi = cyclometer(p0, dist_lr(i, 1), dist_lr(i, 2)); %更新后的后轴中心位姿

p0 = pi; %更新p0

%左轮位置

xl = pi(1) – B/2*cos(pi(3));

yl = pi(2) – B/2*sin(pi(3));

%右轮位置

xr = pi(1) + B/2*cos(pi(3));

yr = pi(2) + B/2*sin(pi(3));

%航向角位置

xh = pi(1) – H*sin(pi(3));

yh = pi(2) + H*cos(pi(3));

plot([xl xr], [yl yr], ‘k’, ‘linewidth’, 2);

plot([pi(1) xh], [pi(2) yh], ‘r’, ‘linewidth’, 1.5);

pause(0.01);

end

hold off

3.2、cyclometer.m

function p1 = cyclometer(p0, dl, dr)

% 轮速计位姿估计模型

% p0(x0,y0,theta0)分别为初始的车辆位置和航向

% dr,dl分别为右轮和左轮的移动距离

% x,y,theta分别为更新后的车辆位置和航向

global B;

ds = (dl + dr) / 2; %车辆后轴中心移动的距离

dtheta = (dr – dl) / B; %车辆转过的角度

p1 = p0 + [-ds * sin(p0(3) + dtheta/2);

ds * cos(p0(3) + dtheta/2);

dtheta];

4、运行结果

4.1、左右轮移动距离随机生成的结果

4.2、绕圆圈左右轮移动距离的结果