自动驾驶之路径跟踪：Carsim/Simulink 联合仿真与运动学 MPC 算法实践-平芜编程栈

自动驾驶，carsim/simulink联合仿真，基于运动学mpc算法的圆形路径跟踪控制，配套视频详细讲解原理和代码。需要自定义路径跟踪的，可以私或去主页自定义路径跟踪的程序。 carsim2019，matlab2018。

在自动驾驶领域，路径跟踪是核心技术之一。今天就跟大家分享一下基于运动学 MPC（模型预测控制）算法实现圆形路径跟踪控制，并通过 Carsim/Simulink 联合仿真来验证效果，同时还准备了配套视频详细讲解原理和代码，相信会对这块感兴趣的小伙伴们有所帮助。

一、Carsim 和 Simulink 环境

本次使用的是 Carsim2019 和 Matlab2018 版本。Carsim 作为一款专业的车辆动力学仿真软件，能够提供非常精确的车辆模型。而 Simulink 则是 Matlab 中强大的系统建模与仿真工具，两者结合，为自动驾驶算法的验证提供了理想平台。

二、运动学 MPC 算法

运动学 MPC 算法是实现路径跟踪的关键。它通过建立车辆运动学模型，预测车辆未来的状态，并根据预测结果优化控制输入，使得车辆尽可能沿着期望路径行驶。

1. 车辆运动学模型

以简单的自行车模型为例，其运动学方程如下：

\begin{cases}

\dot{x} = v \cos(\theta) \\

\dot{y} = v \sin(\theta) \\

\dot{\theta} = \frac{v}{L} \tan(\delta)

\end{cases}

其中，\((x, y)\) 是车辆在平面坐标系中的位置，\(\theta\) 是车辆的航向角，\(v\) 是车辆速度，\(L\) 是车辆轴距，\(\delta\) 是前轮转向角。

2. MPC 优化问题

在每个采样时刻 \(k\)，MPC 算法需要求解以下优化问题：

\begin{align*}

\min{\Delta \delta{k+i|k}, \Delta v{k+i|k}} & \sum{i = 1}^{Np} (x{k+i|k} - x{ref, k+i})^2 + (y{k+i|k} - y{ref, k+i})^2 + \theta{k+i|k}^2 + \\

& \sum{i = 0}^{Nc - 1} \lambda{\delta} \Delta \delta{k+i|k}^2 + \lambda{v} \Delta v{k+i|k}^2

\end{align*}

约束条件包括车辆运动学模型约束、控制输入约束等。这里 \(Np\) 是预测时域，\(Nc\) 是控制时域，\(\lambda{\delta}\) 和 \(\lambda{v}\) 是权重系数，\((x{ref, k+i}, y{ref, k+i})\) 是期望路径上的参考点。

3. 代码实现示例（Matlab）

% MPC 参数设置 Np = 10; % 预测时域 Nc = 2; % 控制时域 lambda_delta = 0.1; % 转向角权重 lambda_v = 0.01; % 速度权重 % 车辆参数 L = 2.9; % 轴距 % 初始化状态 x = 0; y = 0; theta = 0; v = 5; % 初始位置、航向角和速度 delta = 0; % 初始转向角 % 期望圆形路径参数 r = 10; % 半径 center_x = 0; center_y = 0; % 圆心位置 for k = 1:100 % 仿真步数 % 生成期望路径点 t = k * 0.1; % 时间 x_ref = center_x + r * cos(t); y_ref = center_y + r * sin(t); % 构建 MPC 优化问题 prob = optimproblem('ObjectiveSense','min'); % 控制输入变量 delta_k = optimvar('delta_k', Nc, 1, 'LowerBound', -pi/4, 'UpperBound', pi/4); v_k = optimvar('v_k', Nc, 1, 'LowerBound', 0, 'UpperBound', 10); % 状态变量 x_k = optimvar('x_k', Np + 1, 1); y_k = optimvar('y_k', Np + 1, 1); theta_k = optimvar('theta_k', Np + 1, 1); % 初始状态约束 prob.Constraints.initial_state = [x_k(1) == x; y_k(1) == y; theta_k(1) == theta; v_k(1) == v]; % 运动学模型约束 for i = 1:Np prob.Constraints.dynamics(i) = [ x_k(i + 1) == x_k(i) + v_k(i) * cos(theta_k(i)) * 0.1; y_k(i + 1) == y_k(i) + v_k(i) * sin(theta_k(i)) * 0.1; theta_k(i + 1) == theta_k(i) + (v_k(i) / L) * tan(delta_k(i)) * 0.1 ]; } % 目标函数 obj = sum((x_k(2:end) - x_ref).^2) + sum((y_k(2:end) - y_ref).^2) + sum(theta_k(2:end).^2) +... lambda_delta * sum(delta_k.^2) + lambda_v * sum(v_k.^2); prob.Objective = obj; % 求解优化问题 sol = solve(prob); % 获取控制输入 delta = sol.delta_k(1); v = sol.v_k(1); % 更新车辆状态 x = x + v * cos(theta) * 0.1; y = y + v * sin(theta) * 0.1; theta = theta + (v / L) * tan(delta) * 0.1; % 绘图显示 plot(x_ref, y_ref, 'r--'); hold on; plot(x, y, 'bo'); drawnow; end

这段代码实现了基于运动学 MPC 算法的圆形路径跟踪。首先设置了 MPC 参数和车辆参数，然后在每个仿真步中，生成期望路径点，构建并求解 MPC 优化问题，得到控制输入并更新车辆状态，最后实时绘图显示车辆跟踪路径和期望路径。