135动力学模型，横向mpc+纵向mpc simulink和carsim实现任意轨迹跟踪，程序内包含五次多项式式与圆形轨迹两种。

在自动驾驶领域，实现车辆对任意轨迹的精准跟踪是核心任务之一。今天咱就唠唠基于135动力学模型，利用横向MPC（模型预测控制）和纵向MPC，通过Simulink和CarSim联合仿真来达成这一目标，程序里还涵盖了五次多项式轨迹与圆形轨迹哦。

135动力学模型

135动力学模型是对车辆运动进行简化与抽象得到的。它考虑了车辆的主要运动特性，像纵向的加速减速、横向的转向等。这个模型为后续的控制算法提供了一个基础框架，让我们能基于它对车辆的运动进行较为准确的模拟与控制。

横向MPC与纵向MPC

横向MPC

横向MPC主要负责控制车辆的转向，使其能够沿着期望轨迹行驶。它通过预测车辆未来的横向位置和方向，不断调整方向盘转角。咱来看段简单的伪代码示例：

% 定义一些参数
N = 10; % 预测时域
dt = 0.1; % 采样时间

% 初始化状态
x = 0; % 初始横向位置
y = 0; % 初始纵向位置
theta = 0; % 初始航向角

% 期望轨迹
desired_x = [0:0.1:10];
desired_y = sin(desired_x);

for k = 1:length(desired_x)
    % 预测未来状态
    for i = 1:N
        % 这里简单假设车辆运动模型，实际更复杂
        x_pred(i) = x + v * dt * cos(theta); 
        y_pred(i) = y + v * dt * sin(theta); 
        theta_pred(i) = theta + v * dt * delta / L; 
    end
    
    % 计算成本函数，包含横向偏差、航向偏差等
    cost = 0;
    for i = 1:N
        cost = cost + (x_pred(i) - desired_x(k + i))^2 + (y_pred(i) - desired_y(k + i))^2 + (theta_pred(i) - atan2(desired_y(k + i + 1) - desired_y(k + i), desired_x(k + i + 1) - desired_x(k + i)))^2;
    end
    
    % 求解最优控制输入（方向盘转角delta）
    % 这通常会用到优化算法，这里简单示意
    [delta, ~] = fmincon(@(delta) calculate_cost(delta, x, y, theta, desired_x(k:k + N), desired_y(k:k + N), v, L), 0, [], [], [], [], -pi/4, pi/4);
    
    % 更新车辆状态
    x = x + v * dt * cos(theta); 
    y = y + v * dt * sin(theta); 
    theta = theta + v * dt * delta / L; 
end

这段代码首先定义了预测时域和采样时间，初始化车辆状态，以及期望轨迹。在循环中，预测车辆未来状态，计算成本函数，这个成本函数综合考虑了横向偏差、纵向偏差以及航向偏差，目标就是让这些偏差尽可能小。最后求解最优的方向盘转角来更新车辆状态。

纵向MPC

纵向MPC专注于车辆的速度控制，确保车辆在沿着期望轨迹行驶时，速度也能满足要求。同样来段伪代码：

% 定义参数
N = 5; % 预测时域
dt = 0.1; % 采样时间

% 初始化状态
v = 0; % 初始速度

% 期望速度
desired_v = [0:1:10];

for k = 1:length(desired_v)
    % 预测未来速度
    for i = 1:N
        % 简单的速度更新模型，实际更复杂
        v_pred(i) = v + a * dt; 
    end
    
    % 计算成本函数，主要是速度偏差
    cost = 0;
    for i = 1:N
        cost = cost + (v_pred(i) - desired_v(k + i))^2;
    end
    
    % 求解最优控制输入（加速度a）
    % 同样简单示意，实际用优化算法
    [a, ~] = fmincon(@(a) calculate_speed_cost(a, v, desired_v(k:k + N), dt), 0, [], [], [], [], -5, 5);
    
    % 更新速度
    v = v + a * dt; 
end

这里先设定预测时域和采样时间，初始化速度，以及期望速度。循环中预测未来速度，计算基于速度偏差的成本函数，求解最优加速度来更新车辆速度。

Simulink与CarSim联合实现

搭建Simulink模型

在Simulink里，我们要搭建一个能够集成横向MPC和纵向MPC算法的模型。将135动力学模型作为被控对象，把横向MPC和纵向MPC的输出（方向盘转角和加速度）作为输入。同时，需要设置好与CarSim的接口，实现数据交互。

五次多项式轨迹与圆形轨迹

五次多项式轨迹

五次多项式可以很好地描述车辆从一个状态平滑过渡到另一个状态的轨迹。代码实现如下：

% 定义起始和终止位置、速度、加速度
x0 = 0; vx0 = 0; ax0 = 0;
xf = 10; vxf = 0; axf = 0;
t0 = 0; tf = 5;

% 计算五次多项式系数
syms a0 a1 a2 a3 a4 a5
eq1 = a0 + a1 * t0 + a2 * t0^2 + a3 * t0^3 + a4 * t0^4 + a5 * t0^5 == x0;
eq2 = a1 + 2 * a2 * t0 + 3 * a3 * t0^2 + 4 * a4 * t0^3 + 5 * a5 * t0^4 == vx0;
eq3 = 2 * a2 + 6 * a3 * t0 + 12 * a4 * t0^2 + 20 * a5 * t0^3 == ax0;
eq4 = a0 + a1 * tf + a2 * tf^2 + a3 * tf^3 + a4 * tf^4 + a5 * tf^5 == xf;
eq5 = a1 + 2 * a2 * tf + 3 * a3 * tf^2 + 4 * a4 * tf^3 + 5 * a5 * tf^4 == vxf;
eq6 = 2 * a2 + 6 * a3 * tf + 12 * a4 * tf^2 + 20 * a5 * tf^3 == axf;

coeffs = solve([eq1, eq2, eq3, eq4, eq5, eq6], [a0, a1, a2, a3, a4, a5]);

% 生成轨迹点
t = linspace(t0, tf, 100);
x = double(coeffs.a0) + double(coeffs.a1) * t + double(coeffs.a2) * t.^2 + double(coeffs.a3) * t.^3 + double(coeffs.a4) * t.^4 + double(coeffs.a5) * t.^5;

这段代码通过定义起始和终止状态，求解五次多项式的系数，然后生成一系列轨迹点。

圆形轨迹

圆形轨迹也很常见，代码如下：

% 定义圆心和半径
center_x = 0;
center_y = 0;
radius = 5;

% 生成圆形轨迹点
theta = linspace(0, 2 * pi, 100);
x = center_x + radius * cos(theta);
y = center_y + radius * sin(theta);

这里通过定义圆心和半径，利用三角函数生成圆形轨迹的一系列点。

135动力学模型，横向mpc+纵向mpc simulink和carsim实现任意轨迹跟踪，程序内包含五次多项式式与圆形轨迹两种。

通过Simulink与CarSim的联合仿真，基于135动力学模型，结合横向MPC和纵向MPC，我们就能实现车辆对五次多项式轨迹和圆形轨迹等任意轨迹的跟踪啦。这在自动驾驶的研发和测试中有着重要的意义，为实际道路场景下的车辆控制提供了有力的支持。