PS：我这个人代码存档的习惯不太好，几年前的代码虽然有存档，但是没什么注释，自己看都需要重新读，所以代码详解栏目的代码都是现码（搞这个公众号的初衷也有整理自己代码的目的）。如果后期公开的代码和前期公开的代码功能重复，但是性能更好，一般就是我自己暗搓搓地改了，而且很有可能我不会特别申明，请大家多担待。
上期讲到利用离线数据计算横向误差（即位移误差，在笔者的英文论文中常写作displacement error），但是因为我自己以前用的是在线计算的方式（在线计算会拖MATLAB完成仿真的速度），所以上一期的代码不太完善，在这一期进行了修改，并加入了航向误差的计算方法。
代码语言：javascript复制N=30000;
T=0.05;
rr=zeros(N 10,4);
%参考轨迹
for ir=1:1:N
if ir<=5100
rr(ir,1)=19.5 0.1*(ir-1)*T;
rr(ir,2)=80;
rr(ir,3)=0;
rr(ir,4)=0;
elseif ir<=9787
rr(ir,1)=45 15*sin(0.0067*(ir-5100)*T);
rr(ir,2)=65 15*cos(0.0067*(ir-5100)*T);
rr(ir,3)=0-0.0067*(ir-5100)*T;
rr(ir,4)=-0.06666667;
elseif ir<=15787
rr(ir,1)=60;
rr(ir,2)=65-0.1*(ir-9787)*T;
rr(ir,3)=-1.57;
rr(ir,4)=0;
elseif ir<=20473
rr(ir,1)=75-15*cos(0.0067*(ir-15787)*T);
rr(ir,2)=35-15*sin(0.0067*(ir-15787)*T);
rr(ir,3)=-1.57 0.0067*(ir-15787)*T;
rr(ir,4)=0.06666667;
elseif ir<=30473
rr(ir,1)=75 0.1*(ir-20473)*T;
rr(ir,2)=20;
rr(ir,3)=0;
rr(ir,4)=0;
else
rr(ir,1)=150;
rr(ir,2)=20;
rr(ir,3)=0;
rr(ir,4)=0;
end
end
for i=1:1:15505
xr0=x1(i,1);
yr0=y1(i,1);
xrdd=10000;
xrdk=0;
%计算参考路径上与车辆当前位置最近的点，并将该点在参考路径中的位置赋值给xrdk
for ir=1:1:30000
xrd(ir)=abs(sqrt((rr(ir,1)-xr0)^2 (rr(ir,2)-yr0)^2));
if xrd(ir)<xrdd
xrdd=xrd(ir);
xrdk=ir;
xr1=rr(ir,1);
yr1=rr(ir,2);
end
end
%计算参考路径上与车辆当前位置第二近的点
if xrdk>1&&xrdk<30000
xprd21=xrd(xrdk-1);
xprd22=xrd(xrdk 1);
if xprd21<xprd22
xr2=rr(xrdk-1,1);
yr2=rr(xrdk-1,2);
else
xr2=rr(xrdk 1,1);
yr2=rr(xrdk 1,2);
end
elseif xrdk==1
xr2=rr(xrdk 1,1);
yr2=rr(xrdk 1,2);
else
xr2=rr(xrdk-1,1);
yr2=rr(xrdk-1,2);
end
%计算位移误差
an=(yr2-yr1);
bn=(xr2-xr1);
cn=bn*yr1-an*xr1;
e(i,1)=abs(an*xr0-bn*yr0 cn)/sqrt(an^2 bn^2);
%判断位移误差的正负，一般定义车辆在参考路径前进方向的左侧时误差为正
if xrdk<30000
Tmp=(rr(xrdk,2)-rr(xrdk 1,2))*xr0 (rr(xrdk 1,1)-rr(xrdk,1))*yr0 rr(xrdk,1)*rr(xrdk 1,2)-rr(xrdk 1,1)*rr(xrdk,2);
else
Tmp=(rr(xrdk-1,2)-rr(xrdk,2))*xr0 (rr(xrdk,1)-rr(xrdk-1,1))*yr0 rr(xrdk-1,1)*rr(xrdk,2)-rr(xrdk,1)*rr(xrdk-1,2);
end
if Tmp<=0
e(i,1)=-e(i,1);
end
%计算航向
rh=rr(xrdk,3);
if i>1
h=atan((yr0-y1(i-1,1))/(xr0-x1(i-1,1)));
else
h=atan((y1(i 1,1)-yr0)/(x1(i 1,1)-xr0));
end
if h>1%需根据实际行驶状态调整
h=h-3.14;
end
%航向也可以用To Workspace模块直接读出，但是需要注意采样间隔应与x1、y1的To Workspace采样间隔一致。采用这种方法时不再需要专门根据实际情况对航向进行调整。
%计算航向误差
eh(i,1)=h-rh;
%计算里程（用于绘图的横坐标，当然也可以绘制时序图，但是绘制时序图时，x1、y1的To Workspace采样间隔必须固定）
ll(i,1)=xrdk*0.005;
%tt(i,1)=i*0.05;%如果采样间隔为0.05s
end结果：
横坐标里程，纵坐标位移误差（m）
横坐标里程，纵坐标航向误差（rad）
源码
下期将介绍一些MATLAB出图技巧
