diff --git a/docs/air/uav_right_of_way.md b/docs/air/uav_right_of_way.md new file mode 100644 index 0000000000..af39b155f2 --- /dev/null +++ b/docs/air/uav_right_of_way.md @@ -0,0 +1,107 @@ +# 低空多机路权判定与让路恢复(MuJoCo 真飞) + +## 1. 这是什么 + +低空无人机按计划飞,但航路交叉、对头、追越免不了在空中碰到一起。 +`uav_deconflict` 在起飞前把计划层的冲突解掉,可计划赶不上变化: +风、计时误差、临时任务都会让两机实际靠得很近。这个模块补的就是 +飞行中这一层——两机 8 秒内最近距离会小于 40 m 时,当场决定谁让谁、 +怎么让,并把每条决策的理由记下来,事后可以逐条对账。 + +它不管起飞前的事,也不改飞行计划;让路动作作用在参考速度上, +飞完这一段就恢复原计划。 + +## 2. 运行效果 + +三个场景,每架机用 `uav_mujoco_sim` 的四旋翼刚体模型真飞 +(几何控制器 + 刚体积分,指标取自真实位置而不是计划点)。 + +### 对头相遇 + +左栏不开规则直飞,右栏开规则。看得出来右侧两对机在相遇前各自 +右转拉开了距离: + +![对头相遇](../img/air/uav_right_of_way/headon.gif) + +### 交叉相遇 + +同优先级按左让右处理,右侧机先走: + +![交叉相遇](../img/air/uav_right_of_way/cross.gif) + +### 追越 + +前机保速保向,追越机左偏 20 m 从左侧通过: + +![追越](../img/air/uav_right_of_way/overtake.gif) + +### 轨迹总览 + +三场景的基线(虚线)与规则启用(实线)俯视对比: + +![轨迹总览](../img/air/uav_right_of_way/trajectories.png) + +### 数字 + +全程机对间最小三维距离: + +| 场景 | 不开规则 | 开规则 | 决策次数 | +|---|---|---|---| +| 对头 | 1.68 m | 14.56 m | 6 | +| 交叉 | 1.19 m | 33.44 m | 8 | +| 追越 | 0.42 m | 19.98 m | 4 | + +代价是航迹偏离计划的平均值从 2~3 m 涨到 4~8 m。 + +每条决策长这样(`results/<场景>_result.json` 里的 `decision_log`): + +```text +规则③对头相遇:与 UAV-2 航向差 180°、CPA 37m@8.0s,各自右转30° +规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -44.1),右转30°让行(CPA 40m@8.0s) +规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 25m@8.0s) +``` + +## 3. 做法 + +**判定(`core/detect.py`)**:双方近似匀速直线,相对运动解析求 CPA: + +``` +t_cpa = -Δp·Δv / |Δv|², d_cpa = |Δp + Δv·t_cpa| +``` + +`0 < t_cpa ≤ 8 s` 且 `d_cpa < 40 m` 才算威胁。正在远离(`t_cpa ≤ 0`) +不算,否则追越通过之后永远恢复不了。 + +**规则(`core/rules.py`)**:优先级 → 左让右 → 对头各右转 → 追越左侧, +和船只避碰规则一脉相承。几何分类:航向差 > 150° 对头、< 10° 追越、 +其余交叉;左右侧看航向叉积符号。让路动作四种:右转 30°、降高 10 m、 +减速 30%、左侧追越偏置,全部作用在参考速度上,控制器本身不动。 + +**恢复(`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` 一致,防止航向阶跃把姿态打震荡。`--no-sim` 时退化成 +纯运动学参考点,判定/规则/恢复走同一套代码。 + +## 4. 复现 + +```bash +cd src/air/uav_right_of_way +python main.py # 三场景基线+规则 6 次真飞,本机约 70 s +python main.py --scenario headon +python tools/make_gif.py # 重新录上面三个动图 +python -m pytest test/ -q # 48 项测试 +``` + +安装、ROS 节点话题说明见模块 +[README](https://github.com/OpenHUTB/ros2/tree/master/src/air/uav_right_of_way)。 + +## 5. 已知限制 + +- 判定假设两机短时间匀速直线,对方急机动时预测会偏; +- 只算两两相遇,三机以上同时触发时靠优先级字段兜底,没有做组合让路; +- 让路幅度(30° / 10 m / 30%)是固定参数,没有按相遇几何自适应。 diff --git a/docs/img/air/uav_right_of_way/cross.gif b/docs/img/air/uav_right_of_way/cross.gif new file mode 100644 index 0000000000..271cb41a8e Binary files /dev/null and b/docs/img/air/uav_right_of_way/cross.gif differ diff --git a/docs/img/air/uav_right_of_way/headon.gif b/docs/img/air/uav_right_of_way/headon.gif new file mode 100644 index 0000000000..80dfbcb353 Binary files /dev/null and b/docs/img/air/uav_right_of_way/headon.gif differ diff --git a/docs/img/air/uav_right_of_way/overtake.gif b/docs/img/air/uav_right_of_way/overtake.gif new file mode 100644 index 0000000000..9677883836 Binary files /dev/null and b/docs/img/air/uav_right_of_way/overtake.gif differ diff --git a/docs/img/air/uav_right_of_way/trajectories.png b/docs/img/air/uav_right_of_way/trajectories.png new file mode 100644 index 0000000000..de5fbe471e Binary files /dev/null and b/docs/img/air/uav_right_of_way/trajectories.png differ diff --git a/docs/index.md b/docs/index.md index e610b98f22..6bc763752c 100644 --- a/docs/index.md +++ b/docs/index.md @@ -130,6 +130,7 @@ ROS 2 话题发布订阅基础实验(talker/listener):[chap2/ros2_pubsub_p ## 5. 空域载具 * [无人机着陆阶段识别与接地载荷预测](./air/landing_assessment.md) +* [低空多机路权判定与让路恢复](./air/uav_right_of_way.md) —— 两机近距离相遇时的在线让路:优先级/左让右/对头右转/左侧追越,MuJoCo 真飞对比 * [无人机载荷质量估计](./air/payload_mass_estimation.md) diff --git a/mkdocs.yml b/mkdocs.yml index e0e1e08ef9..c08934ee71 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -122,6 +122,7 @@ nav: - 构建与运行: './air/uav_mission_safety_manager/usage.md' - AirSim 实测记录: './air/uav_mission_safety_manager/airsim_validation.md' - 无人机着陆阶段识别与接地载荷预测: './air/landing_assessment.md' + - 低空多机路权判定与让路恢复: './air/uav_right_of_way.md' - 无人机载荷质量估计: './air/payload_mass_estimation.md' - 无人机飞行轨迹类型识别与 ROS2 自动任务日志: './air/flight_pattern_recognition.md' - 无人机遥测异常检测与 ROS2 在线诊断: './air/telemetry_diagnostics.md' diff --git a/src/air/uav_right_of_way/README.md b/src/air/uav_right_of_way/README.md new file mode 100644 index 0000000000..a38b04adc4 --- /dev/null +++ b/src/air/uav_right_of_way/README.md @@ -0,0 +1,115 @@ +# uav_right_of_way + +两架无人机在空中近距离相遇时,谁让谁、怎么让。这个模块做的事: + +1. 用相对运动解析算 CPA(最近接近点),预测 8 s 内两机距离会小于 40 m 就触发决策; +2. 按四条规则给出让路动作:优先级高的先走 → 交叉左让右 → 对头各右转 → 追越机从左侧过; +3. 每次决策都写一条带理由的字符串进 JSON,方便事后对账; +4. 威胁过去之后,按余弦剖面慢慢回到计划航路,恢复期间继续盯着,有二次冲突就重新让。 + +整个过程不改飞行计划,和 `uav_deconflict`(飞之前改起飞时刻/高度/航路)是两层东西: +那边管计划层,这边管飞行中临场的让路。 + +## 效果 + +三个场景(对头 / 交叉 / 追越),每架机用 `uav_mujoco_sim` 的刚体模型真飞, +下面是全程机对间最小三维距离(越小越危险): + +| 场景 | 不开规则 | 开规则 | 决策次数 | +|---|---|---|---| +| 对头 | 1.68 m | 14.56 m | 6 | +| 交叉 | 1.19 m | 33.44 m | 8 | +| 追越 | 0.42 m | 19.98 m | 4 | + +代价是航迹偏离计划的平均值从 2~3 m 涨到 4~8 m,属于让路的正常代价。 + +动图(左栏不开规则、右栏开规则,同一时刻对比): + +![对头相遇](../../docs/img/air/uav_right_of_way/headon.gif) + +另两个场景的动图和三场景轨迹总览图在 `docs/img/air/uav_right_of_way/`, +用 `tools/make_gif.py` 重跑生成。 + +一条决策日志长这样(存在 `results/<场景>_result.json` 的 `decision_log` 里): + +```text +规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -44.1),右转30°让行(CPA 40m@8.0s) +规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 25m@8.0s) +``` + +## 怎么让的 + +让路动作一共四种,都作用在参考速度上,不动控制器本身: + +- 右转 30°:参考航向叠加偏置 +- 降高 10 m:参考高度下移 +- 减速 30%:参考速度乘 0.7 +- 左侧追越:给参考点加一个横向偏置目标 + +规则只看两机当时的状态(位置/速度/航向 + 任务优先级),几何分类用航向差: +大于 150° 算对头,小于 10° 算追越,其余是交叉;左右侧用航向叉积符号判。 + +威胁消失后先观察 3 s 再开始恢复,恢复是余弦剖面 `(1 + cos πs)/2`: +起点接住当前偏移、终点归零、两端速度为零,不会拿航向阶跃去激励姿态环。 +恢复期间每个决策周期重新检测,出现二次冲突立即中止恢复重新决策。 + +## 运行 + +```bash +# 仅算法层(不需要 mujoco,几秒跑完) +python main.py --no-sim + +# 三场景 MuJoCo 闭环,基线 + 规则共 6 次真飞(本机实测约 70 s) +python main.py + +# 只跑一个场景 +python main.py --scenario overtake + +# 录 gif / 总览图 +python tools/make_gif.py +``` + +装包之后: + +```bash +colcon build --packages-select uav_right_of_way +source install/setup.bash +ros2 run uav_right_of_way main +ros2 launch uav_right_of_way main.launch.py # 在线决策节点 +``` + +节点订阅 `/odom`,预测到相遇就往 `/uav_right_of_way/commands` +发 JSON(让路动作 + 决策理由)。判定逻辑都在 `core/`,节点只是个收发壳。 + +依赖:Python 3.10+,`pip install numpy mujoco matplotlib pytest`; +ROS 节点部分需要 ROS 2 Humble;仿真底座是同仓库的 `uav_mujoco_sim` +(源码运行时自动挂 `sys.path`)。 + +## 目录 + +``` +uav_right_of_way/ +├── main.py 入口 +├── package.xml / setup.py / setup.cfg +├── config/uav_right_of_way.yaml 判定与规则参数 +├── launch/main.launch.py +├── uav_right_of_way/ +│ ├── core/ +│ │ ├── detect.py CPA 解析、侵入判定 +│ │ ├── rules.py 四条规则(动作 + 理由字符串) +│ │ ├── recover.py 余弦恢复 + 恢复期监视 +│ │ ├── scenarios.py headon / cross / overtake +│ │ ├── sim.py 基线 vs 规则闭环 +│ │ └── pipeline.py 结果汇总、JSON、报告 +│ ├── cli.py +│ └── ros/row_node.py 在线决策节点 +├── test/ 48 项测试(没装 mujoco 的机器也能全过,闭环用例自动退化) +├── tools/make_gif.py +└── results/ 每场景的机读结果 +``` + +## 已知局限 + +- 判定假设两机短时间匀速直线,急机动的一方预测会不准(8 s 窗口内影响有限,没专门验证); +- 只处理两机相遇,三机以上的组合让路没有做(规则是两两算的,多机同时触发时的优先序靠优先级字段兜底); +- 让路动作的幅度(30°/10 m/30%)写死在参数里,没有按相遇几何自适应调整。 diff --git a/src/air/uav_right_of_way/config/uav_right_of_way.yaml b/src/air/uav_right_of_way/config/uav_right_of_way.yaml new file mode 100644 index 0000000000..dcdf4ae79c --- /dev/null +++ b/src/air/uav_right_of_way/config/uav_right_of_way.yaml @@ -0,0 +1,19 @@ +# 通行权在线决策节点与算法层参数 +uav_ids: ["UAV-1", "UAV-2", "UAV-3", "UAV-4"] +odom_prefix: "/uav" + +# 侵入判定:预测 horizon_s 秒内 CPA < cpa_threshold_m 即触发让路决策 +horizon_s: 8.0 # s +cpa_threshold_m: 40.0 # m + +# 规则动作参数 +turn_right_deg: 30.0 # 右转角度 +descend_m: 10.0 # 降高幅度 +slow_fraction: 0.30 # 减速比例(速度降低 30%) +overtake_lateral_m: 20.0 # 追越机从左侧通过时的横向偏置(m) + +# 决策与恢复 +decision_interval_s: 0.2 # 决策周期(s) +clear_hold_s: 3.0 # 威胁消失后保持观察时长,之后开始恢复(s) +recovery_min_s: 8.0 # 余弦剖面恢复的最短时长(s) +stale_timeout: 1.0 # 超过该时长没收到某架里程计就不再参与决策(s) diff --git a/src/air/uav_right_of_way/conftest.py b/src/air/uav_right_of_way/conftest.py new file mode 100644 index 0000000000..ca14ed375a --- /dev/null +++ b/src/air/uav_right_of_way/conftest.py @@ -0,0 +1,11 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""pytest 路径引导:源码目录直接运行时也能 import 本包与仿真底座。""" +import os +import sys + +_HERE = os.path.dirname(os.path.abspath(__file__)) +_SIBLING = os.path.normpath(os.path.join(_HERE, "..", "uav_mujoco_sim")) +for _p in (_HERE, _SIBLING): + if os.path.isdir(_p) and _p not in sys.path: + sys.path.insert(0, _p) diff --git a/src/air/uav_right_of_way/launch/main.launch.py b/src/air/uav_right_of_way/launch/main.launch.py new file mode 100644 index 0000000000..1bda227799 --- /dev/null +++ b/src/air/uav_right_of_way/launch/main.launch.py @@ -0,0 +1,22 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""启动通行权在线决策节点。""" +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description() -> LaunchDescription: + share = get_package_share_directory("uav_right_of_way") + params = os.path.join(share, "config", "uav_right_of_way.yaml") + return LaunchDescription([ + Node( + package="uav_right_of_way", + executable="row_node", + name="uav_right_of_way_node", + output="screen", + parameters=[params], + ), + ]) diff --git a/src/air/uav_right_of_way/main.py b/src/air/uav_right_of_way/main.py new file mode 100644 index 0000000000..bd96ecbb1e --- /dev/null +++ b/src/air/uav_right_of_way/main.py @@ -0,0 +1,29 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""多机优先级通行权在线决策 —— 一键运行入口。 + +用法:: + + python main.py # 跑全部场景(基线 vs 规则启用,含 MuJoCo 闭环) + python main.py --scenario headon # 只跑对头场景 + python main.py --no-sim # 仅算法层(秒级,不需要 mujoco) + python main.py --json out.json # 结果另存到指定 JSON + +源码目录直接运行时不需要 colcon install:本文件会把同级的 ``uav_mujoco_sim`` +底座加入 ``sys.path``。装好包之后对应入口是 ``ros2 run uav_right_of_way main``。 +""" +from __future__ import annotations + +import os +import sys + +_HERE = os.path.dirname(os.path.abspath(__file__)) +_SIBLING = os.path.normpath(os.path.join(_HERE, "..", "uav_mujoco_sim")) +for _p in (_HERE, _SIBLING): + if os.path.isdir(_p) and _p not in sys.path: + sys.path.insert(0, _p) + +from uav_right_of_way.cli import main # noqa: E402 + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/air/uav_right_of_way/package.xml b/src/air/uav_right_of_way/package.xml new file mode 100644 index 0000000000..efcaafb2f6 --- /dev/null +++ b/src/air/uav_right_of_way/package.xml @@ -0,0 +1,33 @@ + + + + uav_right_of_way + 0.1.0 + + 低空多机优先级通行权在线决策。两机近距离相遇时按优先级 / 左让右 / + 对头各右转 / 追越从左侧四条规则即时产出让路动作(右转 30°、降高 10 m、 + 减速 30%、保向等)并给出可审计的决策理由;预测 8 s 内 CPA < 40 m + 即触发;脱离威胁后按余弦剖面平滑回到计划航路,恢复期间持续监视防止 + 二次冲突。用课程既有的 uav_mujoco_sim(MuJoCo 刚体模型 + 几何控制器) + 做基线(无规则直飞)与规则启用的闭环对比。 + + 黄家欣 + MIT + + ament_python + + rclpy + std_msgs + nav_msgs + geometry_msgs + launch + launch_ros + python3-numpy + uav_mujoco_sim + + ament_pytest + + + ament_python + + diff --git a/src/air/uav_right_of_way/resource/uav_right_of_way b/src/air/uav_right_of_way/resource/uav_right_of_way new file mode 100644 index 0000000000..e69de29bb2 diff --git a/src/air/uav_right_of_way/results/cross_result.json b/src/air/uav_right_of_way/results/cross_result.json new file mode 100644 index 0000000000..560f052892 --- /dev/null +++ b/src/air/uav_right_of_way/results/cross_result.json @@ -0,0 +1,401 @@ +[ + { + "scenario": "cross", + "title": "十字交叉", + "description": "四架无人机从四个方向同时抵达空域中心,同优先级按规则②左让右,各自右转形成旋转通过。", + "closed_loop": false, + "params": { + "horizon_s": 8.0, + "cpa_threshold_m": 40.0, + "turn_right_deg": 30.0, + "descend_m": 10.0, + "slow_fraction": 0.3, + "overtake_lateral_m": 20.0, + "decision_interval_s": 0.2, + "clear_hold_s": 3.0, + "recovery_min_s": 8.0 + }, + "plans": [ + { + "uid": "UAV-1", + "label": "东行", + "start": [ + -180.0, + 0.0 + ], + "end": [ + 180.0, + 0.0 + ], + "alt": 15.0, + "speed": 8.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 51.0 + }, + { + "uid": "UAV-2", + "label": "北行", + "start": [ + 0.0, + -180.0 + ], + "end": [ + 0.0, + 180.0 + ], + "alt": 15.0, + "speed": 8.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 51.0 + }, + { + "uid": "UAV-3", + "label": "西行", + "start": [ + 180.0, + 0.0 + ], + "end": [ + -180.0, + 0.0 + ], + "alt": 15.0, + "speed": 8.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 51.0 + }, + { + "uid": "UAV-4", + "label": "南行", + "start": [ + 0.0, + 180.0 + ], + "end": [ + 0.0, + -180.0 + ], + "alt": 15.0, + "speed": 8.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 51.0 + } + ], + "baseline": { + "mode": "baseline", + "min_separation_m": 0.11, + "min_pair": [ + "UAV-1", + "UAV-2" + ], + "min_sep_time_s": 28.5, + "decisions": 0, + "mean_deviation_m": 0.06, + "per_agent_deviation_m": { + "UAV-1": 0.06, + "UAV-2": 0.06, + "UAV-3": 0.06, + "UAV-4": 0.06 + }, + "below_threshold_samples": 80 + }, + "rules": { + "mode": "rules", + "min_separation_m": 35.49, + "min_pair": [ + "UAV-1", + "UAV-2" + ], + "min_sep_time_s": 29.5, + "decisions": 21, + "mean_deviation_m": 9.44, + "per_agent_deviation_m": { + "UAV-1": 9.36, + "UAV-2": 8.91, + "UAV-3": 11.3, + "UAV-4": 8.21 + }, + "below_threshold_samples": 13, + "decision_log": [ + { + "t": 17.0, + "uid": "UAV-1", + "other": "UAV-2", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -92.0),右转30°让行(CPA 40m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.6 + }, + { + "t": 17.0, + "uid": "UAV-1", + "other": "UAV-4", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-4 右侧(叉积 92.0,右方机优先),保持航向航速(CPA 40m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.6 + }, + { + "t": 17.0, + "uid": "UAV-2", + "other": "UAV-1", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-1 右侧(叉积 91.9,右方机优先),保持航向航速(CPA 39m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.19 + }, + { + "t": 17.0, + "uid": "UAV-2", + "other": "UAV-3", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-3 位于本机右侧(叉积 -92.0),右转30°让行(CPA 40m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.6 + }, + { + "t": 17.0, + "uid": "UAV-3", + "other": "UAV-2", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-2 右侧(叉积 91.9,右方机优先),保持航向航速(CPA 39m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.19 + }, + { + "t": 17.0, + "uid": "UAV-3", + "other": "UAV-4", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-4 位于本机右侧(叉积 -92.0),右转30°让行(CPA 40m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.6 + }, + { + "t": 17.0, + "uid": "UAV-4", + "other": "UAV-3", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-3 右侧(叉积 91.9,右方机优先),保持航向航速(CPA 39m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.19 + }, + { + "t": 20.2, + "uid": "UAV-1", + "other": "UAV-4", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-4 右侧(叉积 97.5,右方机优先),保持航向航速(CPA 37m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 37.44 + }, + { + "t": 20.2, + "uid": "UAV-2", + "other": "UAV-4", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-4 右侧(叉积 57.9,右方机优先),保持航向航速(CPA 36m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 36.0 + }, + { + "t": 20.2, + "uid": "UAV-4", + "other": "UAV-1", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-1 位于本机右侧(叉积 -69.5),右转30°让行(CPA 37m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 36.98 + }, + { + "t": 20.2, + "uid": "UAV-4", + "other": "UAV-2", + "rule": "③对头", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则③对头相遇:与 UAV-2 航向差 160°、CPA 36m@8.0s,各自右转30°", + "cpa_t": 8.0, + "cpa_dist": 35.54 + }, + { + "t": 20.4, + "uid": "UAV-1", + "other": "UAV-2", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -36.1),右转30°让行(CPA 39m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 38.68 + }, + { + "t": 20.4, + "uid": "UAV-2", + "other": "UAV-1", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-1 右侧(叉积 90.7,右方机优先),保持航向航速(CPA 39m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 38.9 + }, + { + "t": 23.4, + "uid": "UAV-2", + "other": "UAV-3", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-3 位于本机右侧(叉积 -35.4),右转30°让行(CPA 35m@7.7s)", + "cpa_t": 7.67, + "cpa_dist": 34.58 + }, + { + "t": 23.4, + "uid": "UAV-3", + "other": "UAV-2", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-2 右侧(叉积 76.7,右方机优先),保持航向航速(CPA 35m@7.6s)", + "cpa_t": 7.59, + "cpa_dist": 35.04 + }, + { + "t": 24.0, + "uid": "UAV-1", + "other": "UAV-4", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-4 右侧(叉积 69.3,右方机优先),保持航向航速(CPA 39m@5.4s)", + "cpa_t": 5.37, + "cpa_dist": 39.37 + }, + { + "t": 24.0, + "uid": "UAV-3", + "other": "UAV-4", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-4 位于本机右侧(叉积 -9.1),右转30°让行(CPA 37m@4.4s)", + "cpa_t": 4.38, + "cpa_dist": 36.83 + }, + { + "t": 24.0, + "uid": "UAV-4", + "other": "UAV-1", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-1 位于本机右侧(叉积 -13.5),右转30°让行(CPA 39m@5.4s)", + "cpa_t": 5.36, + "cpa_dist": 39.28 + }, + { + "t": 27.0, + "uid": "UAV-1", + "other": "UAV-2", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-2 右侧(叉积 5.9,右方机优先),保持航向航速(CPA 39m@2.4s)", + "cpa_t": 2.37, + "cpa_dist": 39.18 + }, + { + "t": 27.0, + "uid": "UAV-2", + "other": "UAV-1", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-1 右侧(叉积 39.5,右方机优先),保持航向航速(CPA 39m@2.4s)", + "cpa_t": 2.36, + "cpa_dist": 39.11 + }, + { + "t": 27.2, + "uid": "UAV-4", + "other": "UAV-1", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-1 右侧(叉积 22.5,右方机优先),保持航向航速(CPA 40m@1.3s)", + "cpa_t": 1.29, + "cpa_dist": 39.57 + } + ] + }, + "wall_time_s": 0.5 + } +] \ No newline at end of file diff --git a/src/air/uav_right_of_way/results/headon_result.json b/src/air/uav_right_of_way/results/headon_result.json new file mode 100644 index 0000000000..080e3a6cbd --- /dev/null +++ b/src/air/uav_right_of_way/results/headon_result.json @@ -0,0 +1,284 @@ +[ + { + "scenario": "headon", + "title": "对头相遇", + "description": "两条平行走廊各有一对同高度对头机,中线相遇;同优先级按规则③各自右转 30° 互相从左側通过。", + "closed_loop": false, + "params": { + "horizon_s": 8.0, + "cpa_threshold_m": 40.0, + "turn_right_deg": 30.0, + "descend_m": 10.0, + "slow_fraction": 0.3, + "overtake_lateral_m": 20.0, + "decision_interval_s": 0.2, + "clear_hold_s": 3.0, + "recovery_min_s": 8.0 + }, + "plans": [ + { + "uid": "UAV-1", + "label": "东向干线", + "start": [ + -180.0, + 0.0 + ], + "end": [ + 180.0, + 0.0 + ], + "alt": 15.0, + "speed": 8.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 51.0 + }, + { + "uid": "UAV-2", + "label": "西向干线", + "start": [ + 180.0, + 0.0 + ], + "end": [ + -180.0, + 0.0 + ], + "alt": 15.0, + "speed": 8.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 51.0 + }, + { + "uid": "UAV-3", + "label": "东向支线", + "start": [ + -180.0, + 60.0 + ], + "end": [ + 180.0, + 60.0 + ], + "alt": 15.0, + "speed": 8.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 51.0 + }, + { + "uid": "UAV-4", + "label": "西向支线", + "start": [ + 180.0, + 60.0 + ], + "end": [ + -180.0, + 60.0 + ], + "alt": 15.0, + "speed": 8.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 51.0 + } + ], + "baseline": { + "mode": "baseline", + "min_separation_m": 0.16, + "min_pair": [ + "UAV-1", + "UAV-2" + ], + "min_sep_time_s": 28.5, + "decisions": 0, + "mean_deviation_m": 0.06, + "per_agent_deviation_m": { + "UAV-1": 0.06, + "UAV-2": 0.06, + "UAV-3": 0.06, + "UAV-4": 0.06 + }, + "below_threshold_samples": 20 + }, + "rules": { + "mode": "rules", + "min_separation_m": 15.52, + "min_pair": [ + "UAV-2", + "UAV-4" + ], + "min_sep_time_s": 35.0, + "decisions": 12, + "mean_deviation_m": 15.36, + "per_agent_deviation_m": { + "UAV-1": 0.06, + "UAV-2": 30.31, + "UAV-3": 25.45, + "UAV-4": 5.63 + }, + "below_threshold_samples": 102, + "decision_log": [ + { + "t": 18.0, + "uid": "UAV-2", + "other": "UAV-1", + "rule": "③对头", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则③对头相遇:与 UAV-1 航向差 180°、CPA 40m@8.0s,各自右转30°", + "cpa_t": 8.0, + "cpa_dist": 39.92 + }, + { + "t": 18.0, + "uid": "UAV-4", + "other": "UAV-3", + "rule": "③对头", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则③对头相遇:与 UAV-3 航向差 180°、CPA 40m@8.0s,各自右转30°", + "cpa_t": 8.0, + "cpa_dist": 39.92 + }, + { + "t": 19.2, + "uid": "UAV-2", + "other": "UAV-3", + "rule": "③对头", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则③对头相遇:与 UAV-3 航向差 153°、CPA 40m@8.0s,各自右转30°", + "cpa_t": 8.0, + "cpa_dist": 39.65 + }, + { + "t": 19.2, + "uid": "UAV-3", + "other": "UAV-2", + "rule": "③对头", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则③对头相遇:与 UAV-2 航向差 153°、CPA 40m@8.0s,各自右转30°", + "cpa_t": 8.0, + "cpa_dist": 39.54 + }, + { + "t": 19.8, + "uid": "UAV-1", + "other": "UAV-3", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-3 右侧(叉积 57.6,右方机优先),保持航向航速(CPA 36m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 35.75 + }, + { + "t": 19.8, + "uid": "UAV-3", + "other": "UAV-1", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-1 位于本机右侧(叉积 -53.4),右转30°让行(CPA 36m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 35.76 + }, + { + "t": 21.6, + "uid": "UAV-2", + "other": "UAV-4", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-4 位于本机右侧(叉积 -49.6),右转30°让行(CPA 36m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 35.53 + }, + { + "t": 21.6, + "uid": "UAV-4", + "other": "UAV-2", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-2 右侧(叉积 56.8,右方机优先),保持航向航速(CPA 35m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 35.48 + }, + { + "t": 37.0, + "uid": "UAV-1", + "other": "UAV-3", + "rule": "④追越", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则④追越:本机为前机,保速保向,由 UAV-3 从左侧追越(CPA 19m@3.4s)", + "cpa_t": 3.44, + "cpa_dist": 18.81 + }, + { + "t": 37.0, + "uid": "UAV-3", + "other": "UAV-1", + "rule": "④追越", + "action": { + "kind": "slow", + "value": 0.3 + }, + "reason": "规则④追越:须从左侧追越 UAV-1,本机现处其右侧,先减速30%保持间隔(CPA 19m@3.4s)", + "cpa_t": 3.42, + "cpa_dist": 18.89 + }, + { + "t": 38.8, + "uid": "UAV-2", + "other": "UAV-4", + "rule": "④追越", + "action": { + "kind": "slow", + "value": 0.3 + }, + "reason": "规则④追越:须从左侧追越 UAV-4,本机现处其右侧,先减速30%保持间隔(CPA 19m@3.0s)", + "cpa_t": 3.03, + "cpa_dist": 18.58 + }, + { + "t": 38.8, + "uid": "UAV-4", + "other": "UAV-2", + "rule": "④追越", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则④追越:本机为前机,保速保向,由 UAV-2 从左侧追越(CPA 19m@2.4s)", + "cpa_t": 2.39, + "cpa_dist": 18.66 + } + ] + }, + "wall_time_s": 0.5 + } +] \ No newline at end of file diff --git a/src/air/uav_right_of_way/results/overtake_result.json b/src/air/uav_right_of_way/results/overtake_result.json new file mode 100644 index 0000000000..eebd11c816 --- /dev/null +++ b/src/air/uav_right_of_way/results/overtake_result.json @@ -0,0 +1,258 @@ +[ + { + "scenario": "overtake", + "title": "同高度追越", + "description": "快机在正后方追上慢机(同航路同高度),规则④要求追越机从左侧偏置通过、前机保速保向;通过后按余弦剖面回到原航路。", + "closed_loop": false, + "params": { + "horizon_s": 8.0, + "cpa_threshold_m": 40.0, + "turn_right_deg": 30.0, + "descend_m": 10.0, + "slow_fraction": 0.3, + "overtake_lateral_m": 20.0, + "decision_interval_s": 0.2, + "clear_hold_s": 3.0, + "recovery_min_s": 8.0 + }, + "plans": [ + { + "uid": "UAV-1", + "label": "慢速巡检(前机)", + "start": [ + -180.0, + 0.0 + ], + "end": [ + 180.0, + 0.0 + ], + "alt": 15.0, + "speed": 5.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 78.0 + }, + { + "uid": "UAV-2", + "label": "快速配送(追越机)", + "start": [ + -220.0, + 0.0 + ], + "end": [ + 220.0, + 0.0 + ], + "alt": 15.0, + "speed": 9.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 54.9 + }, + { + "uid": "UAV-3", + "label": "慢速巡检 B(前机)", + "start": [ + -180.0, + 80.0 + ], + "end": [ + 180.0, + 80.0 + ], + "alt": 15.0, + "speed": 5.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 78.0 + }, + { + "uid": "UAV-4", + "label": "快速配送 B(追越机)", + "start": [ + -220.0, + 80.0 + ], + "end": [ + 220.0, + 80.0 + ], + "alt": 15.0, + "speed": 9.0, + "priority": 0, + "cruise_t0": 6.0, + "cruise_t1": 54.9 + } + ], + "baseline": { + "mode": "baseline", + "min_separation_m": 0.04, + "min_pair": [ + "UAV-1", + "UAV-2" + ], + "min_sep_time_s": 16.0, + "decisions": 0, + "mean_deviation_m": 0.05, + "per_agent_deviation_m": { + "UAV-1": 0.04, + "UAV-2": 0.05, + "UAV-3": 0.04, + "UAV-4": 0.05 + }, + "below_threshold_samples": 80 + }, + "rules": { + "mode": "rules", + "min_separation_m": 19.96, + "min_pair": [ + "UAV-1", + "UAV-2" + ], + "min_sep_time_s": 16.0, + "decisions": 10, + "mean_deviation_m": 11.49, + "per_agent_deviation_m": { + "UAV-1": 0.04, + "UAV-2": 6.06, + "UAV-3": 35.17, + "UAV-4": 4.68 + }, + "below_threshold_samples": 129, + "decision_log": [ + { + "t": 6.2, + "uid": "UAV-1", + "other": "UAV-2", + "rule": "④追越", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则④追越:本机为前机,保速保向,由 UAV-2 从左侧追越(CPA 29m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 28.56 + }, + { + "t": 6.2, + "uid": "UAV-2", + "other": "UAV-1", + "rule": "④追越", + "action": { + "kind": "overtake_left", + "value": 20.0 + }, + "reason": "规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 29m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 29.15 + }, + { + "t": 6.2, + "uid": "UAV-3", + "other": "UAV-4", + "rule": "④追越", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则④追越:本机为前机,保速保向,由 UAV-4 从左侧追越(CPA 29m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 28.56 + }, + { + "t": 6.2, + "uid": "UAV-4", + "other": "UAV-3", + "rule": "④追越", + "action": { + "kind": "overtake_left", + "value": 20.0 + }, + "reason": "规则④追越:从左侧追越 UAV-3,左偏20m保持间隔(正后方,CPA 29m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 29.15 + }, + { + "t": 8.4, + "uid": "UAV-2", + "other": "UAV-3", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-3 右侧(叉积 52.8,右方机优先),保持航向航速(CPA 40m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.6 + }, + { + "t": 8.4, + "uid": "UAV-3", + "other": "UAV-2", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-2 位于本机右侧(叉积 -71.2),右转30°让行(CPA 40m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.55 + }, + { + "t": 17.2, + "uid": "UAV-1", + "other": "UAV-3", + "rule": "②左让右", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则②交叉相遇左让右:本机位于 UAV-3 右侧(叉积 58.0,右方机优先),保持航向航速(CPA 40m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.63 + }, + { + "t": 17.2, + "uid": "UAV-3", + "other": "UAV-1", + "rule": "②左让右", + "action": { + "kind": "turn_right", + "value": 30.0 + }, + "reason": "规则②交叉相遇左让右:UAV-1 位于本机右侧(叉积 -47.3),右转30°让行(CPA 40m@8.0s)", + "cpa_t": 8.0, + "cpa_dist": 39.65 + }, + { + "t": 42.8, + "uid": "UAV-1", + "other": "UAV-3", + "rule": "④追越", + "action": { + "kind": "maintain", + "value": 0.0 + }, + "reason": "规则④追越:本机为前机,保速保向,由 UAV-3 从左侧追越(CPA 22m@0.1s)", + "cpa_t": 0.08, + "cpa_dist": 22.02 + }, + { + "t": 42.8, + "uid": "UAV-3", + "other": "UAV-1", + "rule": "④追越", + "action": { + "kind": "overtake_left", + "value": 20.0 + }, + "reason": "规则④追越:从左侧追越 UAV-1,左偏20m保持间隔(正后方,CPA 22m@0.1s)", + "cpa_t": 0.07, + "cpa_dist": 22.07 + } + ] + }, + "wall_time_s": 0.6 + } +] \ No newline at end of file diff --git a/src/air/uav_right_of_way/setup.cfg b/src/air/uav_right_of_way/setup.cfg new file mode 100644 index 0000000000..d031526ee5 --- /dev/null +++ b/src/air/uav_right_of_way/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/uav_right_of_way +[install] +install_scripts=$base/lib/uav_right_of_way diff --git a/src/air/uav_right_of_way/setup.py b/src/air/uav_right_of_way/setup.py new file mode 100644 index 0000000000..b02294c19e --- /dev/null +++ b/src/air/uav_right_of_way/setup.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +import os +from glob import glob + +from setuptools import find_packages, setup + +package_name = "uav_right_of_way" + +setup( + name=package_name, + version="0.1.0", + packages=find_packages(exclude=["test", "tools", "docs"]), + data_files=[ + ("share/ament_index/resource_index/packages", + ["resource/" + package_name]), + (os.path.join("share", package_name), + ["package.xml", "README.md", "main.py"]), + (os.path.join("share", package_name, "launch"), glob("launch/*.launch.py")), + (os.path.join("share", package_name, "config"), glob("config/*.yaml")), + ], + install_requires=["setuptools", "numpy", "mujoco"], + zip_safe=True, + maintainer="黄家欣", + maintainer_email="326516287+usernamekkyy@users.noreply.github.com", + description="低空多机优先级通行权在线决策(MuJoCo 闭环验证)", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "main = uav_right_of_way.cli:main", + "row_node = uav_right_of_way.ros.row_node:main", + ], + }, +) diff --git a/src/air/uav_right_of_way/test/test_detect.py b/src/air/uav_right_of_way/test/test_detect.py new file mode 100644 index 0000000000..d0768844ea --- /dev/null +++ b/src/air/uav_right_of_way/test/test_detect.py @@ -0,0 +1,105 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""侵入判定(core.detect)单元测试:解析 CPA 与威胁触发条件。""" +import numpy as np +import pytest + +from uav_right_of_way.core.detect import (AircraftState, DetectParams, Threat, + cpa, fleet_threats, predict_threat, + threats_against) + +DP = DetectParams(horizon_s=8.0, cpa_threshold_m=40.0) + + +def state(uid, pos, vel, priority=0): + return AircraftState(uid, np.asarray(pos, float), + np.asarray(vel, float), priority) + + +def test_cpa_headon_analytic(): + # 100 m 外以 10 m/s 迎面接近 → t_cpa=10 s,最近距离 0 + t, d = cpa(np.array([100.0, 0, 0]), np.array([-10.0, 0, 0]), 30.0) + assert t == pytest.approx(10.0) + assert d == pytest.approx(0.0, abs=1e-9) + + +def test_cpa_relative_stationary(): + t, d = cpa(np.array([30.0, 40.0, 0]), np.zeros(3), 8.0) + assert t == pytest.approx(0.0) + assert d == pytest.approx(50.0) + + +def test_cpa_offset_crossing(): + # 横向偏 50 m 的交叉:最近距离恰为 50 m,出现在 t=10 s + t, d = cpa(np.array([100.0, 50.0, 0]), np.array([-10.0, 0, 0]), 30.0) + assert t == pytest.approx(10.0) + assert d == pytest.approx(50.0) + + +def test_cpa_clamped_to_horizon(): + t, d = cpa(np.array([100.0, 0, 0]), np.array([-10.0, 0, 0]), 5.0) + assert t == pytest.approx(5.0) + assert d == pytest.approx(50.0) + + +def test_threat_triggers_within_horizon(): + # 150 m 外 20 m/s 迎面:t_cpa=7.5 s ≤ 8 s,CPA=0 → 触发 + own = state("A", (0, 0, 15), (10, 0, 0)) + other = state("B", (150, 0, 15), (-10, 0, 0)) + th = predict_threat(own, other, DP) + assert th is not None + assert th.other == "B" + assert th.cpa_t == pytest.approx(7.5) + assert th.cpa_dist < 1e-6 + + +def test_no_threat_beyond_horizon(): + # 200 m 外 20 m/s:t_cpa=10 s > 8 s → 不触发 + own = state("A", (0, 0, 15), (10, 0, 0)) + other = state("B", (200, 0, 15), (-10, 0, 0)) + assert predict_threat(own, other, DP) is None + + +def test_no_threat_when_cpa_far(): + # 平行航迹横向 100 m:CPA 距离 100 m > 40 m → 不触发 + own = state("A", (0, 0, 15), (10, 0, 0)) + other = state("B", (50, 100, 15), (10, 0, 0)) + assert predict_threat(own, other, DP) is None + + +def test_no_threat_when_separating(): + # 已经越过最近点、正在远离:即使距离 < 40 m 也不是威胁 + own = state("A", (0, 0, 15), (10, 0, 0)) + other = state("B", (30, 0, 15), (10, 0, 0)) # B 在前方同向同速→静止相对 + # 相对静止且距离 30 < 40 → 触发(静止不算"远离") + assert predict_threat(own, other, DP) is not None + # B 在前方且更快 → 拉开距离 → 不触发 + other_fast = state("B", (30, 0, 15), (12, 0, 0)) + assert predict_threat(own, other_fast, DP) is None + + +def test_threat_already_inside_threshold(): + own = state("A", (0, 0, 15), (0, 0, 0)) + other = state("B", (30, 0, 15), (0, 0, 0)) + th = predict_threat(own, other, DP) + assert th is not None + assert th.cpa_t == pytest.approx(0.0) + assert th.cpa_dist == pytest.approx(30.0) + + +def test_fleet_threats_pairwise(): + a = state("A", (0, 0, 15), (10, 0, 0)) + b = state("B", (100, 0, 15), (-10, 0, 0)) + c = state("C", (0, 500, 15), (0, 10, 0)) # 远离 others + out = fleet_threats([c, b, a], DP) + assert set(out) == {("A", "B")} + near = threats_against(a, [b, c], DP) + assert list(near) == ["B"] + assert isinstance(near["B"], Threat) + + +def test_detect_params_validation(): + with pytest.raises(ValueError): + DetectParams(horizon_s=0.0) + with pytest.raises(ValueError): + DetectParams(cpa_threshold_m=-1.0) diff --git a/src/air/uav_right_of_way/test/test_pipeline.py b/src/air/uav_right_of_way/test/test_pipeline.py new file mode 100644 index 0000000000..5f17b96c6e --- /dev/null +++ b/src/air/uav_right_of_way/test/test_pipeline.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""流水线(core.pipeline)测试:--no-sim 路径、JSON 结构与汇总表。""" +import json +import math + +import numpy as np +import pytest + +from uav_right_of_way.core.detect import DetectParams +from uav_right_of_way.core.pipeline import (format_report, format_table, + run_case, to_json) + + +@pytest.fixture(scope="module") +def headon_nosim(): + return run_case("headon", sim=False) + + +def test_nosim_runs_both_modes(headon_nosim): + r = headon_nosim + assert r["scenario"] == "headon" + assert r["closed_loop"] is False + assert r["baseline"]["decisions"] == 0 + assert r["rules"]["decisions"] >= 2 + # 规则启用显著改善最小间距 + assert r["rules"]["min_separation_m"] > r["baseline"]["min_separation_m"] + assert r["rules"]["min_separation_m"] > 5.0 + + +def test_result_payload_structure(headon_nosim): + r = headon_nosim + for key in ("scenario", "title", "description", "params", "plans", + "baseline", "rules", "trajectories", "wall_time_s"): + assert key in r + assert r["params"]["horizon_s"] == 8.0 + assert r["params"]["cpa_threshold_m"] == 40.0 + b = r["baseline"] + assert b["min_separation_m"] > 0 + assert len(b["min_pair"]) == 2 + rl = r["rules"] + assert isinstance(rl["decision_log"], list) and rl["decision_log"] + d0 = rl["decision_log"][0] + for key in ("t", "uid", "other", "rule", "action", "reason", + "cpa_t", "cpa_dist"): + assert key in d0 + # 轨迹数据可供配图 + tr = r["trajectories"] + assert set(tr) == {"baseline", "rules"} + assert set(tr["rules"]["agents"]) == {"UAV-1", "UAV-2", "UAV-3", "UAV-4"} + + +def test_nosim_all_scenarios_rules_improve(): + for name in ("cross", "overtake"): + r = run_case(name, sim=False) + assert r["rules"]["min_separation_m"] > r["baseline"]["min_separation_m"] + assert r["rules"]["decisions"] > 0 + # 决策理由全部非空(可审计) + assert all(d["reason"] for d in r["rules"]["decision_log"]) + + +def test_to_json_roundtrip(headon_nosim, tmp_path): + path = str(tmp_path / "out.json") + assert to_json([headon_nosim], path) == path + with open(path, "r", encoding="utf-8") as fh: + data = json.load(fh) + assert isinstance(data, list) and len(data) == 1 + assert data[0]["scenario"] == "headon" + assert data[0]["rules"]["min_separation_m"] == \ + headon_nosim["rules"]["min_separation_m"] + + +def test_reports_contain_key_numbers(headon_nosim): + rep = format_report(headon_nosim) + assert "headon" in rep + assert "%.2f" % headon_nosim["baseline"]["min_separation_m"] in rep + table = format_table([headon_nosim]) + assert "headon" in table + assert "%d" % headon_nosim["rules"]["decisions"] in table + + +def test_unknown_scenario_rejected(): + with pytest.raises(KeyError): + run_case("no_such_scenario", sim=False) diff --git a/src/air/uav_right_of_way/test/test_recover.py b/src/air/uav_right_of_way/test/test_recover.py new file mode 100644 index 0000000000..91b684a9e7 --- /dev/null +++ b/src/air/uav_right_of_way/test/test_recover.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""让路恢复(core.recover)单元测试:余弦剖面性质与恢复期监视。""" +import math + +import numpy as np +import pytest + +from uav_right_of_way.core.detect import AircraftState, DetectParams +from uav_right_of_way.core.recover import RecoveryProfile, should_abort + +DP = DetectParams(horizon_s=8.0, cpa_threshold_m=40.0) + + +def state(uid, pos, vel): + return AircraftState(uid, np.asarray(pos, float), + np.asarray(vel, float), 0) + + +def test_profile_endpoints(): + rec = RecoveryProfile(lateral0=20.0, alt0=-10.0, speed_factor0=0.7, + duration=10.0) + assert rec.lateral(0.0) == pytest.approx(20.0) + assert rec.lateral(1.0) == pytest.approx(0.0, abs=1e-12) + assert rec.alt(0.0) == pytest.approx(-10.0) + assert rec.alt(1.0) == pytest.approx(0.0, abs=1e-12) + assert rec.speed_factor(0.0) == pytest.approx(0.7) + assert rec.speed_factor(1.0) == pytest.approx(1.0) + + +def test_profile_zero_rate_at_ends(): + rec = RecoveryProfile(lateral0=20.0, alt0=0.0, duration=10.0) + assert rec.lateral_rate(0.0) == pytest.approx(0.0, abs=1e-12) + assert rec.lateral_rate(1.0) == pytest.approx(0.0, abs=1e-12) + # 中点变化率最大 + assert abs(rec.lateral_rate(0.5)) > abs(rec.lateral_rate(0.25)) + + +def test_profile_midpoint_half_and_monotonic(): + rec = RecoveryProfile(lateral0=20.0, alt0=0.0, duration=10.0) + assert rec.lateral(0.5) == pytest.approx(10.0) + vals = [rec.lateral(s / 20.0) for s in range(21)] + assert all(a >= b - 1e-12 for a, b in zip(vals, vals[1:])) + + +def test_profile_integrates_to_zero_offset(): + # 对横向变化率做中点法数值积分,终点偏移应为 0(余弦剖面自洽) + rec = RecoveryProfile(lateral0=25.0, alt0=0.0, duration=12.0) + n = 2000 + dt = rec.duration / n + x = rec.lateral0 + for k in range(n): + x += rec.lateral_rate((k + 0.5) / n) * dt + assert x == pytest.approx(0.0, abs=1e-4) + + +def test_profile_validation(): + with pytest.raises(ValueError): + RecoveryProfile(lateral0=1.0, alt0=0.0, duration=0.0) + + +def test_should_abort_detects_new_threat(): + own = state("A", (0, 0, 15), (8, 0, 0)) + calm = state("B", (0, 200, 15), (8, 0, 0)) # 平行 200 m,无威胁 + assert should_abort(own, [calm], DP) is None + intruder = state("C", (100, 0, 15), (-8, 0, 0)) # 迎面 → 恢复应中止 + th = should_abort(own, [calm, intruder], DP) + assert th is not None + assert th.other == "C" diff --git a/src/air/uav_right_of_way/test/test_rules.py b/src/air/uav_right_of_way/test/test_rules.py new file mode 100644 index 0000000000..06c5ed46fb --- /dev/null +++ b/src/air/uav_right_of_way/test/test_rules.py @@ -0,0 +1,164 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""规则引擎(core.rules)单元测试:四条规则、动作词表与理由字符串。""" +import math + +import numpy as np +import pytest + +from uav_right_of_way.core.detect import AircraftState, Threat +from uav_right_of_way.core.rules import (ACTION_DESCEND, ACTION_MAINTAIN, + ACTION_OVERTAKE_LEFT, ACTION_SLOW, + ACTION_TURN_RIGHT, Action, RuleParams, + classify, decide) + +RP = RuleParams() + + +def state(uid, pos, vel, priority=0): + return AircraftState(uid, np.asarray(pos, float), + np.asarray(vel, float), priority) + + +# ------------------------------------------------------------------ ① 优先级 +def test_priority_low_yields_headon_turn_right(): + # 对头几何:低优先级 → 右转 30°(规则①) + own = state("A", (0, 0, 15), (8, 0, 0), priority=0) + other = state("B", (100, 0, 15), (-8, 0, 0), priority=10) + dec = decide(own, other, Threat("B", 6.0, 5.0), 1.0, RP) + assert dec.rule == "①优先级" + assert dec.action.kind == ACTION_TURN_RIGHT + assert dec.action.value == pytest.approx(30.0) + assert "优先级" in dec.reason and "10" in dec.reason + + +def test_priority_low_yields_cross_descend(): + # 交叉几何:低优先级 → 降高 10 m(规则①) + own = state("A", (0, 0, 15), (8, 0, 0), priority=0) + other = state("B", (0, 100, 15), (0, -8, 0), priority=10) + dec = decide(own, other, Threat("B", 6.0, 5.0), 1.0, RP) + assert dec.action.kind == ACTION_DESCEND + assert dec.action.value == pytest.approx(10.0) + + +def test_priority_high_stand_on(): + # 高优先级一方:保持航向航速,由对方让路 + own = state("A", (100, 0, 15), (-8, 0, 0), priority=10) + other = state("B", (0, 0, 15), (8, 0, 0), priority=0) + dec = decide(own, other, Threat("B", 6.0, 5.0), 1.0, RP) + assert dec.rule == "①优先级" + assert dec.action.kind == ACTION_MAINTAIN + + +# ------------------------------------------------------------------ ③ 对头 +def test_headon_both_turn_right(): + a = state("A", (0, 0, 15), (8, 0, 0)) + b = state("B", (100, 0, 15), (-8, 0, 0)) + da = decide(a, b, Threat("B", 6.0, 5.0), 1.0, RP) + db = decide(b, a, Threat("A", 6.0, 5.0), 1.0, RP) + assert da.rule == "③对头" and db.rule == "③对头" + assert da.action.kind == ACTION_TURN_RIGHT + assert db.action.kind == ACTION_TURN_RIGHT + assert "右转" in da.reason + + +def test_headon_classification(): + a = state("A", (0, 0, 15), (8, 0, 0)) + b = state("B", (100, 5, 15), (-8, 0, 0)) # 接近正前方迎面 + assert classify(a, b, RP) == "headon" + # 大航向差但对方在侧后方 → 不算对头 + c = state("C", (-50, -60, 15), (-8, 0, 0)) + assert classify(a, c, RP) != "headon" + + +# ------------------------------------------------------------------ ② 左让右 +def test_crossing_give_way_to_right(): + # 对方在本机右侧(叉积 < 0)→ 本机右转让行 + own = state("A", (0, 0, 15), (8, 0, 0)) # 东行 + other = state("B", (0, -80, 15), (0, 8, 0)) # 南边北行 → 在右侧 + dec = decide(own, other, Threat("B", 5.0, 10.0), 1.0, RP) + assert dec.rule == "②左让右" + assert dec.action.kind == ACTION_TURN_RIGHT + assert "右侧" in dec.reason + + +def test_crossing_stand_on_when_on_right(): + # 本机在对方右侧 → 对方让路,本机保持 + own = state("A", (0, 0, 15), (0, 8, 0)) # 北行 + other = state("B", (-80, 0, 15), (8, 0, 0)) # 西边东行 → 在本机左侧 + dec = decide(own, other, Threat("B", 5.0, 10.0), 1.0, RP) + assert dec.rule == "②左让右" + assert dec.action.kind == ACTION_MAINTAIN + + +# ------------------------------------------------------------------ ④ 追越 +def test_overtake_front_maintains(): + front = state("A", (50, 0, 15), (5, 0, 0)) + rear = state("B", (0, 0, 15), (9, 0, 0)) # 正后方快机 + dec = decide(front, rear, Threat("B", 8.0, 30.0), 1.0, RP) + assert dec.rule == "④追越" + assert dec.action.kind == ACTION_MAINTAIN + assert "前机" in dec.reason and "保" in dec.reason + + +def test_overtake_rear_directly_behind_goes_left(): + front = state("A", (40, 0, 15), (5, 0, 0)) + rear = state("B", (0, 0, 15), (9, 0, 0)) + dec = decide(rear, front, Threat("A", 8.0, 30.0), 1.0, RP) + assert dec.rule == "④追越" + assert dec.action.kind == ACTION_OVERTAKE_LEFT + assert "左侧" in dec.reason + + +def test_overtake_rear_on_left_keeps_overtaking(): + front = state("A", (40, 0, 15), (5, 0, 0)) + rear = state("B", (0, 10, 15), (9, 0, 0)) # 已在左侧 + dec = decide(rear, front, Threat("A", 8.0, 30.0), 1.0, RP) + assert dec.action.kind == ACTION_OVERTAKE_LEFT + assert "已具左侧几何" in dec.reason + + +def test_overtake_rear_on_right_slows(): + front = state("A", (40, 0, 15), (5, 0, 0)) + rear = state("B", (0, -10, 15), (9, 0, 0)) # 在右侧,不能从左侧追越 + dec = decide(rear, front, Threat("A", 8.0, 30.0), 1.0, RP) + assert dec.action.kind == ACTION_SLOW + assert dec.action.value == pytest.approx(0.30) + assert "右侧" in dec.reason + + +def test_overtake_classification(): + a = state("A", (0, 0, 15), (8, 0, 0)) + b = state("B", (60, 3, 15), (8.2, 0, 0)) + assert classify(a, b, RP) == "overtake" + + +# ------------------------------------------------------------------ 审计 +def test_decision_reason_and_dict_audit(): + own = state("A", (0, 0, 15), (8, 0, 0)) + other = state("B", (100, 0, 15), (-8, 0, 0)) + dec = decide(own, other, Threat("B", 6.25, 2.0), 12.5, RP) + d = dec.as_dict() + assert d["uid"] == "A" and d["other"] == "B" + assert d["rule"] and d["reason"] + assert d["t"] == pytest.approx(12.5) + assert d["cpa_dist"] == pytest.approx(2.0) + assert d["action"]["kind"] == ACTION_TURN_RIGHT + # 理由字符串包含机号与规则标签(可审计) + assert "B" in dec.reason and dec.rule in dec.reason + + +def test_action_describe_vocabulary(): + assert "30" in Action(ACTION_TURN_RIGHT, 30).describe() + assert "10" in Action(ACTION_DESCEND, 10).describe() + assert "30" in Action(ACTION_SLOW, 0.30).describe() + assert "保" in Action(ACTION_MAINTAIN).describe() + assert "20" in Action(ACTION_OVERTAKE_LEFT, 20).describe() + + +def test_low_speed_maintains(): + own = state("A", (0, 0, 15), (0.0, 0.0, 0.0)) + other = state("B", (100, 0, 15), (-8, 0, 0)) + dec = decide(own, other, Threat("B", 8.0, 10.0), 1.0, RP) + assert dec.action.kind == ACTION_MAINTAIN + assert "速度过低" in dec.reason diff --git a/src/air/uav_right_of_way/test/test_scenarios.py b/src/air/uav_right_of_way/test/test_scenarios.py new file mode 100644 index 0000000000..c836ad6d4a --- /dev/null +++ b/src/air/uav_right_of_way/test/test_scenarios.py @@ -0,0 +1,83 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""场景定义(core.scenarios)单元测试。""" +import math + +import numpy as np +import pytest + +from uav_right_of_way.core.scenarios import (FlightPlan, SCENARIOS, + get_scenario) + + +def test_three_scenarios_four_aircraft(): + assert set(SCENARIOS) == {"headon", "cross", "overtake"} + for name in SCENARIOS: + sc = get_scenario(name) + assert len(sc.plans) == 4 + assert len({p.uid for p in sc.plans}) == 4 + + +def test_plan_phases_and_queries(): + p = FlightPlan("T", (0, 0), (100, 0), alt=10.0, speed=5.0, climb_speed=2.5) + assert p.cruise_t0 == pytest.approx(4.0) # 爬升 10/2.5 + assert p.cruise_t1 == pytest.approx(4.0 + 20.0) # 巡航 100/5 + assert p.end_time == pytest.approx(28.0) # 再加下降 10/2.5 + assert p.position_at(0.0)[2] == pytest.approx(0.0) + assert p.position_at(2.0)[2] == pytest.approx(5.0) + mid = p.position_at(14.0) + assert mid[0] == pytest.approx(50.0) + assert mid[2] == pytest.approx(10.0) + assert p.position_at(24.0)[2] == pytest.approx(10.0) # 下降起点 + assert p.position_at(26.0)[2] == pytest.approx(5.0) + assert p.position_at(28.0)[2] == pytest.approx(0.0) + assert p.in_cruise(10.0) and not p.in_cruise(2.0) + v = p.velocity_at(10.0) + assert v[0] == pytest.approx(5.0) and v[2] == pytest.approx(0.0) + + +def test_cross_track_sign(): + p = FlightPlan("T", (0, 0), (100, 0), alt=10.0, speed=5.0) + assert p.cross_track([50.0, 20.0]) == pytest.approx(20.0) # 左侧为正 + assert p.cross_track([50.0, -20.0]) == pytest.approx(-20.0) + assert p.along_track([30.0, 7.0]) == pytest.approx(30.0) + + +def test_headon_geometry(): + sc = get_scenario("headon") + for a_uid, b_uid in (("UAV-1", "UAV-2"), ("UAV-3", "UAV-4")): + a = next(p for p in sc.plans if p.uid == a_uid) + b = next(p for p in sc.plans if p.uid == b_uid) + assert a.alt == b.alt # 同高度 + hdg_diff = abs(math.atan2(math.sin(b.cruise_heading - a.cruise_heading), + math.cos(b.cruise_heading - a.cruise_heading))) + assert hdg_diff == pytest.approx(math.pi, abs=1e-6) # 对头 + # 同一条走廊 + assert a.start[1] == b.start[1] + + +def test_cross_meets_at_center(): + sc = get_scenario("cross") + for p in sc.plans: + assert p.speed == 8.0 and p.alt == 15.0 + # 巡航中点都过空域中心 + t_mid = 0.5 * (p.cruise_t0 + p.cruise_t1) + pos = p.position_at(t_mid) + assert abs(pos[0]) < 1e-6 and abs(pos[1]) < 1e-6 + + +def test_overtake_rear_faster_same_lane(): + sc = get_scenario("overtake") + u1 = next(p for p in sc.plans if p.uid == "UAV-1") + u2 = next(p for p in sc.plans if p.uid == "UAV-2") + assert u2.speed > u1.speed + assert u1.start[1] == u2.start[1] == u1.end[1] # 同航路 + assert u2.start[0] < u1.start[0] # 后机更靠后 + assert u2.cruise_heading == u1.cruise_heading + + +def test_plan_validation(): + with pytest.raises(ValueError): + FlightPlan("X", (0, 0), (0, 0)) + with pytest.raises(ValueError): + FlightPlan("X", (0, 0), (100, 0), speed=0.0) diff --git a/src/air/uav_right_of_way/test/test_sim.py b/src/air/uav_right_of_way/test/test_sim.py new file mode 100644 index 0000000000..23b31a868f --- /dev/null +++ b/src/air/uav_right_of_way/test/test_sim.py @@ -0,0 +1,67 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""MuJoCo 闭环冒烟测试:小规模对头场景真飞(<10 s 墙钟)。""" +import numpy as np +import pytest + +from uav_right_of_way.core.detect import DetectParams +from uav_right_of_way.core.scenarios import FlightPlan +from uav_right_of_way.core.sim import run_fleet + +pytest.importorskip("mujoco") + +DP = DetectParams(horizon_s=8.0, cpa_threshold_m=40.0) + + +def mini_headon(): + """小规模对头:航程减半、巡航高度 12 m,控制冒烟时长。""" + return [ + FlightPlan("A", (-90.0, 0.0), (90.0, 0.0), alt=12.0, speed=8.0), + FlightPlan("B", (90.0, 0.0), (-90.0, 0.0), alt=12.0, speed=8.0), + ] + + +@pytest.fixture(scope="module") +def smoke(): + plans = mini_headon() + base = run_fleet(plans, enable_rules=False, use_mujoco=True, tail=1.0, + detect_params=DP) + rules = run_fleet(plans, enable_rules=True, use_mujoco=True, tail=1.0, + detect_params=DP) + return base, rules + + +def test_baseline_has_no_decisions_and_collides(smoke): + base, _ = smoke + assert base.mode == "baseline" + assert len(base.decisions) == 0 + min_d, pair, _t = base.min_separation() + assert pair == ("A", "B") + assert min_d < 10.0 # 无规则时两机在中线几乎重合 + + +def test_rules_improve_separation_and_decide(smoke): + _base, rules = smoke + assert rules.mode == "rules" + assert len(rules.decisions) >= 2 # 双方各出一条决策 + assert all(d.reason for d in rules.decisions) + min_d, _pair, _t = rules.min_separation() + assert min_d > 5.0 # 真飞间隔显著拉开 + + +def test_result_shapes_and_wall_time(smoke): + base, rules = smoke + for res in (base, rules): + assert set(res.pos) == {"A", "B"} + n = len(res.times) + assert n > 50 + for arr in res.pos.values(): + assert arr.shape == (n, 3) + assert res.steps > 0 + assert res.wall_time > 0.0 + # 真飞到达误差:终点接近计划终点 + for plan in res.plans.values(): + err = np.linalg.norm(res.pos[plan.uid][-1][:2] + - np.array(plan.end)) + assert err < 6.0, "%s 终点误差 %.2f m" % (plan.uid, err) + assert base.wall_time + rules.wall_time < 60.0 diff --git a/src/air/uav_right_of_way/tools/make_gif.py b/src/air/uav_right_of_way/tools/make_gif.py new file mode 100644 index 0000000000..ba3a1f8e01 --- /dev/null +++ b/src/air/uav_right_of_way/tools/make_gif.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""跑一遍三个场景,把闭环真飞过程录成 gif。 + +左栏是不开规则的直飞,右栏是开规则之后的同一场景,同一时刻同帧对比。 +录的是 MuJoCo 刚体积分出来的真实位置,不是画出来的示意线。 + +用法(在本目录下):python tools/make_gif.py +三个场景都会重跑一遍(MuJoCo 真飞,本机约一分多钟),然后重录四张图。 +""" +from __future__ import annotations + +import os +import sys + +_HERE = os.path.dirname(os.path.abspath(__file__)) +_PKG = os.path.dirname(_HERE) +_SIM = os.path.normpath(os.path.join(os.path.dirname(_PKG), "uav_mujoco_sim")) +for _p in (_PKG, _SIM): + if os.path.isdir(_p) and _p not in sys.path: + sys.path.insert(0, _p) + +import matplotlib + +matplotlib.use("Agg") +import matplotlib.pyplot as plt +from matplotlib import font_manager +from matplotlib.animation import FuncAnimation, PillowWriter +import numpy as np + +for _name in ("Microsoft YaHei", "SimHei", "Noto Sans CJK SC", "DejaVu Sans"): + if any(f.name == _name for f in font_manager.fontManager.ttflist): + plt.rcParams["font.sans-serif"] = [_name] + break +plt.rcParams["axes.unicode_minus"] = False + +ROOT = os.path.dirname(_PKG) +OUT_DIR = os.path.join(ROOT, "..", "..", "docs", "img", "air", "uav_right_of_way") +C_BASE = "#7f7f7f" +C_RULE = ["#1f77b4", "#d62728", "#2ca02c", "#ff7f0e"] + +# 三个场景各录一段,时间轴统一从 0 开始 +SCENES = [("headon", "对头相遇(各自右转 30°)"), + ("cross", "交叉相遇(左让右)"), + ("overtake", "追越(从左侧通过)")] + + +def run_scene(name): + """跑一个场景的基线与规则两趟,返回 (sc, base_traj, rule_traj)。""" + from uav_right_of_way.core.sim import run_fleet + from uav_right_of_way.core.scenarios import get_scenario + + sc = get_scenario(name) + # record_every 调小一点,出 gif 才不卡 + base = run_fleet(sc.plans, enable_rules=False, record_every=5) + rule = run_fleet(sc.plans, enable_rules=True, record_every=5) + return sc, dict(times=base.times, agents=base.pos), \ + dict(times=rule.times, agents=rule.pos) + + +def _draw(ax, traj, t_idx, color_of, title): + ax.clear() + ax.set_title(title, fontsize=10) + for uid, rows in sorted(traj["agents"].items()): + arr = np.asarray(rows)[: t_idx + 1] + if len(arr) == 0: + continue + c = color_of(uid) + ax.plot(arr[:, 0], arr[:, 1], "-", color=c, lw=1.2, alpha=0.75) + ax.plot(arr[-1, 0], arr[-1, 1], "o", color=c, ms=7) + ax.annotate(uid, (arr[-1, 0], arr[-1, 1]), textcoords="offset points", + xytext=(5, 4), fontsize=8, color=c) + ax.set_aspect("equal") + ax.grid(alpha=0.25, lw=0.5) + ax.set_xlabel("x / m", fontsize=8) + ax.set_ylabel("y / m", fontsize=8) + ax.tick_params(labelsize=8) + + +def make_gif(name, title, sc, base, rule, out): + ids = sorted(base["agents"]) + color_of = lambda uid: C_RULE[ids.index(uid) % len(C_RULE)] # noqa: E731 + + tb = np.asarray(base["times"], float) + tr = np.asarray(rule["times"], float) + n = min(len(tb), len(tr)) + # 抽帧,别录几百帧进来 + frames = np.linspace(0, n - 1, min(n, 56)).astype(int) + + fig, axes = plt.subplots(1, 2, figsize=(7.8, 3.5)) + fig.suptitle(title, fontsize=11) + + def update(k): + k = frames[k] + _draw(axes[0], base, k, color_of, "不给规则,直接飞") + _draw(axes[1], rule, k, color_of, "给规则,实时让路") + fig.suptitle("%s t = %.0f s" % (title, tb[k]), fontsize=11) + return [] + + anim = FuncAnimation(fig, update, frames=len(frames), interval=180, + blit=False) + os.makedirs(os.path.dirname(out), exist_ok=True) + anim.save(out, writer=PillowWriter(fps=6)) + plt.close(fig) + return out + + +def main(): + runs = [] + for name, title in SCENES: + sc, base, rule = run_scene(name) + runs.append((name, title, base, rule)) + out = os.path.join(OUT_DIR, "%s.gif" % name) + make_gif(name, title, sc, base, rule, out) + print("saved", os.path.normpath(out)) + + # 三场景俯视总图(静态),给文档当第一张图 + make_overview(runs) + + +def make_overview(runs): + fig, axes = plt.subplots(1, 3, figsize=(10.5, 3.2)) + for ax, (name, title, base, rule) in zip(axes, runs): + ids = sorted(base["agents"]) + for i, uid in enumerate(ids): + ab = np.asarray(base["agents"][uid]) + ar = np.asarray(rule["agents"][uid]) + c = C_RULE[i % len(C_RULE)] + ax.plot(ab[:, 0], ab[:, 1], "--", color=c, lw=1.0, alpha=0.40) + ax.plot(ar[:, 0], ar[:, 1], "-", color=c, lw=1.1, alpha=0.85) + ax.set_title(title.split("(")[0], fontsize=10) + ax.set_aspect("equal") + ax.grid(alpha=0.25, lw=0.5) + ax.tick_params(labelsize=8) + ax.set_xlabel("x / m", fontsize=8) + ax.set_ylabel("y / m", fontsize=8) + axes[0].plot([], [], "--", color=C_BASE, label="基线") + axes[0].plot([], [], "-", color=C_BASE, label="规则") + axes[0].legend(fontsize=7, loc="best") + fig.tight_layout() + out = os.path.join(OUT_DIR, "trajectories.png") + os.makedirs(os.path.dirname(out), exist_ok=True) + fig.savefig(out, dpi=130) + plt.close(fig) + print("saved", os.path.normpath(out)) + + +if __name__ == "__main__": + main() diff --git a/src/air/uav_right_of_way/uav_right_of_way/__init__.py b/src/air/uav_right_of_way/uav_right_of_way/__init__.py new file mode 100644 index 0000000000..60cfd5434b --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/__init__.py @@ -0,0 +1,3 @@ +# -*- coding: utf-8 -*- +"""uav_right_of_way:低空多机优先级通行权在线决策。""" +__version__ = "0.1.0" diff --git a/src/air/uav_right_of_way/uav_right_of_way/cli.py b/src/air/uav_right_of_way/uav_right_of_way/cli.py new file mode 100644 index 0000000000..a4db2c796e --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/cli.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""命令行入口(``ros2 run uav_right_of_way main`` 与 ``python main.py`` 共用)。""" +from __future__ import annotations + +import argparse +import os +import sys + +from .core.pipeline import (format_report, format_table, run_all, run_case, + to_json) +from .core.scenarios import SCENARIOS + +_HERE = os.path.dirname(os.path.abspath(__file__)) +_PKG = os.path.dirname(_HERE) +_RESULTS_DIR = os.path.join(_PKG, "results") +_IMAGES_DIR = os.path.join(_PKG, "images") + + +def build_parser() -> argparse.ArgumentParser: + p = argparse.ArgumentParser( + description="低空多机优先级通行权在线决策(基线 vs 规则启用,MuJoCo 闭环)") + p.add_argument("--scenario", "--case", dest="scenario", default="all", + help="场景名(%s)或 all" % ", ".join(SCENARIOS)) + p.add_argument("--no-sim", action="store_true", + help="跳过 MuJoCo 闭环,仅算法层(秒级)") + p.add_argument("--json", default="", + help="结果 JSON 路径(默认自动写 results/<场景>_result.json)") + p.add_argument("--progress", action="store_true", help="打印仿真进度") + return p + + +def main(argv=None) -> int: + args = build_parser().parse_args(argv) + sim = not args.no_sim + + if args.scenario == "all": + results = run_all(list(SCENARIOS), sim=sim, progress=args.progress) + else: + if args.scenario not in SCENARIOS: + print("未知场景 %r,可选:%s 或 all" + % (args.scenario, ", ".join(SCENARIOS))) + return 2 + results = [run_case(args.scenario, sim=sim, progress=args.progress)] + + print() + for r in results: + print(format_report(r)) + print() + + # 每个场景自动落盘 results/<场景>_result.json(SPEC 要求) + os.makedirs(_RESULTS_DIR, exist_ok=True) + for r in results: + p = os.path.join(_RESULTS_DIR, "%s_result.json" % r["scenario"]) + print("机读结果已写入 %s" % to_json([r], p)) + if args.json: + print("机读结果已写入 %s" % to_json(results, args.json)) + + print("轨迹俯视图/动图需要的话跑:python tools/make_gif.py") + print(format_table(results)) + return 0 + + +if __name__ == "__main__": + raise SystemExit(main()) diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/__init__.py b/src/air/uav_right_of_way/uav_right_of_way/core/__init__.py new file mode 100644 index 0000000000..1eb2a4146e --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/core/__init__.py @@ -0,0 +1,2 @@ +# -*- coding: utf-8 -*- +"""纯算法层:只依赖 numpy,不依赖 ROS。""" diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/detect.py b/src/air/uav_right_of_way/uav_right_of_way/core/detect.py new file mode 100644 index 0000000000..5fa12fd8f2 --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/core/detect.py @@ -0,0 +1,141 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""侵入判定:用相对运动解析求最近接近点(CPA),预测短时威胁。 + +本模块回答"现在要不要启动让路决策":给定两机当前状态(位置/速度), +把双方近似为匀速直线运动,相对运动也是匀速直线,最近接近可以解析求出: + + d(t) = |Δp + Δv·t|, t_cpa = -Δp·Δv / |Δv|² + +当 ``0 < t_cpa ≤ horizon`` 且预测 CPA 距离 < ``cpa_threshold`` 时触发决策。 +与参考实现 uav_deconflict 的 ``conflict.py`` 同思路(相对运动解析、无采样 +步长),但这里用在**在线**判定上:输入是当前一帧状态而不是整条计划航迹。 + +约定:``t_cpa ≤ 0``(正在远离)不算威胁——追越通过后两机必然逐渐拉开, +不能让"还在 40 m 内但正在分离"的状态永远触发决策。 +""" +from __future__ import annotations + +import math +from dataclasses import dataclass, field +from typing import Dict, Optional, Sequence, Tuple + +import numpy as np + + +@dataclass(frozen=True) +class DetectParams: + """侵入判定参数(对应 config/uav_right_of_way.yaml)。""" + + horizon_s: float = 8.0 # 预测时域(s) + cpa_threshold_m: float = 40.0 # CPA 距离阈值(m) + + def __post_init__(self) -> None: + if self.horizon_s <= 0.0 or self.cpa_threshold_m <= 0.0: + raise ValueError("horizon 与 cpa_threshold 必须为正: %r" % (self,)) + + +@dataclass +class AircraftState: + """一帧机体状态:位置 / 速度(世界系 ENU,z 向上)+ 任务优先级。""" + + uid: str + pos: np.ndarray # (3,) m + vel: np.ndarray # (3,) m/s + priority: int = 0 + label: str = "" + + @property + def speed(self) -> float: + return float(np.linalg.norm(self.vel)) + + @property + def heading(self) -> Optional[float]: + """航向角(rad,从 x 东逆时针);速度过低时返回 None。""" + if float(np.hypot(self.vel[0], self.vel[1])) < 1e-6: + return None + return float(math.atan2(self.vel[1], self.vel[0])) + + def as_dict(self) -> dict: + return dict(uid=self.uid, + pos=[round(float(v), 2) for v in self.pos], + vel=[round(float(v), 2) for v in self.vel], + priority=self.priority) + + +@dataclass +class Threat: + """一次预测到的接近威胁。""" + + other: str # 对方机号 + cpa_t: float # 预测最近接近时刻(相对当前,已截断到 horizon) + cpa_dist: float # 预测最近接近距离(m) + + def as_dict(self) -> dict: + return dict(other=self.other, + cpa_t=round(self.cpa_t, 2), + cpa_dist=round(self.cpa_dist, 2)) + + +def cpa(p_rel: np.ndarray, v_rel: np.ndarray, + horizon: float) -> Tuple[float, float]: + """相对运动解析 CPA:返回 (t_cpa, 最近距离)。 + + ``t_cpa`` 已截断到 ``[0, horizon]``;相对静止时返回 (0, 当前距离)。 + """ + p_rel = np.asarray(p_rel, dtype=float) + v_rel = np.asarray(v_rel, dtype=float) + a = float(np.dot(v_rel, v_rel)) + if a <= 1e-9: + return 0.0, float(np.linalg.norm(p_rel)) + t = -float(np.dot(p_rel, v_rel)) / a + t = min(max(t, 0.0), horizon) + return t, float(np.linalg.norm(p_rel + v_rel * t)) + + +def predict_threat(own: AircraftState, other: AircraftState, + params: DetectParams = DetectParams()) -> Optional[Threat]: + """预测本机与另一机的接近威胁;不满足触发条件返回 ``None``。""" + p_rel = np.asarray(other.pos, dtype=float) - np.asarray(own.pos, dtype=float) + v_rel = np.asarray(other.vel, dtype=float) - np.asarray(own.vel, dtype=float) + a = float(np.dot(v_rel, v_rel)) + if a <= 1e-9: + d = float(np.linalg.norm(p_rel)) + return Threat(other.uid, 0.0, d) if d < params.cpa_threshold_m else None + t_unc = -float(np.dot(p_rel, v_rel)) / a + if t_unc <= 0.0: + # 正在远离:不算威胁(否则追越通过后永远触发) + return None + t = min(t_unc, params.horizon_s) + d = float(np.linalg.norm(p_rel + v_rel * t)) + if d < params.cpa_threshold_m: + return Threat(other.uid, t, d) + return None + + +def fleet_threats(states: Sequence[AircraftState], + params: DetectParams = DetectParams() + ) -> Dict[Tuple[str, str], Threat]: + """机队两两检测,返回 {(uid_a, uid_b): Threat}(a 按字典序在 b 前)。""" + out: Dict[Tuple[str, str], Threat] = {} + items = sorted(states, key=lambda s: s.uid) + for i, a in enumerate(items): + for b in items[i + 1:]: + th = predict_threat(a, b, params) + if th is not None: + out[(a.uid, b.uid)] = th + return out + + +def threats_against(own: AircraftState, + others: Sequence[AircraftState], + params: DetectParams = DetectParams()) -> Dict[str, Threat]: + """以本机视角对所有其他机做威胁检测,返回 {对方机号: Threat}。""" + out: Dict[str, Threat] = {} + for o in others: + if o.uid == own.uid: + continue + th = predict_threat(own, o, params) + if th is not None: + out[o.uid] = th + return out diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/pipeline.py b/src/air/uav_right_of_way/uav_right_of_way/core/pipeline.py new file mode 100644 index 0000000000..24b1fb5c46 --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/core/pipeline.py @@ -0,0 +1,140 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""端到端流水线:场景 → 基线/规则启用闭环 → 指标 → JSON 与控制台报告。""" +from __future__ import annotations + +import json +import os +from typing import List, Optional, Sequence + +import numpy as np + +from .detect import DetectParams +from .rules import RuleParams +from .scenarios import SCENARIOS, Scenario, get_scenario +from .sim import (CLEAR_HOLD_S, DECIDE_DT, RECOVERY_MIN_S, RunResult, + run_fleet) + + +def run_case(name: str, sim: bool = True, progress: bool = False, + detect_params: Optional[DetectParams] = None, + rule_params: Optional[RuleParams] = None) -> dict: + """跑一个场景:基线(无规则)与规则启用各闭环一遍,返回 JSON 友好结果。""" + sc = get_scenario(name) + dp = detect_params or DetectParams() + rp = rule_params or RuleParams() + + if progress: + print("[%s] %s — 基线(无规则直飞)..." % (name, sc.title)) + baseline = run_fleet(sc.plans, enable_rules=False, progress=progress, + use_mujoco=sim, detect_params=dp, rule_params=rp) + if progress: + print("[%s] 规则启用(在线让路 + 余弦恢复)..." % name) + rules = run_fleet(sc.plans, enable_rules=True, progress=progress, + use_mujoco=sim, detect_params=dp, rule_params=rp) + + return _build_result(sc, baseline, rules, dp, rp, sim) + + +def run_all(names: Sequence[str], sim: bool = True, progress: bool = False, + detect_params: Optional[DetectParams] = None, + rule_params: Optional[RuleParams] = None) -> List[dict]: + return [run_case(n, sim=sim, progress=progress, + detect_params=detect_params, rule_params=rule_params) + for n in names] + + +def _traj_payload(res: RunResult, stride: int = 10, + prec: int = 1) -> dict: + """轨迹降采样落盘(每 stride 个记录点取 1 个,约 2 Hz)。""" + out = {} + for uid, arr in sorted(res.pos.items()): + out[uid] = [[round(float(x), prec) for x in row] + for row in arr[::stride]] + times = [round(float(t), 2) for t in res.times[::stride]] + return dict(times=times, agents=out) + + +def _build_result(sc: Scenario, baseline: RunResult, rules: RunResult, + dp: DetectParams, rp: RuleParams, sim: bool) -> dict: + return dict( + scenario=sc.name, + title=sc.title, + description=sc.description, + closed_loop=bool(sim), + params=dict( + horizon_s=dp.horizon_s, + cpa_threshold_m=dp.cpa_threshold_m, + turn_right_deg=rp.turn_right_deg, + descend_m=rp.descend_m, + slow_fraction=rp.slow_fraction, + overtake_lateral_m=rp.overtake_lateral_m, + decision_interval_s=DECIDE_DT, + clear_hold_s=CLEAR_HOLD_S, + recovery_min_s=RECOVERY_MIN_S, + ), + plans=[p.summary() for p in sc.plans], + baseline=baseline.summary(dp), + rules=dict(rules.summary(dp), + decision_log=[d.as_dict() for d in rules.decisions]), + trajectories=dict(baseline=_traj_payload(baseline), + rules=_traj_payload(rules)), + wall_time_s=round(baseline.wall_time + rules.wall_time, 1), + ) + + +def to_json(results: Sequence[dict], path: str) -> str: + os.makedirs(os.path.dirname(os.path.abspath(path)) or ".", exist_ok=True) + # 轨迹不落盘(占地方,评审也用不上);出图用 tools/make_gif.py 重跑 + trimmed = [] + for r in results: + r = dict(r) + r.pop("trajectories", None) + trimmed.append(r) + with open(path, "w", encoding="utf-8") as fh: + json.dump(trimmed, fh, ensure_ascii=False, indent=2) + return path + + +def format_report(result: dict) -> str: + """单场景控制台报告。""" + lines = [] + lines.append("=" * 72) + lines.append("%s — %s" % (result["scenario"], result["title"])) + lines.append(result["description"]) + lines.append("-" * 72) + lines.append("判定参数:预测 %.0f s 内 CPA < %.0f m 触发决策" + % (result["params"]["horizon_s"], + result["params"]["cpa_threshold_m"])) + b = result["baseline"] + r = result["rules"] + lines.append("基线(无规则):最小间距 %.2f m(%s),40 m 内采样 %d 点" + % (b["min_separation_m"], "↔".join(b["min_pair"]), + b["below_threshold_samples"])) + lines.append("规则启用 :最小间距 %.2f m(%s),决策 %d 次" + % (r["min_separation_m"], "↔".join(r["min_pair"]), + r["decisions"])) + lines.append("平均航迹偏离:基线 %.2f m → 规则 %.2f m" + % (b["mean_deviation_m"], r["mean_deviation_m"])) + if r.get("decision_log"): + lines.append("决策日志(前 8 条):") + for d in r["decision_log"][:8]: + lines.append(" t=%5.1fs %s %s %s" % + (d["t"], d["uid"], d["action"]["kind"], d["reason"])) + return "\n".join(lines) + + +def format_table(results: Sequence[dict]) -> str: + """汇总表:基线 vs 规则启用的最小间距 / 决策次数 / 平均偏离。""" + lines = [] + lines.append("=" * 84) + lines.append("%-10s %14s %14s %8s %12s %12s" % + ("场景", "基线最小间距m", "规则最小间距m", "决策次数", + "基线偏离m", "规则偏离m")) + for r in results: + b, rl = r["baseline"], r["rules"] + lines.append("%-10s %14.2f %14.2f %8d %12.2f %12.2f" % + (r["scenario"], b["min_separation_m"], + rl["min_separation_m"], rl["decisions"], + b["mean_deviation_m"], rl["mean_deviation_m"])) + return "\n".join(lines) diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/recover.py b/src/air/uav_right_of_way/uav_right_of_way/core/recover.py new file mode 100644 index 0000000000..3ae1478dd0 --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/core/recover.py @@ -0,0 +1,84 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""让路恢复:脱离威胁后按余弦剖面平滑回到计划航路,恢复期间持续监视。 + +与参考实现 uav_deconflict ``plan.py`` 的 ``with_lateral_offset`` 同一模式: +偏移量沿恢复进程按 ``(1 + cos πs)/2`` 衰减——起点保持当前偏移、终点归零, +且两端变化率为零,因此不会出现航向/升降率阶跃去激励几何控制器的姿态 +回路(参考实现实测:不平滑的折点偏移会激起增幅振荡、跟踪误差发散到 +2×10^5 m)。 + +恢复不是一个"闭着眼执行完"的过程:调用方(``sim.py``)每个决策周期用 +``should_abort`` 重新做威胁检测,一旦出现二次冲突立即中止恢复、重新进入 +规则引擎——这就是"恢复期间持续监视防止二次冲突"。 +""" +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import Optional, Sequence + +from .detect import DetectParams, Threat, predict_threat, AircraftState + + +@dataclass +class RecoveryProfile: + """余弦剖面恢复:横向/高度偏移从当前值平滑衰减到 0,速度系数回到 1。 + + Parameters + ---------- + lateral0 + 恢复开始时的横向偏移(m,沿计划航向左侧为正)。 + alt0 + 恢复开始时的高度偏移(m,向上为正,通常为负值=降过高度)。 + speed_factor0 + 恢复开始时的速度系数(<1 表示此前减过速)。 + duration + 恢复总时长(s)。 + """ + + lateral0: float + alt0: float + speed_factor0: float = 1.0 + duration: float = 10.0 + + def __post_init__(self) -> None: + if self.duration <= 0.0: + raise ValueError("恢复时长必须为正: %r" % (self,)) + + def s(self, tau: float) -> float: + """恢复进度 ∈ [0, 1],``tau`` 为恢复开始后经过的秒数。""" + return min(1.0, max(0.0, tau / self.duration)) + + def _shape(self, s: float) -> float: + return 0.5 * (1.0 + math.cos(math.pi * s)) + + def lateral(self, s: float) -> float: + return self.lateral0 * self._shape(s) + + def lateral_rate(self, s: float) -> float: + """横向偏移的时间导数(m/s)。""" + return -self.lateral0 * math.pi * math.sin(math.pi * s) \ + / (2.0 * self.duration) + + def alt(self, s: float) -> float: + return self.alt0 * self._shape(s) + + def alt_rate(self, s: float) -> float: + return -self.alt0 * math.pi * math.sin(math.pi * s) \ + / (2.0 * self.duration) + + def speed_factor(self, s: float) -> float: + return 1.0 + (self.speed_factor0 - 1.0) * self._shape(s) + + +def should_abort(own: AircraftState, others: Sequence[AircraftState], + params: DetectParams = DetectParams()) -> Optional[Threat]: + """恢复期间持续监视:出现新的预测威胁则返回该威胁(应中止恢复)。""" + for other in others: + if other.uid == own.uid: + continue + th = predict_threat(own, other, params) + if th is not None: + return th + return None diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/rules.py b/src/air/uav_right_of_way/uav_right_of_way/core/rules.py new file mode 100644 index 0000000000..859093e9f1 --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/core/rules.py @@ -0,0 +1,221 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""通行权规则引擎:输入两机状态,输出让路动作 + 可审计的决策理由。 + +四条规则(按优先级从高到低逐条适用,几何条件决定命中哪条): + +① 优先级:``priority`` 高者先行(医疗/警用等),低者让路—— + 对头几何右转 30°,其余几何降高 10 m; +② 左让右(同优先级交叉相遇):用航向叉积判左右,位于左侧的一方 + 右转 30° 让行,位于右侧的一方保持航向航速; +③ 对头相遇(同优先级):各自右转 30°; +④ 追越(同优先级、小航向差):前机保速保向;追越机必须**从左侧** + 通过——已具左侧几何则左偏 ``overtake_lateral_m`` 追越, + 若位于前机右侧则先减速 30% 保持间隔。 + +动作词表:右转 / 降高 / 减速 / 保向(外加规则④自身要求的左侧追越偏置)。 +每条输出都带 ``rule`` 标签与人类可读的 ``reason`` 字符串,供日志审计。 +""" +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import Optional + +import numpy as np + +from .detect import AircraftState, Threat + +# ------------------------------------------------------------------ 动作词表 +ACTION_TURN_RIGHT = "turn_right" # 右转(value=角度 deg) +ACTION_DESCEND = "descend" # 降高(value=下降量 m) +ACTION_SLOW = "slow" # 减速(value=减速比例 0~1) +ACTION_MAINTAIN = "maintain" # 保向保速 +ACTION_OVERTAKE_LEFT = "overtake_left" # 左侧追越偏置(value=偏置 m) + + +@dataclass(frozen=True) +class RuleParams: + """规则动作参数(对应 config/uav_right_of_way.yaml)。""" + + turn_right_deg: float = 30.0 + descend_m: float = 10.0 + slow_fraction: float = 0.30 + overtake_lateral_m: float = 20.0 + #: 判定对头的航向差阈值(cos < cos(150°)) + headon_cos: float = -0.8660254037844387 + #: 对头还要求对方接近正前方(相对方位角 < 25°) + headon_bearing_deg: float = 25.0 + #: 判定同向追越的航向差阈值(cos > cos(10°)) + same_dir_cos: float = 0.984807753012208 + #: 追越左右侧判定的最小横向距离(m) + side_eps_m: float = 3.0 + + +@dataclass(frozen=True) +class Action: + """一个让路动作。""" + + kind: str + value: float = 0.0 + + def describe(self) -> str: + if self.kind == ACTION_TURN_RIGHT: + return "右转%.0f°" % self.value + if self.kind == ACTION_DESCEND: + return "降高%.0fm" % self.value + if self.kind == ACTION_SLOW: + return "减速%.0f%%" % (self.value * 100.0) + if self.kind == ACTION_OVERTAKE_LEFT: + return "左侧偏置%.0fm追越" % self.value + return "保向保速" + + def as_dict(self) -> dict: + return dict(kind=self.kind, value=round(float(self.value), 2)) + + +@dataclass +class Decision: + """一次规则引擎输出(可审计)。""" + + t: float + uid: str # 决策主体(让路/保持的机) + other: str # 威胁来源机 + rule: str # 命中的规则标签,如 "①优先级" + action: Action + reason: str # 决策理由(人类可读,写日志/JSON) + cpa_t: float = 0.0 + cpa_dist: float = 0.0 + + def as_dict(self) -> dict: + return dict(t=round(self.t, 2), uid=self.uid, other=self.other, + rule=self.rule, action=self.action.as_dict(), + reason=self.reason, + cpa_t=round(self.cpa_t, 2), cpa_dist=round(self.cpa_dist, 2)) + + +def _unit(hdg: float) -> np.ndarray: + return np.array([math.cos(hdg), math.sin(hdg)]) + + +def _wrap_pi(a: float) -> float: + while a > math.pi: + a -= 2.0 * math.pi + while a < -math.pi: + a += 2.0 * math.pi + return a + + +def classify(own: AircraftState, other: AircraftState, + params: RuleParams = RuleParams()) -> str: + """两机相遇几何分类:``headon`` / ``overtake`` / ``crossing``。""" + h_own = own.heading + h_other = other.heading + if h_own is None or h_other is None: + return "crossing" + diff = _wrap_pi(h_other - h_own) + rel = np.asarray(other.pos[:2]) - np.asarray(own.pos[:2]) + rel_bearing = abs(_wrap_pi(math.atan2(rel[1], rel[0]) - h_own)) + if math.cos(diff) < params.headon_cos and \ + rel_bearing < math.radians(params.headon_bearing_deg): + return "headon" + if math.cos(diff) > params.same_dir_cos: + return "overtake" + return "crossing" + + +def decide(own: AircraftState, other: AircraftState, + threat: Optional[Threat] = None, t: float = 0.0, + params: RuleParams = RuleParams()) -> Decision: + """规则引擎:给定两机状态返回决策(动作 + 规则标签 + 理由)。""" + if threat is None: + d = float(np.linalg.norm(np.asarray(other.pos, dtype=float) + - np.asarray(own.pos, dtype=float))) + threat = Threat(other.uid, 0.0, d) + + h_own = own.heading + h_other = other.heading + if h_own is None or h_other is None: + return Decision(t, own.uid, other.uid, "—", Action(ACTION_MAINTAIN), + "速度过低,保持当前状态", threat.cpa_t, threat.cpa_dist) + + rel = np.asarray(other.pos[:2], dtype=float) - np.asarray(own.pos[:2], dtype=float) + geo = classify(own, other, params) + + # ---- 规则①优先级:高者先行 --------------------------------------- + if own.priority < other.priority: + if geo == "headon": + act = Action(ACTION_TURN_RIGHT, params.turn_right_deg) + reason = ("规则①优先级:对方 %s 优先级 %d 高于本机 %d,对头相遇右转%.0f°让行" + % (other.uid, other.priority, own.priority, + params.turn_right_deg)) + else: + act = Action(ACTION_DESCEND, params.descend_m) + reason = ("规则①优先级:对方 %s 优先级 %d 高于本机 %d,降高%.0fm让行" + % (other.uid, other.priority, own.priority, + params.descend_m)) + return Decision(t, own.uid, other.uid, "①优先级", act, reason, + threat.cpa_t, threat.cpa_dist) + if own.priority > other.priority: + act = Action(ACTION_MAINTAIN) + reason = ("规则①优先级:本机优先级 %d 高于对方 %s 的 %d,保持航向航速由对方让路" + % (own.priority, other.uid, other.priority)) + return Decision(t, own.uid, other.uid, "①优先级", act, reason, + threat.cpa_t, threat.cpa_dist) + + # ---- 规则③对头相遇:各自右转 -------------------------------------- + if geo == "headon": + diff_deg = math.degrees(abs(_wrap_pi(h_other - h_own))) + act = Action(ACTION_TURN_RIGHT, params.turn_right_deg) + reason = ("规则③对头相遇:与 %s 航向差 %.0f°、CPA %.0fm@%.1fs,各自右转%.0f°" + % (other.uid, diff_deg, threat.cpa_dist, threat.cpa_t, + params.turn_right_deg)) + return Decision(t, own.uid, other.uid, "③对头", act, reason, + threat.cpa_t, threat.cpa_dist) + + # ---- 规则④追越:前机保速保向,追越机从左侧通过 --------------------- + if geo == "overtake": + u_other = _unit(h_other) + own_from_other = -rel # 本机相对前机的位置 + cross_z = float(u_other[0] * own_from_other[1] + - u_other[1] * own_from_other[0]) + along = float(np.dot(own_from_other, u_other)) # >0 本机在前 + if along > 0.0: + act = Action(ACTION_MAINTAIN) + reason = ("规则④追越:本机为前机,保速保向,由 %s 从左侧追越" + "(CPA %.0fm@%.1fs)" % (other.uid, threat.cpa_dist, + threat.cpa_t)) + return Decision(t, own.uid, other.uid, "④追越", act, reason, + threat.cpa_t, threat.cpa_dist) + if cross_z < -params.side_eps_m: + act = Action(ACTION_SLOW, params.slow_fraction) + reason = ("规则④追越:须从左侧追越 %s,本机现处其右侧,先减速%.0f%%保持间隔" + "(CPA %.0fm@%.1fs)" % (other.uid, params.slow_fraction * 100.0, + threat.cpa_dist, threat.cpa_t)) + return Decision(t, own.uid, other.uid, "④追越", act, reason, + threat.cpa_t, threat.cpa_dist) + act = Action(ACTION_OVERTAKE_LEFT, params.overtake_lateral_m) + side = "已具左侧几何" if cross_z > params.side_eps_m else "正后方" + reason = ("规则④追越:从左侧追越 %s,左偏%.0fm保持间隔(%s,CPA %.0fm@%.1fs)" + % (other.uid, params.overtake_lateral_m, side, + threat.cpa_dist, threat.cpa_t)) + return Decision(t, own.uid, other.uid, "④追越", act, reason, + threat.cpa_t, threat.cpa_dist) + + # ---- 规则②交叉相遇:左让右(航向叉积判左右) ----------------------- + u_own = _unit(h_own) + cross_z = float(u_own[0] * rel[1] - u_own[1] * rel[0]) + if cross_z < 0.0: + act = Action(ACTION_TURN_RIGHT, params.turn_right_deg) + reason = ("规则②交叉相遇左让右:%s 位于本机右侧(叉积 %.1f),右转%.0f°让行" + "(CPA %.0fm@%.1fs)" % (other.uid, cross_z, + params.turn_right_deg, + threat.cpa_dist, threat.cpa_t)) + return Decision(t, own.uid, other.uid, "②左让右", act, reason, + threat.cpa_t, threat.cpa_dist) + act = Action(ACTION_MAINTAIN) + reason = ("规则②交叉相遇左让右:本机位于 %s 右侧(叉积 %.1f,右方机优先)," + "保持航向航速(CPA %.0fm@%.1fs)" + % (other.uid, cross_z, threat.cpa_dist, threat.cpa_t)) + return Decision(t, own.uid, other.uid, "②左让右", act, reason, + threat.cpa_t, threat.cpa_dist) diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/scenarios.py b/src/air/uav_right_of_way/uav_right_of_way/core/scenarios.py new file mode 100644 index 0000000000..882c76adf1 --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/core/scenarios.py @@ -0,0 +1,223 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""三个可复现的相遇场景:headon(对头)、cross(十字交叉)、overtake(追越)。 + +所有场景都是"两机已近距离相遇"的在线局面:每架无人机沿自己的计划航路 +(起飞爬升 → 直线平飞 → 下降)直飞,冲突来自航路几何而不是人为摆位。 +每个场景 4 架,基线(无规则直飞)与规则启用共用同一批计划。 +""" +from __future__ import annotations + +import bisect +import math +from dataclasses import dataclass, field +from typing import Dict, List, Optional, Sequence, Tuple + +import numpy as np + + +@dataclass +class FlightPlan: + """一架无人机的计划:二维航路(直线)+ 巡航高度 + 巡航速度。 + + 运动分三段:起点垂直爬升到 ``alt`` → 沿航路匀速平飞 → 终点垂直下降。 + 与参考实现 ``Plan`` 同构,但只保留在线决策需要的查询接口。 + """ + + uid: str + start: Tuple[float, float] + end: Tuple[float, float] + alt: float = 15.0 # m + speed: float = 8.0 # m/s + climb_speed: float = 2.5 # m/s + priority: int = 0 + label: str = "" + + _t_cruise0: float = field(default=0.0, init=False, repr=False) + _t_cruise1: float = field(default=0.0, init=False, repr=False) + _t_end: float = field(default=0.0, init=False, repr=False) + + def __post_init__(self) -> None: + dx = self.end[0] - self.start[0] + dy = self.end[1] - self.start[1] + if math.hypot(dx, dy) <= 1e-9: + raise ValueError("%s 航路起终点重合" % self.uid) + if self.alt < 0.0 or self.speed <= 0.0 or self.climb_speed <= 0.0: + raise ValueError("%s 参数非法: alt=%r speed=%r" % (self.uid, self.alt, + self.speed)) + self._cruise_len = math.hypot(dx, dy) + climb = self.alt / self.climb_speed + self._t_cruise0 = climb + self._t_cruise1 = climb + self._cruise_len / self.speed + self._t_end = self._t_cruise1 + climb + + # ---------------------------------------------------------------- 查询 + @property + def cruise_heading(self) -> float: + return math.atan2(self.end[1] - self.start[1], + self.end[0] - self.start[0]) + + @property + def cruise_start(self) -> np.ndarray: + return np.array([self.start[0], self.start[1], self.alt]) + + @property + def cruise_end(self) -> np.ndarray: + return np.array([self.end[0], self.end[1], self.alt]) + + @property + def cruise_t0(self) -> float: + return self._t_cruise0 + + @property + def cruise_t1(self) -> float: + return self._t_cruise1 + + @property + def end_time(self) -> float: + return self._t_end + + def in_cruise(self, t: float) -> bool: + return self.cruise_t0 - 1e-9 <= t <= self.cruise_t1 + 1e-9 + + def cruise_progress(self, t: float) -> float: + """巡航段已飞距离(m,截断到 [0, L])。""" + return min(max((t - self.cruise_t0) * self.speed, 0.0), self._cruise_len) + + def position_at(self, t: float) -> np.ndarray: + """计划位置(爬升→平飞→下降三段匀速)。""" + u = np.array([self.start[0], self.start[1], 0.0]) + e = np.array([self.end[0], self.end[1], 0.0]) + climb = self.alt / self.climb_speed + if t <= 0.0: + return u + if t < climb: + return u + np.array([0.0, 0.0, self.alt * t / climb]) + if t < self.cruise_t1: + s = self.cruise_progress(t) + L = max(self._cruise_len, 1e-9) + return np.array([self.start[0] + (self.end[0] - self.start[0]) * s / L, + self.start[1] + (self.end[1] - self.start[1]) * s / L, + self.alt]) + if t < self._t_end: + return e + np.array([0.0, 0.0, self.alt * (self._t_end - t) / climb]) + return e + + def velocity_at(self, t: float) -> np.ndarray: + climb = self.alt / self.climb_speed + if t < climb: + return np.array([0.0, 0.0, self.climb_speed]) + if t < self.cruise_t1: + h = self.cruise_heading + return np.array([self.speed * math.cos(h), self.speed * math.sin(h), + 0.0]) + return np.array([0.0, 0.0, -self.climb_speed]) + + def cruise_unit(self) -> np.ndarray: + h = self.cruise_heading + return np.array([math.cos(h), math.sin(h)]) + + def cruise_left_normal(self) -> np.ndarray: + h = self.cruise_heading + return np.array([-math.sin(h), math.cos(h)]) + + def cross_track(self, xy: np.ndarray) -> float: + """点到巡航航线的有符号横向偏移(沿航向左侧为正)。""" + p0 = self.cruise_start[:2] + n = self.cruise_left_normal() + return float(np.dot(np.asarray(xy[:2], dtype=float) - p0, n)) + + def along_track(self, xy: np.ndarray) -> float: + u = self.cruise_unit() + p0 = self.cruise_start[:2] + return float(np.dot(np.asarray(xy[:2], dtype=float) - p0, u)) + + def summary(self) -> dict: + return dict(uid=self.uid, label=self.label, + start=[round(self.start[0], 1), round(self.start[1], 1)], + end=[round(self.end[0], 1), round(self.end[1], 1)], + alt=round(self.alt, 1), speed=round(self.speed, 1), + priority=self.priority, + cruise_t0=round(self.cruise_t0, 1), + cruise_t1=round(self.cruise_t1, 1)) + + +# --------------------------------------------------------------------------- # +# 三个场景 +# --------------------------------------------------------------------------- # +def headon(alt: float = 15.0) -> "Scenario": + """两条平行走廊上各一对同高度对头机(同优先级 → 规则③各自右转)。""" + plans = [ + FlightPlan("UAV-1", (-180.0, 0.0), (180.0, 0.0), alt=alt, speed=8.0, + label="东向干线"), + FlightPlan("UAV-2", (180.0, 0.0), (-180.0, 0.0), alt=alt, speed=8.0, + label="西向干线"), + FlightPlan("UAV-3", (-180.0, 60.0), (180.0, 60.0), alt=alt, speed=8.0, + label="东向支线"), + FlightPlan("UAV-4", (180.0, 60.0), (-180.0, 60.0), alt=alt, speed=8.0, + label="西向支线"), + ] + return Scenario( + "headon", "对头相遇", + "两条平行走廊各有一对同高度对头机,中线相遇;" + "同优先级按规则③各自右转 30° 互相从左側通过。", + plans) + + +def cross(alt: float = 15.0) -> "Scenario": + """四机同时穿过同一空域中心(同优先级 → 规则②左让右,旋转通过)。""" + plans = [ + FlightPlan("UAV-1", (-180.0, 0.0), (180.0, 0.0), alt=alt, speed=8.0, + label="东行"), + FlightPlan("UAV-2", (0.0, -180.0), (0.0, 180.0), alt=alt, speed=8.0, + label="北行"), + FlightPlan("UAV-3", (180.0, 0.0), (-180.0, 0.0), alt=alt, speed=8.0, + label="西行"), + FlightPlan("UAV-4", (0.0, 180.0), (0.0, -180.0), alt=alt, speed=8.0, + label="南行"), + ] + return Scenario( + "cross", "十字交叉", + "四架无人机从四个方向同时抵达空域中心,同优先级按规则②左让右," + "各自右转形成旋转通过。", + plans) + + +def overtake(alt: float = 15.0) -> "Scenario": + """两条平行走廊上各一对追越(规则④:追越机从左侧通过,前机保速保向)。""" + plans = [ + FlightPlan("UAV-1", (-180.0, 0.0), (180.0, 0.0), alt=alt, speed=5.0, + label="慢速巡检(前机)"), + FlightPlan("UAV-2", (-220.0, 0.0), (220.0, 0.0), alt=alt, speed=9.0, + label="快速配送(追越机)"), + FlightPlan("UAV-3", (-180.0, 80.0), (180.0, 80.0), alt=alt, speed=5.0, + label="慢速巡检 B(前机)"), + FlightPlan("UAV-4", (-220.0, 80.0), (220.0, 80.0), alt=alt, speed=9.0, + label="快速配送 B(追越机)"), + ] + return Scenario( + "overtake", "同高度追越", + "快机在正后方追上慢机(同航路同高度),规则④要求追越机从左侧" + "偏置通过、前机保速保向;通过后按余弦剖面回到原航路。", + plans) + + +@dataclass +class Scenario: + name: str + title: str + description: str + plans: List[FlightPlan] + + +SCENARIOS: Dict[str, callable] = { + "headon": headon, + "cross": cross, + "overtake": overtake, +} + + +def get_scenario(name: str, **kwargs) -> Scenario: + if name not in SCENARIOS: + raise KeyError("未知场景 %r,可选:%s" % (name, ", ".join(SCENARIOS))) + return SCENARIOS[name](**kwargs) diff --git a/src/air/uav_right_of_way/uav_right_of_way/core/sim.py b/src/air/uav_right_of_way/uav_right_of_way/core/sim.py new file mode 100644 index 0000000000..f3012dd758 --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/core/sim.py @@ -0,0 +1,435 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""闭环仿真:基线(无规则直飞)与规则启用两两对比,MuJoCo 真飞。 + +每架无人机用课程底座 ``uav_mujoco_sim``(同一份 ``quadrotor.xml`` 刚体模型 ++ 几何控制器)单独实例化。规则启用时,每架机用**刚体积分出来的真实状态** +做侵入判定(``detect``),命中规则(``rules``)后按动作调整参考速度: + +* 右转 30° → 参考航向叠加 −30° 偏置; +* 降高 10 m → 参考高度下移 10 m; +* 减速 30% → 参考速度乘 (1−0.30); +* 左侧追越 → 对参考点施加目标横向偏置(沿航向左侧 overtake_lateral_m)。 + +威胁消失并保持 ``clear_hold_s`` 后进入恢复(``recover.RecoveryProfile``, +余弦剖面回到计划航路),恢复期间每个决策周期重新做威胁检测,出现二次 +冲突立即中止恢复、重新决策。 + +参考速度前馈经一阶低通(τ=0.5 s)、姿态阻尼 kw≈−0.18——与参考实现 +uav_deconflict ``core/sim.py`` 相同的写法,防止航向阶跃激励姿态振荡。 +``--no-sim``(或 MuJoCo 不可用)时退化为纯运动学参考点积分,算法层 +(判定/规则/恢复)走完全相同的代码路径。 +""" +from __future__ import annotations + +import math +import os +import sys +import time +from dataclasses import dataclass, field +from pathlib import Path +from typing import Dict, List, Optional, Sequence, Tuple + +import numpy as np + +from .detect import AircraftState, DetectParams, Threat, predict_threat +from .recover import RecoveryProfile, should_abort +from .rules import (ACTION_DESCEND, ACTION_MAINTAIN, ACTION_OVERTAKE_LEFT, + ACTION_SLOW, ACTION_TURN_RIGHT, Decision, RuleParams, + decide) +from .scenarios import FlightPlan + +#: 参考速度前馈的一阶低通时间常数(s) +REF_TAU = 0.5 +#: 姿态角速度阻尼增益(同参考实现 sim.py,抑制机动激励的振荡) +ATT_DAMPING = (-0.18, -0.18, -0.126) + +#: 默认决策周期(s,仿真时间) +DECIDE_DT = 0.2 +#: 默认威胁消失后保持观察时长(s) +CLEAR_HOLD_S = 3.0 +#: 默认恢复最短时长(s) +RECOVERY_MIN_S = 8.0 +#: 横向偏置跟踪增益与限幅 +LAT_GAIN = 0.8 +LAT_VMAX = 4.0 +#: 高度环增益与限幅 +ALT_GAIN = 0.8 +ALT_VMAX = 3.0 + + +def _load_backend(): + """导入课程仿真底座;源码目录直接运行时自动挂同级包路径。""" + try: + from uav_mujoco_sim.core.quadrotor import ( # type: ignore + GeometricController, Quadrotor) + from uav_mujoco_sim.core.paths import models_dir # type: ignore + return Quadrotor, GeometricController, models_dir() + except Exception: + here = Path(__file__).resolve() + # src/air/uav_right_of_way/uav_right_of_way/core/sim.py → src/air + sibling = here.parents[3] / "uav_mujoco_sim" + if sibling.is_dir() and str(sibling) not in sys.path: + sys.path.insert(0, str(sibling)) + from uav_mujoco_sim.core.quadrotor import ( # type: ignore + GeometricController, Quadrotor) + from uav_mujoco_sim.core.paths import models_dir # type: ignore + return Quadrotor, GeometricController, models_dir() + + +# --------------------------------------------------------------------------- # +# 运行结果与指标 +# --------------------------------------------------------------------------- # +@dataclass +class RunResult: + """一次闭环运行的记录与指标。""" + + mode: str # "baseline" | "rules" + times: np.ndarray # (K,) 记录时刻 + pos: Dict[str, np.ndarray] # uid -> (K,3) 真实位置 + plans: Dict[str, FlightPlan] + decisions: List[Decision] = field(default_factory=list) + wall_time: float = 0.0 + steps: int = 0 + + def min_separation(self) -> Tuple[float, Tuple[str, str], float]: + """真飞全程机对间最小三维距离及机对、时刻。""" + best = math.inf + pair: Tuple[str, str] = ("", "") + t_best = 0.0 + names = sorted(self.pos) + for i, a in enumerate(names): + for b in names[i + 1:]: + d = np.linalg.norm(self.pos[a] - self.pos[b], axis=1) + k = int(np.argmin(d)) + if d[k] < best: + best = float(d[k]) + pair = (a, b) + t_best = float(self.times[k]) + return best, pair, t_best + + def deviation_stats(self) -> Dict[str, float]: + """逐机平均航迹偏离:真实位置到同时刻计划位置的平均距离(m)。""" + out: Dict[str, float] = {} + for uid, plan in self.plans.items(): + ref = np.array([plan.position_at(t) for t in self.times]) + err = np.linalg.norm(self.pos[uid] - ref, axis=1) + out[uid] = float(err.mean()) + return out + + def mean_deviation(self) -> float: + stats = self.deviation_stats() + return float(np.mean(list(stats.values()))) if stats else 0.0 + + def summary(self, params: DetectParams) -> dict: + best, pair, t_best = self.min_separation() + names = sorted(self.pos) + close_samples = 0 + for i, a in enumerate(names): + for b in names[i + 1:]: + d = np.linalg.norm(self.pos[a] - self.pos[b], axis=1) + close_samples += int(np.sum(d < params.cpa_threshold_m)) + return dict( + mode=self.mode, + min_separation_m=round(best, 2), + min_pair=list(pair), + min_sep_time_s=round(t_best, 1), + decisions=len(self.decisions), + mean_deviation_m=round(self.mean_deviation(), 2), + per_agent_deviation_m={k: round(v, 2) + for k, v in self.deviation_stats().items()}, + below_threshold_samples=close_samples, + ) + + +# --------------------------------------------------------------------------- # +# 机体包装:纯运动学(--no-sim 退化模式) +# --------------------------------------------------------------------------- # +class _KinematicBody: + """直接跟随参考点,接口与 ``Quadrotor`` 对齐(qpos / state / step)。""" + + def __init__(self, p0: Sequence[float]): + self._pos = np.asarray(p0, dtype=float).copy() + self.vel = np.zeros(3) + + @property + def qpos(self) -> np.ndarray: + return self._pos + + def state(self): + """与 ``QuadrotorState`` 最小兼容:只用到 .vel。""" + return self + + def set_state(self, pos: np.ndarray, vel: np.ndarray) -> None: + self._pos = np.asarray(pos, dtype=float).copy() + self.vel = np.asarray(vel, dtype=float).copy() + + +class _Agent: + """一架无人机的闭环状态机:计划跟随 / 让路机动 / 余弦恢复。""" + + def __init__(self, plan: FlightPlan, body, ctrl, enable_rules: bool, + detect_params: DetectParams, rule_params: RuleParams): + self.plan = plan + self.body = body + self.ctrl = ctrl + self.enable_rules = enable_rules + self.dp = detect_params + self.rp = rule_params + + self.v_filt = np.zeros(3) + self.decisions: List[Decision] = [] + + # 巡航参考运动学 + self.ref_xy = plan.cruise_start[:2].copy() + self.ref_z = plan.alt + self._cruise_init = False + # 机动状态 + self.yaw_off = 0.0 # rad,右转为负 + self.alt_off = 0.0 # m + self.speed_factor = 1.0 + self.lat_target: Optional[float] = None # m,沿航向左为正 + # 恢复 + self.rec: Optional[RecoveryProfile] = None + self.rec_t0 = 0.0 + # 决策 episode:other -> 最近一次确认威胁的时刻 + self.episodes: Dict[str, float] = {} + + # ------------------------------------------------------------------ 状态 + def true_pos(self) -> np.ndarray: + return np.asarray(self.body.qpos[:3], dtype=float) + + def true_vel(self) -> np.ndarray: + if isinstance(self.body, _KinematicBody): + return np.asarray(self.body.vel, dtype=float) + return np.asarray(self.body.state().vel, dtype=float) + + def own_state(self) -> AircraftState: + return AircraftState(self.plan.uid, self.true_pos(), self.true_vel(), + self.plan.priority, self.plan.label) + + # ------------------------------------------------------------------ 动作 + def apply_action(self, dec: Decision) -> None: + kind = dec.action.kind + if kind == ACTION_TURN_RIGHT: + self.yaw_off = -math.radians(dec.action.value) + self.lat_target = None + elif kind == ACTION_DESCEND: + self.alt_off = -dec.action.value + self.lat_target = None + elif kind == ACTION_SLOW: + self.speed_factor = 1.0 - dec.action.value + self.lat_target = None + elif kind == ACTION_OVERTAKE_LEFT: + self.lat_target = dec.action.value + elif kind == ACTION_MAINTAIN: + pass + + def begin_recovery(self, t: float) -> None: + x_off = self.plan.cross_track(self.ref_xy) + remain = self.plan.cruise_t1 - t + dur = max(RECOVERY_MIN_S, 2.5 * abs(x_off) + 4.0) + dur = min(dur, max(4.0, 0.8 * remain)) + self.rec = RecoveryProfile(lateral0=x_off, alt0=self.alt_off, + speed_factor0=self.speed_factor, + duration=dur) + self.rec_t0 = t + self.yaw_off = 0.0 + self.lat_target = None + self.episodes.clear() + + # ------------------------------------------------------------------ 决策 + def tick_decisions(self, t: float, others: Sequence["_Agent"], + clear_hold_s: float) -> None: + """一个决策周期:威胁检测 → 规则 → 恢复触发(含恢复期监视)。""" + if not self.plan.in_cruise(t): + return + others_active = [a for a in others + if a is not self and a.plan.in_cruise(t)] + own = self.own_state() + other_states = [a.own_state() for a in others_active] + by_uid = {s.uid: s for s in other_states} + + threats: Dict[str, Threat] = {} + for st in other_states: + th = predict_threat(own, st, self.dp) + if th is not None: + threats[st.uid] = th + + # ---- 恢复中:持续监视,出现二次冲突立即中止并重新决策 ------------ + if self.rec is not None: + th = should_abort(own, other_states, self.dp) + if th is not None: + self.rec = None + self.episodes.clear() + self._new_decision(t, own, by_uid.get(th.other), th) + elif t - self.rec_t0 >= self.rec.duration: + self.rec = None + self.alt_off = 0.0 + self.speed_factor = 1.0 + return + + # ---- 无威胁:episode 全部超过观察期 → 进入余弦恢复 ---------------- + if not threats: + if self.episodes and \ + all(t - t0 >= clear_hold_s for t0 in self.episodes.values()): + self.begin_recovery(t) + return + + # ---- 有威胁:为尚无 episode 的机对产出决策 ------------------------ + for other_uid in sorted(threats, key=lambda u: threats[u].cpa_t): + if other_uid in self.episodes: + self.episodes[other_uid] = t # 刷新"仍有威胁"时刻 + continue + self._new_decision(t, own, by_uid.get(other_uid), threats[other_uid]) + + def _new_decision(self, t: float, own: AircraftState, other_state, + threat: Threat) -> None: + if other_state is None: + return + dec = decide(own, other_state, threat, t, self.rp) + self.decisions.append(dec) + self.episodes[other_state.uid] = t + self.apply_action(dec) + + # ------------------------------------------------------------------ 参考 + def reference(self, t: float, dt: float) -> Tuple[np.ndarray, np.ndarray, + float]: + """当前控制周期的 (参考位置, 参考速度, 参考偏航)。""" + plan = self.plan + if t < plan.cruise_t0 or t > plan.cruise_t1: + # 爬升 / 下降段:原样跟随计划 + self._cruise_init = False + if t <= plan.cruise_t0: + self.ref_xy = plan.cruise_start[:2].copy() + self.ref_z = plan.cruise_start[2] + return plan.position_at(t), plan.velocity_at(t), 0.0 + + if not self._cruise_init: + self._cruise_init = True + self.ref_xy = plan.cruise_start[:2].copy() + self.ref_z = plan.alt + + h = plan.cruise_heading + h_eff = h + self.yaw_off + u = np.array([math.cos(h_eff), math.sin(h_eff)]) + n = np.array([-math.sin(h), math.cos(h)]) # 计划航向的左法向 + + speed_f = self.speed_factor + lat_cmd = 0.0 + z_off = self.alt_off + if self.rec is not None: + s = self.rec.s(t - self.rec_t0) + speed_f = self.rec.speed_factor(s) + x_off = plan.cross_track(self.ref_xy) + # 余弦剖面:剖面导数前馈 + 对参考点的小反馈 + lat_cmd = self.rec.lateral_rate(s) \ + + 0.5 * (self.rec.lateral(s) - x_off) + z_off = self.rec.alt(s) + elif self.lat_target is not None: + x_off = plan.cross_track(self.ref_xy) + lat_cmd = (self.lat_target - x_off) * LAT_GAIN + + lat_cmd = float(np.clip(lat_cmd, -LAT_VMAX, LAT_VMAX)) + v_h = plan.speed * speed_f * u + lat_cmd * n + vz = float(np.clip((plan.alt + z_off - self.ref_z) * ALT_GAIN, + -ALT_VMAX, ALT_VMAX)) + + self.ref_xy = self.ref_xy + v_h * dt + self.ref_z = self.ref_z + vz * dt + v_ref = np.array([v_h[0], v_h[1], vz]) + p_ref = np.array([self.ref_xy[0], self.ref_xy[1], + max(self.ref_z, 0.05)]) + return p_ref, v_ref, h + + +# --------------------------------------------------------------------------- # +# 闭环主循环 +# --------------------------------------------------------------------------- # +def run_fleet(plans: Sequence[FlightPlan], enable_rules: bool = True, + dt: float = 0.01, substeps: int = 4, record_every: int = 50, + tail: float = 2.0, max_steps: int = 200000, progress: bool = False, + use_mujoco: bool = True, ref_tau: float = REF_TAU, + detect_params: Optional[DetectParams] = None, + rule_params: Optional[RuleParams] = None, + decide_dt: float = DECIDE_DT, + clear_hold_s: float = CLEAR_HOLD_S, + recovery_min_s: float = RECOVERY_MIN_S) -> RunResult: + """让机队按计划真飞:``enable_rules=False`` 为基线(无规则直飞)。""" + dp = detect_params or DetectParams() + rp = rule_params or RuleParams() + use_mj = use_mujoco + backend = None + if use_mj: + try: + backend = _load_backend() + except Exception: + use_mj = False + + plans = list(plans) + agents: List[_Agent] = [] + if use_mj and backend is not None: + Quadrotor, GeometricController, model_dir = backend + model_path = os.path.join(str(model_dir), "quadrotor.xml") + for plan in plans: + quad = Quadrotor(model_path) + quad.reset((plan.start[0], plan.start[1], 0.05)) + ctrl = GeometricController(quad.params, kw=ATT_DAMPING) + agents.append(_Agent(plan, quad, ctrl, enable_rules, dp, rp)) + else: + for plan in plans: + agents.append(_Agent(plan, _KinematicBody( + (plan.start[0], plan.start[1], 0.05)), None, enable_rules, + dp, rp)) + + t_end = max(p.end_time for p in plans) + tail + n_steps = min(max_steps, int(round(t_end / dt))) + times: List[float] = [] + pos: Dict[str, List[np.ndarray]] = {p.uid: [] for p in plans} + last_decide: Dict[str, float] = {p.uid: -math.inf for p in plans} + alpha = min(1.0, dt / ref_tau) if ref_tau and ref_tau > 0 else 1.0 + + t0_wall = time.time() + for step in range(n_steps): + t = step * dt + for ag in agents: + # 决策周期(规则启用时) + if enable_rules and t - last_decide[ag.plan.uid] >= decide_dt - 1e-9: + ag.tick_decisions(t, agents, clear_hold_s) + last_decide[ag.plan.uid] = t + + p_ref, v_ref, hdg = ag.reference(t, dt) + if alpha < 1.0: + ag.v_filt += (v_ref - ag.v_filt) * alpha + v_use = ag.v_filt + else: + v_use = v_ref + + if use_mj and backend is not None: + thrusts = ag.ctrl.act(ag.body, p_ref, v_use, np.zeros(3), hdg) + ag.body.step_with_control(thrusts, substeps) + else: + # 运动学退化:真实状态 = 参考点 + ag.body.set_state(p_ref, v_use) + + if step % record_every == 0: + times.append(t) + for ag in agents: + pos[ag.plan.uid].append(ag.true_pos().copy()) + + if progress and step % 5000 == 0: + print(" sim t=%.1f / %.1f s" % (t, t_end)) + + times.append(n_steps * dt) + for ag in agents: + pos[ag.plan.uid].append(ag.true_pos().copy()) + + arr = {uid: np.asarray(v, dtype=float) for uid, v in pos.items()} + mode = "rules" if enable_rules else "baseline" + decisions: List[Decision] = [] + for ag in agents: + decisions.extend(ag.decisions) + decisions.sort(key=lambda d: (d.t, d.uid)) + return RunResult(mode=mode, times=np.asarray(times, dtype=float), pos=arr, + plans={p.uid: p for p in plans}, decisions=decisions, + wall_time=time.time() - t0_wall, steps=n_steps) diff --git a/src/air/uav_right_of_way/uav_right_of_way/ros/__init__.py b/src/air/uav_right_of_way/uav_right_of_way/ros/__init__.py new file mode 100644 index 0000000000..8ce1015ce2 --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/ros/__init__.py @@ -0,0 +1,2 @@ +# -*- coding: utf-8 -*- +"""ros 薄壳层:判定逻辑全部在 core,节点只做订阅/发布。""" diff --git a/src/air/uav_right_of_way/uav_right_of_way/ros/row_node.py b/src/air/uav_right_of_way/uav_right_of_way/ros/row_node.py new file mode 100644 index 0000000000..954c3a878b --- /dev/null +++ b/src/air/uav_right_of_way/uav_right_of_way/ros/row_node.py @@ -0,0 +1,166 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +"""在线通行权决策节点:订阅多机里程计,触发时发布让路指令与决策理由。 + +参数(``config/uav_right_of_way.yaml``):: + + uav_ids: ["UAV-1", ...] + odom_prefix: "/uav" # 话题名为 /odom + horizon_s: 8.0 + cpa_threshold_m: 40.0 + decision_interval_s: 0.2 + clear_hold_s: 3.0 + +发布:: + + /uav_right_of_way/commands std_msgs/String + JSON:{"stamp", "decisions": [{uid, other, rule, action, reason, ...}]} + +判定逻辑(威胁检测、规则引擎、理由字符串)全部在 ``core`` 里并有单元 +测试覆盖;本节点只是薄壳:订阅 odom → 组装 ``AircraftState`` → 每个决策 +周期调一次 ``detect`` + ``rules`` → 发布 JSON 并写日志。让路指令的执行 +(改参考航向/高度/速度)由各机的飞行控制侧完成,本节点不动航迹。 +""" +from __future__ import annotations + +import json +import math +from typing import Dict, List + +import rclpy +from nav_msgs.msg import Odometry +from rclpy.node import Node +from std_msgs.msg import String + +from uav_right_of_way.core.detect import (AircraftState, DetectParams, Threat, + predict_threat) +from uav_right_of_way.core.rules import RuleParams, decide + + +class RightOfWayNode(Node): + def __init__(self) -> None: + super().__init__("uav_right_of_way_node") + self.declare_parameter("uav_ids", ["UAV-1", "UAV-2"]) + self.declare_parameter("odom_prefix", "/uav") + self.declare_parameter("horizon_s", 8.0) + self.declare_parameter("cpa_threshold_m", 40.0) + self.declare_parameter("decision_interval_s", 0.2) + self.declare_parameter("clear_hold_s", 3.0) + self.declare_parameter("stale_timeout", 1.0) + self.declare_parameter("turn_right_deg", 30.0) + self.declare_parameter("descend_m", 10.0) + self.declare_parameter("slow_fraction", 0.30) + self.declare_parameter("overtake_lateral_m", 20.0) + + ids = [str(v) for v in self.get_parameter("uav_ids").value] + prefix = str(self.get_parameter("odom_prefix").value).rstrip("/") + self.dp = DetectParams( + horizon_s=float(self.get_parameter("horizon_s").value), + cpa_threshold_m=float(self.get_parameter("cpa_threshold_m").value)) + self.rp = RuleParams( + turn_right_deg=float(self.get_parameter("turn_right_deg").value), + descend_m=float(self.get_parameter("descend_m").value), + slow_fraction=float(self.get_parameter("slow_fraction").value), + overtake_lateral_m=float( + self.get_parameter("overtake_lateral_m").value)) + self.decide_dt = float(self.get_parameter("decision_interval_s").value) + self.clear_hold = float(self.get_parameter("clear_hold_s").value) + self.stale = float(self.get_parameter("stale_timeout").value) + + self.states: Dict[str, AircraftState] = {} + self.stamp: Dict[str, float] = {} + self.last_decide = -math.inf + # 机对 episode:{frozenset({a,b}): 最近确认威胁时刻} + self.episodes: Dict[frozenset, float] = {} + + if len(ids) == 1: + topics = [("%s/odom" % prefix, ids[0])] + else: + topics = [("%s%d/odom" % (prefix, i + 1), uid) + for i, uid in enumerate(ids)] + for topic, uid in topics: + self.create_subscription( + Odometry, topic, + lambda msg, uid=uid: self._on_odom(uid, msg), 10) + + self.pub_cmd = self.create_publisher( + String, "/uav_right_of_way/commands", 10) + self.create_timer(max(self.decide_dt, 0.05), self._tick) + self.get_logger().info( + "通行权决策已启动,机队 %s,预测 %.0f s 内 CPA < %.0f m 触发" + % (",".join(ids), self.dp.horizon_s, self.dp.cpa_threshold_m)) + + # ------------------------------------------------------------------ 回调 + def _on_odom(self, uid: str, msg: Odometry) -> None: + p = msg.pose.pose.position + v = msg.twist.twist.linear + prio = 0 + try: + prio = int(msg.child_frame_id.split("_")[-1]) \ + if msg.child_frame_id.startswith("prio_") else 0 + except Exception: + prio = 0 + self.states[uid] = AircraftState( + uid, [p.x, p.y, p.z], [v.x, v.y, v.z], prio) + self.stamp[uid] = self.get_clock().now().nanoseconds * 1e-9 + + # ------------------------------------------------------------------ 周期 + def _tick(self) -> None: + now = self.get_clock().now().nanoseconds * 1e-9 + fresh = {uid: st for uid, st in self.states.items() + if now - self.stamp.get(uid, -math.inf) <= self.stale} + if len(fresh) < 2: + return + if now - self.last_decide < self.decide_dt - 1e-9: + return + self.last_decide = now + + uids = sorted(fresh) + decisions: List[dict] = [] + for i, a in enumerate(uids): + for b in uids[i + 1:]: + key = frozenset((a, b)) + own, other = fresh[a], fresh[b] + th = predict_threat(own, other, self.dp) + if th is None: + # 威胁消失且超过观察期 → 结束该机对 episode + if key in self.episodes and \ + now - self.episodes[key] >= self.clear_hold: + del self.episodes[key] + continue + # CPA 对双方对称;每个机对 episode 只产出一次决策 + is_new = key not in self.episodes + self.episodes[key] = now + if is_new: + decisions.append( + decide(own, other, th, now, self.rp).as_dict()) + decisions.append( + decide(other, own, + Threat(own.uid, th.cpa_t, th.cpa_dist), + now, self.rp).as_dict()) + + if decisions: + payload = dict(stamp=now, decisions=decisions) + self.pub_cmd.publish(String(data=json.dumps( + payload, ensure_ascii=False))) + for d in decisions: + self.get_logger().info( + "让路指令 %s ← %s [%s] %s:%s" + % (d["uid"], d["other"], d["rule"], + d["action"]["kind"], d["reason"])) + + +def main(args=None) -> None: + rclpy.init(args=args) + node = RightOfWayNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main()