SUPER 避障飞行测试流程 | SUPER Obstacle-Avoidance Flight Test Procedure

SUPER Obstacle-Avoidance Flight Test Procedure | 无人机避障飞行测试流程

概述

本测试使用：

Livox MID-360S
FAST-LIO SLAM
PX4 + MAVROS
SUPER planner
super_to_mavros_position 桥接

目标：测试 SUPER 短轨迹运动和避障能力，同时保留 RC 飞行员控制作为紧急后备。

警告重要：使用真实飞行规划器配置 click_slow_f450.yaml，不要使用 bench 配置如 click_slow_f450_bench_r020.yaml。

测试区域设置

推荐首次设置：
无人机悬停点 → 1.5m 净空 → 障碍物/墙壁

侧向净空：
左侧：至少 0.8–1.0m 开放
右侧：至少 0.8–1.0m 开放

推荐悬停高度： 0.6–0.8m

首次目标距离： 0.20m
成功后尝试：0.50m
不要从靠近墙壁的长距离目标开始。

终端 1 — 启动正常 SLAM 堆栈 + Bag

tmux kill-session -t slam 2>/dev/null || true
~/start_slam_takeoff_test_mid360s.sh

等待约 60 秒后再继续。

终端 2 — 飞行前健康检查

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

echo "=== FAST-LIO ==="
timeout 5 ros2 topic hz /Odometry

echo "=== REGISTERED CLOUD ==="
timeout 5 ros2 topic hz /cloud_registered

echo "=== PX4 EV INPUT ==="
timeout 5 ros2 topic hz /mavros/odometry/out

echo "=== PX4 LOCAL ODOMETRY ==="
timeout 5 ros2 topic hz /mavros/local_position/odom

echo "=== VEHICLE STATE ==="
ros2 topic echo /mavros/state --once

预期：
/Odometry: ~10 Hz
/cloud_registered: ~10 Hz
/mavros/odometry/out: ~10 Hz
/mavros/local_position/odom: ~30 Hz
车辆状态：已连接、已上锁、通常为 STABILIZED 或 POSCTL

终端 3 — 启动 SUPER Planner

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

ros2 run super_planner fsm_node \
 --ros-args \
 -p config_name:=click_slow_f450.yaml \
 > /tmp/super_planner_obstacle.log 2>&1

保持此终端运行。

终端 4 — 监控 SUPER Planner

tail -f /tmp/super_planner_obstacle.log | grep -E 'Get goal|SUCCESS|FAILED|min dis|GeneratePolytope|PlanFromRest|FOLLOW_TRAJ|Traj finish'

良好信号：
Get goal · GenerateExpTrajectory SUCCESS · FOLLOW_TRAJ · ReplanOnce succeed · Traj finish

不良信号：
GeneratePolytopeFromLine failed · PlanFromRest failed · min dis... · FAILED · OPT_FAILED

如果反复出现不良信号，不要继续自主运动。

终端 5 — 启动 SUPER-to-MAVROS 桥接

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

ros2 run drobotics_super_bridge super_to_mavros_position \
 --ros-args \
 -p setpoint_rate_hz:=20.0 \
 -p odom_timeout_s:=0.50 \
 -p super_command_timeout_s:=0.50 \
 -p max_odom_jump_m:=0.75 \
 -p max_command_jump_m:=0.50 \
 -p max_mission_xy_m:=2.00 \
 -p max_mission_z_m:=0.75 \
 -p max_setpoint_speed_mps:=0.10 \
 -p max_yaw_rate_rps:=0.50 \
 -p follow_super_z:=false \
 -p follow_super_yaw:=false

预期消息：Prestreaming current PX4 XYZ and yaw at 20.0 Hz.

在 Position 模式下手动起飞，稳定后选择 OFFBOARD。

通过终端 2 — 确认设定点正在发布

echo "=== SETPOINT RATE ==="
timeout 6 ros2 topic hz /mavros/setpoint_position/local

echo "=== SETPOINT PUBLISHER ==="
ros2 topic info /mavros/setpoint_position/local --verbose

预期：约 20 Hz · 发布者：super_to_mavros_position

飞行操作

警告准备 RC 接管。

1. 在 POSCTL 中解锁
2. 手动起飞
3. 在约 0.6–0.8m 处稳定悬停
4. 确认无马桶效应或盘旋
5. 切换至 OFFBOARD
6. 等待 1-2 秒，无人机应保持位置
7. 从终端 6 发布 SUPER 目标

