📚 课程目录第4章基于3D点云的避障算法集成

4. 基于3D点云的避障算法集成

任务定义

在ArduPilot/Copter 4.5及以上版本中，集成基于3D点云的避障算法。目标是通过激光雷达（如Livox Mid-40）或RGB-D相机获取环境3D点云，经机载计算机（树莓派4B/Orin Nano）处理后，将障碍物信息注入ArduPilot的避障系统，使无人机在自动模式下能够绕开障碍物飞行。最终验证方式：无人机在模拟环境中面对墙体时，航向发生偏转并绕行通过。

系统架构与数据流

本案例采用“机载计算机+飞控”分离架构。机载计算机运行点云处理与避障决策算法，通过串口（TELEM2）以MAVLink协议与飞控通信。飞控端启用OA_TYPE=2（外置避障），接收机载计算机发送的障碍物距离数据。

飞控端配置

通过Mission Planner地面站连接飞控，在配置参数中设置以下关键参数：

SERIAL2_PROTOCOL = 2      // MAVLink2
SERIAL2_BAUD     = 921    // 921600波特率
OA_TYPE          = 2      // 使用外置避障
OA_DB_OUTPUT     = 1      // 输出避障方向
PRX_TYPE         = 1      // 距离传感器启用
AVOID_ENABLE     = 7      // 启用水平+垂直避障

机载计算机端实现

在机载计算机上创建Python脚本obstacle_avoidance_3d.py，实现点云处理与避障决策。核心流程：接收点云→体素滤波→地面分割→障碍物聚类→生成距离数据→MAVLink发送。

#!/usr/bin/env python3
import numpy as np
import open3d as o3d
from pymavlink import mavutil
import time

# ---------- MAVLink连接 ----------
master = mavutil.mavlink_connection('/dev/ttyUSB0', baud=921600)
master.wait_heartbeat()
print("飞控已连接")

# ---------- 点云处理参数 ----------
VOXEL_SIZE = 0.2        # 体素滤波尺寸(m)
GROUND_Z   = -0.3       # 地面高度阈值(m)
MAX_RANGE  = 10.0       # 最大探测距离(m)
FOV_ANGLE  = 60         # 避障视场角(度)

def process_pointcloud(points):
    """输入Nx3点云，输出障碍物距离数组(0-360度)"""
    # 1. 体素滤波降采样
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd = pcd.voxel_down_sample(VOXEL_SIZE)
    
    # 2. 去除地面点
    pts = np.asarray(pcd.points)
    pts = pts[pts[:, 2] > GROUND_Z]
    
    # 3. 按角度分箱，计算最近障碍物距离
    distances = np.full(360, MAX_RANGE, dtype=np.float32)
    if len(pts) == 0:
        return distances
    
    # 计算方位角(度)
    angles = np.degrees(np.arctan2(pts[:, 1], pts[:, 0]))
    angles = (angles + 360) % 360
    
    # 计算水平距离
    horiz_dist = np.sqrt(pts[:, 0]**2 + pts[:, 1]**2)
    
    # 限制在视场角内
    mask = np.abs(angles - 90) < FOV_ANGLE  # 假设机头朝+Y
    angles, horiz_dist = angles[mask], horiz_dist[mask]
    
    # 分箱取最小值
    for ang, dist in zip(angles, horiz_dist):
        idx = int(ang)
        if dist < distances[idx]:
            distances[idx] = dist
    
    return distances

def send_distance_data(distances):
    """通过MAVLink发送DISTANCE_SENSOR消息"""
    # 每45度取一个代表值，共8个方向
    for i in range(8):
        sector_start = i * 45
        sector_end = sector_start + 45
        sector_dist = np.min(distances[sector_start:sector_end])
        
        master.mav.distance_sensor_send(
            0,                      # 时间戳(ms)
            0,                      # 最小距离(cm)
            int(MAX_RANGE * 100),   # 最大距离(cm)
            int(sector_dist * 100), # 当前距离(cm)
            0,                      # 类型:超声波
            0,                      # ID
            i + 1,                  # 朝向(1=前, 3=右, 5=后, 7=左)
            0)                      # 协方差

# ---------- 主循环 ----------
def main():
    # 模拟点云数据源(实际使用中替换为LiDAR驱动)
    # 此处生成一个前方5米处有墙体的场景
    print("开始避障运行...")
    
    while True:
        # 模拟生成点云：前方5米处有一面墙
        x = np.full(1000, 5.0)
        y = np.random.uniform(-3, 3, 1000)
        z = np.random.uniform(-0.5, 2.0, 1000)
        points = np.column_stack([x, y, z])
        
        # 处理点云
        distances = process_pointcloud(points)
        
        # 发送给飞控
        send_distance_data(distances)
        
        # 打印前方距离
        front_dist = np.min(distances[80:100])
        print(f"前方障碍物距离: {front_dist:.2f} m")
        
        time.sleep(0.1)  # 10Hz更新率

if __name__ == "__main__":
    main()

运行与验证

运行步骤：

将上述Python脚本复制到机载计算机，安装依赖：pip install open3d pymavlink numpy
用USB线连接飞控TELEM2口与机载计算机，确认串口设备号（如/dev/ttyUSB0）
启动脚本：python3 obstacle_avoidance_3d.py
在Mission Planner中打开模拟器（或连接真实无人机），切换到Auto模式并设置一个前方有障碍物的任务

验证标准：

脚本控制台持续输出“前方障碍物距离: 5.00 m”，数值稳定无跳变
Mission Planner的MAVLink Inspector中可看到DISTANCE_SENSOR消息持续到达，orientation字段值1-8对应8个方向
无人机在自动飞行接近墙体时，航向自动偏转，飞行轨迹绕过障碍物区域
当障碍物距离小于安全阈值（默认2m）时，无人机减速并悬停等待

故障排查：若飞控未响应避障指令，检查OA_TYPE是否已设为2，且PRX_TYPE已启用；确认MAVLink消息中的坐标系与飞控期望一致（机头方向为0度，顺时针增加）。