ArduPilot_VTOL系统深度技术分析

ArduPilot VTOL系统技术分析

作者：李荣江
邮箱地址：2780304724@qq.com
时间：2025年8月1日

一、VTOL系统架构概述

1.1 系统设计理念

ArduPilot的VTOL（Vertical Take-Off and Landing，垂直起降）系统采用了高度模块化的设计架构，支持三种主要的VTOL构型：

1. 标准四旋翼飞机（Standard QuadPlane）

   • 固定翼飞机 + 独立的多旋翼升力系统
   • 最成熟、应用最广泛的VTOL方案
   • 典型代表：垂直起降固定翼无人机

2. 倾转旋翼（Tiltrotor）

   • 旋翼可在垂直和水平方向之间倾转
   • 效率高但机械复杂度大
   • 典型代表：V-22 鱼鹰式倾转旋翼机

3. 尾座式（Tailsitter）

   • 整机在垂直和水平姿态之间转换
   • 结构简单但控制复杂
   • 典型代表：Convair XFY Pogo

1.2 核心类层次结构

// 基础VTOL控制器
class QuadPlane {
    // 推力类型枚举
    enum class ThrustType : uint8_t {
        SLT=0,        // Separate Lift Thrust (标准四旋翼飞机)
        TAILSITTER,   // 尾座式
        TILTROTOR,    // 倾转旋翼
    };
    
    // 核心组件
    AP_MotorsMulticopter *motors;           // 多旋翼电机控制
    AC_AttitudeControl_Multi *attitude_control; // 姿态控制器
    AC_PosControl *pos_control;             // 位置控制器
    AC_WPNav *wp_nav;                       // 航点导航
    AC_Loiter *loiter_nav;                  // 悬停导航
    
    // 特殊功能组件
    Tailsitter tailsitter;                  // 尾座式控制
    Tiltrotor tiltrotor;                    // 倾转旋翼控制
    VTOL_Assist assist;                     // VTOL辅助功能
};

// 过渡控制基类
class Transition {
public:
    virtual void update() = 0;              // 更新过渡状态
    virtual void VTOL_update() = 0;         // VTOL模式更新
    virtual bool complete() const = 0;      // 过渡是否完成
    virtual MAV_VTOL_STATE get_mav_vtol_state() const = 0;
};

// 标准过渡控制（用于标准四旋翼飞机和倾转旋翼）
class SLT_Transition : public Transition {
    enum {
        TRANSITION_AIRSPEED_WAIT,   // 等待空速
        TRANSITION_TIMER,           // 定时过渡
        TRANSITION_DONE            // 过渡完成
    } transition_state;
};

// 尾座式过渡控制
class Tailsitter_Transition : public Transition {
    enum {
        TRANSITION_ANGLE_WAIT_FW,   // 等待前飞角度
        TRANSITION_ANGLE_WAIT_VTOL, // 等待垂直角度
        TRANSITION_DONE            // 过渡完成
    } transition_state;
};

二、VTOL飞行模式与状态机

2.1 飞行模式体系

VTOL系统扩展了标准固定翼的飞行模式，增加了"Q"系列模式（Q代表QuadPlane）：

// VTOL特有飞行模式
enum class Mode::Number : uint8_t {
    QSTABILIZE = 17,   // Q稳定模式 - 手动油门，自稳定姿态
    QHOVER = 18,       // Q悬停模式 - 自动高度保持
    QLOITER = 19,      // Q留待模式 - GPS定点悬停
    QLAND = 20,        // Q着陆模式 - 自动垂直着陆
    QRTL = 21,         // Q返航模式 - 返回起飞点并着陆
    QAUTOTUNE = 22,    // Q自动调参 - PID参数自动优化
    QACRO = 23,        // Q特技模式 - 速率控制
    QGUIDED = 24,      // Q引导模式 - 外部控制
};

2.2 过渡状态机实现

2.2.1 前飞过渡（VTOL → Forward Flight）

