本文描述了一种简单的循迹行驶方案，并通过Matlab simulink实现，实车测试通过。

运动轨迹规划的工作的一部分就是循迹，本文的模型只披露了循迹部分的算法，对于轨迹规划的算法进行了屏蔽。对这部分模块的输入，选择了已经提前录制好的地图地理数据作为参考线轨迹（reference line）.

接下来看一下总的结构：

有点复杂，接下来每个部分分别解释：

提前录制好车辆轨迹的经纬度坐标和对应的每个点的地球坐标系heading angle,存储到矩阵store中。

从车辆总线接入相关信号：包括车辆真实转角，真实heading angle(依旧是在地球坐标系下的)，车辆所在的经纬度信息， 通过WGS84地球坐标系的转换方式，讲车辆起点作为转换后的笛卡尔坐标系远点，正北方向为Y轴方向。

function [X,Y]=transformation1(lat,lon,B0,L0)
% 调用格式：[X,Y]=transformation(lat,lon,B0,L0)
% 输入： lat为纬度，lon为经度（单位为弧度），B0：坐标原点的纬度x，L0：坐标原点的经度y，单位为弧度((rad);
% 输出：X为水平直角坐标， Y为纵向直角坐标，单位为米(m);
 B=lat*pi/180;
 L=lon*pi/180;
 B0=B0*pi/180;
 L0=L0*pi/180;
% B0=B0*pi/180;L0=L0*pi/180;%弧度转换为度数
a=6378137; b=6356752.3142;% a:地球椭球体长半轴，b:地球椭球体短半轴，单位为米(m); constant
e=sqrt(1-(b/a)^2);ee=sqrt((a/b)^2-1);%偏心率 constant
K=((a^2*cos(B0))/b)/sqrt(1+(ee)^2*(cos(B0))^2); %constant
X=K*(L-L0); 
Y0=K*log(tan(pi/4+B0/2)*((1-e*sin(B0))/(1+e*sin(B0)))^(e/2));
Y=K*log(tan(pi/4+B/2)*((1-e*sin(B))/(1+e*sin(B)))^(e/2))-Y0;%%这行有问题 -3.12145e6
end

% a=6378137;% a：地球椭球体长半轴，单位为米(m);
% b=6356752.3142;% b：地球椭球体短半轴，单位为米(m);
% e=sqrt(1-(b/a)^2);%第一偏心率
% ee=sqrt((a/b)^2-1);%第二偏心率
% K=(((a^2*cos(B0))/b)/sqrt(1+(ee)^2*(cos(B0))^2));
% X=K*(L-L0); %水平直角坐标值
% Y=K*log(tan(pi/4+B/2)*((1-e*sin(B))/(1+e*sin(B)))^(e/2));%纵向直角坐标值
%%

对应的坐标转换公式如图所示。
接下来会有一个初始化。找到车辆当前的位置在轨迹上的什么位置：

输出的K值，就是提前对轨迹上的点做好的标号。

比如k= 134, 那么车辆就是在134号点附近。

这里还有一个状态机，具体不做详细叙述，主要作用就是用于状态机的切换，比如待机，开机，关机等，可以不用管。

主要的函数在这里，这个函数输出的信息就是，当前时刻，车辆与预定轨迹的横向误差error， 这个误差在后面的控制中是要被用到的，还有delta_heading, 就是车辆当前的heading angle 与离你最近的参考点上的heading angle 的差值，这个也是要被后面的控制用到的，其他的Location， 就是你当前在哪一个点附近。LFx,y,z 暂时用不到，先不用管。
详细的内容可以下载资源打开看：
https://download.csdn.net/download/gophae/11840135

接下里是control, control 由三部分组成， 一个是PID control， input 是横向误差。

第二部分是一个预瞄的控制

function y = fcn(error, pld,fd)
%#codegen
y= fd * atan(error/(pld)) *180/pi;

最后一部分就是delta_heading 的前馈控制。

以上的控制思想，参考我的博文
https://blog.csdn.net/gophae/article/details/100012763

最后将期望的方向盘转角通过总线输出：

总线方面的接受和发送通讯我就不详细叙述了。