MATLAB路径规划仿真 轨迹规划，船舶轨迹跟踪控制，数学模

MATLAB路径规划仿真 轨迹规划，船舶轨迹跟踪控制，数学模

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

以下是基于两轮差速小车模型的路径规划和轨迹跟踪控制代码。我们将使用两种方法实现：

1. PID控制：通过PID控制器对航向角进行控制，使小车走向目标。
2. LQR算法：通过线性二次型调节器（LQR）优化控制输入，使小车跟踪目标轨迹。

---

1. 基于PID控制的轨迹跟踪

数学模型

两轮差速小车的动力学模型可以简化为：
[
\dot{x} = v \cos(\theta), \quad \dot{y} = v \sin(\theta), \quad \dot{\theta} = \frac{v}{L}(u_r - u_l)
]
其中：

• (x, y) 是小车的位置，
• (\theta) 是小车的航向角，
• (v) 是小车的速度，
• (L) 是轮距，
• (u_r, u_l) 是左右轮的速度。

我们可以将问题转化为通过PID控制器调整航向角误差，使小车朝向目标点。

Matlab代码

% PID控制的小车轨迹跟踪
clear; clc;

% 参数设置
dt = 0.1; % 时间步长
T = 20; % 总仿真时间
L = 0.5; % 小车轮距
v = 1.0; % 小车速度 (m/s)

% 初始状态 [x, y, theta]
state = [0; 0; 0]; % 起点坐标和初始航向角
target = [10; 10]; % 目标点坐标

% PID参数
Kp = 1.0;
Ki = 0.01;
Kd = 0.1;

% 初始化变量
x_history = state(1);
y_history = state(2);
theta_history = state(3);

integral_error = 0;
prev_error = 0;

for t = 0:dt:T
    % 计算误差
    dx = target(1) - state(1);
    dy = target(2) - state(2);
    target_angle = atan2(dy, dx); % 目标航向角
    error = angle_diff(target_angle, state(3)); % 航向角误差
    
    % PID控制
    integral_error = integral_error + error * dt;
    derivative_error = (error - prev_error) / dt;
    omega = Kp * error + Ki * integral_error + Kd * derivative_error; % 角速度
    
    % 更新状态
    state(3) = state(3) + omega * dt; % 更新航向角
    state(1) = state(1) + v * cos(state(3)) * dt; % 更新x
    state(2) = state(2) + v * sin(state(3)) * dt; % 更新y
    
    % 记录轨迹
    x_history = [x_history, state(1)];
    y_history = [y_history, state(2)];
    theta_history = [theta_history, state(3)];
    
    % 更新误差
    prev_error = error;
end

