纯跟踪算法软件在环仿真（Prescan+Carsim+matlab）

1.软件环境搭建

算法平台搭建的大致思想为，首先在Prescan中搭建环境并放置传感器/或采用Prescan的simulink Self Port模块用于提取自车信息，首先在Prescan中搭建场景：

放置路面和如何添加轨迹的操作不是本文重点，由于控制是处于最下层，只有在定位和轨迹规划完成后才能进行运动控制，故首先需要绘制一条轨迹；

在Prescan中选择Carsim的动力学：

此工程为我们用Carsim生成的动力学的模型路径，打开Prescan中的Carsim模型设置：

上图为笔者自己的路径，注意此模型文件夹在Prescan中而不是在Carsim中，打开Carsim：

设置模块的输入输出：

由于我们只做横向控制，故输入只关注方向盘转角。

输出从上到下分别为X,Y,Z的坐标，以及沿着X,Y,Z的运动速度，和Roll，Pitch，Yaw三个角度。

Prescan发送到Simulink中，配置好工程的simfile.sim路径为Carsim工程的路径：

若要对simulink联合仿真工程中Carsim的接口进行更新，更新顺序为：首先更改Carsim的输入输出接口，然后Send to simulink即可完成对动力学接口的更新。

首先不采用carsim动力学，采用Prescan自身动力学，在simulink中添加toWorkspace模块后，运行Prescan工程：

在运行完工程后，编写如下脚本将全局路径点的数据保存到workspace，为防止后面运行时，toWorkspace模块将之前已保存好的全局轨迹点覆盖，故在保存.mat数据后将simulink中toWorkspace模块删去：

path_x=pathX.Data;
path_y=pathY.Data;
save('pathInfo.mat','path_y','path_x','path_y');

在simulink中，需对carsim的输出量做如下处理：

其中MuxState1模块为对Prescan的状态进行更新的模块，Carsim+Prescan的模型在环中，大致的信号流为：

2.纯跟踪算法代码

在左侧的Prescan的Self Port模块中，由于Rotz的坐标系和车辆的坐标系规定相同，故使用RotZ作为车辆航向角，不使用Heading：

其中X,Y为车辆本时刻的坐标，path_x，path_y为规划轨迹，输出angel为前轮转角，steer为转向盘转角。算法代码如下：

function angle  = Puirsuite(x_t,y_t,yawDeg,x_ref,y_ref)
yaw=yawDeg*pi/180;
path_length=length(x_ref);
L=3.165;%车长
h=1.87;%车宽
curve_view_dis=4;    % 前视距离
ld=0;
arpha=0;
%%判断自车位置
 Distant=zeros(path_length,1);
 for i=1:1:path_length
     Distant(i)=(x_ref(i)-x_t)^2+(y_ref(i)-y_t)^2;
    end
 [~, Location]= min(Distant); 
%%计算预瞄点
 distance=0;
 count=1;
 for k=Location:1:path_length
       distance=distance+sqrt((x_ref(k)-x_ref(k+1))^2+(y_ref(k)-y_ref(k+1))^2); 
       count=count+1;
      if distance>=curve_view_dis         %自车与某点距离大于前视距离认为此时的点就是前视点
         break;
      end
   end  
   x_view=x_ref(Location+count);                 %找到前视点的坐标
   y_view=y_ref(Location+count);
   xpf=(x_view-x_t)*cos(yaw)+(y_view-y_t)*sin(yaw); 
   ypf=(x_view-x_t)*(-sin(yaw))+(y_view-y_t)*cos(yaw);
   ld=sqrt(xpf^2+ypf^2);                        %前视点到自车的距离  
   arpha=atan(ypf/xpf); 
   if ld== 0
      angle=0;
   else
       R = ld/(2*sin(arpha));                                 %转弯半径
      angle = atan( L/R)*(180/pi);                            %算出前轮转角
   end
     
end

算法详解如下：首先采用for循环遍历当前车辆位置和所有点之间的距离，并找出距离本车最近的点，将其索引记为Location，在定位的基础上将Location+1的路径索引点作为当下进行一次控制的基础点。

在此基础上，从基础点像后续的规划路径点进行遍历，选择距离大于的点作为预瞄点。由上图的几何关系不难得到：

故要求前轮转角，实际上就是求图中跟踪算法的跟踪半径R，由以下式子确定：

对的确定，涉及到世界坐标系与车辆坐标系的转换，纯跟踪算法确定的车辆坐标系原点为车轮右后轮的坐标，即图中点A的坐标。首先将世界坐标系原点移动到A点有：

将坐标变换表示为矩阵如下：

最后，需要仿真工程和源码的，可关注我私信领取。