基于非线性MPC的无人机轨迹跟踪控制系统

MATLAB基于非线性MPC的无人机轨迹跟踪控制系统
商品内容

本项目为完整的无人机三维轨迹跟踪与姿态控制系统，采用非线性模型预测控制（MPC）算法，结合状态反馈线性化与LPV建模，实现在 MATLAB 中的动态飞行控制仿真与可视化。

无人机三维轨迹跟踪与姿态控制是一个复杂的系统工程，涉及到动力学建模、控制器设计、仿真验证等多个方面。非线性模型预测控制（MPC）是一种强大的控制策略，特别适用于具有多输入多输出（MIMO）、非线性特性的系统，如无人机。

】

1. 动力学模型

首先，我们需要建立无人机的动力学模型。为了简化问题，我们假设无人机是一个六自由度（6-DOF）刚体，其运动方程可以表示为：

$$\begin{array}{rl}\dot{x}& =v_x\\ \dot{y}& =v_y\\ \dot{z}& =v_z\\ {\dot{v}}_x& =\frac{T}{m}\cdot (\sin (\varphi )\cos (\theta )+\sin (\psi )\cos (\varphi )\sin (\theta ))\\ {\dot{v}}_y& =\frac{T}{m}\cdot (-\sin (\varphi )\cos (\psi )+\sin (\theta )\cos (\varphi )\cos (\psi ))\\ {\dot{v}}_z& =\frac{T}{m}\cdot (-\cos (\varphi )\cos (\theta ))-g\end{array}$$

其中：

• $ x, y, z $ 是无人机在三维空间中的位置；
• $ v_x, v_y, v_z $ 是速度；
• $ T $ 是总推力；
• $ m $ 是无人机的质量；
• $ \phi, \theta, \psi $ 分别是滚转角、俯仰角和偏航角；
• $ g $ 是重力加速度。

2. 控制器设计：非线性MPC

我们将使用非线性MPC来控制无人机沿着给定的三维轨迹飞行。MPC的核心思想是在每个时间步长内求解一个优化问题，以预测未来的系统行为并计算最优控制输入。

MATLAB代码示例

% 非线性MPC无人机轨迹跟踪控制系统
clear; clc; close all;

% 参数设置
m = 1.0;          % 无人机质量 (kg)
g = 9.81;         % 重力加速度 (m/s^2)
dt = 0.1;         % 时间步长 (s)
T_sim = 30;       % 总仿真时间 (s)
N = 10;           % 预测步数

% 初始状态 [x, y, z, vx, vy, vz]
initial_state = [0, 0, 0, 0, 0, 0];

% 目标轨迹参数
t_ref = 0:dt:T_sim;
x_ref = 5 * sin(0.1 * t_ref);   % x方向正弦波
y_ref = 5 * cos(0.1 * t_ref);   % y方向余弦波
z_ref = 2 * t_ref / T_sim;      % z方向线性上升

% 定义MPC优化问题
mpcobj = nlmpc(6, 4);  % 6个状态变量，4个控制输入（T, phi, theta, psi）

% 设置状态方程
mpcobj.Model.StateFcn = @(x, u, ~) stateEquations(x, u, m, g);

% 设置目标轨迹
mpcobj.Ts = dt;
mpcobj.PredictionHorizon = N;
mpcobj.ControlHorizon = 5;

% 设置权重矩阵
mpcobj.Weights.OutputVariables = [1, 1, 1, 0, 0, 0];  % 轨迹误差权重
mpcobj.Weights.ManipulatedVariablesRate = 0.1 * ones(1, 4);  % 控制变化率权重

% 设置初始猜测
u0 = [0.5, 0, 0, 0];  % 初始控制输入

% 初始化状态
x = initial_state;

% 存储结果
positions = zeros(length(t_ref), 3);
controls = zeros(length(t_ref), 4);

% 仿真循环
for k = 1:length(t_ref)
    % 获取当前参考轨迹点
    ref = [x_ref(k), y_ref(k), z_ref(k)];
    
    % 设置参考信号
    mpcobj.Model.Nominal.X = [ref, 0, 0, 0];
    
    % 求解MPC优化问题
    [u, info] = nlmpcmove(mpcobj, x, u0);
    
    % 更新状态
    x = ode45(@(t, x) stateEquations(x, u, m, g), [0 dt], x)';
    x = x(end, :);
    
    % 记录数据
    positions(k, :) = x(1:3);
    controls(k, :) = u;
    
    % 更新初始猜测
    u0 = u;
end

% 绘制结果
figure;
plot3(positions(:,1), positions(:,2), positions(:,3), 'b', 'LineWidth', 2);
hold on;
plot3(x_ref, y_ref, z_ref, 'r--', 'LineWidth', 2);
xlabel('X'); ylabel('Y'); zlabel('Z');
title('无人机三维轨迹跟踪');
legend('实际轨迹', '参考轨迹');
grid on;

