📚 课程目录第2章基于视觉的障碍物检测与避障

2. 基于视觉的障碍物检测与避障

任务说明

在ArduPilot固件中集成视觉障碍物检测模块，通过机载摄像头获取图像，识别前方障碍物并生成避障指令。最终实现：当检测到障碍物距离小于安全阈值时，飞控自动切换至避障模式并输出转向指令，同时在地面站显示检测结果。

硬件与依赖

Pixhawk 4飞控（运行ArduPilot 4.1+）
树莓派4B（运行Ubuntu 20.04，作为视觉处理单元）
CSI摄像头（640×480@30fps）
串口连接：树莓派GPIO14(TX)→Pixhawk TELEM2 RX，GPIO15(RX)→TELEM2 TX，共地
依赖库：OpenCV 4.5、MAVSDK-Python 0.11

实现步骤

1. 视觉检测节点（树莓派端）

创建 vision_obstacle.py，使用YOLOv4-tiny检测障碍物，通过MAVLink协议将障碍物距离与方位发送至飞控：

import cv2
import numpy as np
import time
from pymavlink import mavutil

# 加载YOLO模型
net = cv2.dnn.readNet("yolov4-tiny.weights", "yolov4-tiny.cfg")
layer_names = net.getLayerNames()
output_layers = [layer_names[i[0]-1] for i in net.getUnconnectedOutLayers()]

# 连接飞控（TELEM2波特率57600）
master = mavutil.mavlink_connection('/dev/serial0', baud=57600)
master.wait_heartbeat()

# 摄像头参数（水平FOV=62°）
FOV_H = np.radians(62)
IMG_W = 640
FOCAL_X = IMG_W / (2 * np.tan(FOV_H/2))

cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, IMG_W)
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)

while True:
    ret, frame = cap.read()
    if not ret:
        continue
    
    h, w = frame.shape[:2]
    blob = cv2.dnn.blobFromImage(frame, 0.00392, (416, 416), (0, 0, 0), True, crop=False)
    net.setInput(blob)
    detections = net.forward(output_layers)
    
    obstacle_found = False
    min_dist = 999.0
    obstacle_angle = 0.0
    
    for detection in detections:
        for obj in detection:
            scores = obj[5:]
            class_id = np.argmax(scores)
            confidence = scores[class_id]
            # 只检测人(0)、自行车(1)、汽车(2)
            if confidence > 0.5 and class_id in [0, 1, 2]:
                center_x = int(obj[0] * w)
                center_y = int(obj[1] * h)
                box_w = int(obj[2] * w)
                box_h = int(obj[3] * h)
                
                # 基于框宽估算距离（假设目标平均宽度1.8m）
                target_width_m = 1.8
                dist = (target_width_m * FOCAL_X) / box_w
                
                # 计算目标相对光轴的角度
                angle = np.arctan((center_x - IMG_W/2) / FOCAL_X)
                
                if dist < min_dist:
                    min_dist = dist
                    obstacle_angle = angle
                    obstacle_found = True
                
                cv2.rectangle(frame, (center_x-box_w//2, center_y-box_h//2),
                            (center_x+box_w//2, center_y+box_h//2), (0,255,0), 2)
                cv2.putText(frame, f"{dist:.1f}m", (center_x-20, center_y-30),
                           cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0,255,0), 2)
    
    # 发送障碍物距离信息（MAVLink DISTANCE_SENSOR消息）
    if obstacle_found:
        master.mav.distance_sensor_send(
            0,                      # time_boot_ms（由飞控忽略）
            10,                     # min_distance (cm)
            500,                    # max_distance (cm)
            int(min_dist*100),      # current_distance (cm)
            0,                      # type: MAV_DISTANCE_SENSOR_LASER
            0,                      # id
            0,                      # orientation: 前向
            0,                      # covariance
            0,                      # horizontal_fov
            0,                      # vertical_fov
            [0,0,0]                 # quaternion
        )
        
        # 同时发送障碍物方位（自定义MAVLink消息，使用COMMAND_LONG模拟）
        master.mav.command_long_send(
            master.target_system, master.target_component,
            mavutil.mavlink.MAV_CMD_CONDITION_YAW,
            0, obstacle_angle, 0, 0, 0, 0, 0
        )
    
    cv2.imshow("Obstacle Detection", frame)
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

cap.release()
cv2.destroyAllWindows()

2. ArduPilot端避障逻辑配置

在飞控参数中启用避障功能，并设置安全距离阈值：

# 通过Mission Planner或MAVProxy连接飞控后执行
param set PRX_TYPE 1          # 启用距离传感器
param set PRX_ORIENT 0        # 传感器朝前
param set PRX_MIN_CM 20       # 最小检测距离20cm
param set PRX_MAX_CM 500      # 最大检测距离5m
param set AVOID_ENABLE 1      # 启用避障
param set AVOID_MARGIN 2.0    # 安全距离2m
param set AVOID_BEHAVE 1      # 避障行为：水平避让
param set WPNAV_SPEED 500     # 巡航速度5m/s
param set WPNAV_ACCEL 100     # 加速度1m/s²
param save

3. 启动与运行

# 树莓派端（需root权限访问串口）
sudo chmod 666 /dev/serial0
python3 vision_obstacle.py

# 飞控端（已预装ArduPilot固件）
# 在Mission Planner中连接飞控，确认PRX_TYPE=1且AVOID_ENABLE=1生效

验证方法

静态测试：在飞控前方1.5m处放置一个0.5m×0.5m纸箱，观察树莓派终端输出，应显示检测框并标注距离约1.5m；Mission Planner的Status页面中PRX.curr应显示150cm。
动态避障测试：将飞控设置为Loiter模式，在飞行路径上放置障碍物，当无人机接近至2m安全距离时，应自动横向偏移绕过障碍物；Mission Planner地图中可看到航迹偏移。
数据验证：在Mission Planner的MAVLink Inspector中查看DISTANCE_SENSOR消息，确认current_distance字段随障碍物距离变化，且orientation=0表示前向检测。

预期结果：当障碍物进入5m检测范围时，飞控在2m处触发避障动作，无人机横向偏移量≥1m后恢复原航线，全程无人工干预。