% 绘制结果
figure;
plot(x_history, y_history, 'b-', 'LineWidth', 2);
hold on;
plot(target(1), target(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
xlabel('X Position (m)');
ylabel('Y Position (m)');
title('PID-Based Trajectory Tracking');
legend('Trajectory', 'Target');
grid on;
axis equal;

% 辅助函数：计算角度差
function diff = angle_diff(angle1, angle2)
    diff = mod(angle1 - angle2 + pi, 2*pi) - pi;
end

---

2. 基于LQR算法的轨迹跟踪

数学模型

我们可以通过离散化的状态空间模型表示小车动力学：
[
\begin{bmatrix}
x_{k+1} \
y_{k+1} \
\theta_{k+1}
\end{bmatrix}

\begin{bmatrix}
1 & 0 & -v \sin(\theta_k) \Delta t \
0 & 1 & v \cos(\theta_k) \Delta t \
0 & 0 & 1
\end{bmatrix}
\begin{bmatrix}
x_k \
y_k \
\theta_k
\end{bmatrix}
+
\begin{bmatrix}
\cos(\theta_k) \Delta t \
\sin(\theta_k) \Delta t \
\frac{\Delta t}{L}
\end{bmatrix}
u_k
]

Matlab代码

% LQR控制的小车轨迹跟踪
clear; clc;

% 参数设置
dt = 0.1; % 时间步长
T = 20; % 总仿真时间
L = 0.5; % 小车轮距
v = 1.0; % 小车速度 (m/s)

% 初始状态 [x, y, theta]
state = [0; 0; 0]; % 起点坐标和初始航向角
target = [10; 10]; % 目标点坐标

% LQR权重矩阵
Q = diag([1, 1, 0.1]); % 状态权重
R = 0.1; % 控制输入权重

% 初始化变量
x_history = state(1);
y_history = state(2);
theta_history = state(3);

for t = 0:dt:T
    % 计算误差
    dx = target(1) - state(1);
    dy = target(2) - state(2);
    error = [dx; dy; angle_diff(atan2(dy, dx), state(3))];
    
    % 离散化状态空间模型
    A = [1 0 -v*sin(state(3))*dt; ...
         0 1 v*cos(state(3))*dt; ...
         0 0 1];
    B = [cos(state(3))*dt; sin(state(3))*dt; dt/L];
    
    % 计算LQR增益矩阵
    K = dlqr(A, B, Q, R);
    
    % 控制输入
    u = -K * error;
    
    % 更新状态
    state = A * state + B * u;
    
    % 记录轨迹
    x_history = [x_history, state(1)];
    y_history = [y_history, state(2)];
    theta_history = [theta_history, state(3)];
end

% 绘制结果
figure;
plot(x_history, y_history, 'b-', 'LineWidth', 2);
hold on;
plot(target(1), target(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
xlabel('X Position (m)');
ylabel('Y Position (m)');
title('LQR-Based Trajectory Tracking');
legend('Trajectory', 'Target');
grid on;
axis equal;

% 辅助函数：计算角度差
function diff = angle_diff(angle1, angle2)
    diff = mod(angle1 - angle2 + pi, 2*pi) - pi;
end

---

结果说明

1. PID控制：适用于简单的航向角误差修正，适合初学者理解。
2. LQR控制：通过优化状态和控制输入，提供更平滑的轨迹跟踪效果。

假设初始航向角为 (\pi/8)（约22.5度），目标点为 ((10, 10))。

% 参数设置
dt = 0.1; % 时间步长
T = 20; % 总仿真时间
L = 0.5; % 小车轮距
v = 1.0; % 小车速度 (m/s)

% 初始状态 [x, y, theta]
state = [0; 0; pi/8]; % 起点坐标和初始航向角
target = [10; 10]; % 目标点坐标

% PID参数
Kp = 1.0;
Ki = 0.01;
Kd = 0.1;

% 初始化变量
x_history = state(1);
y_history = state(2);
theta_history = state(3);

integral_error = 0;
prev_error = 0;

for t = 0:dt:T
    % 计算误差
    dx = target(1) - state(1);
    dy = target(2) - state(2);
    target_angle = atan2(dy, dx); % 目标航向角
    error = angle_diff(target_angle, state(3)); % 航向角误差
    
    % PID控制
    integral_error = integral_error + error * dt;
    derivative_error = (error - prev_error) / dt;
    omega = Kp * error + Ki * integral_error + Kd * derivative_error; % 角速度
    
    % 更新状态
    state(3) = state(3) + omega * dt; % 更新航向角
    state(1) = state(1) + v * cos(state(3)) * dt; % 更新x
    state(2) = state(2) + v * sin(state(3)) * dt; % 更新y
    
    % 记录轨迹
    x_history = [x_history, state(1)];
    y_history = [y_history, state(2)];
    theta_history = [theta_history, state(3)];
    
    % 更新误差
    prev_error = error;
end

% 绘制结果
figure;
plot(x_history, y_history, 'b-', 'LineWidth', 2);
hold on;
plot(target(1), target(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
xlabel('X Position (m)');
ylabel('Y Position (m)');
title('PID-Based Trajectory Tracking');
legend('Trajectory', 'Target');
grid on;
axis equal;

% 辅助函数：计算角度差
function diff = angle_diff(angle1, angle2)
    diff = mod(angle1 - angle2 + pi, 2*pi) - pi;
end

代码解释：

1. 参数设置:

   • dt: 时间步长。
   • T: 总仿真时间。
   • L: 小车轮距。
   • v: 小车速度。
   • state: 初始状态 [x, y, theta]。
   • target: 目标点坐标。

2. PID参数:

   • Kp, Ki, Kd: 比例、积分、微分增益。

3. 初始化变量:

   • x_history, y_history, theta_history: 存储轨迹历史。
   • integral_error, prev_error: 积分误差和前一时刻的误差。

4. 主循环:

   • 计算当前位置到目标点的误差。
   • 使用PID控制器计算角速度 omega。
   • 更新小车的状态。
   • 记录轨迹。

5. 绘制结果:

   • 绘制小车的轨迹和目标点。

6. 辅助函数:

   • angle_diff: 计算两个角度之间的最小角度差。

假设初始航向角为 (\pi/20)（约9度），目标点为 ((10, 10))。

以下是完整的MATLAB代码：

% 参数设置
dt = 0.1; % 时间步长
T = 20; % 总仿真时间
L = 0.5; % 小车轮距
v = 1.0; % 小车速度 (m/s)

% 初始状态 [x, y, theta]
state = [0; 0; pi/20]; % 起点坐标和初始航向角
target = [10; 10]; % 目标点坐标

% PID参数
Kp = 1.0;
Ki = 0.01;
Kd = 0.1;

% 初始化变量
x_history = state(1);
y_history = state(2);
theta_history = state(3);

integral_error = 0;
prev_error = 0;

for t = 0:dt:T
    % 计算误差
    dx = target(1) - state(1);
    dy = target(2) - state(2);
    target_angle = atan2(dy, dx); % 目标航向角
    error = angle_diff(target_angle, state(3)); % 航向角误差
    
    % PID控制
    integral_error = integral_error + error * dt;
    derivative_error = (error - prev_error) / dt;
    omega = Kp * error + Ki * integral_error + Kd * derivative_error; % 角速度
    
    % 更新状态
    state(3) = state(3) + omega * dt; % 更新航向角
    state(1) = state(1) + v * cos(state(3)) * dt; % 更新x
    state(2) = state(2) + v * sin(state(3)) * dt; % 更新y
    
    % 记录轨迹
    x_history = [x_history, state(1)];
    y_history = [y_history, state(2)];
    theta_history = [theta_history, state(3)];
    
    % 更新误差
    prev_error = error;
end

% 绘制结果
figure;
plot(x_history, y_history, 'b-', 'LineWidth', 2);
hold on;
plot(target(1), target(2), 'ro', 'MarkerSize', 10, 'MarkerFaceColor', 'r');
xlabel('X Position (m)');
ylabel('Y Position (m)');
title('PID-Based Trajectory Tracking');
legend('Trajectory', 'Target');
grid on;
axis equal;

% 辅助函数：计算角度差
function diff = angle_diff(angle1, angle2)
    diff = mod(angle1 - angle2 + pi, 2*pi) - pi;
end

代码解释：

1. 参数设置:

   • dt: 时间步长。
   • T: 总仿真时间。
   • L: 小车轮距。
   • v: 小车速度。
   • state: 初始状态 [x, y, theta]。
   • target: 目标点坐标。

2. PID参数:

   • Kp, Ki, Kd: 比例、积分、微分增益。

3. 初始化变量:

   • x_history, y_history, theta_history: 存储轨迹历史。
   • integral_error, prev_error: 积分误差和前一时刻的误差。

4. 主循环:

   • 计算当前位置到目标点的误差。
   • 使用PID控制器计算角速度 omega。
   • 更新小车的状态。
   • 记录轨迹。

5. 绘制结果:

   • 绘制小车的轨迹和目标点。

6. 辅助函数:

   • angle_diff: 计算两个角度之间的最小角度差。