void SLT_Transition::update()
{
    const uint32_t now = millis();
    
    // 获取目标空速和当前空速
    float aspeed;
    bool have_airspeed = quadplane.ahrs.airspeed_estimate(aspeed);
    
    // 检查辅助电机状态
    quadplane.assisted_flight = quadplane.assistance.should_assist(aspeed);
    
    switch (transition_state) {
    case TRANSITION_AIRSPEED_WAIT:
        // 阶段1：等待空速达到阈值
        quadplane.throttle_wait = false;
        
        // 设置目标姿态
        quadplane.set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
        
        // 检查空速条件
        if (have_airspeed && aspeed > plane.aparm.airspeed_min) {
            // 空速足够，进入定时过渡阶段
            transition_state = TRANSITION_TIMER;
            transition_start_ms = now;
            
            // 记录倾转角度（倾转旋翼专用）
            if (quadplane.tiltrotor.enabled()) {
                airspeed_reached_tilt = quadplane.tiltrotor.current_tilt;
            }
        }
        
        // 检查低空速超时
        if (have_airspeed && aspeed < plane.aparm.airspeed_min * 0.5f) {
            if (transition_low_airspeed_ms == 0) {
                transition_low_airspeed_ms = now;
            }
        } else {
            transition_low_airspeed_ms = 0;
        }
        
        // 空速过低超时处理
        if (transition_low_airspeed_ms != 0 && 
            (now - transition_low_airspeed_ms) > (quadplane.transition_failure.timeout*1000)) {
            handle_transition_failure();
        }
        break;
        
    case TRANSITION_TIMER:
        // 阶段2：定时过渡阶段
        if ((now - transition_start_ms) > (unsigned)quadplane.transition_time_ms) {
            // 过渡时间结束
            transition_state = TRANSITION_DONE;
            
            // 关闭VTOL电机
            if (!quadplane.tiltrotor.has_vtol_motor() && 
                !quadplane.tailsitter.enabled()) {
                quadplane.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::SHUT_DOWN);
            }
        }
        break;
        
    case TRANSITION_DONE:
        // 阶段3：过渡完成
        break;
    }
}

// 倾转旋翼专用过渡逻辑
void Tiltrotor::slew(float target_angle)
{
    // 计算倾转速率限制
    float max_change = tilt_max_change(target_angle > current_tilt);
    
    // 应用速率限制
    float new_tilt = current_tilt;
    if (target_angle > current_tilt) {
        new_tilt = MIN(target_angle, current_tilt + max_change);
    } else {
        new_tilt = MAX(target_angle, current_tilt - max_change);
    }
    
    current_tilt = new_tilt;
    
    // 输出到舵机
    SRV_Channels::set_output_scaled(SRV_Channel::k_motor_tilt, current_tilt * 100);
}

2.2.2 垂直过渡（Forward Flight → VTOL）

void SLT_Transition::VTOL_update()
{
    const uint32_t now = millis();
    
    // 设置VTOL模式标志
    quadplane.in_vtol_mode = true;
    
    // 启动VTOL电机
    quadplane.motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
    
    // 运行姿态控制器
    quadplane.attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(
        plane.nav_roll_cd, 
        plane.nav_pitch_cd,
        quadplane.get_desired_yaw_rate_cds()
    );
    
    // 倾转旋翼回到垂直位置
    if (quadplane.tiltrotor.enabled()) {
        quadplane.tiltrotor.slew(0);
    }
    
    // 保持高度
    quadplane.hold_hover(quadplane.get_pilot_desired_climb_rate_cms());
}

2.3 尾座式过渡控制

尾座式VTOL的过渡控制更加复杂，需要处理整机姿态的大幅度变化：

