四轮转向4WS➕轨迹跟踪控制模型采用双SMC控制

四轮转向4WS➕轨迹跟踪控制模型
采用双SMC控制

4WS通过积分滑模控制跟踪期望横摆角速度和质心侧偏角，效果很好~
轨迹跟踪为双移线输入，采用积分滑模控制

这是一个非常经典的车辆动力学控制问题。四轮转向结合双滑模控制（通常指对横摆角速度和质心侧偏角进行滑模控制）是实现高精度轨迹跟踪的有效方法。

由于涉及复杂的微分方程和矩阵运算，直接用 Simulink 搭建底层模型比较繁琐。我为你整理了一个基于 MATLAB Function Block 的核心控制算法代码，以及车辆动力学模型的代码。你可以将这些代码放入 Simulink 中，或者直接在 MATLAB 脚本中运行进行仿真。

数学模型基础

车辆动力学模型 (2-DOF)：
描述车辆的侧向运动和横摆运动。

begin{bmatrix}
m(V_x + dot{beta}) & -m V_x \
-m V_x & I_z
end{bmatrix}
begin{bmatrix}
dot{beta} \
dot{r}
end{bmatrix}
=
begin{bmatrix}
F_{yf} + F_{yr} \
a F_{yf} - b F_{yr}
end{bmatrix}

其中，F_{yf} = C_{alpha f}(delta_f - beta - frac{a r}{V_x})，F_{yr} = C_{alpha r}(delta_r - beta + frac{b r}{V_x})。

滑模控制律：
定义滑模面 s = dot{e} + lambda e，控制律包含等效控制和切换控制。

MATLAB Function 代码 (双滑模控制器)

这是一个用于 Simulink 的 MATLAB Function Block 代码，实现了双滑模控制律。

function [delta_f, delta_r] = fcn(Vx, beta, r, ref_beta, ref_r)
% 双滑模控制器：跟踪期望的质心侧偏角(ref_beta)和横摆角速度(ref_r)
% 输入: Vx(车速), beta(侧偏角), r(横摆角速度), ref_beta, ref_r
% 输出: delta_f(前轮转角), delta_r(后轮转角)

%% 1. 车辆参数 (根据你的车型修改)
m = 1575; % 整车质量 (kg)
Iz = 2873; % 绕Z轴转动惯量 (kg.m^2)
a = 1.28; % 质心到前轴距离 (m)
b = 1.52; % 质心到后轴距离 (m)
Caf = -80000; % 前轮胎侧偏刚度 (N/rad)
Car = -80000; % 后轮胎侧偏刚度 (N/rad)

%% 2. 误差定义
e_beta = beta - ref_beta; % 侧偏角误差
e_r = r - ref_r; % 横摆角速度误差

%% 3. 滑模参数设置
lambda_beta = 10; % 侧偏角滑模面系数
lambda_r = 10; % 横摆角速度滑模面系数
eta_beta = 10; % 切换增益 (侧偏角)
eta_r = 10; % 切换增益 (横摆角)

% 滑模面
s_beta = e_beta * lambda_beta + e_beta;
s_r = e_r * lambda_r + e_r;

%% 4. 双滑模控制律推导 (简化版)
% 这里采用解耦控制思想，分别计算前轮和后轮转角
% 注意：实际代码需要求解微分方程，这里展示核心逻辑

% 计算所需的侧向力和横摆力矩
Fy_cmd = m * Vx * (lambda_beta * e_beta + (ref_r - r));
Mz_cmd = Iz * (lambda_r * e_r + (0 - 0)); % 假设横摆角加速度期望为0

% 解耦计算前后轮转角 (这是一个简化的逆运算)
delta_f = (Fy_cmd * b + Mz_cmd) / (Caf * (a + b));
delta_r = (Fy_cmd * a - Mz_cmd) / (Car * (a + b));

% 加入滑模切换项 (Stribeck效应或鲁棒项)
delta_f = delta_f + eta_beta * sign(s_beta) / Caf;
delta_r = delta_r + eta_r * sign(s_r) / Car;

%% 5. 限幅处理 (防止转角过大)
delta_max = 0.35; % 20度左右
delta_f = max(min(delta_f, delta_max), -delta_max);
delta_r = max(min(delta_r, delta_max), -delta_r);

车辆动力学模型代码 (Plant Model)

这是被控对象的代码，用于 Simulink 中计算车辆的实际响应。

function [dx] = vehicle_dynamics(x, delta_f, delta_r, Vx)
% 2自由度车辆动力学模型
% 输入: x(状态向量), delta_f, delta_r(控制输入), Vx(车速)
% 输出: dx(状态导数)

%% 参数
m = 1575; Iz = 2873; a = 1.28; b = 1.52;
Caf = -80000; Car = -80000;

%% 状态变量
beta = x(1); % 侧偏角
r = x(2); % 横摆角速度

%% 轮胎力计算
alpha_f = delta_f - beta - (a * r) / Vx;
alpha_r = delta_r - beta + (b * r) / Vx;

