差速机器人的纯轨迹跟踪仿真（Matlab）

差速机器人的纯轨迹跟踪仿真（Matlab）

刚入门，有的地方不对，烦请大家指正。

1 差速机器人运动模型

1.1 运动学分析建模

运动特性为两轮差速驱动，其底部后方两个同构驱动轮的转动为其提供动力，前方的随动轮起支撑作用并不推动其运动，如图1两轮差速驱动示意图所示。

定义其左右驱动轮的中心分别为ωl和ωr，且车体坐标系中这两点在惯性坐标系下移动的线速度为vl和vr，理想情况下即为左右轮转动时做圆周运动的线速度。该值可以通过电机驱动接口输出的角转速Φl、Φr和驱动轮半径r求得，即：

令两驱动轮中心连线的中点为机器的基点C，C点在大地坐标系XOY下的坐标为（x,y），机器的瞬时线速度为vc，瞬时角速度为ωc，姿态角θ即为vc与X轴夹角。此时，机器位姿信息可用适量P = [x,y,θ]T表示。机器人瞬时线速度为vc可以表示为：

令左右轮间距为l，且机器瞬时旋转中心为Oc，转动半径即为C到Oc的距离R。机器在做同轴圆周运动时，左右轮及基点所处位置在该圆周运动中的角速度相同ωl=ωr=ωc，到旋转中心的半径不同，有：

则机器的瞬时角速度ωc可以表示为：

1.2 差速机器人的运动状态方程

在驱动轮与地面接触运动为纯滚动无滑动情况下，机器的运动学模型可以表示为：

2 控制器设计：（pure pursuit）纯轨迹算法

Pure pursuit方法的依据是使下图所示的单车模型以适当的轮转向δ运动，并恰好使机器人轮轴中心经过当前的路点。这样一来，我们就可以根据当前的路点以及单车几何模型计算去当前的期望的轮转向角δ。
我们有以下几何关系：

3程序源码（Matlab）

3.1 分为几个步骤

1、经纬度轨迹数据的导入与筛除
2、数据转化到笛卡尔坐标系和轨迹的平移及变换
3、轨迹跟踪算法
4、处理跟踪轨迹数据

3.2 代码

%筛除错误数据%
new_GPCHC = [];
a = 1;
for i = 1:3431

    if abs(m.GPCHC(i,13)-noproave_lon)<0.04
       new_GPCHC(a,1) = m.GPCHC(i,13);
       new_GPCHC(a,2) = m.GPCHC(i,14);
    a = a+1;
    end
    
end
position = new_GPCHC(:,1:2);
%筛除错误数据结束%

%MATLAB程序实现经纬度转换成笛卡尔坐标%：
M_PI=3.14159265358979323846;
L = 6381372 * M_PI * 2;     %地球周长  
W = L;                      % 平面展开后，x轴等于周长  
H = L / 2;                  % y轴约等于周长一半  
mill = 2.3;                 % 米勒投影中的一个常数，范围大约在正负2.3之间  

n=size(position,1);
new_position=[];
for i =1:n
    lon=position(i,1);
    lat=position(i,2);
    x = lon * M_PI / 180; % 将经度从度数转换为弧度  
    y = lat * M_PI / 180; %将纬度从度数转换为弧度  
    y1 = 1.25 * log(tan(0.25 * M_PI + 0.4 * y)); % 米勒投影的转换  
    % 弧度转为实际距离  
    dikaerX = (W / 2) + (W / (2 * M_PI)) * x ; %笛卡尔坐标x
    dikaerY = (H / 2) - (H / (2 * mill)) * y1 ;%笛卡尔坐标y
    new_position(i,1)=dikaerX;
    new_position(i,2)=dikaerY;
    fprintf('第%d个点的',i)
    fprintf('坐标是=(%f %f);',new_position(i,1),new_position(i,2))
    fprintf('\n')
end
%经纬度轨迹完全转化为笛卡尔坐标系%

