MATLAB 实现小车路径规划与轨迹跟踪控制

MATLAB路径规划仿真轨迹规划，船舶轨迹跟踪控制，数学模型基于两轮差速的小车模型，用PID环节对航向角进行控制，迫使小车走向目标，或用PID环节对航向角和距离进行控制，迫使小车走向目标 LQR 算法可自行小车起点坐标

在智能小车的研究领域，路径规划和轨迹跟踪控制是关键技术。今天咱们就来聊聊基于 MATLAB 的相关实现，这里涉及基于两轮差速的小车模型，以及 PID 控制和 LQR 算法。

两轮差速小车模型

两轮差速小车是常见的移动机器人模型。它通过左右轮的速度差来实现转向。假设小车的轴距为 \( L \)，左轮速度为 \( vl \)，右轮速度为 \( vr \)，那么小车的线速度 \( v \) 和角速度 \( \omega \) 可以表示为：

\[ v = \frac{vl + vr}{2} \]

\[ \omega = \frac{vr - vl}{L} \]

在 MATLAB 中，我们可以这样简单定义小车的模型参数：

matlab 复制代码

L = 0.3; % 轴距，单位：米

这里设定了小车的轴距为 0.3 米，后续计算小车运动状态时会用到这个参数。

PID 控制实现航向角控制

PID 控制是一种经典且应用广泛的控制算法。在小车路径规划中，我们可以用它来控制航向角，使小车走向目标。PID 控制器的输出 \( u \) 由比例（P）、积分（I）和微分（D）三部分组成，公式如下：

\[ u(t) = Kp e(t) + K i \int*{0}^{t} e(\tau) d\tau + K*d \frac{de(t)}{dt} \]

其中 \( e(t) \) 是误差，即目标值与当前值的差， \( Kp \)、 \( Ki \)、 \( K_d \) 分别是比例、积分、微分系数。

假设我们已经知道小车当前航向角 \( \theta \) 和目标航向角 \( \theta*{target} \)，误差 \( e = \theta*{target} - \theta \)。在 MATLAB 中实现一个简单的离散 PID 控制代码如下：

matlab 复制代码

% PID 参数
Kp = 1;
Ki = 0.1;
Kd = 0.01;
dt = 0.01; % 时间步长

e_prev = 0;
I = 0;
for k = 1:num_steps
    e = theta_target - theta(k);
    P_term = Kp * e;
    I = I + e * dt;
    I_term = Ki * I;
    D_term = Kd * (e - e_prev) / dt;
    u(k) = P_term + I_term + D_term;
    e_prev = e;
    % 根据控制量 u(k) 更新小车状态，这里省略具体更新代码
end

上述代码初始化了 PID 的三个参数 \( Kp \)、 \( K i \)、 \( Kd \) 以及时间步长 \( dt \)。在循环中，计算每一步的误差 \( e \)，然后分别算出比例项 \( P term \)、积分项 \( Iterm \) 和微分项 \( Dterm \)，最终得到控制量 \( u(k) \)。不过这里省略了根据控制量更新小车实际状态的具体代码，实际应用中需要根据小车运动学模型来实现。

PID 控制实现航向角和距离控制

除了单纯控制航向角，我们还可以同时对航向角和距离进行控制。假设小车当前位置 \( (x, y) \)，目标位置 \( (x*{target}, y* {target}) \)，首先计算距离误差 \( d*{error} = \sqrt{(x*{target} - x)^2 + (y_{target} - y)^2} \)。然后结合航向角误差，综合计算 PID 的误差项。

matlab 复制代码

% 假设小车当前位置 (x, y) 和目标位置 (x_target, y_target)
x = 0; y = 0; % 初始位置
x_target = 5; y_target = 5; % 目标位置
d_error = sqrt((x_target - x)^2 + (y_target - y)^2);
theta_error = theta_target - theta;
% 综合误差计算，这里简单加权相加，实际需调整权重
total_error = 0.5 * d_error + 0.5 * theta_error;

在这段代码中，我们先计算了距离误差 \( d*{error} \) 和航向角误差 \( theta*error \)，然后通过简单加权得到综合误差 \( total_error \)，实际应用中需要根据具体情况调整权重以达到更好的控制效果。之后就可以像之前一样，基于这个综合误差进行 PID 控制计算。

LQR 算法

LQR（线性二次型调节器）是一种最优控制算法。它通过求解 Riccati 方程，找到使二次型性能指标最小化的反馈控制律。对于线性化后的两轮差速小车模型，状态方程可以表示为 \( \dot{\mathbf{x}} = \mathbf{A}\mathbf{x} + \mathbf{B}\mathbf{u} \)，其中 \( \mathbf{x} \) 是状态向量，包含位置、速度、航向角等信息， \( \mathbf{u} \) 是控制输入向量， \( \mathbf{A} \) 和 \( \mathbf{B} \) 是系统矩阵。

在 MATLAB 中使用 LQR 函数非常方便，假设我们已经定义好 \( \mathbf{A} \)、 \( \mathbf{B} \) 矩阵，以及权重矩阵 \( \mathbf{Q} \) 和 \( \mathbf{R} \)：

matlab 复制代码

% 假设已定义 A, B 矩阵
Q = [1 0 0; 0 1 0; 0 0 1]; % 状态权重矩阵
R = 0.1; % 控制输入权重矩阵
[K, S, E] = lqr(A, B, Q, R);

上述代码调用 lqr 函数，传入系统矩阵 \( \mathbf{A} \)、 \( \mathbf{B} \) 以及权重矩阵 \( \mathbf{Q} \)、 \( \mathbf{R} \)，得到反馈增益矩阵 \( K \)，后续就可以根据这个增益矩阵来计算控制输入 \( \mathbf{u} = -K\mathbf{x} \)。

设定小车起点坐标并仿真

我们可以自行设定小车的起点坐标，假设起点为 \( (0, 0) \)，目标点为 \( (5, 5) \)。在 MATLAB 中可以结合上述控制算法进行路径规划的仿真。这里以简单的基于 PID 控制航向角和距离的情况为例：

matlab 复制代码

% 设定起点和目标点
start_x = 0; start_y = 0;
target_x = 5; target_y = 5;
% 初始化小车状态
x = start_x; y = start_y;
theta = 0; % 初始航向角
% 仿真循环
num_steps = 1000;
for k = 1:num_steps
    % 计算距离和航向角误差
    d_error = sqrt((target_x - x)^2 + (target_y - y)^2);
    theta_error = atan2(target_y - y, target_x - x) - theta;
    total_error = 0.5 * d_error + 0.5 * theta_error;
    % PID 控制计算
    % 省略具体 PID 计算代码，参考前面 PID 控制部分
    % 根据控制量更新小车状态
    % 省略具体更新代码
    % 记录小车位置，用于绘图
    x_history(k) = x;
    y_history(k) = y;
end
% 绘图展示小车轨迹
figure;
plot(x_history, y_history);
xlabel('X 坐标');
ylabel('Y 坐标');
title('小车路径规划轨迹');

这段代码设定了小车起点和目标点，在仿真循环中不断计算误差，通过 PID 控制得到控制量并更新小车状态，同时记录小车位置，最后绘图展示小车的轨迹。

通过以上方法，我们在 MATLAB 中实现了基于两轮差速小车模型的路径规划与轨迹跟踪控制，PID 控制和 LQR 算法都为小车智能行驶提供了有效的手段，希望这些内容能为你的研究或项目带来帮助。

以上代码仅为示意性代码，实际应用中需根据具体需求和模型进行完善与调试。