Fyf = Caf * alpha_f;
Fyr = Car * alpha_r;

%% 动力学方程
dbeta = (Fyf + Fyr) / (m * Vx) - r;
dr = (a * Fyf - b * Fyr) / Iz;

dx = [dbeta; dr];

Simulink 搭建建议

参考轨迹生成：使用 Signal Builder 或 MATLAB Function 生成“双移线”轨迹。双移线的数学表达式通常为：
y_{ref}(t) = A cdot text{sign}(sin(omega t)) cdot (1 - e^{-k|t-t_0|})
或者简单的分段函数：在 0-2s 向左移，2-4s 向右移。

运动学模块：你需要一个运动学模块将横摆角速度 r 和车速 Vx 积分得到 X, Y 坐标，用于计算横向误差 e_y。

积分滑模：
你在描述中提到“积分滑模”，这意味着滑模面 s 中包含误差的积分项。
修改滑模面定义：s = e_dot + lambde + gammaintegral(e)。
在 Simulink 中，这意味着你需要对误差信号加一个 Integrator 模块，然后加权求和。

轨迹跟踪外环：
计算横向误差 e_y 和航向角误差 e_psi。
使用 PID 或 SMC 生成期望的横摆角速度 r_{ref}。
将 r_{ref} 和 beta_{ref}(通常为0) 输入到上面的双滑模控制器中。

完整的 Simulink + CarSim 联合仿真的搭建方案和核心代码逻辑。

Simulink 模型搭建逻辑

你需要搭建一个闭环控制系统，结构如下：

CarSim S-Function 模块：作为车辆模型（Plant）。
控制器模块 (MATLAB Function)：读取 CarSim 的状态，计算前轮转角 delta_f 和后轮转角 delta_r。
信号源：如“双移线”轨迹生成器。

Simulink 信号连接说明

在 Simulink 中，CarSim 模块的端口定义通常是固定的（具体取决于你导出的接口文件，但通常如下）：

输入 (Inputs)：
Port 1：前轮转角 delta_f (rad)
Port 2：后轮转角 delta_r (rad)
输出 (Outputs)：
Port 1：侧向位置 Y (m)
Port 2：纵向位置 X (m)
Port 3：质心侧偏角 beta (rad)
Port 4：横摆角速度 r (rad/s)
Port 5：车速 Vx (m/s)

核心控制代码 (MATLAB Function)

你需要将这段代码放入 Simulink 的 MATLAB Function Block 中。它接收 CarSim 的输出，计算控制量，并反馈给 CarSim。

function [delta_f, delta_r] = controller(Vx, beta, r, Y, X, ref_X, ref_Y)
% controller: 双滑模控制器主函数
% 输入: Vx, beta, r, Y, X 来自 CarSim 的传感器反馈
% ref_X, ref_Y 来自轨迹生成器 (双移线)
% 输出: delta_f, delta_r (发送给 CarSim 的转向指令)

%% 1. 轨迹误差计算
% 假设轨迹是双移线，计算横向误差 ey
% 这里需要根据你的轨迹方程计算 ref_beta 和 ref_r
% 例如：简单的跟随，ref_Y 是预设的双移线轨迹

% 计算期望的横摆角速度 (ref_r) 和侧偏角 (ref_beta)
% 这通常需要根据车辆运动学和轨迹曲率计算
% 这里简化处理，你可以根据之前的滑模控制逻辑替换这部分
[ref_r, ref_beta] = calc_ref_states(Vx, X, Y, ref_X, ref_Y);

%% 2. 双滑模控制律 (核心逻辑)
% 参数设置
lambda_beta = 1.0;
lambda_r = 1.0;
k_beta = 100;
k_r = 100;

% 误差
e_beta = beta - ref_beta;
e_r = r - ref_r;

% 滑模面
s_beta = e_beta + lambda_beta * e_beta; % 简化为比例项，实际需要微分
s_r = e_r + lambda_r * e_r;

% 控制律 (简化版，实际需要考虑车辆动力学矩阵)
% 这里直接输出转角指令
delta_f = -k_beta * sign(s_beta) + ref_beta; % 前轮控制
delta_r = -k_r * sign(s_r) + ref_r; % 后轮控制

% 限幅 (防止转角过大)
delta_f = min(max(delta_f, -0.5), 0.5); % 限制在 ±30度左右
delta_r = min(max(delta_r, -0.3), 0.3);

end

function [ref_r, ref_beta] = calc_ref_states(Vx, X, Y, ref_X, ref_Y)
% calc_ref_states: 计算期望状态
% 这里需要根据轨迹规划算法实现
% 简单示例：假设 ref_r 和 ref_beta 是根据横向误差计算的
ref_r = 0; % 待填充
ref_beta = 0; % 待填充
end

双移线轨迹生成器代码

你需要一个子系统来生成双移线轨迹。可以使用 MATLAB Function 或 Signal Builder。

