基于carsim Simulink联合仿真的车辆换道轨迹规划与轨迹跟踪模型

基于carsim Simulink联合仿真的车辆换道轨迹规划与轨迹跟踪模型
仿真环境设置的道路由2车道组成，策略目标为换道超车规划与控制。道路方向和x轴方向平行。车道1中心线的y坐标是4。车道2中心线的y坐标是0。
变道轨迹采用5次多项式拟合，在程序中，通过生成多段轨迹之后再组合成一段轨迹做为期望轨迹来进行控制。
车辆动力学仿真模型采用侧向二自由度模型，横纵向MPC控制，将纵向控制误差模型和横向控制误差模型集成到一起，误差状态变量分别对应
横向误差lateral_error
横向误差率 lateral_error_rate
航向误差 heading_error
航向误差率 heading_error_rate
速度误差speed_error
位置误差station_error。
包含carsim/simulink联合仿真和纯simulink仿真两种实现方式。

在车辆动力学仿真和控制系统开发中，CarSim 和 Simulink 的联合仿真是一种常见的方法。以下是一个基于 CarSim 和 Simulink 联合仿真的车辆换道轨迹规划与轨迹跟踪模型的设计思路和代码示例。

---

设计思路

1. 轨迹规划：

   • 使用五次多项式或三次样条插值生成平滑的换道轨迹。
   • 输入包括初始位置、目标位置、初始速度、目标速度等。
   • 输出为纵向位移 ( x(t) ) 和横向位移 ( y(t) ) 的时间序列。

2. 轨迹跟踪控制：

   • 使用纯追踪算法（Pure Pursuit）或模型预测控制（MPC）实现轨迹跟踪。
   • 输入为目标轨迹点和当前车辆状态（位置、速度、航向角等）。
   • 输出为前轮转角（Steering Angle）和驱动扭矩（Throttle/Torque）。

3. CarSim 与 Simulink 联合仿真：

   • 在 CarSim 中定义车辆动力学模型。
   • 在 Simulink 中实现轨迹规划和轨迹跟踪控制逻辑。
   • 通过接口将 Simulink 的控制信号传递给 CarSim，并将 CarSim 的车辆状态反馈到 Simulink。

---

代码实现

1. 轨迹规划模块（MATLAB/Simulink）

function [x, y] = generate_trajectory(initial_pos, target_pos, initial_speed, target_speed, time)
    % 输入参数：
    % initial_pos: 初始位置 [x0, y0]
    % target_pos: 目标位置 [xf, yf]
    % initial_speed: 初始速度 (m/s)
    % target_speed: 目标速度 (m/s)
    % time: 时间序列 (秒)

    % 计算轨迹参数
    t = time;
    T = max(t);
    a0 = initial_pos(1); % 初始x位置
    a1 = initial_speed;  % 初始x速度
    a2 = 0;              % 初始x加速度
    a3 = (10*(target_pos(1)-initial_pos(1)) - (4*T*initial_speed + 6*T*target_speed)) / T^3;
    a4 = (-15*(target_pos(1)-initial_pos(1)) + (7*T*initial_speed + 8*T*target_speed)) / T^4;
    a5 = (6*(target_pos(1)-initial_pos(1)) - (3*T*initial_speed + 3*T*target_speed)) / T^5;

    b0 = initial_pos(2); % 初始y位置
    b1 = 0;              % 假设初始y速度为0
    b2 = 0;              % 初始y加速度
    b3 = (10*(target_pos(2)-initial_pos(2))) / T^3;
    b4 = (-15*(target_pos(2)-initial_pos(2))) / T^4;
    b5 = (6*(target_pos(2)-initial_pos(2))) / T^5;

    % 生成轨迹
    x = a0 + a1*t + a2*t.^2 + a3*t.^3 + a4*t.^4 + a5*t.^5;
    y = b0 + b1*t + b2*t.^2 + b3*t.^3 + b4*t.^4 + b5*t.^5;
end

2. 纯追踪轨迹跟踪模块（Simulink）

function delta = pure_pursuit_control(x_ref, y_ref, x_car, y_car, yaw_car, L)
    % 输入参数：
    % x_ref, y_ref: 参考轨迹点
    % x_car, y_car: 当前车辆位置
    % yaw_car: 当前车辆航向角
    % L: 车辆轴距

    % 计算参考点与车辆的距离
    dx = x_ref - x_car;
    dy = y_ref - y_car;
    distance = sqrt(dx^2 + dy^2);

    % 计算目标点角度
    alpha = atan2(dy, dx) - yaw_car;

    % 计算前轮转角
    delta = atan2(2*L*sin(alpha), distance);
end
```![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/7351c5f34d3f4b3187d5d75a35c07b4a.png)


#### **3. CarSim 与 Simulink 接口**
- 在 CarSim 中设置车辆模型，并导出所需的状态变量（如车速、横摆角速度、位置等）。
- 在 Simulink 中创建输入输出接口，将轨迹跟踪模块的输出（如转向角和驱动扭矩）传递给 CarSim。

---

### **Simulink 模型结构**
1. **轨迹规划模块**：
   - 输入：初始位置、目标位置、速度等。
   - 输出：参考轨迹 \( x(t) \) 和 \( y(t) \)。

2. **轨迹跟踪模块**：
   - 输入：参考轨迹、车辆当前状态。
   - 输出：前轮转角和驱动扭矩。

3. **CarSim 接口模块**：
   - 输入：Simulink 控制信号。
   - 输出：CarSim 车辆状态。

---

### **运行步骤**
1. 在 MATLAB 中运行轨迹规划函数 `generate_trajectory`，生成参考轨迹。
2. 在 Simulink 中加载轨迹数据，并将其作为轨迹跟踪模块的输入。
3. 配置 CarSim 与 Simulink 的联合仿真接口。
4. 运行仿真，观察车辆是否能够准确跟踪参考轨迹。

---![在这里插入图片描述](https://i-blog.csdnimg.cn/direct/95b502fcecfc4812b3fe4d5fbaedd32f.png)


### **注意事项**
1. **参数调优**：
   - 调整车速、轴距等参数以适应具体车辆模型。
   - 调整纯追踪算法中的预瞄距离以提高跟踪精度。

2. **仿真环境配置**：
   - 确保 CarSim 和 Simulink 的版本兼容。
   - 正确配置 CarSim 的输入输出接口。

3. **实时性**：
   - 如果需要实时仿真，确保计算量在硬件性能允许范围内。

---

以上代码和设计思路仅供参考，实际应用中需根据具体需求进行调整和优化。