1. 项目概述
作为一名无人机控制系统的开发者,我最近在AirSim仿真环境中实现了一套完整的四旋翼无人机控制系统。这个项目基于PX4飞控的核心算法,结合浙大开源代码进行了改进,删除了硬件相关部分和状态机结构,专注于控制算法的实现与优化。
系统采用串级控制结构,包含位置环、速度环、姿态环和角速度环,最终输出PWM信号控制无人机。输入数据包括IMU(100Hz)、里程计(100Hz,由GPS和IMU通过卡尔曼滤波融合得到)以及来自Ego-planner的目标点/轨迹。输出为0~1范围的PWM信号,控制频率为50Hz。
2. 系统架构设计
2.1 控制模块划分
整个控制系统采用经典的串级控制架构,分为四个主要环节:
- 位置/速度环:处理目标位置与当前状态的偏差,生成期望加速度
- 姿态环:将期望加速度转换为机体姿态(四元数表示)
- 角速度环:控制机体达到期望姿态的旋转速率
- PWM输出:将控制指令分配到四个电机
这种分层设计的好处是每层只需关注单一控制目标,简化了参数整定过程,同时保证了系统的稳定性。
2.2 通信与接口
系统通过AirSim API与仿真环境交互。AirSim启动后会开启一个本地server,控制器通过这个server获取传感器数据并发送控制指令。开发环境为Ubuntu 20.04和ROS1。
注意:AirSim使用NED坐标系(北-东-下),而ROS通常使用ENU坐标系(东-北-上)。这个差异会导致很多问题,特别是在姿态控制环节,需要特别注意坐标转换。
3. 控制器实现详解
3.1 位置/速度环控制
位置环采用P控制,将位置误差转换为目标速度:
cpp复制Eigen::Vector3d pos_err = pos_sp - odom.p;
Eigen::Vector3d target_vel = vel_sp + Kp_pos.cwiseProduct(pos_err);
速度环采用PID控制,增加了积分项以消除稳态误差:
cpp复制// 积分项更新 (限幅防饱和)
for (int i=0; i<3; i++) {
vel_int_(i) += Ki_vel(i) * vel_err(i) * dt;
vel_int_(i) = std::max(-Max_I_vel(i), std::min(Max_I_vel(i), vel_int_(i)));
}
// 计算期望加速度 (P + I + 前馈)
Eigen::Vector3d target_acc = acc_sp + Kp_vel.cwiseProduct(vel_err) + vel_int_;
参数选择依据:
- 位置环带宽设为0.95 rad/s
- 速度环带宽设为2.5 rad/s
- 积分限幅防止积分饱和(约等于允许的最大持续侧向加速度补偿)
3.2 姿态环控制
姿态环采用四元数表示,核心是将期望加速度方向转换为机体姿态:
cpp复制// 根据期望加速度方向,计算目标机体姿态
Eigen::Vector3d z_b_des = target_acc.normalized();
Eigen::Vector3d x_c_des(std::cos(yaw_sp), std::sin(yaw_sp), 0.0);
Eigen::Vector3d y_b_des = z_b_des.cross(x_c_des).normalized();
Eigen::Vector3d x_b_des = y_b_des.cross(z_b_des).normalized();
Eigen::Matrix3d R_des;
R_des << x_b_des, y_b_des, z_b_des;
u.q = Eigen::Quaterniond(R_des);
关键修复:AirSim与ROS的坐标系差异导致姿态控制反向的问题。解决方案是在计算完目标四元数后,对Pitch角取负:
cpp复制Eigen::Vector3d euler = u.q.toRotationMatrix().eulerAngles(2, 1, 0); // Yaw, Pitch, Roll
u.q = Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(-euler(1), Eigen::Vector3d::UnitY()) *
Eigen::Vector3d::UnitX());
3.3 角速度环控制
角速度环采用PID控制,增加了前馈和抗积分饱和机制:
cpp复制// 角速度误差
Eigen::Vector3d rate_error = target_body_rates - current_w;
// 计算角加速度
Eigen::Vector3d angular_accel = (current_w - last_w_) / dt;
// 带抗饱和的积分更新
for (int i = 0; i < 3; i++) {
bool sat_pos = (norm_torque_(i) >= 1.0);
bool sat_neg = (norm_torque_(i) <= -1.0);
double current_err = rate_error(i);
if (sat_pos && current_err > 0) current_err = 0.0;
if (sat_neg && current_err < 0) current_err = 0.0;
double i_factor = current_err / (400.0 * M_PI / 180.0);
i_factor = std::max(0.0, 1.0 - i_factor * i_factor);
rate_int_(i) += i_factor * Ki_rate(i) * current_err * dt;
rate_int_(i) = std::max(-Max_I_rate(i), std::min(Max_I_rate(i), rate_int_(i)));
}
// 最终控制量计算
norm_torque_ = Kp_rate.cwiseProduct(rate_error)
+ rate_int_
- Kd_rate.cwiseProduct(angular_accel)
+ Kff_rate.cwiseProduct(target_body_rates);
3.4 控制分配与PWM输出
将控制量分配到四个电机,需要考虑X型四旋翼的混控矩阵:
cpp复制// 混控矩阵定义
const RotorMixer quad_x_config[4] = {
{ -0.707107, -0.707107, 1.000000, 1.000000 }, // 电机1
{ 0.707107, 0.707107, 1.000000, 1.000000 }, // 电机2
{ 0.707107, -0.707107, -1.000000, 1.000000 }, // 电机3
{ -0.707107, 0.707107, -1.000000, 1.000000 } // 电机4
};
// 计算各电机输出
for (int i = 0; i < 4; i++) {
outputs[i] = ctrl_roll * quad_x_config[i].roll_scale +
ctrl_pitch * quad_x_config[i].pitch_scale +
ctrl_yaw * quad_x_config[i].yaw_scale +
ctrl_thrust * quad_x_config[i].thrust_scale;
if (outputs[i] > max_output) {
max_output = outputs[i];
}
}
// 限幅处理
if (max_output > 1.0) {
double exceed_amt = max_output - 1.0;
ctrl_thrust = std::max(0.05, ctrl_thrust - exceed_amt);
// 重新计算输出
for (int i = 0; i < 4; i++) {
outputs[i] = ctrl_roll * quad_x_config[i].roll_scale +
ctrl_pitch * quad_x_config[i].pitch_scale +
ctrl_yaw * quad_x_config[i].yaw_scale +
ctrl_thrust * quad_x_config[i].thrust_scale;
}
}
// 最终PWM输出
for (int i = 0; i < 4; i++) {
pwm_out.pwm[i] = std::min(std::max(outputs[i], 0.05), 1.0);
}
4. 与EgoPlanner集成的问题与解决方案
4.1 坐标系不一致问题
AirSim使用NED坐标系,而EgoPlanner使用ENU坐标系,这导致了多个问题:
- 俯仰角方向相反
- 定位数据需要转换
- 雷达点云坐标系不匹配
解决方案是在控制器内部进行坐标转换,确保所有数据使用同一坐标系:
cpp复制// 定位数据转换示例
Eigen::Vector3d ned_to_enu(const Eigen::Vector3d& ned) {
return Eigen::Vector3d(ned.y(), ned.x(), -ned.z());
}
// 姿态数据转换示例
Eigen::Quaterniond ned_to_enu(const Eigen::Quaterniond& q_ned) {
Eigen::Matrix3d R = q_ned.toRotationMatrix();
Eigen::Matrix3d R_enu;
R_enu << R(1,1), R(1,0), -R(1,2),
R(0,1), R(0,0), -R(0,2),
-R(2,1),-R(2,0), R(2,2);
return Eigen::Quaterniond(R_enu);
}
4.2 建图优化
原EgoPlanner代码使用固定大小的数组存储地图,当地图较大时会崩溃。改进方案:
- 改用动态内存分配
- 实现地图的实时更新机制
- 增加地图压缩算法减少内存占用
cpp复制// 动态地图数据结构示例
class DynamicMap {
public:
DynamicMap(double resolution, double max_range)
: resolution_(resolution), max_range_(max_range) {}
void update(const pcl::PointCloud<pcl::PointXYZ>& cloud) {
// 动态更新地图数据
for (const auto& point : cloud) {
// 转换为地图坐标
Eigen::Vector3i idx = posToIndex(Eigen::Vector3d(point.x, point.y, point.z));
// 动态扩展地图
if (!map_data_.count(idx)) {
map_data_[idx] = true; // 标记为障碍物
}
}
}
private:
std::unordered_map<Eigen::Vector3i, bool> map_data_;
double resolution_;
double max_range_;
};
4.3 轨迹跟踪震动问题
初期集成时发现无人机在跟踪EgoPlanner生成的轨迹时会出现剧烈震动,主要原因是:
- 定位误差(特别是姿态角计算不准确)
- 坐标系转换不完整
- 控制参数不匹配
解决方案:
- 重新检查姿态角计算逻辑
- 确保所有坐标系转换一致
- 调整控制参数,特别是角速度环的增益
cpp复制// 修正后的姿态角计算
double fromQuaternion2yaw(Eigen::Quaterniond q) {
// 确保使用正确的坐标系转换
Eigen::Vector3d euler = q.toRotationMatrix().eulerAngles(2, 1, 0);
return euler(0); // yaw
}
5. 参数整定与性能优化
5.1 控制参数选择
控制参数基于物理模型推导得出,主要考虑因素包括:
- 无人机质量:0.9kg
- 轴距:0.18m
- 推重比:5.6
- 期望闭环带宽
cpp复制// 位置/速度环参数
Eigen::Vector3d Kp_pos{0.95, 0.95, 1.0}; // P_pos: [1/s]
Eigen::Vector3d Kp_vel{2.5, 2.5, 3.0}; // P_vel: [1/s]
Eigen::Vector3d Ki_vel{1.0, 1.0, 1.5}; // I_vel
Eigen::Vector3d Max_I_vel{2.0, 2.0, 3.0}; // 积分限幅
// 姿态环参数
Eigen::Vector3d Kp_att_base{5.0, 5.0, 2.0}; // 姿态P增益
Eigen::Vector3d max_rate{3.84, 3.84, 3.49}; // 最大角速度[rad/s]
double yaw_w = 0.4; // 偏航权重
// 角速度环参数
Eigen::Vector3d Kp_rate{0.0368, 0.0543, 0.4690}; // P增益
Eigen::Vector3d Ki_rate{0.3680, 0.5430, 1.0000}; // I增益
Eigen::Vector3d Kd_rate{0.0010, 0.0010, 0.0}; // D增益
Eigen::Vector3d Max_I_rate{0.3, 0.3, 0.3}; // 积分限幅
5.2 性能优化技巧
-
传感器数据处理:
- IMU数据需要进行低通滤波去除高频噪声
- GPS数据可以与视觉里程计融合提高定位精度
- 使用卡尔曼滤波融合多传感器数据
-
实时性保证:
- 控制循环严格保持100Hz频率
- 使用高精度定时器
- 关键计算部分使用SIMD指令优化
-
调试工具:
- 实现丰富的调试信息输出
- 支持ROS话题发布关键数据
- 设计可视化工具监控控制过程
cpp复制// 调试信息发布示例
debug_msg_.des_v_x = pos_sp(0);
debug_msg_.des_v_y = pos_sp(1);
debug_msg_.des_v_z = pos_sp(2);
debug_msg_.des_a_x = odom.p(0);
debug_msg_.des_a_y = odom.p(1);
debug_msg_.des_a_z = odom.p(2);
debug_msg_.des_q_x = pos_sp(0) - odom.p(0);
debug_msg_.des_q_y = pos_sp(1) - odom.p(1);
debug_msg_.des_q_z = pos_sp(2) - odom.p(2);
debug_msg_.des_q_w = u.q.w();
debug_msg_.des_thr = ctrl_thrust;
6. 常见问题与解决方案
6.1 无人机响应迟钝
可能原因:
- 控制频率不足
- 传感器延迟过大
- 控制参数过于保守
解决方案:
- 确保控制循环运行在100Hz
- 检查传感器数据时间戳,处理延迟
- 适当增加位置环和速度环的增益
6.2 轨迹跟踪震荡
可能原因:
- 姿态环增益过高
- 角速度环积分饱和
- 传感器噪声过大
解决方案:
- 降低姿态环P增益
- 调整角速度环积分限幅
- 增加传感器滤波
6.3 紧急情况处理
设计应急处理机制:
- 失控检测:持续监控状态估计与控制指令的偏差
- 安全策略:超过阈值时切换为悬停模式
- 自动降落:电池电量不足或通信中断时执行
cpp复制// 安全监控示例
bool SafetyMonitor::checkEmergency(const Odom_Data_t& odom,
const Controller_Output_t& u) {
// 检查姿态异常
if (std::abs(u.q.toRotationMatrix().eulerAngles(2,1,0)[1]) > M_PI/4) {
return true; // 俯仰角超过45度
}
// 检查高度异常
if (odom.p.z() < 0.1 || odom.p.z() > 5.0) {
return true; // 高度异常
}
// 检查控制量饱和
if (u.thrust < 0.1 || u.thrust > 0.9) {
return true; // 推力饱和
}
return false;
}
7. 未来改进方向
-
算法升级:
- 引入模型预测控制(MPC)提高轨迹跟踪性能
- 实现传感器融合定位(Fasilio算法等)
- 增加自适应控制应对不同飞行条件
-
感知增强:
- 融合多传感器点云数据(激光雷达+双目视觉)
- 改进避障算法处理动态障碍物
- 实现语义分割辅助导航
-
系统优化:
- 移植到ROS2提升实时性能
- 支持硬件在环(HIL)测试
- 开发更完善的仿真测试场景
在实际开发过程中,我发现无人机控制系统是一个复杂的多学科交叉领域,需要平衡理论分析与工程实践。特别是当集成不同来源的算法模块时,坐标系、接口协议、时间同步等细节问题往往会成为主要挑战。建议在项目初期就建立完善的测试框架和调试工具,这将大大提升开发效率。