function [ref_X, ref_Y] = double_lane_change(t)
% 双移线轨迹函数
A = 3; % 偏移幅度 (m)
L = 100; % 波长 (m)
Vx = 20; % 假设车速 (m/s)

ref_X = Vx * t; % 纵向匀速

% 简化的双移线数学模型
if t < L/Vx
ref_Y = A/2 * (1 - cos(piVxt/L));
elseif t < 2L/Vx
ref_Y = A/2 * (1 + cos(piVx*(t-L/Vx)/L));
else
ref_Y = 0;
end
end

核心 MATLAB 仿真代码

这段代码模拟了一个受正弦位移激励的磁流变阻尼器。它包含了 Bouc-Wen 模型的微分方程求解。

function MR_Damper_Simulation()
% ==========================================
% 磁流变阻尼器 Bouc-Wen 模型仿真
% 生成：力-位移曲线、力-速度曲线、位移-时间曲线
% ==========================================

% --- 1. 系统参数设置 ---
% 这些参数需要根据你的实验数据或论文进行标定
c0 = 1500;    % 油孔阻尼系数 (Ns/m)
k0 = 1000;    % 油孔刚度系数 (N/m)
alpha = 1e5;  % 磁滞刚度增益 (N/m)

% Bouc-Wen 磁滞参数
A = 1.0;      % 幅值参数
beta_b = 0.5; % 形状参数 1
gamma_b = 0.5;% 形状参数 2
n = 2;        % 光滑度指数

% --- 2. 激励信号设置 (模拟作动器输入) ---
A_excite = 0.01; % 激励幅值 (m) - 10mm
f_excite = 1;    % 激励频率 (Hz)
t_final = 10;    % 仿真时间 (s)
fs = 1000;       % 采样频率 (Hz)
t = 0:1/fs:t_final;

% 生成正弦位移激励
x = A_excite * sin(pif_excite*t);
% 计算速度 (使用差分近似，避免相位误差)
dx = gradient(x) * fs;

% --- 3. 求解 Bouc-Wen 模型 ---
% 初始化状态变量 z
z = zeros(size(t));
Fmr = zeros(size(t));

% 简单的欧拉积分法求解 dz/dt
for k = 2:length(t)
    % Bouc-Wen 磁滞演化方程
    dz = dx(k) - beta_babs(dx(k)abs(z(k-1))^nsign(z(k-1)) ...
         gamma_dx(k)abs(z(k-1))^n;
    z(k) = z(k-1) + dz * (1/fs);

    % 计算阻尼力
    Fmr(k) = c(dx(k) - 0) + k0(z(k) - x(k)) + alpha*z(k);
end

% --- 4. 绘图展示 (复现截图效果) ---
figure('Color', 'w');

% 左上：力-位移曲线 (滞回环)
subplot(2,2,1);
plot(x, Fmr, 'LineWidth', 1.5);
xlabel('位移 (m)');
ylabel('阻尼力 (N)');
title('力-位移滞回曲线');
grid on;
axis equal; % 保持纵横比，清晰显示曲线形状

% 右上：力-速度曲线
subplot(2,2,2);
plot(dx, Fmr, 'LineWidth', 1.5);
xlabel('速度 (m/s)');
ylabel('阻尼力 (N)');
title('力-速度关系曲线');
grid on;

% 左下：位移-时间曲线
subplot(2,2,3);
plot(t, x, 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('位移 (m)');
title('输入位移信号');
grid on;

% 右下：力-时间曲线
subplot(2,2,4);
plot(t, Fmr, 'LineWidth', 1.5);
xlabel('时间 (s)');
ylabel('阻尼力 (N)');
title('输出阻尼力响应');
grid on;

% 调整布局
set(gcf, 'Position', [100, 100, 1000, 800]);

end

代码逻辑解析

Bouc-Wen 动力学方程：代码中的核心是 dz 的计算部分。这部分代码实现了 Bouc-Wen 模型的非线性磁滞特性：
dot{z} = Adot{x} - beta|dot{x}||z|^{n}text{sgn}(z) - gammadot{x}|z|^{n}
通过改变 beta_b 和 gamma_b，你可以调整滞回环的饱满度（即阻尼器的耗能能力）。

积分求解：使用了简单的欧拉法（z(k) = z(k-1) + dz * dt）来求解状态变量 z。如果你的 Simulink 模型搭建好了，通常使用 Integrator 模块即可。

绘图技巧：
axis equal：在左上角的图中非常重要，它确保 X 轴和 Y 轴的单位长度相同，这样滞回环的“S”型或“8”字形特征才能清晰显示，不会被拉伸。
gradient：用于计算速度 dx。相比于 diff，gradient 能更好地保持数据点的数量对齐，避免绘图时维度不匹配。

如何在 Simulink 中使用

将 x 和 dx 作为输入端口。
使用 MATLAB Function Block 封装 dz 和 Fmr 的计算逻辑。
使用 Integrator 模块对 dz 积分得到 z。
将 Fmr 作为输出端口连接到 Scope 或 XY Graph。