void Tailsitter_Transition::update()
{
    const uint32_t now = millis();
    const float pitch_angle = ahrs.pitch_sensor * 0.01f;
    
    switch (transition_state) {
    case TRANSITION_ANGLE_WAIT_FW:
        // 从垂直到水平的过渡
        {
            // 计算目标俯仰角
            float target_pitch = tailsitter.transition_angle_fw;
            
            // 应用速率限制
            float pitch_rate = tailsitter.transition_rate_fw;
            float max_change = pitch_rate * dt;
            
            // 限制俯仰角变化
            float new_pitch = constrain_float(
                target_pitch,
                pitch_angle - max_change,
                pitch_angle + max_change
            );
            
            // 设置导航俯仰角
            plane.nav_pitch_cd = new_pitch * 100;
            
            // 检查是否达到目标角度
            if (fabsf(pitch_angle - target_pitch) < 5.0f) {
                transition_state = TRANSITION_DONE;
            }
        }
        break;
        
    case TRANSITION_ANGLE_WAIT_VTOL:
        // 从水平到垂直的过渡
        {
            // 目标是垂直姿态（90度）
            float target_pitch = tailsitter.transition_angle_vtol;
            
            // 计算所需的俯仰变化
            float pitch_change = wrap_180(target_pitch - pitch_angle);
            
            // 应用过渡速率
            float pitch_rate = tailsitter.transition_rate_vtol;
            float max_change = pitch_rate * dt;
            pitch_change = constrain_float(pitch_change, -max_change, max_change);
            
            // 更新姿态目标
            plane.nav_pitch_cd = (pitch_angle + pitch_change) * 100;
            
            // 检查过渡完成
            if (fabsf(pitch_angle - target_pitch) < 5.0f) {
                transition_state = TRANSITION_DONE;
            }
            
            // 油门补偿
            float throttle_boost = (90.0f - fabsf(pitch_angle)) / 90.0f;
            throttle_boost *= tailsitter.transition_throttle_vtol;
            motors->set_throttle(throttle_boost);
        }
        break;
    }
}

三、VTOL控制算法实现

3.1 位置控制器

VTOL模式下使用多旋翼的位置控制算法：

void QuadPlane::vtol_position_controller()
{
    // 更新惯性导航
    inertial_nav.update();
    
    // 获取当前位置和速度
    const Vector3f& curr_pos = inertial_nav.get_position_neu_cm();
    const Vector3f& curr_vel = inertial_nav.get_velocity_neu_cms();
    
    // 运行位置控制器
    pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(),
                                       wp_nav->get_wp_acceleration());
    pos_control->set_max_speed_accel_z(pilot_speed_z_max_up,
                                       pilot_speed_z_max_dn,
                                       pilot_accel_z);
    
    // 更新Z轴控制器
    run_z_controller();
    
    // 更新XY轴控制器
    run_xy_controller();
    
    // 获取姿态角度目标
    float nav_roll_cd, nav_pitch_cd;
    pos_control->get_lean_angles_to_accel(nav_roll_cd, nav_pitch_cd);
    
    // 发送到姿态控制器
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(
        nav_roll_cd,
        nav_pitch_cd,
        get_desired_yaw_rate_cds()
    );
}

// Z轴（高度）控制器
void QuadPlane::run_z_controller()
{
    // 获取目标爬升率
    float target_climb_rate = get_pilot_desired_climb_rate_cms();
    
    // 地形跟随
    float terr_offset = 0;
    if (terrain_enabled()) {
        float terr_alt;
        if (terrain.height_above_terrain(terr_alt, true)) {
            terr_offset = terr_alt * 100 - inertial_nav.get_position_z_up_cm();
        }
    }
    
    // 设置目标
    pos_control->set_pos_target_z_from_climb_rate_cm(
        target_climb_rate,
        false,
        terr_offset
    );
    
    // 运行Z轴控制器
    pos_control->update_z_controller();
}

3.2 姿态控制算法

3.2.1 标准VTOL姿态控制

void QuadPlane::multicopter_attitude_rate_update(float yaw_rate_cds)
{
    // 设置YAW速率目标
    attitude_control->input_euler_rate_yaw_euler_angle_pitch_roll(
        yaw_rate_cds,
        plane.nav_pitch_cd,
        plane.nav_roll_cd
    );
    
    // 运行速率控制器
    attitude_control->rate_controller_run();
}

// 姿态控制增益调度
void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
{
    // 根据油门调整姿态控制权重
    float throttle = motors.get_throttle();
    float throttle_mix = constrain_float(throttle * 2.0f, 0.0f, 1.0f);
    
    // 油门越高，姿态控制权重越大
    _throttle_rpy_mix = throttle_mix;
}

3.2.2 尾座式姿态控制

尾座式VTOL需要特殊的姿态映射：

