浙江省机器人竞赛“空中机器人”赛项系列文章（三）：动态避障

在上一篇文章中你的无人机学会了“看”——利用3D激光雷达判断前方是否有障碍。但这还远远不够。当省赛场地中的O2、O6立柱开始规律移动当你的无人机需要在狭窄通道中与动态障碍共舞时简单的“停”或“绕”将瞬间失效。动态避障是空中机器人从“自动化”迈向“智能化”的关键一跃也是本届省赛的核心难点之一。灵智实验室在本篇将为你彻底揭开动态避障的技术面纱。我们将深入解析控制库 offboard_control_lib.py中已实现的3D点云多目标跟踪与动态窗口算法DWA手把手教你如何让无人机拥有“预判”能力在复杂动态环境中游刃有余。回顾从静态感知到动态挑战系列二中我们使用了 process_3d_radar_data函数。它是一个出色的“静态快照”分析器能告诉你此刻前方是否畅通。然而对于移动的障碍物它存在天然局限无目标标识无法区分障碍物A和B更无法知道上一帧的A就是这一帧的A。无速度信息无法获知障碍物的运动速度和方向因而无法预测其下一步位置。决策滞后基于瞬间状态的决策在高速动态场景下极易失效可能导致无人机“撞向障碍物的未来位置”。要解决这些问题我们需要一套完整的感知-跟踪-预测-规划流水线。这正是我们控制库中 pointcloud_callback和 compute_dwa等函数所构建的体系。第一部分智能感知——3D点云多目标跟踪流水线当3D激光雷达的原始点云数据流入系统控制库通过一系列精密的处理步骤将其转化为对环境中每一个障碍物的结构化认知。整个过程如下图所示形成了一个从数据到决策的完整闭环1.1 点云预处理去芜存菁原始点云包含大量噪声。preprocess_pointcloud函数是第一位“清道夫”1.地面过滤 (ground_z_thresh): 去除接近地面的点FLU坐标系中Z值较小的点这些点通常来自地面反射对避障无意义。2.距离过滤 (min_range, max_range): 只保留有效测距范围内的点忽略过近可能是机身和过远的噪声。3.前方扇形聚焦 (forward_range, lateral_range): 这是关键优化并非处理所有方向点云而是只关注无人机正前方扇形区域内的点。这大幅减少了计算量让算法能运行在机载计算机上。参数 forward_range6和 lateral_range8意味着关注前方6米、左右各4米范围内的障碍物。1.2 聚类与跟踪识别并锁定每一个目标预处理后的点云是一团“散沙”。我们需要知道哪些点属于同一个物体。DBSCAN聚类cluster_points函数使用DBSCAN算法。它将彼此距离小于 dbscan_eps(例如0.35米) 的点归为同一簇。这能很好地将一个障碍物如直径0.4米的柱子的点云从背景中分离出来并计算出该簇的中心点(x, y, z)和包围盒尺寸(dx, dy, dz)。多目标跟踪track_and_compute_speed是核心。它通过数据关联解决“哪一个是哪一个”的问题。关联将当前帧检测到的障碍物中心点与上一帧跟踪列表中的障碍物位置进行匹配基于最近邻原则阈值association_threshold。匹配成功则认为是同一个物体更新其信息匹配失败则视为新出现的障碍物分配新ID。速度估计对于匹配上的障碍物根据其位置变化和两帧之间的时间差 dt可以计算出瞬时速度。通过低通滤波参数alpha平滑速度数据得到稳定可靠的速度估计 (vx, vy, vz)。这是预测的基础。状态管理每个被跟踪的障碍物都有一个miss_count。如果连续多帧max_miss_frames未被检测到则将其从跟踪列表中移除意味着它可能已离开视野或消失。1.3 障碍物分类与决策支持跟踪之后我们对障碍物有了深刻认知。classify_and_print_obstacles函数根据尺寸和速度对其进行智能分类墙 (walls): 尺寸dx, dy, dz任一大于0.5米的物体。通常是场地边界。静止柱子 (static_pillars): 尺寸较小且速度小于0.1 m/s的物体。例如场地中固定的障碍柱。移动柱子 (moving_pillars): 尺寸较小但速度大于0.1 m/s的物体。这就是省赛中的动态障碍O2、O6分类带来了巨大的决策优势。例如在代码的 pointcloud_callback末尾我们调用 widest_corridor_between_walls函数。这个函数利用静止柱子static_pillars的位置信息在两侧的“墙”之间计算出当前最宽阔、可安全通行的走廊中轴线坐标y_star。这为无人机提供了高层导航的绝佳参考路径——沿着最宽阔的地方走总是更安全。第二部分智慧决策——动态窗口算法DWA详解感知系统告诉我们“世界是什么样”而DWA算法则决定“现在该怎么飞”。compute_dwa函数实现了这一经典局部规划器。2.1 DWA的核心思想DWA的智慧在于它不在庞大的地理空间中搜索路径而是在无人机当前可达的速度空间 (vx, vy, w) 中进行搜索。它模拟无人机在未来短时间内predict_time如2秒以不同速度组合运动产生的轨迹并给每条轨迹打分最后选择分数最高的那组速度来执行。2.2 评分标准安全、快速、对准目标DWA为每条模拟轨迹从三个维度评分1.朝向目标 (heading_score): 轨迹终点朝向与目标点方向的接近程度。分值越高越对准目标。2.前进速度 (vel_score): 轨迹的平均速度大小。在安全的前提下鼓励更快移动。3.远离障碍 (clear_score): 轨迹上所有点与最近障碍物的距离。距离越远越安全。4.抵达目标 (goal_score): 轨迹终点与局部目标点的距离。鼓励靠近目标。最终得分是这些分项的加权和alpha, beta, gamma。通过调整权重你可以定义无人机的“性格”是激进还是保守。2.3 与感知的融合DWA算法依赖感知系统提供的障碍物信息。在代码中它通过将当前2D激光雷达数据lidar_scan转换为障碍物点列表并构建KD-Tree (cKDTree) 来快速查询模拟轨迹上的点与所有障碍物的最近距离。这正是动态避障的关键用于碰撞检测的障碍物信息是实时更新的包含了移动障碍物的最新位置。第三部分实战整合——在省赛任务中应用动态避障理论已备现在来看如何在你自己的比赛脚本中调用这些强大的功能。3.1 启用高级感知与跟踪控制库的感知跟踪功能默认在 pointcloud_callback中通过一个开关 self.mod控制。你需要先在主程序中打开它def main(): vehicle Vehicle() vehicle.drone.arm() vehicle.drone.takeoff(1.5) # 关键步骤启用动态感知与跟踪模式 vehicle.drone.mod True try: # 此时vehicle.drone的以下属性将被实时更新 # - static_pillars: 字典包含所有静止柱子的ID、位置、尺寸、速度。 # - moving_pillars: 字典包含所有移动柱子的ID、位置、尺寸、速度。 # - corridor_y: 浮点数基于当前静止柱子计算出的建议走廊中轴y坐标。 # - pillars_avoidance: 列表只包含位于前方避障区域(1x4.5, y0.5)的静止柱子坐标。 time.sleep(1.0) # 给感知系统一点时间初始化 # 示例打印当前检测到的移动柱子 if vehicle.drone.moving_pillars: for oid, data in vehicle.drone.moving_pillars.items(): pos data[pos] vel data[vel] print(f移动柱子 ID{oid}: 位置[{pos[0]:.1f}, {pos[1]:.1f}], 速度[{vel[0]:.1f}, {vel[1]:.1f}] m/s) else: print(未检测到移动柱子。)3.2 调用DWA进行动态避障飞行基础的位置指令 fly_to_trajectory_setpoint是“盲飞”。要启用避障必须使用DWA版本# 目标点穿越动态障碍区前往(4.0, 3.0, 1.5) target_x, target_y, target_z 4.0, 3.0, 1.5 target_yaw math.radians(90.0) # 使用DWA避障模式飞向目标 # 此函数内部会1. 检查前方安全距离2. 触发DWA计算3. 发布速度控制指令。 success vehicle.drone.fly_to_trajectory_setpoint_dwa(target_x, target_y, target_z, target_yaw, timeout30.0) if success: print(成功抵达目标并规避了所有动态障碍) else: print(飞行任务超时或中断。)fly_to_trajectory_setpoint_dwa函数内部逻辑是智能切换的当雷达检测到前方safe_dist默认1.5米内有障碍时自动切换到DWA速度控制模式。当障碍物离开恢复为位置控制模式直飞目标。在DWA模式下它每秒调用compute_dwa函数数次根据最新的目标位置和实时障碍物信息计算出最优的 (vx_enu, vy_enu, vz_enu, yaw_rate_enu)速度指令实现流畅绕行。3.3 完整实战代码示例结合以上所有知识点一个应对省赛动态区域的完整任务片段如下import rclpy import math from offboard_control_lib import Vehicle import time import numpy as np def main(): vehicle Vehicle() vehicle.drone.arm() vehicle.drone.takeoff(1.5) vehicle.drone.mod True # 启用高级感知跟踪 time.sleep(2.0) # 等待感知系统稳定 try: print( 阶段1: 前往动态区域边缘 ) # 先飞到动态区域前的观测点 vehicle.drone.fly_to_trajectory_setpoint(1.0, vehicle.drone.corridor_y, 1.5, math.radians(90.0)) print(f已到达观测点建议走廊中轴: y{vehicle.drone.corridor_y:.2f}) print( 阶段2: 观察动态障碍物 ) for _ in range(5): # 观察5秒 if vehicle.drone.moving_pillars: for oid, data in vehicle.drone.moving_pillars.items(): pos data[pos] if 1.0 pos[0] 4.0: # 如果移动柱子进入目标区域 print(f监测到动态柱子{oid}在区域内速度{np.linalg.norm(data[vel]):.2f}m/s) time.sleep(1) print( 阶段3: 启动DWA穿越动态区域 ) # 目标点设在动态区域另一侧 final_x, final_y 4.5, vehicle.drone.corridor_y success vehicle.drone.fly_to_trajectory_setpoint_dwa(final_x, final_y, 1.5, math.radians(90.0), timeout45.0) if success: print( 阶段4: 穿越成功准备降落 ) vehicle.drone.hover(2.0) vehicle.drone.land() else: print(!!! 穿越动态区域失败执行紧急悬停 !!!) vehicle.drone.hover(5.0) # 此处可加入紧急处理逻辑如尝试原路返回 vehicle.drone.land(vehicle.drone.home_position[0], vehicle.drone.home_position[1], 0.0) except KeyboardInterrupt: print(程序被用户中断紧急降落...) vehicle.drone.land() finally: vehicle.close() if __name__ __main__: main()总结与下篇预告通过本篇你已经掌握了让无人机在动态环境中生存的核心科技实时3D多目标跟踪赋予了无人机识别与预测移动障碍物的“慧眼”动态窗口算法DWA则赋予了它在瞬息万变的环境中实时规划局部安全路径的“大脑”。二者结合你的无人机已具备应对省赛中最棘手的动态障碍挑战的能力。然而一个顶尖的空中机器人不仅需要“避得开”还需要“穿得快”。省赛中的竞速门是比拼速度与精度的关键环节。在下一篇 系列四竞速门穿越篇 中灵智实验室将带你探索视觉识别精准定位如何利用机载相机在高速运动中稳定检测并定位竞速门框。高速穿越控制策略如何设计轨迹生成与控制算法让无人机以最优姿态和速度平稳、精准地穿过狭窄的门框。感知-控制的紧耦合如何将视觉识别结果实时反馈给飞控实现闭环的穿越控制确保百发百中。从“避障”到“穿门”让我们共同挑战速度与精度的极限。灵智实验室深耕机器人感知与决策核心技术助力创新赋能未来。