% 状态方程函数
function dxdt = stateEquations(x, u, m, g)
    % 解析状态变量
    x_pos = x(1);
    y_pos = x(2);
    z_pos = x(3);
    vx = x(4);
    vy = x(5);
    vz = x(6);
    
    % 解析控制输入
    T = u(1);        % 推力
    phi = u(2);      % 滚转角
    theta = u(3);    % 俯仰角
    psi = u(4);      % 偏航角
    
    % 计算加速度
    ax = T/m * (sin(phi)*cos(theta) + sin(psi)*cos(phi)*sin(theta));
    ay = T/m * (-sin(phi)*cos(psi) + sin(theta)*cos(phi)*cos(psi));
    az = T/m * (-cos(phi)*cos(theta)) - g;
    
    % 返回导数
    dxdt = [vx; vy; vz; ax; ay; az];
end

3. 说明

• MPC控制器：本示例使用了MATLAB的nlmpc工具箱来构建非线性MPC控制器。nlmpc允许用户直接定义非线性状态方程，并通过优化算法求解最优控制输入。

• 状态方程：状态方程描述了无人机的动力学行为，包括位置、速度以及加速度的变化。

• 轨迹跟踪：参考轨迹被定义为一个三维路径（例如正弦波、余弦波和线性上升），MPC控制器会根据这个参考轨迹调整无人机的控制输入，使其尽可能接近目标轨迹。

• 权重矩阵：MPC控制器中的权重矩阵决定了轨迹误差和控制变化率的重要性。可以根据具体需求进行调整。

• 仿真结果：最终绘制出无人机的实际轨迹与参考轨迹的对比图，展示了控制器的性能。

4. 扩展功能

• 避障：可以在MPC的目标函数中加入障碍物规避项，通过惩罚靠近障碍物的行为来实现避障。

• LPV建模：如果需要更精确的控制，可以考虑使用线性参数变增益（LPV）建模方法，将非线性系统分解为多个局部线性模型，并在不同工作点之间进行切换。

• 可视化：可以使用MATLAB的动画功能（如animatedline或scatter）来实时显示无人机的飞行过程。

以下是一个基于PID控制的MATLAB代码示例，用于模拟无人机在z轴上的位置和速度跟踪：

% 无人机z轴位置和速度跟踪仿真

clear; clc; close all;

% 参数设置
m = 1.0;          % 无人机质量 (kg)
g = 9.81;         % 重力加速度 (m/s^2)
dt = 0.1;         % 时间步长 (s)
T_sim = 100;      % 总仿真时间 (s)

% PID控制器参数
Kp = 1.5;
Ki = 0.1;
Kd = 0.5;

% 初始状态 [z, z_dot]
initial_state = [0, 0];

% 目标轨迹参数
t_ref = 0:dt:T_sim;
z_ref = 5 * sin(0.2 * t_ref);   % z方向正弦波参考轨迹

% 初始化变量
z_position = zeros(size(t_ref));
z_velocity = zeros(size(t_ref));
integral_error = 0;
previous_error = 0;

% 仿真循环
for k = 1:length(t_ref)
    % 当前时间点
    t = t_ref(k);
    
    % 获取当前参考值
    z_ref_k = z_ref(k);
    
    % 计算误差
    error = z_ref_k - initial_state(1);
    
    % 积分项
    integral_error = integral_error + error * dt;
    
    % 微分项
    derivative_error = (error - previous_error) / dt;
    
    % PID控制律
    control_input = Kp * error + Ki * integral_error + Kd * derivative_error;
    
    % 更新状态
    z_acceleration = (control_input - m * g) / m;
    z_velocity(k) = initial_state(2) + z_acceleration * dt;
    z_position(k) = initial_state(1) + z_velocity(k) * dt;
    
    % 更新状态向量
    initial_state = [z_position(k), z_velocity(k)];
    
    % 更新上一时刻误差
    previous_error = error;
end

% 计算参考速度
z_dot_ref = diff([0; z_ref]) / dt;

% 绘制结果
figure;
subplot(2, 1, 1);
plot(t_ref, z_ref, 'b--', 'LineWidth', 2);
hold on;
plot(t_ref, z_position, 'r', 'LineWidth', 2);
xlabel('time [s]');
ylabel('z-position [m]');
title('z-position');
legend('z-ref', 'z-position');

subplot(2, 1, 2);
plot(t_ref, [0; z_dot_ref], 'b--', 'LineWidth', 2);
hold on;
plot(t_ref, z_velocity, 'r', 'LineWidth', 2);
xlabel('time [s]');
ylabel('z-velocity [m/s]');
title('z-velocity');
legend('z-dot-ref', 'z-velocity');

说明

• PID控制器：本示例使用了经典的PID控制策略来跟踪给定的参考轨迹。PID控制器通过调整比例、积分和微分项来最小化系统误差。

• 动力学模型：假设无人机在垂直方向上的运动只受到重力和控制输入的影响，其加速度可以通过牛顿第二定律计算得出。

• 参考轨迹：目标轨迹被定义为一个正弦波，表示无人机在z轴上的期望位置变化。

• 仿真结果：最终绘制出无人机的实际位置和速度与参考值的对比图，展示了控制器的性能。