void Tailsitter::output()
{
    if (!enabled() || !active()) {
        return;
    }
    
    // 获取姿态四元数
    Quaternion q;
    quadplane.ahrs.get_quaternion(q);
    
    // 尾座式姿态转换矩阵
    // 机体坐标系: X前 Y右 Z下
    // 尾座式垂直时: X上 Y右 Z后
    Matrix3f rotation_tailsitter;
    rotation_tailsitter.from_euler(0, radians(90), 0);
    
    // 转换到尾座式坐标系
    Quaternion q_tailsitter;
    q_tailsitter = q * rotation_tailsitter;
    
    // 提取欧拉角
    float roll, pitch, yaw;
    q_tailsitter.to_euler(roll, pitch, yaw);
    
    // 应用控制增益缩放
    if (in_vtol_transition()) {
        // 过渡期间的增益调度
        float transition_scale = get_transition_scale();
        roll *= transition_scale * VTOL_roll_scale;
        pitch *= transition_scale * VTOL_pitch_scale;
        yaw *= transition_scale * VTOL_yaw_scale;
    }
    
    // 计算控制面偏转
    float aileron_out = roll * 4500.0f;
    float elevator_out = pitch * 4500.0f;
    float rudder_out = yaw * 4500.0f;
    
    // 输出到舵机
    SRV_Channels::set_output_scaled(SRV_Channel::k_aileron, aileron_out);
    SRV_Channels::set_output_scaled(SRV_Channel::k_elevator, elevator_out);
    SRV_Channels::set_output_scaled(SRV_Channel::k_rudder, rudder_out);
}

3.3 推力矢量控制

倾转旋翼的推力矢量控制实现：

void Tiltrotor::vectoring()
{
    // 仅在倾转旋翼模式下执行
    if (!enabled() || !_is_vectored) {
        return;
    }
    
    // 获取姿态控制输出
    float roll_out = attitude_control->get_roll_out();
    float pitch_out = attitude_control->get_pitch_out();
    float yaw_out = attitude_control->get_yaw_out();
    
    // 计算倾转角度调整
    float tilt_left = current_tilt;
    float tilt_right = current_tilt;
    
    // 横滚控制通过差动倾转实现
    tilt_left += roll_out * vectored_hover_gain;
    tilt_right -= roll_out * vectored_hover_gain;
    
    // 偏航控制通过差动倾转实现
    float yaw_differential = yaw_out * tilt_yaw_angle;
    tilt_left += yaw_differential;
    tilt_right -= yaw_differential;
    
    // 应用倾转限制
    tilt_left = constrain_float(tilt_left, 0, max_angle_deg);
    tilt_right = constrain_float(tilt_right, 0, max_angle_deg);
    
    // 输出到倾转舵机
    SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorLeft, tilt_left * 100);
    SRV_Channels::set_output_scaled(SRV_Channel::k_tiltMotorRight, tilt_right * 100);
}

// 推力补偿算法
void Tiltrotor::tilt_compensate(float *thrust, uint8_t num_motors)
{
    // 计算倾转角度的余弦值
    float tilt_cos = cosf(radians(current_tilt));
    float tilt_sin = sinf(radians(current_tilt));
    
    // 垂直推力补偿
    if (tilt_cos > 0.1f) {
        float thrust_scale = 1.0f / tilt_cos;
        thrust_scale = constrain_float(thrust_scale, 1.0f, 2.0f);
        
        // 应用补偿
        for (uint8_t i = 0; i < num_motors; i++) {
            if (is_motor_tilting(i)) {
                thrust[i] *= thrust_scale;
            }
        }
    }
    
    // 前向推力分量
    float forward_thrust = 0;
    for (uint8_t i = 0; i < num_motors; i++) {
        if (is_motor_tilting(i)) {
            forward_thrust += thrust[i] * tilt_sin;
        }
    }
    
    // 设置前向油门
    SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, forward_thrust * 100);
}

四、VTOL导航与路径规划

4.1 VTOL自动起飞

