Carsim-Simulink无人驾驶Stanley算法实现

一 基础准备

1 Carsim中的坐标系

• 为惯性坐标系，yaw的角度是以x轴为0°，逆时针为正，范围0~360°。
• 车辆的起始位置为路径上的第一个点（默认是，可以修改），但是航向角默认不是0，是跟随路径变化的，默认起步状态下车辆航向与路径的航向相同。

2 Matlab中的atan vs. atan2

对于tan(θ) = y / x:

• θ = atan(y / x)求出的θ取值范围是[-PI/2, PI/2]。
• θ = atan2(y , x)求出的θ取值范围是[-PI, PI]。
• atan2 的优点在于 如果 x2-x1等于0 依然可以计算，但是atan函数就会导致程序出错；

一般情况下用atan即可，当对所求出角度的取值范围有特殊要求时，应使用atan2。
如：有两个点 point(x1,y1), 和 point(x2,y2); 那么这两个点形成的斜率的角度计算方法分别是：

float angle = atan( (y2-y1)/(x2-x1) );
% 或
float angle = atan2( y2-y1, x2-x1 );

3. 双移线测试

• 车辆的操控性测试中，经常采用的测试是：Double Lane Test双移线试验、Fish Hoock鱼钩试验、J-TURN试验项目是NHTSA（美国交通运输安全委员会）用来评价研究汽车侧翻的，可见这是非常极端的试验。对于适不适合用来评估trajectory tracking问题的测试输入，先把问题留在这。
• 仍然给出一种国内的DLT的测试工况供设计参考路径测试输入参考：
  
• 下面给出了carsim中设置参考路径为双移线的方法：
  

4. Carsim中采用外部转向控制输入的设置

在carsim+simulink联合仿真中，由于轨迹跟踪控制器是在simulink中运行的，轨迹跟踪控制器算法产生的控制量一般是线性车辆模型（单轨模型）中的转向角。
Carsim接收外部的转向角作为控制车辆转向的输入信号，如下图设置输入信号为IMP_STEER_SW，但注意：该信号是 方向盘角度。

5. Carsim中关于Path参考路径的设置

主要在Libraries->Path and Road Surfaces 里面进行路径设置，可以参考如下资料：
https://www.pianshen.com/article/69151025183/
路径里面有两种坐标：

• S-L坐标：S是沿着路径的长度，L是距离路径最近点的距离（与路径最近点切线的垂直线方向上的距离，如下图）。
• X-Y坐标：全局坐标系。
  

二 Stanley算法的原理

• 基本原理是基于几何的方法，可以阅读以下资料：
  https://blog.csdn.net/renyushuai900/article/details/98460758
• 关于python中实现的stanley, 可以阅读以下资料：
  https://blog.csdn.net/gophae/article/details/102765091
• Matlab2020也有Stanley的案例，可以阅读以下资料
  https://www.mathworks.com/help/driving/ref/lateralcontrollerstanleyfunction.html
• 纯Matlab实现的Stanley算法，可以参考以下资料：
  https://blog.csdn.net/caokaifa/article/details/91483376