x_goal = real(new_position(:,1))-23615000;
y_goal = real(new_position(:,2))+6941400;
y_goal = y_goal/10;
%%%%%%%%%%%%%%%%%%%%%%%%%%路径加载完毕%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%载入路径%
cx = x_goal;
cy = y_goal;
%载入参数%
k = 1;        % 前视距离增益
Lfc = 1.0;       % 预见性距离
Kp = 1.0 ;       % 速度比例增益
dt = 0.4  ;      % 时间
L = 0.4  ;       %  轮轴距
target_speed = 2; %目标速度
T = 100000;       %仿真时间
qwe = 1200;
%设置预处理过程%
x_process = [cx(1,1)];
y_process = [cy(1,1)];
i = 1;
N = length(x_process);
x =cx(1,1);
y = cy(1,1); 
yaw = 0;
v = 0;
time = 0;
Lf = k * v + Lfc;
f1 = figure(1)
  
while ((T > time)&&(N~=qwe))
    N = length(x_process);
    target_index = calc_target_current_distance(x,y,cx,cy);
   %计算控制量%
    ai = PIDcontrol(target_speed, v,Kp);
    di = pure_pursuit_control(x,y,yaw,v,cx,cy,target_index,k,Lfc,L,Lf);
    
    [x,y,yaw,v] = updatestate(x,y,yaw,v, ai, di,dt,L)
    time = time + dt;
    x_process = [x_process;x];
    y_process = [y_process;y];
    plot(cx,cy,'b',x,y,'r*')
    title('跟踪过程')
    drawnow
    hold on
end
f2 = figure(2)
plot(x_goal,y_goal,'b',x_process,y_process,'r')
title('跟踪路径')
legend('预定轨迹','实际跟踪路径');

%筛选出距离最近的点%
newx = zeros(qwe+1,1);
newy = zeros(qwe+1,1);
locat = 0;
for p = 1:(qwe+1)
    Dis = zeros(3429,1);
    for q = 1:3429
    Dis(q,1) =  (x_goal(q,1)-x_process(p,1))^2 + (y_goal(q,1)-y_process(p,1))^2;
    end
    [~, locat]= min(Dis);
    newx(p,1) = x_goal(locat,1);
    newy(p,1) = y_goal(locat,1);
end
%处理数据，计算均方差%
sum = 0;
for i = 1:(qwe+1)
    sum = sum + sqrt((newx(i,1)-x_process(i,1))^2+(newy(i,1)*10-y_process(i,1)*10)^2);
end
 ave = sum/(qwe+1);
 fprintf('轨迹跟踪的均方差为%f\n',ave);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%函数定义%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [x, y, yaw, v] = updatestate(x, y, yaw, v, a, delta,dt,L) %更新当前差速机器人状态%
    x = x + v * cos(yaw) * dt;
    y = y + v * sin(yaw) * dt;
    yaw = yaw + v / L * tan(delta) * dt;
    v = v + a * dt;
end

function [a] = PIDcontrol(target_v, current_v, Kp)
    a = Kp * (target_v - current_v);
end

function [delta] = pure_pursuit_control(x,y,yaw,v,cx,cy,index,k,Lfc,L,Lf)
    tx = cx(index);
    ty = cy(index);
    
    alpha = atan2((ty-y),(tx-x))-yaw;
    
    Lf = k * v + Lfc;
   delta = atan(2*L * sin(alpha)/Lf)  ;
end

function [index] = calc_target_current_distance(x,y, cx,cy)
  N =  length(cx);  %计算目标横轴点的所有点数%
  Distance = zeros(N,1);  %初始化距离矩阵%
for i = 1:N
  Distance(i) =  sqrt((cx(i)-x)^2 + (cy(i)-y)^2); %计算出每个目标点到当前点的距离%
end
  [~, location]= min(Distance);
  index = location;
  index = index+1 ;
end

3.3 效果

谢谢！