bool QuadPlane::do_vtol_takeoff(const AP_Mission::Mission_Command& cmd)
{
    // 检查GPS状态
    if (!plane.AP_Plane::verify_takeoff_fix()) {
        return false;
    }
    
    // 设置起飞参数
    plane.auto_state.takeoff_altitude_rel_cm = cmd.content.location.alt;
    plane.auto_state.takeoff_speed_time_ms = 0;
    plane.auto_state.takeoff_complete = false;
    
    // 计算起飞位置
    Location takeoff_loc = cmd.content.location;
    if (takeoff_loc.lat == 0 && takeoff_loc.lng == 0) {
        // 使用当前位置
        takeoff_loc = plane.current_loc;
        takeoff_loc.alt = plane.auto_state.takeoff_altitude_rel_cm;
    }
    
    // 设置目标位置
    set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
    wp_nav->wp_and_spline_init();
    
    // 设置垂直起飞目标
    Vector3p target_pos;
    if (!takeoff_loc.get_vector_from_origin_NEU(target_pos)) {
        return false;
    }
    
    // 配置垂直速度
    pos_control->set_max_speed_accel_z(wp_nav->get_default_speed_up(),
                                       wp_nav->get_default_speed_down(),
                                       wp_nav->get_wp_acceleration());
    
    // 开始起飞
    wp_nav->set_wp_destination(target_pos.tofloat());
    
    // 记录起飞时间
    plane.takeoff_state.start_time_ms = millis();
    
    return true;
}

// 起飞控制器
void QuadPlane::takeoff_controller()
{
    if (!in_vtol_takeoff()) {
        return;
    }
    
    // 运行位置控制
    setup_target_position();
    run_xy_controller();
    
    // 检查是否达到目标高度
    const float alt_diff = plane.current_loc.alt - plane.auto_state.takeoff_altitude_rel_cm;
    if (alt_diff > -50) {
        // 接近目标高度，准备过渡
        plane.auto_state.takeoff_complete = true;
        
        // 如果设置了前飞速度，开始过渡
        if (plane.auto_state.takeoff_speed_time_ms > 0) {
            transition->restart();
        }
    }
    
    // 运行Z轴控制器
    pos_control->update_z_controller();
    
    // 姿态控制
    attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(
        wp_nav->get_roll(),
        wp_nav->get_pitch(),
        get_desired_yaw_rate_cds()
    );
}

4.2 VTOL精准着陆

bool QuadPlane::do_vtol_land(const AP_Mission::Mission_Command& cmd)
{
    // 设置着陆参数
    plane.auto_state.wp_distance = plane.current_loc.get_distance(cmd.content.location);
    plane.auto_state.wp_proportion = 0;
    plane.auto_state.next_wp_crosstrack = false;
    plane.auto_state.crosstrack = true;
    
    // 计算着陆点
    Location land_loc = cmd.content.location;
    if (land_loc.lat == 0 && land_loc.lng == 0) {
        // 使用当前位置着陆
        land_loc = plane.current_loc;
        land_loc.alt = 0;
    }
    
    // 设置着陆目标
    Vector3p target_pos;
    land_loc.get_vector_from_origin_NEU(target_pos);
    wp_nav->set_wp_destination(target_pos.tofloat());
    
    // 进入着陆模式
    plane.set_mode(plane.mode_qland, ModeReason::MISSION_CMD);
    
    return true;
}

// 着陆速率控制
float QuadPlane::landing_descent_rate_cms(float height_above_ground)
{
    // 分段着陆速率
    float descent_rate;
    
    if (height_above_ground > land_final_alt) {
        // 高空快速下降
        descent_rate = land_speed_cms;
    } else {
        // 低空缓慢着陆
        // 使用平方根曲线实现平滑减速
        float land_proportion = height_above_ground / land_final_alt;
        descent_rate = land_speed_cms * sqrtf(land_proportion);
        
        // 最小下降速率限制
        descent_rate = MAX(descent_rate, 10);
    }
    
    // 应用用户输入修正
    float pilot_throttle = get_pilot_land_throttle();
    if (pilot_throttle > 0.7f) {
        // 允许加速下降
        descent_rate *= (1.0f + (pilot_throttle - 0.7f) * 2.0f);
    } else if (pilot_throttle < 0.3f) {
        // 允许减速或悬停
        descent_rate *= pilot_throttle / 0.3f;
    }
    
    return descent_rate;
}

// 着陆检测
bool QuadPlane::check_land_complete()
{
    // 检查多个条件
    if (motors->get_throttle() > get_throttle_thrust_max() * 0.3f) {
        // 油门过高，未着陆
        return false;
    }
    
    // 高度变化率检查
    float climb_rate = inertial_nav.get_velocity_z_up_cms();
    if (fabsf(climb_rate) > 50) {
        // 垂直速度过大
        return false;
    }
    
    // 加速度检查（检测地面接触）
    float accel_variance = ahrs.get_accel_ef().length_squared();
    if (accel_variance > 3.0f) {
        // 振动过大，可能在空中
        return false;
    }
    
    // 使用着陆检测器
    return land_detector(1500);
}

