Skip to content

feat(air): 新增低空多机路权判定与让路恢复 - #1010

Open
usernamekkyy wants to merge 1 commit into
OpenHUTB:masterfrom
usernamekkyy:homework/uav_right_of_way
Open

usernamekkyy wants to merge 1 commit into
OpenHUTB:masterfrom
usernamekkyy:homework/uav_right_of_way

Conversation

@usernamekkyy

Copy link
Copy Markdown
Contributor

模块简介

新增 src/air/uav_right_of_way:两架无人机在空中近距离相遇时的在线让路决策。

uav_deconflict 在起飞前把计划层的冲突解掉(改起飞时刻/高度/航路),但计划赶不上变化——风、计时误差、临时任务都会让两机在空中实际靠近。这个模块补的是飞行中这一层:预测 8 s 内两机最近距离小于 40 m 就触发决策,按四条规则(优先级 → 左让右 → 对头各右转 → 追越从左侧)给出动作,并把每条决策的理由字符串落盘,事后可逐条对账。威胁过去后按余弦剖面回到计划航路,恢复期间继续检测,有二次冲突就重新让。全程不改飞行计划。

让路动作四种,都作用在参考速度上,控制器本身不动:右转 30°、降高 10 m、减速 30%、左侧追越偏置(20 m)。

效果动图

三个场景各录了一段,左栏不开规则直飞、右栏开规则,同一时刻同帧对比。录的是 MuJoCo 刚体积分出来的真实位置(由 tools/make_gif.py 重新跑仿真生成,不是示意动画):

场景 动图
对头相遇 对头相遇
交叉相遇 交叉相遇
追越 追越

三场景轨迹总览(虚线基线、实线规则启用):

轨迹总览

结果

全程机对间最小三维距离(越小越危险):

场景 不开规则 开规则 决策次数
对头 1.68 m 14.56 m 6
交叉 1.19 m 33.44 m 8
追越 0.42 m 19.98 m 4

代价是航迹偏离计划的平均值从 23 m 涨到 48 m,属于让路的正常开销。

一条决策日志长这样(results/<场景>_result.json 的 decision_log):

规则③对头相遇:与 UAV-2 航向差 180°、CPA 37m@8.0s,各自右转30°
规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -44.1),右转30°让行(CPA 40m@8.0s)
规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 25m@8.0s)

复现

cd src/air/uav_right_of_way
python main.py                  # 三场景基线+规则 6 次真飞,本机约 70 s
python main.py --scenario overtake
python tools/make_gif.py        # 重录上面三个动图和总览图
python -m pytest test/ -q       # 48 项测试

测试在只装 numpy/matplotlib/pyyaml/pytest 的环境也能全过(闭环用例自动退化成运动学参考点跑通),不需要 ROS 2,也不需要装 MuJoCo。

主要改动

  • core/detect.py:相对运动解析求 CPA,0 < t_cpa ≤ 8 s 且 d_cpa < 40 m 判侵入;正在远离不算威胁
  • core/rules.py:四条规则引擎,几何分类看航向差(>150° 对头、<10° 追越、其余交叉),左右侧用航向叉积符号
  • core/recover.py:威胁消失后观察 3 s,余弦剖面 (1 + cos πs)/2 恢复,两端速度为零不激励姿态环
  • core/sim.py:每架机独立实例化 uav_mujoco_sim 的 Quadrotor + GeometricController;参考速度前馈一阶低通 τ=0.5 s、姿态阻尼 kw≈−0.18,与 uav_deconflict/core/sim.py 同一写法
  • ros/row_node.py:在线决策节点,订阅 <prefix><n>/odom,命中规则就往 /uav_right_of_way/commands 发 JSON(动作+理由);判定逻辑全在 core/,节点是收发薄壳
  • tools/make_gif.py:跑一遍三场景,录 gif 和总览图
  • docs/air/uav_right_of_way.md 文档页;mkdocs.yml、docs/index.md 各加一行导航(插在着陆载荷预测之后,与在审 PR 的插入位置错开)

已知限制

  • 判定假设两机短时间匀速直线,对方急机动时预测会偏(8 s 窗口内影响有限,没有专门验证)
  • 只算两两相遇,三机以上同时触发时靠优先级字段兜底,没做组合让路
  • 让路幅度(30° / 10 m / 30%)是固定参数,没有按相遇几何自适应调整

提交前检查

  • 37 个文件、+3523 行、删除 0 行,与 master 无冲突(本地 git merge upstream/master 实测自动合并通过)
  • python -m pytest test/ -q:48 passed(6.6 s,本机带 MuJoCo;干净环境 48 passed,闭环用例自动退化)
  • 三场景闭环结果、决策日志随分支提交(results/*.json),动图用仓库内相对路径引用,不依赖 fork raw 链接

两机 8 s 内 CPA < 40 m 时按四条规则(优先级/左让右/对头右转/左侧
追越)即时给出让路动作,决策理由逐条落盘可审计;脱离威胁后按余弦
剖面回到计划航路,恢复期间持续监视二次冲突。三场景 MuJoCo 真飞
对比:对头 1.68→14.56 m,交叉 1.19→33.44 m,追越 0.42→19.98 m。
48 项单元测试通过。

作业信息:黄家欣 2535030010
@usernamekkyy

Copy link
Copy Markdown
Contributor Author

@zinniahhh @2196744532-bot

@usernamekkyy

Copy link
Copy Markdown
Contributor Author

@xingwunian

@LinFengCi LinFengCi left a comment

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

审核未通过,需修改后重提。

主要问题:缺少大模型使用声明(仓库 README L58 明确要求“使用大模型需要在最后声明,并对提交内容负全部责”)。

  • 你的同批其他 PR(#1275/#1274/#1273/#1272)均在描述与 README 末尾明确写了“使用 OpenAI Codex 辅助……提交者复核并对提交内容负责”。本 PR 描述中未见任何大模型声明。
  • 若该模块使用了大模型(含 Codex 等)辅助生成/整理,请按仓库要求在 PR 描述末尾补充声明,点名具体工具并明确“提交者复核并对内容负责”;若确实未使用,也请简要说明。

其余均合规,已本地复现:pytest 45 passed + 1 skipped(无 ROS/MuJoCo 的干净环境下核心逻辑跑通),文件结构(src/air/uav_right_of_way + docs/air)、命名、入口 main.py + launch/main.launch.py、导航栏与首页跳转、运行效果图(cross/headon/overtake.gif 及 trajectories.png,均<10MB)均符合规范。补完声明后即可批准合并。

This branch has not been deployed

No deployments
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

2 participants