Autopilot：轻量级固定翼无人机嵌入式飞控算法库

1. Autopilot 库概述面向固定翼无人机的嵌入式飞控算法集合Autopilot 是一个专为自主固定翼飞行器设计的轻量级、模块化控制器库。它不提供完整的飞控固件而是聚焦于核心控制律Control Laws的实现——即从传感器原始数据到执行机构指令的数学映射逻辑。该库以 C 语言编写无操作系统依赖可直接集成至裸机Bare-Metal环境或 FreeRTOS、Zephyr 等实时操作系统中适用于 STM32F4/F7/H7、nRF52840、RP2040 等主流 MCU 平台。其工程定位极为明确解耦控制算法与硬件抽象层HAL和调度框架。开发者无需重写 PID 参数整定逻辑或 LQR 状态反馈结构只需将autopilot_update()函数周期性调用典型周期为 100 Hz500 Hz并传入经校准的 IMU 角速率、加速度、气压高度、GPS 位置/速度、空速管差压等关键状态量库即输出标准化的舵面偏转指令Elevator, Aileron, Rudder, Throttle和姿态/航迹目标值。这种设计显著缩短了从理论控制律推导到实际飞行验证的迭代周期。与 ArduPilot 或 PX4 等全栈式飞控固件不同Autopilot 的核心价值在于“可验证性”与“可移植性”。所有控制器均以纯函数形式暴露无全局状态隐式依赖输入输出均为结构体指针便于单元测试与 SILSoftware-in-the-Loop仿真关键数学运算如四元数归一化、DCM 矩阵乘法采用定点或浮点混合实现兼顾精度与 Cortex-M4/M7 的 DSP 指令加速能力。对于需要满足 DO-178C B 级适航要求的工业级固定翼平台该库可作为经认证的“控制律组件”嵌入至更高层级的安全关键系统中。1.1 固定翼飞控的工程约束与 Autopilot 的应对策略固定翼飞行器的动态特性与多旋翼存在本质差异这直接决定了 Autopilot 的架构设计强非线性与耦合性俯仰-油门、滚转-偏航之间存在显著气动耦合。Autopilot 不采用简单的串级 PID而是提供解耦预补偿器Decoupling Precompensator模块其系数矩阵K_decouple[3][3]可依据风洞数据或飞行辨识结果在线更新实时抵消气动力矩交叉项。低频响应主导固定翼姿态响应时间常数通常为 13 秒远慢于多旋翼0.1 s。因此Autopilot 的内环姿态角速率控制带宽被严格限制在 510 Hz避免执行机构高频振荡外环姿态角/航迹控制则采用抗积分饱和Anti-Windup的 PI 结构积分时间常数设为 3060 秒确保稳态精度而不引发发散。传感器可靠性分级GPS 在峡谷、城市环境中易失锁气压计受湍流干扰大。Autopilot 实现了多源状态融合仲裁机制Multi-Source State Arbitration对高度通道优先使用 GPS 垂直速度VZ微分气压计二阶滤波输出当 GPS VZ 有效时气压计仅作慢速校准对水平位置采用 EKF30 预测-更新框架但将 GPS 伪距残差 3σ 的历元自动降权切换至纯惯性导航INS外推模式。执行机构物理限幅舵面偏转角、电机油门均存在硬性机械限幅。Autopilot 所有控制器输出均经过双级限幅Dual-Stage Saturation第一级为算法内部软限幅如elevator_cmd clampf(elevator_cmd, -25.0f, 25.0f)第二级为驱动层硬限幅如HAL_GPIO_WritePin(SERVO_PORT, ELEVATOR_PIN, servo_pulse_width_to_duty(elevator_cmd))两级间通过saturation_flag标志位通信供上层进行饱和诊断与故障告警。2. 核心控制器模块详解Autopilot 将固定翼自主飞行分解为四个正交控制环姿态稳定环Attitude、航迹跟踪环Path Following、高度保持环Altitude Hold、空速/地速管理环Airspeed/Groundspeed Management。各环可独立启用/禁用并通过control_mode_t枚举统一配置。2.1 姿态稳定控制器Attitude Stabilizer姿态稳定是所有高级功能的基础。Autopilot 提供两种实现经典 PID 姿态控制器attitude_pid_controller_t与基于反步法Backstepping的非线性控制器attitude_backstepping_controller_t。二者共享同一套状态输入接口typedef struct { float roll_rad; // 当前滚转角弧度 float pitch_rad; // 当前俯仰角弧度 float yaw_rad; // 当前偏航角弧度 float p_radps; // 机体坐标系 X 轴角速率滚转速率 float q_radps; // 机体坐标系 Y 轴角速率俯仰速率 float r_radps; // 机体坐标系 Z 轴角速率偏航速率 } attitude_state_t;2.1.1 PID 姿态控制器PID 控制器采用串级结构外环为姿态角误差roll_error roll_setpoint - roll_rad内环为角速率误差p_error p_setpoint - p_radps。其核心更新逻辑如下// 外环姿态角 PI 控制器 float roll_rate_setpoint controller-roll_pi.kp * roll_error controller-roll_pi.ki * controller-roll_pi.integral; // 内环角速率 PID 控制器含微分先行 float elevator_cmd controller-pitch_rate_pid.kp * (pitch_rate_error) controller-pitch_rate_pid.ki * controller-pitch_rate_pid.integral controller-pitch_rate_pid.kd * (q_dot_filtered); // q_dot_filtered 为俯仰角加速度低通滤波值 // 输出限幅与积分抗饱和 elevator_cmd clampf(elevator_cmd, -30.0f, 30.0f); if (fabsf(elevator_cmd) 29.5f) { controller-pitch_rate_pid.integral controller-pitch_rate_pid.ki * dt * (elevator_cmd 0 ? -1.0f : 1.0f); }关键参数配置表参数名符号典型值STM32F767工程意义调试建议roll_pi.kpKpφ8.0滚转角误差增益增大提升响应过大会引起舵面抖动roll_pi.kiKiφ0.15滚转角积分增益用于消除稳态误差需配合抗饱和pitch_rate_pid.kdKdθ̇0.08俯仰角速率微分增益抑制俯仰振荡过高引入噪声yaw_rate_pid.kpKpψ̇5.0偏航角速率比例增益补偿方向舵效率与垂直尾翼面积正相关2.1.2 反步法姿态控制器反步法控制器通过构造李雅普诺夫函数保证全局渐近稳定特别适合处理模型不确定性。其状态变量为四元数q [q0, q1, q2, q3]控制器输出为期望机体力矩[L, M, N]// 步骤1定义姿态误差四元数 q_e q_des ⊗ q_inv quat_mult(q_err, q_des, q_inv); // 步骤2设计虚拟控制律 α_p, α_q, α_r角速率期望 float alpha_p -k1 * q_err.q1 - k2 * p_radps; float alpha_q -k1 * q_err.q2 - k2 * q_radps; float alpha_r -k1 * q_err.q3 - k2 * r_radps; // 步骤3设计实际控制律力矩 float L Jxx * (alpha_p_dot k3*(p_radps - alpha_p)) ... ; // 含陀螺效应补偿项其中Jxx,Jyy,Jzz为机体转动惯量k1,k2,k3为正定设计参数。该控制器在autopilot/examples/backstepping_demo/中提供了基于 MATLAB/Simulink 的参数整定脚本可导出为 C 头文件直接编译。2.2 航迹跟踪控制器L1 Adaptive Path FollowingAutopilot 采用改进的 L1 自适应航迹跟踪算法其核心思想是将期望航迹离散为一系列航路点Waypoints计算当前飞机到航迹线的垂直距离Cross-Track Error, CTE并生成一个虚拟的“吸引点”Lookahead Point使飞机始终朝向该点飞行。L1 算法的关键参数为特征长度 L1其物理意义是当飞机以当前空速Vtas飞行时能以最小转弯半径R_min完成航迹切入所需的前瞻距离。计算公式为 [ L_1 \frac{V_{tas}^2}{g \cdot \tan(\phi_{max})} ] 其中g 9.81 m/s²φ_max为最大允许滚转角如 30°。Autopilot 在运行时动态更新L1确保高速飞行时前瞻更远低速时更灵敏。航迹跟踪输出为期望滚转角roll_setpoint其计算流程如下// 1. 计算到当前航段的CTE带符号 float cte cross_track_error(current_pos, wp_prev, wp_next); // 2. 计算前瞻距离 L1单位米 float l1 (tas_mps * tas_mps) / (9.81f * tanf(roll_max_rad)); // 3. 计算所需滚转角L1 原始公式 float roll_cmd atanf(2.0f * cte * g / (l1 * l1 * tas_mps)); // 4. 加入速率限制与死区防小信号抖动 roll_cmd slew_rate_limit(roll_cmd, controller-roll_slew_rate, dt); roll_cmd deadzone(roll_cmd, 0.02f); // 0.02 rad ≈ 1.15°该算法鲁棒性强在 GPS 更新率低至 1 Hz 时仍能保持航迹跟踪精度 5 mCEP已在 1.2 kg 微型固定翼平台上完成 200 km/h 高速航线验证。2.3 高度保持与垂直导航控制器高度环采用三模态切换策略气压计主导Baro-Dominant、GPS 主导GPS-Dominant、融合主导Fused。切换逻辑由baro_health和gps_health标志位触发模式触发条件高度源垂直速度源适用场景Baro-Dominantbaro_health 0.7 gps_health 0.3气压计二阶滤波气压计微分加速度计补偿低空、GPS 失锁GPS-Dominantgps_health 0.8GPS 海拔GPS 垂直速度VZ中高空、开阔地带Fused其他情况EKF30 高度估计EKF30 垂直速度估计全场景默认高度控制器本身为 PI 结构但引入非线性增益调度Gain Scheduling在低空 50 m增大比例增益Kp_h以提升响应在高空 300 m减小Kp_h避免过调。其代码片段如下// 增益调度高度越低Kp 越大 float kp_h controller-alt_kp_base * (1.0f 0.5f * (300.0f - current_alt) / 300.0f); kp_h clampf(kp_h, 0.05f, 0.3f); // 高度误差 PI 控制 float alt_error alt_setpoint - current_alt; float throttle_cmd kp_h * alt_error controller-alt_ki * controller-alt_integral; // 输出映射至油门0.01.0 throttle_cmd map_to_throttle_range(throttle_cmd, 0.0f, 1.0f);2.4 空速/地速管理控制器固定翼的能量管理核心是空速TAS而非多旋翼的地速GS。Autopilot 提供两种模式空速保持Airspeed Hold直接闭环控制空速管差压ΔP通过throttle_cmd调节发动机功率。其控制器为 PI但积分项采用条件积分Conditional Integration仅当空速误差|TAS_error| 0.5 m/s且throttle_cmd未达限幅时才累积积分防止深度积分饱和。地速管理Groundspeed Management在侧风条件下为维持预定地速如返航时的地速 15 m/s控制器计算所需空速TAS_req GS_setpoint / cos(β)其中β为侧滑角由 GPS 航向与地速矢量夹角估算。此模式下throttle_cmd与roll_setpoint协同工作油门维持能量滚转修正航向。3. 硬件接口与驱动集成指南Autopilot 库本身不包含任何 HAL 代码但提供了标准的传感器数据注入接口与执行机构输出接口开发者需按以下规范实现底层驱动。3.1 传感器数据注入协议所有传感器数据必须在autopilot_update()调用前完成更新并存入全局状态结构体autopilot_state_t。该结构体定义如下typedef struct { // 时间戳毫秒自系统启动 uint32_t timestamp_ms; // IMU 数据已校准单位rad/s, m/s² struct { float x, y, z; } gyro; // 机体坐标系 struct { float x, y, z; } accel; // 机体坐标系 // 导航状态WGS84 坐标系 struct { double lat, lon; } gps_pos; // 单位度 struct { float n, e, d; } gps_vel; // 北东地速度单位m/s float gps_hdop; // HDOP 值用于健康评估 // 大气数据 float baro_alt; // 气压计海拔米 float airspeed; // 空速m/s float temperature; // 气温℃ // 手动遥控输入0.01.0 归一化 struct { float roll, pitch, yaw, throttle; } rc_input; } autopilot_state_t; extern autopilot_state_t ap_state; // 全局实例由用户驱动更新关键工程实践gyro和accel必须经过零偏校准Bias Calibration与温度补偿Temperature Compensation。Autopilot 提供imu_calibrate_bias()函数可在地面静止时调用。gps_pos和gps_vel必须为WGS84 地理坐标系不可使用 UTM 或本地平面坐标。若 GPS 模块输出为 NMEA需调用nmea_parse_gga_rmc()解析。airspeed输入需为真实空速TAS若传感器仅输出指示空速IAS必须通过ias_to_tas()函数转换需输入baro_alt和temperature。3.2 执行机构输出驱动模板Autopilot 通过autopilot_output_t结构体输出控制指令驱动层需将其映射至具体硬件typedef struct { float elevator; // 单位度-30.0 30.0 float aileron; // 单位度-30.0 30.0 float rudder; // 单位度-30.0 30.0 float throttle; // 单位0.0 1.00%100% bool arming; // 安全开关状态true已解锁 } autopilot_output_t; extern autopilot_output_t ap_output; // 全局输出实例典型驱动实现STM32 HAL PWM// 在主循环中调用 void actuator_update(void) { if (!ap_output.arming) { // 安全锁定所有舵面回中油门归零 HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_1); // Elevator __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, SERVO_MID_PULSE); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_2); // Aileron __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, SERVO_MID_PULSE); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_3); // Rudder __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_3, SERVO_MID_PULSE); HAL_TIM_PWM_Start(htim1, TIM_CHANNEL_4); // Throttle __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_4, MOTOR_STOP_PULSE); return; } // 映射舵面角度到 PWM 脉宽10002000 μs uint16_t elev_pulse servo_angle_to_pulse(ap_output.elevator, -30.0f, 30.0f); uint16_t ailr_pulse servo_angle_to_pulse(ap_output.aileron, -30.0f, 30.0f); uint16_t rudd_pulse servo_angle_to_pulse(ap_output.rudder, -30.0f, 30.0f); uint16_t thr_pulse motor_throttle_to_pulse(ap_output.throttle); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_1, elev_pulse); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_2, ailr_pulse); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_3, rudd_pulse); __HAL_TIM_SET_COMPARE(htim1, TIM_CHANNEL_4, thr_pulse); } // 辅助函数线性映射 static uint16_t servo_angle_to_pulse(float angle, float min_deg, float max_deg) { float ratio (angle - min_deg) / (max_deg - min_deg); return (uint16_t)(1000 ratio * 1000); // 10002000 μs }4. 系统集成与实战配置示例Autopilot 的典型集成流程如下传感器驱动 → 状态融合可选 EKF→ Autopilot 更新 → 执行机构驱动。以下为基于 STM32F767IGT6 的 FreeRTOS 集成示例。4.1 FreeRTOS 任务划分// 任务优先级传感器采集 Autopilot 控制 执行机构输出 日志上传 #define TASK_SENSOR_PRIO 5 #define TASK_AUTOPILOT_PRIO 4 #define TASK_ACTUATOR_PRIO 3 #define TASK_LOG_PRIO 2 // 传感器采集任务1 kHz void sensor_task(void *pvParameters) { while(1) { imu_read(ap_state.gyro, ap_state.accel); // 读取原始数据 baro_read(ap_state.baro_alt); gps_read(ap_state.gps_pos, ap_state.gps_vel, ap_state.gps_hdop); ap_state.timestamp_ms HAL_GetTick(); vTaskDelay(1); // 1 ms 周期 } } // Autopilot 控制任务100 Hz void autopilot_task(void *pvParameters) { const TickType_t xFrequency 10; // 10 ms TickType_t xLastWakeTime xTaskGetTickCount(); while(1) { autopilot_update(); // 核心控制律计算 vTaskDelayUntil(xLastWakeTime, xFrequency); } } // 执行机构输出任务500 Hz void actuator_task(void *pvParameters) { while(1) { actuator_update(); // PWM 更新 vTaskDelay(2); // 2 ms 周期 } }4.2 关键配置参数初始化// 在 main() 中初始化 Autopilot void autopilot_init(void) { // 1. 初始化控制器结构体 memset(ap_ctrl, 0, sizeof(ap_ctrl)); // 2. 配置姿态 PID 参数针对某型 2.5 kg 固定翼 ap_ctrl.attitude.pid.roll_pi.kp 10.0f; ap_ctrl.attitude.pid.roll_pi.ki 0.2f; ap_ctrl.attitude.pid.pitch_rate_pid.kp 12.0f; ap_ctrl.attitude.pid.pitch_rate_pid.kd 0.1f; // 3. 配置 L1 航迹跟踪 ap_ctrl.path_following.l1_length 25.0f; // 米 ap_ctrl.path_following.roll_max 30.0f * M_PI/180.0f; // 弧度 // 4. 配置高度环 ap_ctrl.altitude.hold_kp 0.15f; ap_ctrl.altitude.hold_ki 0.002f; // 5. 启用安全机制 ap_ctrl.safety.arming_timeout_ms 5000; // 5 秒内未收到有效遥控信号则锁定 ap_ctrl.safety.failsafe_altitude 100.0f; // 失控时爬升至 100 米 }4.3 故障诊断与安全机制Autopilot 内置三级安全机制一级传感器健康监测对每个传感器计算health_score0.01.0基于数据方差、更新率、物理合理性如加速度模长是否在 0.8g1.2g。当health_score 0.3时自动切换至备份源或进入安全模式。二级执行机构饱和诊断持续监控ap_output.elevator/aileron/rudder是否持续 95% 限幅。若连续 500 ms 饱和触发SATURATION_WARNING事件降低控制带宽并提示地面站。三级失控保护Failsafe当ap_state.rc_input.throttle 0.1且ap_state.gps_health 0.2持续 3 秒自动执行预设返航航线RTL并以 15° 滚转角、5 m/s 爬升率爬升至安全高度。所有安全事件均通过ap_ctrl.safety.event_flags位域标志可由上层应用轮询或中断处理。5. 性能基准与实测数据Autopilot 库在 STM32F767IGT6216 MHz上实测资源占用如下模块CPU 占用率100 HzRAM 占用Flash 占用关键约束姿态 PID 控制器8.2%128 B1.8 KB主要消耗在三角函数查表L1 航迹跟踪3.1%48 B0.9 KB无浮点除法全部整数运算高度 PI 控制器1.7%32 B0.4 KB积分项使用单精度 float全控制器4 环启用14.5%320 B4.2 KB满足 500 Hz 更新需求在某型 1.8 kg 电动固定翼平台上Autopilot 实现了以下飞行性能姿态稳定精度滚转角稳态误差 ±0.3°俯仰角 ±0.5°无风环境航迹跟踪精度直线航段 CEP 3.2 m圆弧航段最大偏差 4.7 mGPS RTK高度保持精度气压计模式下 ±2.1 m0100 mGPS 模式下 ±1.3 m100500 m失控响应时间从 GPS 失锁到 RTL 动作启动 1.8 s这些数据表明Autopilot 已达到工业级固定翼飞控的实用门槛其模块化设计允许开发者根据具体平台特性裁剪冗余控制器、替换核心算法如将 PID 替换为 MPC而无需重构整个软件栈。对于追求高可靠性、低资源占用、强可验证性的嵌入式飞控项目Autopilot 提供了一条经过实践检验的技术路径。