4.3 过渡点计算与路径规划

// 计算从当前速度到悬停的制动距离
float QuadPlane::stopping_distance(float ground_speed_squared) const
{
    // 使用动能定理计算制动距离
    // s = v² / (2*a)
    const float decel = transition_decel;
    
    if (decel <= 0) {
        // 使用默认减速度
        return ground_speed_squared / (2.0f * 2.0f); // 2 m/s²
    }
    
    return ground_speed_squared / (2.0f * decel);
}

// 计算过渡开始点
float QuadPlane::transition_threshold(void)
{
    // 基于巡航速度计算安全过渡距离
    const float cruise_speed = plane.aparm.airspeed_cruise;
    const float stop_dist = stopping_distance(sq(cruise_speed));
    
    // 添加安全余量
    const float safety_margin = 1.5f;
    
    return stop_dist * safety_margin;
}

// 更新着陆定位
void QuadPlane::update_land_positioning(void)
{
    // 仅在VTOL着陆模式下执行
    if (!in_vtol_land_descent()) {
        return;
    }
    
    // 获取到着陆点的距离和方位
    Vector2f target_vec = plane.next_WP_loc.get_distance_NE(plane.current_loc);
    float distance = target_vec.length();
    
    if (distance > transition_threshold()) {
        // 距离过远，使用固定翼模式接近
        plane.nav_controller->update_waypoint(plane.current_loc, plane.next_WP_loc);
    } else {
        // 进入VTOL控制范围
        // 计算减速目标
        float target_speed = safe_sqrt(2.0f * transition_decel * distance);
        
        // 设置位置目标
        Vector3p target_pos;
        plane.next_WP_loc.get_vector_from_origin_NEU(target_pos);
        wp_nav->set_wp_destination_NED(target_pos.tofloat());
        
        // 限制水平速度
        pos_control->set_max_speed_xy(target_speed * 100);
    }
}

五、VTOL安全保护机制

5.1 过渡失败处理

void SLT_Transition::handle_transition_failure()
{
    // 记录失败
    if (!quadplane.transition_failure.warned) {
        gcs().send_text(MAV_SEVERITY_CRITICAL, "Transition failed!");
        quadplane.transition_failure.warned = true;
    }
    
    // 执行失败动作
    switch (quadplane.transition_failure.action) {
    case QuadPlane::TRANS_FAIL::ACTION::QLAND:
        // 强制VTOL着陆
        plane.set_mode(plane.mode_qland, ModeReason::TRANSITION_FAILED);
        break;
        
    case QuadPlane::TRANS_FAIL::ACTION::QRTL:
        // VTOL返航
        plane.set_mode(plane.mode_qrtl, ModeReason::TRANSITION_FAILED);
        break;
    }
}

5.2 VTOL辅助（Assist）系统

class VTOL_Assist {
public:
    // 检查是否需要辅助
    bool should_assist(float aspeed) {
        // 速度辅助
        if (speed > 0 && aspeed < speed) {
            return true;
        }
        
        // 姿态角度辅助
        if (angle > 0) {
            float roll = degrees(ahrs.roll);
            float pitch = degrees(ahrs.pitch);
            
            if (fabsf(roll) > angle || fabsf(pitch) > angle) {
                return true;
            }
        }
        
        // 高度损失辅助
        if (alt_lost > 0) {
            float alt_change = (current_alt - last_alt);
            if (alt_change < -alt_lost) {
                return true;
            }
        }
        
        return false;
    }
    
private:
    AP_Float speed;     // 最小空速阈值
    AP_Float angle;     // 最大姿态角
    AP_Float alt_lost;  // 允许高度损失
};

5.3 电机失效保护

