自动驾驶车辆静态动态避障。
全网最详细解读！！！

采用matlab2023+carsim2016联合仿真。

可以对静态和动态障碍物进行跟踪，有两套规划代码，选择不同的规划代码即可。MPC算法。

采用分层规划控制，规划一层，跟踪一层。

simulink中可以自行设置一下参数：
跟综层预测时域，控制时域
规划层预测时域，控制时域，避障函数权重。

预留滑移率观察接口，可自行在carsim中设置输出滑移率。

可观察每个控制周期的计算时间，便于后期加入创新点后性能对比。

carsim中输出：距离，前轮转角，轮胎滑移率，横向加速度，横摆率，轮胎力，纵向速度等参数，实时绘图。

自动驾驶车辆的静态和动态避障是一个复杂的问题，涉及感知、规划和控制等多个层次。MPC（Model Predictive Control）是一种常用的控制策略，用于处理具有约束条件的系统，并能够预测未来的状态。下面我将提供一个简化的MATLAB/Simulink框架来实现基于MPC的避障控制。

1. 创建Simulink模型

首先，在MATLAB中打开Simulink并创建一个新的模型：

new_system('Autonomous_Vehicle_MPC');
open_system('Autonomous_Vehicle_MPC');

2. 添加必要的模块

在Simulink库浏览器中，添加以下模块到您的模型中：

• MPC Controller：用于实现MPC控制。
• S-Function 或 MATLAB Function Block：用于编写自定义的避障逻辑。
• Sum：用于计算误差信号。
• Scope：用于可视化输出结果。
• Inport/Outport：用于与Carsim进行联合仿真。

3. 配置MPC控制器

使用mpc函数创建一个MPC控制器对象，并设置其参数：

% 定义车辆动力学模型
A = [0 1; 0 0]; % 简单的状态转移矩阵
B = [0; 1];     % 控制输入矩阵
C = [1 0];      % 输出矩阵
D = 0;          % 直接传递矩阵
Ts = 0.1;       % 采样时间

% 创建MPC控制器
mpcobj = mpc(ss(A,B,C,D,Ts), Ts);

% 设置权重
mpcobj.Weights.MV = 0.1;   % 控制动作变化的权重
mpcobj.Weights.MVrate = 0.5;
mpcobj.Weights.OV = 1;     % 输出变量的权重

% 设置约束
mpcobj.MV.Min = -1;        % 控制输入最小值
mpcobj.MV.Max = 1;         % 控制输入最大值

4. 实现避障逻辑

在Simulink中使用S-Function或MATLAB Function Block来实现避障逻辑。这里我们以MATLAB Function Block为例：

function controlSignal = avoidObstacle(distanceToObstacle, currentSpeed)
    % 这里只是一个简单的避障逻辑示例
    if distanceToObstacle < 5 % 如果距离障碍物小于5米
        if currentSpeed > 0
            controlSignal = -0.5; % 减速
        else
            controlSignal = 0.5;  % 加速通过
        end
    else
        controlSignal = 0; % 正常行驶
    end
end

5. 动态调整MPC参数

您可以在Simulink中动态调整MPC控制器的参数，例如预测时域和控制时域：

% 调整预测时域和控制时域
mpcobj.PredictionHorizon = 10; % 预测时域
mpcobj.ControlHorizon = 5;     % 控制时域

6. 连接模块并配置仿真参数

将各个模块按照控制逻辑连接起来，并设置合适的仿真参数：

set_param('Autonomous_Vehicle_MPC', 'StopTime', '30'); % 设置停止时间
set_param('Autonomous_Vehicle_MPC', 'Solver', 'ode45'); % 设置求解器

7. 运行仿真并观察结果

运行仿真并使用Scope模块观察控制效果：

sim('Autonomous_Vehicle_MPC');

8. 数据记录与分析

您可以使用To Workspace模块将仿真数据保存到MATLAB工作区，并进行进一步分析：

% 记录数据
simout = sim('Autonomous_Vehicle_MPC', 'ReturnWorkspaceOutputs', 'on');

9. Carsim集成

为了与Carsim集成，您需要在Carsim中设置相应的输入输出接口，并确保Simulink模型能够接收来自Carsim的数据并发送控制命令。

这个示例仅提供了基本的框架，实际应用中需要根据具体的车辆模型和环境进行详细的建模和调优。对于更复杂的场景，可能还需要考虑更多的传感器数据融合、路径规划算法以及更精细的动力学模型。

我将提供一个MATLAB代码示例，用于生成类似的轨迹跟踪图。这个示例将模拟自动驾驶车辆在不同控制策略下的避障行为，并绘制出相应的轨迹。

MATLAB 代码示例

% 参数设置
x = 50:300; % X轴范围
num_trials = 10; % 不同的试验次数或控制策略数量
obstacle_x = 150; % 障碍物位置
obstacle_width = 20; % 障碍物宽度

% 初始化数据存储
trajectories = zeros(length(x), num_trials);

% 生成不同的轨迹
for i = 1:num_trials
    y = sin((x - obstacle_x) / 50) * (exp(-(x - obstacle_x).^2 / 1000)); % 基础轨迹函数
    
    % 添加随机扰动以模拟不同的控制策略
    noise = randn(size(y)) * 0.2;
    trajectories(:, i) = y + noise;
    
    % 在障碍物区域添加额外的偏移
    idx = find(x >= obstacle_x - obstacle_width/2 & x <= obstacle_x + obstacle_width/2);
    trajectories(idx, i) = trajectories(idx, i) + 1;
end

% 绘制轨迹
figure;
hold on;

% 绘制每条轨迹
for i = 1:num_trials
    plot(x, trajectories(:, i), 'Color', rand(1,3));
end

% 绘制障碍物区域
rectangle('Position', [obstacle_x - obstacle_width/2, -2, obstacle_width, 4], ...
          'FaceColor', 'k', 'EdgeColor', 'none');

% 绘制参考线
plot([obstacle_x, obstacle_x], [-2, 4], 'k--');
plot([obstacle_x - obstacle_width/2, obstacle_x - obstacle_width/2], [-2, 4], 'k--');
plot([obstacle_x + obstacle_width/2, obstacle_x + obstacle_width/2], [-2, 4], 'k--');

% 设置坐标轴标签和标题
xlabel('X (m)');
ylabel('Y (m)');
title('Autonomous Vehicle Trajectories with Obstacle Avoidance');

% 设置坐标轴范围
axis([50 300 -2 5]);

% 添加网格
grid on;

% 保存图形
saveas(gcf, 'autonomous_vehicle_trajectories.png');

代码解释

1. 参数设置：定义了X轴范围、试验次数（即不同的控制策略）、障碍物的位置和宽度。
2. 初始化数据存储：创建了一个矩阵来存储所有轨迹的数据。
3. 生成不同的轨迹：
   • 使用一个基础的正弦函数作为轨迹模型，并添加随机噪声来模拟不同的控制策略。
   • 在障碍物区域内添加额外的偏移，以模拟避障行为。
4. 绘制轨迹：使用plot函数绘制每条轨迹，并用不同颜色表示。
5. 绘制障碍物区域：使用rectangle函数绘制障碍物区域。
6. 绘制参考线：使用plot函数绘制垂直虚线，表示障碍物边界。
7. 设置坐标轴标签和标题：使用xlabel、ylabel和title函数设置坐标轴标签和图形标题。
8. 设置坐标轴范围：使用axis函数设置坐标轴范围。
9. 添加网格：使用grid on命令添加网格。
10. 保存图形：使用saveas函数保存生成的图形。

以更好地模拟自动驾驶车辆的避障行为。