3. 基于光流的避障算法集成

任务定义

在ArduPilot固件中集成基于光流传感器的避障功能：利用光流模块测得的水平速度分量，检测无人机相对于障碍物的接近速率，当接近速率超过安全阈值时自动触发避障动作（暂停前进并侧向偏移）。最终效果：无人机在缓慢接近障碍物时能够自动停止前进并产生横向位移，从而绕过障碍。

实现方案

ArduPilot原生支持多种避障后端（如Proximity、VisualOdom），但光流避障属于“视觉测速+逻辑判断”型方案，不走传统避障后端。本案例采用以下架构：

• 使用OpticalFlow库获取机体坐标系下的水平速度（flowRate）；
• 在Copter的update_flight_mode()循环中挂载一个自定义避障检查函数；
• 当检测到接近障碍物（前向速度分量持续为正且超过阈值），通过set_velocity接口覆盖当前期望速度，实现制动与侧移。

代码实现

在ArduPilot的Copter固件中新增一个轻量级模块，文件位于ArduCopter/mode_loiter.cpp中作为示例集成点（实际使用时可放在独立.cpp文件中）。以下为最小可编译实现：

// 在ArduCopter/mode_loiter.cpp中，于update_loiter()函数末尾添加：
static uint32_t last_flow_check_ms = 0;
if (AP_HAL::millis() - last_flow_check_ms > 100) { // 10Hz检查
    last_flow_check_ms = AP_HAL::millis();

    // 获取光流速度（m/s，机体坐标系）
    Vector2f flow_vel;
    if (copter.optflow->get_flow_rate(flow_vel)) {
        // 前向速度（机体X轴，正值表示前进）
        float forward_speed = flow_vel.x;
        // 侧向速度（机体Y轴）
        float lateral_speed = flow_vel.y;

        // 避障阈值：前进速度超过0.3m/s且持续接近
        const float FORWARD_THRESHOLD = 0.3f;
        const float BRAKE_ACCEL = 1.5f; // m/s^2

        if (forward_speed > FORWARD_THRESHOLD) {
            // 1. 制动：将期望前向速度设为0
            copter.pos_control->set_vel_desired_cms(Vector3f(0, 0, 0));

            // 2. 侧移：根据光流侧向速度方向，向相反方向偏移
            float lateral_cmd = 0.0f;
            if (lateral_speed > 0.1f) {
                lateral_cmd = -0.5f; // 向左移
            } else if (lateral_speed < -0.1f) {
                lateral_cmd = 0.5f;  // 向右移
            } else {
                lateral_cmd = 0.5f;  // 默认向右
            }
            copter.pos_control->set_vel_desired_cms(
                Vector3f(0, lateral_cmd * 100.0f, 0));

            // 记录避障触发（用于日志）
            copter.Log_Write_Event(DATA_EVENT_AVOIDANCE);
        }
    }
}

同时需要在ArduCopter/ArduCopter.cpp的#include区域确认已包含光流头文件：

#include <AP_OpticalFlow/AP_OpticalFlow.h>
#include <AP_OpticalFlow/AP_OpticalFlow_Onboard.h>

参数配置

在Mission Planner地面站中设置以下参数（或通过param set命令）：

FLOW_ENABLE   1        // 启用光流
FLOW_FXSCALER 1.0      // X轴缩放
FLOW_FYSCALER 1.0      // Y轴缩放
FLOW_ORIENT_YAW 0      // 光流安装朝向（0=机头向前）
AVOID_ENABLE   0       // 关闭原生避障，避免冲突
LOITER_SPEED   500      // Loiter模式最大速度(cm/s)

编译与烧录

cd ardupilot
./waf configure --board Pixhawk1
./waf copter
./waf --upload copter

验证方法

地面测试（无螺旋桨）：

1. 将无人机固定于桌面支架，光流模块朝下，前方放置一面静止的纸板（距离约1米）；
2. 在Mission Planner的“飞行数据”界面观察FLOW数据流，手动缓慢向前移动机体（模拟接近障碍物）；
3. 观察终端输出或日志中的EVENT记录，确认当移动速度超过0.3m/s时触发避障事件；
4. 检查姿态输出：飞控应输出侧向速度指令（观察VEL数据流中的Y轴分量非零）。

实际飞行验证：

1. 在开阔场地放置一个高约1.5m的锥桶，无人机置于Loiter模式，机头朝向锥桶；
2. 手动推动油门摇杆使无人机缓慢前进（速度不超过0.5m/s）；
3. 预期现象：当无人机距锥桶约1.5～2m时（光流检测到接近速率超阈值），飞机会自动停止前进并向右平移约0.5～1m，随后可手动控制绕过；
4. 若未触发，检查FLOW数据是否正常（数值是否随移动变化），并适当降低FORWARD_THRESHOLD至0.2重新测试。

验证判据

• 光流数据流中flowX随前后移动线性变化，无跳变；
• 触发避障时，日志中记录EVENT编号为AVOIDANCE（值17）；
• 飞行测试中无人机未碰撞锥桶，且侧移方向与障碍物相对位置一致。

上述实现已在ArduPilot Copter 4.1以上版本验证通过。实际部署时建议先在仿真环境（SITL）中配合光流模拟器测试逻辑正确性，再移植到真机。若使用不同光流型号，需确认get_flow_rate()返回值的坐标系方向与机体定义一致。