📚 课程目录第1章基于距离传感器的静态障碍物检测与避障

1. 基于距离传感器的静态障碍物检测与避障

任务定义

在ArduPilot固件中集成一个基于距离传感器的静态障碍物检测与避障模块。目标：当无人机前方2米内出现障碍物时，自动停止前向飞行并悬停，障碍物移除后恢复原飞行指令。本案例使用AP_Periph方式实现传感器数据注入，通过AP_Avoidance库完成避障逻辑，最终在SITL仿真中验证完整闭环。

实现方案

采用ArduPilot的AP_Avoidance前端与AP_Proximity后端架构。距离传感器数据通过MAVLink的DISTANCE_SENSOR消息注入，由AP_Proximity_MAVLink后端解析，再经AP_Avoidance生成制导指令覆盖原飞控指令。静态障碍物场景下，仅需处理水平面正前方的探测扇区。

代码实现

在ArduPilot的libraries/AP_Avoidance目录下新增静态避障策略文件：

// libraries/AP_Avoidance/AP_Avoidance_StaticObstacle.h
#pragma once
#include <AP_Avoidance/AP_Avoidance.h>

class AP_Avoidance_StaticObstacle : public AP_Avoidance {
public:
    AP_Avoidance_StaticObstacle() : AP_Avoidance() {}
    
    // 仅处理正前方±30°扇区，距离阈值2米
    bool obstacle_in_front() const {
        AP_Proximity &prox = AP::proximity();
        if (!prox.is_enabled()) return false;
        
        float distance = 0;
        if (!prox.get_horizontal_distance(0, distance)) return false; // 0°为正前方
        return (distance < 2.0f);
    }
    
protected:
    // 覆写制导决策：有障碍物时输出停止，否则输出原指令
    bool handle_avoidance(const Location ¤t_loc,
                          const Vector3f &target_vel,
                          Vector3f &avoid_vel) override {
        if (obstacle_in_front()) {
            avoid_vel.zero();  // 悬停
            return true;       // 已处理
        }
        return false;          // 不干预原指令
    }
};

在SITL仿真中，通过Python脚本模拟距离传感器数据注入：

# sitl_obstacle_test.py
from pymavlink import mavutil
import time, math

master = mavutil.mavlink_connection('tcp:127.0.0.1:5760')
master.wait_heartbeat()

def send_distance(distance_m):
    """发送DISTANCE_SENSOR消息，id=0表示正前方传感器"""
    master.mav.distance_sensor_send(
        time_boot_ms=int(time.time()*1000),
        min_distance=20,      # 0.2m
        max_distance=400,     # 4m
        current_distance=int(distance_m*100),
        type=mavutil.mavlink.MAV_DISTANCE_SENSOR_LASER,
        id=0,
        orientation=mavutil.mavlink.MAV_SENSOR_ROTATION_NONE,
        covariance=0
    )

# 测试序列：前方3米→2米→1.5米→3米
for dist in [3.0, 2.0, 1.5, 3.0]:
    send_distance(dist)
    time.sleep(3)
    print(f"Sent distance: {dist}m")

落地配置

在SITL启动参数中启用避障与距离传感器后端：

# 启动SITL（终端1）
sim_vehicle.py -v ArduCopter --map --console

# 在MAVProxy中设置参数（终端2）
param set AVOID_ENABLE 1
param set PRX_TYPE 4        # MAVLink后端
param set PRX_ORIENT 0      # 仅正前方
param set PRX_MIN 0.2
param set PRX_MAX 4.0
param set AVOID_MARGIN 2.0  # 避障距离阈值
reboot

编译并运行测试脚本：

cd ardupilot
./waf configure --board sitl
./waf copter
sim_vehicle.py -v ArduCopter --console --map
# 等待飞控就绪后，另开终端执行：
python sitl_obstacle_test.py

验证方法

在MAVProxy中执行以下操作并观察现象：

# 1. 切换到LOITER模式并解锁
mode loiter
arm throttle
# 2. 发送前向速度指令
rc 3 1500
# 3. 观察避障行为
# 当脚本发送距离1.5m时，无人机应停止前向运动
# 当距离恢复3m时，无人机恢复前向飞行

预期验证结果：

当注入距离≥2m时，无人机正常执行前向飞行指令，地面站显示地速>0.5m/s
当注入距离<2m时，无人机自动悬停，地速降至0.1m/s以下，MAVProxy输出AVOID: stopping日志
距离恢复至3m后，无人机在2秒内恢复前向飞行
通过proximity status命令可查看传感器健康状态为OK

关键实现说明

本案例通过AP_Avoidance_StaticObstacle类覆写制导决策函数，将距离传感器数据直接映射为飞控速度指令。传感器数据注入采用MAVLink标准协议，无需修改飞控底层驱动，便于在真实硬件上通过外接数传模块快速部署。避障阈值AVOID_MARGIN可根据机型制动距离调整，本案例中2米对应SITL默认四旋翼的急停距离。