1. 四旋翼无人机系统概述
四旋翼无人机作为典型的欠驱动系统,其动力学特性一直是控制领域的研究热点。这类飞行器通过四个旋翼转速的协调控制实现六自由度的空间运动,具有结构简单、机动性强等特点,在航拍、巡检、物流等领域有广泛应用。
我从事无人机控制系统开发已有七年时间,从早期的PID调参到现在的现代控制方法应用,深刻体会到建立准确数学模型的重要性。一个完整的四旋翼控制系统通常包含以下几个核心模块:
- 动力学建模:建立姿态与位置的运动方程
- 状态估计:通过传感器数据获取飞行器状态
- 控制算法:生成电机控制指令
- 仿真验证:在虚拟环境中测试算法性能
本次分享将重点介绍基于MATLAB的完整实现方案,包含从建模到控制的全部流程。所有代码都经过实际仿真验证,可直接用于学术研究或工程开发。
2. 四旋翼动力学建模
2.1 坐标系定义与转换
建立准确的动力学模型首先需要明确定义坐标系系统。我们通常采用以下两种坐标系:
-
地面惯性坐标系(I系):
- 原点:地面任意固定点
- X轴:指向地理北向
- Y轴:指向地理东向
- Z轴:垂直地面向下
-
机体坐标系(B系):
- 原点:无人机质心
- X轴:指向机头方向
- Y轴:指向右侧机臂
- Z轴:垂直机身向下
两坐标系间的转换通过旋转矩阵实现:
matlab复制% 欧拉角到旋转矩阵的转换
function R = euler2rotmat(phi, theta, psi)
R = [cos(psi)*cos(theta) - sin(psi)*cos(phi)+cos(psi)*sin(theta)*sin(phi) ...
sin(psi)*sin(phi)+cos(psi)*sin(theta)*cos(phi);
sin(psi)*cos(theta) cos(psi)*cos(phi)+sin(psi)*sin(theta)*sin(phi) ...
-cos(psi)*sin(phi)+sin(psi)*sin(theta)*cos(phi);
-sin(theta) cos(theta)*sin(phi) cos(theta)*cos(phi)];
end
2.2 刚体动力学方程
考虑四旋翼为刚体,其动力学方程可分为平移运动和旋转运动两部分:
-
平移动力学:
[
m\ddot{\mathbf{p}} = m\mathbf{g} + \mathbf{R}\mathbf{F}
]
其中:- ( m ):无人机质量
- ( \mathbf{p} ):位置向量
- ( \mathbf{g} ):重力加速度
- ( \mathbf{R} ):旋转矩阵
- ( \mathbf{F} ):机体坐标系下的总推力
-
旋转动力学:
[
\mathbf{I}\dot{\boldsymbol{\omega}} + \boldsymbol{\omega}\times\mathbf{I}\boldsymbol{\omega} = \mathbf{M}
]
其中:- ( \mathbf{I} ):惯性矩阵
- ( \boldsymbol{\omega} ):角速度向量
- ( \mathbf{M} ):总力矩
在MATLAB中实现时,需要注意以下几点:
- 惯性矩阵应考虑电机和螺旋桨的转动惯量
- 对于小型无人机,科里奥利力项通常可以忽略
- 坐标系转换时要确保方向一致性
3. 带积分动作的LQR控制器设计
3.1 系统线性化
LQR控制要求系统在平衡点附近线性化。对于四旋翼,我们选择悬停状态作为平衡点:
-
悬停状态定义:
- 滚转/俯仰角:0°
- 偏航角:任意固定值
- 高度:恒定
- 水平速度:0
-
状态变量选择:
[
\mathbf{x} = [x \ y \ z \ \phi \ \theta \ \psi \ \dot{x} \ \dot{y} \ \dot{z} \ p \ q \ r]^T
] -
控制输入:
[
\mathbf{u} = [U_1 \ U_2 \ U_3 \ U_4]^T
]
分别对应总推力和三个轴向力矩
线性化后的状态空间方程:
matlab复制A = [zeros(6) eye(6);
zeros(3,9) [0 g 0;-g 0 0;0 0 0];
zeros(3,12)];
B = [zeros(6,4);
[0 0 0 0; 0 0 0 0; 1/m 0 0 0];
[0 1/Ixx 0 0; 0 0 1/Iyy 0; 0 0 0 1/Izz]];
3.2 积分动作引入
为消除稳态误差,我们在LQR中引入积分环节:
-
扩展状态向量:
[
\mathbf{x}_{ext} = \left[\begin{array}{c}
\mathbf{x} \
\int e
\end{array}\right]
]
其中( e )为输出误差 -
扩展后的系统方程:
[
\dot{\mathbf{x}}{ext} = \left[\begin{array}{cc}
A & 0 \
-C & 0
\end{array}\right]\mathbf{x} + \left[\begin{array}{c}
B \
0
\end{array}\right]\mathbf{u}
]
MATLAB实现代码片段:
matlab复制% 扩展系统矩阵
A_ext = [A zeros(12,4); -C zeros(4)];
B_ext = [B; zeros(4)];
% 设计LQR权重矩阵
Q = diag([10 10 20 5 5 1 1 1 1 1 1 1 50 50 100 1]);
R = diag([0.1 1 1 1]);
% 求解Riccati方程
[K_ext,~,~] = lqr(A_ext, B_ext, Q, R);
K = K_ext(:,1:12);
Ki = K_ext(:,13:end);
3.3 控制器实现要点
在实际应用中需要注意:
-
积分抗饱和处理:
- 设置积分限幅
- 在误差较大时暂停积分
- 使用clamping方法防止windup
-
权重矩阵调整经验:
- 先调整Q中对角元素,再微调R
- 位置相关权重通常大于姿态
- 积分项权重应适当大于状态项
-
采样时间选择:
- 通常取10-50ms
- 需要与传感器更新率匹配
- 太短会导致计算负担,太长影响性能
4. 扩展卡尔曼滤波(EKF)状态估计
4.1 传感器模型
典型四旋翼传感器配置包括:
-
IMU(惯性测量单元):
- 三轴加速度计
- 三轴陀螺仪
- 测量模型:
[
\mathbf{a}_{meas} = \mathbf{a} + \mathbf{b}_a + \mathbf{n}a
]
[
\boldsymbol{\omega} = \boldsymbol{\omega} + \mathbf{b}_g + \mathbf{n}_g
]
-
气压计:
- 测量高度
- 易受气流扰动影响
-
磁力计:
- 测量地磁方向
- 易受电子设备干扰
4.2 EKF算法流程
EKF分为预测和更新两个步骤:
-
预测步骤:
[
\hat{\mathbf{x}}k^- = f(\hat{\mathbf{x}}, \mathbf{u}{k-1})
]
[
\mathbf{P}k^- = \mathbf{F}\mathbf{P}\mathbf{F}_{k-1}^T + \mathbf{Q}
] -
更新步骤:
[
\mathbf{K}_k = \mathbf{P}_k^-\mathbf{H}_k^T(\mathbf{H}_k\mathbf{P}_k^-\mathbf{H}_k^T + \mathbf{R})^{-1}
]
[
\hat{\mathbf{x}}_k = \hat{\mathbf{x}}_k^- + \mathbf{K}_k(\mathbf{z}_k - h(\hat{\mathbf{x}}_k^-))
]
[
\mathbf{P}_k = (\mathbf{I} - \mathbf{K}_k\mathbf{H}_k)\mathbf{P}_k^-
]
MATLAB实现关键代码:
matlab复制function [x_est, P] = ekf_update(x_pred, P_pred, z, Q, R)
% 状态转移雅可比
F = compute_jacobian_f(x_pred);
% 观测雅可比
H = compute_jacobian_h(x_pred);
% 卡尔曼增益
K = P_pred * H' / (H * P_pred * H' + R);
% 状态更新
x_est = x_pred + K * (z - h(x_pred));
% 协方差更新
P = (eye(length(x_pred)) - K * H) * P_pred;
% 确保对称性
P = (P + P') / 2;
end
4.3 实现注意事项
-
初始状态不确定性设置:
- 位置不确定性:1-2m
- 速度不确定性:0.5m/s
- 姿态不确定性:5-10°
-
过程噪声Q调整:
- 通常取对角矩阵
- 线性加速度噪声:0.1-0.5 m/s²
- 角速度噪声:0.01-0.05 rad/s
-
观测噪声R调整:
- 加速度计:0.05-0.2 m/s²
- 陀螺仪:0.005-0.01 rad/s
- 气压计:0.5-1 m
5. 非线性动力学仿真
5.1 仿真框架搭建
完整的仿真系统应包含以下模块:
- 动力学模型
- 传感器模拟
- 状态估计器
- 控制器
- 可视化
MATLAB仿真主循环结构:
matlab复制% 初始化
x = x0;
x_est = x0_est;
P = P0;
for k = 1:N
% 控制计算
u = controller(x_est, ref);
% 动力学更新
x = dynamics(x, u, dt);
% 传感器模拟
z = sensor_model(x);
% 状态估计
[x_est, P] = ekf(x_est, P, z, u, Q, R, dt);
% 数据记录
log_data(k) = pack_data(x, x_est, u, z);
% 实时可视化
if mod(k,10)==0
update_plot(log_data(k));
end
end
5.2 典型仿真场景
-
悬停测试:
- 评估稳态性能
- 检查积分项效果
- 观察估计误差
-
阶跃响应:
- 位置阶跃
- 姿态阶跃
- 评估动态性能
-
轨迹跟踪:
- 圆形轨迹
- 8字形轨迹
- 评估跟踪精度
5.3 仿真结果分析要点
-
时域指标:
- 上升时间
- 超调量
- 稳态误差
-
频域指标(通过FFT分析):
- 带宽
- 相位裕度
- 抗干扰能力
-
状态估计性能:
- 估计误差统计
- 收敛速度
- 对噪声的敏感性
6. 实际应用中的经验分享
6.1 参数调试技巧
-
质量与惯性参数测量:
- 使用精密电子秤测量质量
- 通过摆动实验估计惯性矩
- 考虑电池电量变化的影响
-
电机特性标定:
- 静态推力测试
- 转速-油门曲线拟合
- 考虑电池电压补偿
-
控制器调试步骤:
- 先调姿态环,再调位置环
- 先比例,后积分
- 室内测试时降低增益
6.2 常见问题排查
-
发散振荡:
- 检查传感器延时
- 降低控制器增益
- 增加滤波器截止频率
-
稳态误差:
- 检查积分项是否生效
- 验证模型准确性
- 检查执行器饱和
-
估计偏差:
- 重新校准传感器
- 调整噪声协方差
- 检查初始状态设置
6.3 性能优化建议
-
代码实现优化:
- 使用预先计算的雅可比矩阵
- 矩阵运算向量化
- 减少动态内存分配
-
多速率处理:
- IMU高频处理(500Hz-1kHz)
- 视觉/位置中频更新(20-50Hz)
- 控制器中频运行(100-200Hz)
-
硬件选择建议:
- 选择低噪声IMU
- 确保传感器同步
- 考虑计算单元性能