Skip to content
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
107 changes: 107 additions & 0 deletions docs/air/uav_right_of_way.md
Original file line number Diff line number Diff line change
@@ -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%)是固定参数,没有按相遇几何自适应。
Binary file added docs/img/air/uav_right_of_way/cross.gif
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added docs/img/air/uav_right_of_way/headon.gif
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added docs/img/air/uav_right_of_way/overtake.gif
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Binary file added docs/img/air/uav_right_of_way/trajectories.png
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
1 change: 1 addition & 0 deletions docs/index.md
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,7 @@ ROS 2 话题发布订阅基础实验(talker/listener):[chap2/ros2_pubsub_p

## 5. 空域载具 <span id='air_vehicle'></span>
* [无人机着陆阶段识别与接地载荷预测](./air/landing_assessment.md)
* [低空多机路权判定与让路恢复](./air/uav_right_of_way.md) —— 两机近距离相遇时的在线让路:优先级/左让右/对头右转/左侧追越,MuJoCo 真飞对比

* [无人机载荷质量估计](./air/payload_mass_estimation.md)

Expand Down
1 change: 1 addition & 0 deletions mkdocs.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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'
Expand Down
115 changes: 115 additions & 0 deletions src/air/uav_right_of_way/README.md
Original file line number Diff line number Diff line change
@@ -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 # 在线决策节点
```

节点订阅 `<prefix><n>/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%)写死在参数里,没有按相遇几何自适应调整。
19 changes: 19 additions & 0 deletions src/air/uav_right_of_way/config/uav_right_of_way.yaml
Original file line number Diff line number Diff line change
@@ -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)
11 changes: 11 additions & 0 deletions src/air/uav_right_of_way/conftest.py
Original file line number Diff line number Diff line change
@@ -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)
22 changes: 22 additions & 0 deletions src/air/uav_right_of_way/launch/main.launch.py
Original file line number Diff line number Diff line change
@@ -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],
),
])
29 changes: 29 additions & 0 deletions src/air/uav_right_of_way/main.py
Original file line number Diff line number Diff line change
@@ -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())
33 changes: 33 additions & 0 deletions src/air/uav_right_of_way/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>uav_right_of_way</name>
<version>0.1.0</version>
<description>
低空多机优先级通行权在线决策。两机近距离相遇时按优先级 / 左让右 /
对头各右转 / 追越从左侧四条规则即时产出让路动作(右转 30°、降高 10 m、
减速 30%、保向等)并给出可审计的决策理由;预测 8 s 内 CPA &lt; 40 m
即触发;脱离威胁后按余弦剖面平滑回到计划航路,恢复期间持续监视防止
二次冲突。用课程既有的 uav_mujoco_sim(MuJoCo 刚体模型 + 几何控制器)
做基线(无规则直飞)与规则启用的闭环对比。
</description>
<maintainer email="326516287+usernamekkyy@users.noreply.github.com">黄家欣</maintainer>
<license>MIT</license>

<buildtool_depend>ament_python</buildtool_depend>

<exec_depend>rclpy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>nav_msgs</exec_depend>
<exec_depend>geometry_msgs</exec_depend>
<exec_depend>launch</exec_depend>
<exec_depend>launch_ros</exec_depend>
<exec_depend>python3-numpy</exec_depend>
<exec_depend>uav_mujoco_sim</exec_depend>

<test_depend>ament_pytest</test_depend>

<export>
<build_type>ament_python</build_type>
</export>
</package>
Empty file.
Loading