void QuadPlane::motors_output(bool run_rate_controller)
{
    // 检查电机状态
    if (!motors->armed() || motors->get_spool_state() == AP_Motors::SpoolState::SHUT_DOWN) {
        // 电机未解锁或关闭
        return;
    }
    
    // 运行速率控制器
    if (run_rate_controller) {
        attitude_control->rate_controller_run();
    }
    
    // 混控输出
    motors->output();
    
    // 监测电机健康状态
    for (uint8_t i = 0; i < motors->get_num_motors(); i++) {
        if (motors->get_motor_mask() & (1U << i)) {
            // 检查电机输出
            float motor_out = motors->get_motor_out(i);
            
            if (motor_out > 0.95f) {
                // 电机饱和警告
                motor_saturation_warning(i);
            }
        }
    }
    
    // 尾座式特殊输出
    if (tailsitter.enabled()) {
        tailsitter.output();
    }
    
    // 倾转旋翼控制
    if (tiltrotor.enabled()) {
        tiltrotor.continuous_update();
        tiltrotor.vectoring();
    }
}

六、VTOL参数调优指南

6.1 关键参数配置

// 过渡参数
AP_GROUPINFO("TRANSITION_MS", 1, QuadPlane, transition_time_ms, 5000),
AP_GROUPINFO("TRANS_DECEL", 2, QuadPlane, transition_decel, 2.0),

// 辅助参数
AP_GROUPINFO("ASSIST_SPEED", 3, VTOL_Assist, speed, 0),
AP_GROUPINFO("ASSIST_ANGLE", 4, VTOL_Assist, angle, 30),

// 倾转旋翼参数
AP_GROUPINFO("TILT_MAX", 5, Tiltrotor, max_angle_deg, 90),
AP_GROUPINFO("TILT_RATE_UP", 6, Tiltrotor, max_rate_up_dps, 40),
AP_GROUPINFO("TILT_RATE_DN", 7, Tiltrotor, max_rate_down_dps, 20),

// 尾座式参数
AP_GROUPINFO("TAILSIT_ANGLE", 8, Tailsitter, transition_angle_fw, 45),
AP_GROUPINFO("TAILSIT_RATE", 9, Tailsitter, transition_rate_fw, 50),

6.2 调试与日志

// VTOL专用日志结构
struct PACKED log_QControl_Tuning {
    LOG_PACKET_HEADER;
    uint64_t time_us;
    float    throttle_in;       // 输入油门
    float    angle_boost;       // 角度补偿
    float    throttle_out;      // 输出油门
    float    throttle_hover;    // 悬停油门
    float    desired_alt;       // 期望高度
    float    inav_alt;         // 惯导高度
    int32_t  baro_alt;         // 气压高度
    int16_t  target_climb_rate; // 目标爬升率
    int16_t  climb_rate;       // 实际爬升率
    float    throttle_mix;     // 油门混合
    uint8_t  transition_state; // 过渡状态
    uint8_t  assist;          // 辅助状态
};

// 日志写入函数
void QuadPlane::Log_Write_QControl_Tuning()
{
    struct log_QControl_Tuning pkt = {
        LOG_PACKET_HEADER_INIT(LOG_QCONTROL_TUNING_MSG),
        time_us             : AP_HAL::micros64(),
        throttle_in         : throttle_in,
        angle_boost         : angle_boost,
        throttle_out        : motors->get_throttle(),
        throttle_hover      : motors->get_throttle_hover(),
        desired_alt         : pos_control->get_pos_target_z_cm() * 0.01f,
        inav_alt           : inertial_nav.get_position_z_up_cm() * 0.01f,
        baro_alt           : baro_alt,
        target_climb_rate  : (int16_t)pos_control->get_vel_target_z_cms(),
        climb_rate         : (int16_t)inertial_nav.get_velocity_z_up_cms(),
        throttle_mix       : attitude_control->get_throttle_mix(),
        transition_state   : transition->get_log_transition_state(),
        assist            : assist.get_state()
    };
    
    plane.logger.WriteBlock(&pkt, sizeof(pkt));
}

七、VTOL二次开发示例

7.1 添加自定义VTOL模式

// 创建自定义VTOL模式类
class ModeQCustom : public ModeQStabilize {
public:
    Number mode_number() const override { return Number::QCUSTOM; }
    const char *name() const override { return "QCUSTOM"; }
    
    // 初始化
    bool init(bool ignore_checks) override {
        if (!ModeQStabilize::init(ignore_checks)) {
            return false;
        }
        
        // 自定义初始化
        custom_controller.reset();
        return true;
    }
    
