SUPER功能包理解

徐凤年_2026-07-26 15:18

功能包	主要功能	输入/输出
`quadrotor_msgs`	无人机位置指令、多项式轨迹等自定义消息	被规划器和仿真器共同使用
`marsim_render`	使用 PCD 和 OpenGL 模拟激光雷达点云	输入地图和位姿，输出局部点云
`perfect_drone_sim`	理想无人机动力学、里程计与点云仿真	订阅 `/planning/pos_cmd`，发布 `/lidar_slam/odom`、`/cloud_registered`
`mission_planner`	航点任务管理，按顺序发送目标点	订阅里程计，发布 `/planning/click_goal`
`rog_map`	概率占据地图、膨胀地图、滑动地图、ESDF	读取点云和里程计，向规划器提供碰撞查询
`super_planner`	FSM、A*、安全走廊、探索/备用轨迹、MINCO 优化	订阅目标，使用 ROG-Map，发布位置指令

核心数据流是：

复制代码

benchmark.txt
    -> mission_planner
    -> /planning/click_goal
    -> super_planner/fsm_node
    -> A* + 安全走廊 + MINCO
    -> /planning/pos_cmd
    -> perfect_drone
    -> /lidar_slam/odom + /cloud_registered
    -> ROG-Map
    -> super_planner

启动launch文件包括三组配置：

复制代码

waypoint_mission -> benchmark.txt
perfect_drone    -> high_speed.yaml
fsm_node         -> static_high_speed.yaml

包括四组具体配置文件：

任务参数： $waypoint.yaml (line 1)$ (/root/super_ws/src/SUPER/mission_planner/config/waypoint.yaml:1)
航点列表： $benchmark.txt (line 1)$ (/root/super_ws/src/SUPER/mission_planner/data/benchmark.txt:1)
仿真和雷达： $high_speed.yaml (line 1)$ (/root/super_ws/src/SUPER/mars_uav_sim/perfect_drone_sim/config/high_speed.yaml:1)
规划器和 ROG-Map： $static_high_speed.yaml (line 1)$ (/root/super_ws/src/SUPER/super_planner/config/static_high_speed.yaml:1)

导航参数

bash 复制代码

static_high_speed.yaml 分为五部分：

    fsm：目标话题、指令话题、重规划频率。
    super_planner：规划范围、安全走廊、机器人半径、备用轨迹开关。
    traj_opt：速度、加速度、角速度、推力限制及优化权重。
    astar：A* 缓冲区大小、启发函数、对角搜索。
    rog_map：地图全部参数。

规划过程大致为：A* 搜索引导路径，CorridorGenerator/CIRI 构造无碰撞多面体走廊，ExpTrajOpt 优化主轨迹，BackupTrajOpt 生成安全制动轨迹，YawTrajOpt 生成航向轨迹。