终端 6 — 发布小型避障目标

首次测试：仅前进 0.20m

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

read CURRENT_X CURRENT_Y CURRENT_Z < <(
 ros2 topic echo /Odometry --once --field pose.pose.position |
 awk '/^x:/ {x=$2} /^y:/ {y=$2} /^z:/ {z=$2} END {print x, y, z}'
)

GOAL_X=$(awk -v x="$CURRENT_X" 'BEGIN {printf "%.6f", x + 0.20}')

echo "Current: $CURRENT_X $CURRENT_Y $CURRENT_Z"
echo "Goal: $GOAL_X $CURRENT_Y $CURRENT_Z"

ros2 topic pub \
 --rate 2 \
 --times 3 \
 --qos-reliability best_effort \
 /goal_pose \
 geometry_msgs/msg/PoseStamped \
 "{header: {frame_id: camera_init}, pose: {position: {x: $GOAL_X, y: $CURRENT_Y, z: $CURRENT_Z}, orientation: {w: 1.0}}}"

终端 4 — 观察结果

良好结果：
Get goal → GenerateExpTrajectory SUCCESS → FOLLOW_TRAJ → ReplanOnce succeed → Traj finish

如果前方路径被阻挡且一侧开放，SUPER 应在有足够净空时生成弯曲或侧滑轨迹。

逐步增加距离

如果 0.20m 成功，尝试 0.50m

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

read CURRENT_X CURRENT_Y CURRENT_Z < <(
 ros2 topic echo /Odometry --once --field pose.pose.position |
 awk '/^x:/ {x=$2} /^y:/ {y=$2} /^z:/ {z=$2} END {print x, y, z}'
)

GOAL_X=$(awk -v x="$CURRENT_X" 'BEGIN {printf "%.6f", x + 0.50}')

echo "Current: $CURRENT_X $CURRENT_Y $CURRENT_Z"
echo "Goal: $GOAL_X $CURRENT_Y $CURRENT_Z"

ros2 topic pub \
 --rate 2 \
 --times 3 \
 --qos-reliability best_effort \
 /goal_pose \
 geometry_msgs/msg/PoseStamped \
 "{header: {frame_id: camera_init}, pose: {position: {x: $GOAL_X, y: $CURRENT_Y, z: $CURRENT_Z}, orientation: {w: 1.0}}}"

如果 0.50m 成功，尝试 0.75m

source /opt/ros/humble/setup.bash
source ~/slam_ws/install/setup.bash
source ~/super_ws/install/setup.bash

read CURRENT_X CURRENT_Y CURRENT_Z < <(
 ros2 topic echo /Odometry --once --field pose.pose.position |
 awk '/^x:/ {x=$2} /^y:/ {y=$2} /^z:/ {z=$2} END {print x, y, z}'
)

GOAL_X=$(awk -v x="$CURRENT_X" 'BEGIN {printf "%.6f", x + 0.75}')

echo "Current: $CURRENT_X $CURRENT_Y $CURRENT_Z"
echo "Goal: $GOAL_X $CURRENT_Y $CURRENT_Z"

ros2 topic pub \
 --rate 2 \
 --times 3 \
 --qos-reliability best_effort \
 /goal_pose \
 geometry_msgs/msg/PoseStamped \
 "{header: {frame_id: camera_init}, pose: {position: {x: $GOAL_X, y: $CURRENT_Y, z: $CURRENT_Z}, orientation: {w: 1.0}}}"

规划器解读

如果 SUPER 输出：
min dis to obstacle is only: 0.332
或 min dis to obstacle is only: 0.384
且你的真实配置中 robot_r: 0.45

规划器正确拒绝路径，因为净空小于所需的安全半径。这不是控制器故障，意味着路径太窄。

紧急规则

如果有任何异常感觉：

1. 切换回 POSCTL
2. 轻轻降低油门
3. 降落
4. 上锁

警告不要在 OFFBOARD 中挣扎抵抗。POSCTL 是逃生模式。

平台：Jetson · ROS2 Humble · Livox MID-360S · FAST-LIO · PX4 · SUPER Planner
(c) 卓博泰科技 · SUPER 避障飞行测试流程

SUPER Obstacle-Avoidance Flight Test Procedure

Related Drobotics guides