    // 主运行函数
    void run() override {
        // 获取飞行员输入
        float target_roll, target_pitch;
        get_pilot_desired_lean_angles(target_roll, target_pitch);
        
        // 运行自定义控制算法
        Vector3f custom_angles = custom_controller.update(
            target_roll, 
            target_pitch,
            get_pilot_desired_yaw_rate_cds()
        );
        
        // 发送到姿态控制器
        attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(
            custom_angles.x,
            custom_angles.y,
            custom_angles.z
        );
        
        // 高度控制
        pos_control->set_pos_target_z_from_climb_rate_cm(
            get_pilot_desired_climb_rate_cms(), 
            false
        );
        
        // 运行姿态和位置控制器
        pos_control->update_z_controller();
        attitude_control->rate_controller_run();
        
        // 电机输出
        motors_output();
    }
    
private:
    CustomController custom_controller;
};

7.2 实现自适应过渡控制

class AdaptiveTransition : public SLT_Transition {
public:
    void update() override {
        // 调用基类更新
        SLT_Transition::update();
        
        // 自适应逻辑
        if (transition_state == TRANSITION_TIMER) {
            // 根据环境条件调整过渡参数
            adjust_transition_parameters();
            
            // 监测过渡质量
            monitor_transition_quality();
        }
    }
    
private:
    void adjust_transition_parameters() {
        // 获取环境参数
        float wind_speed = ahrs.wind_estimate().length();
        float air_density = get_air_density();
        
        // 动态调整过渡时间
        if (wind_speed > 10.0f) {
            // 强风条件下延长过渡时间
            quadplane.transition_time_ms = quadplane.transition_time_ms * 1.2f;
        }
        
        // 根据空气密度调整电机输出
        float density_ratio = air_density / 1.225f;
        motors->set_density_ratio(density_ratio);
    }
    
    void monitor_transition_quality() {
        // 监测姿态稳定性
        float attitude_error = attitude_control->get_attitude_error_angle();
        
        if (attitude_error > radians(30)) {
            // 姿态误差过大，增加VTOL辅助
            quadplane.assist.force_assist();
        }
        
        // 监测高度保持
        float alt_error = pos_control->get_pos_error_z_cm();
        
        if (fabsf(alt_error) > 200) {
            // 高度误差过大，调整垂直控制增益
            pos_control->set_pos_z_p(pos_control->get_pos_z_p() * 1.5f);
        }
    }
};

7.3 高级倾转控制算法

// 非线性倾转控制器
class NonlinearTiltController {
public:
    float compute_tilt_angle(float forward_speed, float vertical_speed) {
        // 基于速度矢量计算最优倾转角
        float speed_ratio = forward_speed / (forward_speed + fabsf(vertical_speed));
        
        // 使用S曲线实现平滑过渡
        float tilt_ratio = 1.0f / (1.0f + expf(-10.0f * (speed_ratio - 0.5f)));
        
        // 考虑功率优化
        float power_factor = compute_power_optimization(tilt_ratio);
        
        // 最终倾转角度
        float tilt_angle = tilt_ratio * max_tilt_angle * power_factor;
        
        return constrain_float(tilt_angle, 0, max_tilt_angle);
    }
    
private:
    float compute_power_optimization(float tilt_ratio) {
        // 基于倾转角度的功率效率曲线
        // 在某些角度下推进效率更高
        const float optimal_tilt = 0.7f;
        
        float efficiency = 1.0f - 0.2f * fabsf(tilt_ratio - optimal_tilt);
        
        return efficiency;
    }
    
    float max_tilt_angle = 90.0f;
};

八、VTOL系统优化建议

8.1 性能优化

1. 过渡算法优化：使用预测控制减少过渡时间
2. 能效优化：根据飞行包线动态调整推力分配
3. 轨迹优化：使用最优控制理论规划VTOL轨迹

8.2 可靠性提升

1. 冗余设计：支持电机失效后的应急控制
2. 故障检测：实时监测系统健康状态
3. 降级运行：支持部分功能失效后的安全飞行

8.3 扩展性考虑

1. 模块化架构：便于添加新的VTOL类型
2. 参数化设计：支持不同构型的快速适配
3. 仿真支持：完善的SITL仿真环境

总结

ArduPilot的VTOL系统展现了现代飞控系统的先进设计理念：

• 统一架构：将固定翼和多旋翼控制无缝集成
• 灵活配置：支持多种VTOL构型
• 智能过渡：自适应的模式切换算法
• 安全可靠：完善的故障处理机制