UAV 経路計画の競合解決アルゴリズムの概要
無人航空機 (UAV) が単一機の運用からクラスター連携に進化するにつれて、複数の UAV が同じ空域でタスクを実行する場合、経路の競合は避けられない中心的な問題となっています。 紛争解決 とは、各ドローンの軌道や意思決定を調整して紛争状態を解消し、飛行の安全を確保しながらミッションを継続的に完了することを指します。この記事では、幾何学的な手法から深層強化学習に至るまで、競合の特定と解決のための主流のアルゴリズム フレームワークを体系的に整理し、各テクノロジーの核となる考え方、利点、欠点、進化を探ります。
1. 競合の定義と分類
1.1 パスの競合とは何ですか?
マルチ UAV システムでは、競合 は、2 つ以上の UAV が時空次元で同じ空間位置 (または安全な隔離距離未満) を同時に占有している状態を指します。正式には:
このうち、
1.2 紛争の分類
| タイプ | 説明 | 典型的なシナリオ |
|---|---|---|
| 宇宙紛争 | 軌道は空間で交差します | ルートの交差、反対便 |
| 時間と空間の対立 | 軌跡は時間次元で重なり合う | 次々と同じ空域に進入 |
| 速度の競合 | 相対速度が安全しきい値を超えています | 追い上げシナリオ |
| 激しい紛争 | 縦方向の対立 | 交差点を持ち上げる |
| 動的競合 | 移動障害物による衝突 (他の航空機) | 空中遭遇 |
1.3 競合の指標
- 競合までの時間: 競合が発生するまでの残り時間を予測します。
- 紛争確率: 不確実性を考慮した紛争リスク評価
- 最小分離距離: 軌道間の最も近い距離
- 解決時間: 解決アクションが有効になるまでに必要な時間
2. 競合識別アルゴリズム競合の特定は競合を解決するための予備ステップであり、その中心となるのは競合予測であり、実際に競合が発生する前に競合が発生するかどうかを判断します。
2.1 幾何学的予測手法
最も直感的な方法は、幾何学的計算に基づく空間検出です。
import numpy as np
def detect_conflict_2D(traj_i, traj_j, safe_radius=5.0):
"""
检测两条轨迹是否发生空间冲突
traj_i, traj_j: shape (N, 3) 的轨迹数组，每行为 (x, y, z)
safe_radius: 安全隔离距离 (m)
返回: (是否冲突, 最小间隔距离, 冲突时间点索引)
"""
min_dist = float('inf')
conflict_time = -1
for t in range(len(traj_i)):
dist = np.linalg.norm(traj_i[t] - traj_j[t])
if dist < min_dist:
min_dist = dist
if dist < safe_radius:
conflict_time = t
is_conflict = min_dist < safe_radius
return is_conflict, min_dist, conflict_time2.2 速度障害
速度障害物 (VO) は、ロボット工学分野で最も古典的な衝突検出および予測方法です。これは、Fioretti & Fraichard (1999) によって UAV 分野に導入されました。
核となるアイデア: スピード空間に「禁止エリア」を構築する。ドローンの現在の速度ベクトルがこの領域内にある場合、間違いなく衝突が発生します。
ここで、
def velocity_obstacle(p_i, v_i, p_j, v_j, r_safe=5.0):
"""
计算第 i 架 UAV 的速度障碍区域
p_i, p_j: 位置向量
v_i, v_j: 速度向量
r_safe: 安全半径
"""
rel_pos = p_j - p_i
rel_vel = v_i - v_j
dist = np.linalg.norm(rel_pos)
if dist == 0:
return None
# 相对位置的夹角（安全圆柱的视角）
theta = np.arcsin(r_safe / dist)
# 障碍扇区的两条边向量
dir_pos = rel_pos / dist
perp_dir = np.array([-dir_pos[1], dir_pos[0]])
# 两条边界速度向量
v_left = v_j + np.linalg.norm(v_j) * (np.cos(theta) * dir_pos + np.sin(theta) * perp_dir)
v_right = v_j + np.linalg.norm(v_j) * (np.cos(theta) * dir_pos - np.sin(theta) * perp_dir)
return v_left, v_right # VO 的两条边界
def is_in_vo(v_i, v_left, v_right, v_j):
"""判断速度 v_i 是否落在 VO 区域内"""
# 转换到相对坐标系
rel_v = v_i - v_j
rel_left = v_left - v_j
rel_right = v_right - v_j
# 检查 rel_v 是否在 rel_left 和 rel_right 之间
cross_left = np.cross(rel_left, rel_v)
cross_right = np.cross(rel_right, rel_v)
return np.sign(cross_left) == np.sign(cross_right)2.3 不確実性を認識した競合検出
実際のシステムでは、位置情報には GPS 誤差やセンサーノイズなどの不確実性が含まれることがよくあります。 確率的競合検出 競合判断に確率分布を導入します。
ここで、
- モンテカルロ サンプリング: 多数の確率分布をサンプリングした後の競合率の統計
- 線形検証ツール (LVT): ガウス分布の仮定に基づく確率矛盾の分析的近似
- 確率的到達可能セット: 確率的制御理論に基づいたセット表現
3. 競合解決アルゴリズム
3.1 幾何学的手法
3.1.1 Rate Obstacle 法 (Rate Obstacle / VO 補正)
VO ベースの排除戦略: VO エリアを回避できる目標速度
def vo_resolution(p_i, v_i, p_j, v_j, v_max=20.0, r_safe=5.0):
"""
基于 Velocity Obstacle 的冲突消解
返回满足速度约束且避开 VO 的新速度
"""
vo = velocity_obstacle(p_i, v_i, p_j, v_j, r_safe)
if vo is None:
return v_i
v_left, v_right = vo
# 所有候选速度（速度空间中均匀采样）
best_v = v_i
min_dist_to_vo = float('inf')
for speed in np.linspace(0, v_max, 20):
for angle in np.linspace(0, 2*np.pi, 36):
v_candidate = speed * np.array([np.cos(angle), np.sin(angle)])
# 跳过落在 VO 内的速度
if is_in_vo(v_candidate, v_left, v_right, v_j):
continue
# 选择最接近原始速度方向且最"远离"VO 的速度
dist_to_original = np.linalg.norm(v_candidate - v_i)
# 到 VO 边界的距离
dist_to_vo = min(
np.linalg.norm(v_candidate - v_left),
np.linalg.norm(v_candidate - v_right)
)
# 优化目标：尽量接近原速度，同时远离 VO
score = dist_to_vo - 0.5 * dist_to_original
if dist_to_vo > min_dist_to_vo and dist_to_vo > 1.0:
min_dist_to_vo = dist_to_vo
best_v = v_candidate
return best_v3.1.2 人工ポテンシャル場法（人工ポテンシャル場）
ドローンを「ポテンシャル場」内を移動する荷電粒子と考えてください。
- ターゲットポイントが魅力を生み出す
- 障害物/他のドローンは反発力を生成します
その中には:
利点: 計算が速く、リアルタイム制御に適しています。 短所: 極小値に陥りやすい (2 つのドローンが互いに「押すことができない」場合、振動します)
改善の方向性:
- ポテンシャル フィールドの整形: 極小値を避けるためにポテンシャル フィールドの形状を調整します。
- 複数の仮想ポテンシャルフィールド: 仮想障害物を導入してトラップエリアの周囲に経路を案内します
- ハイブリッド手法: A* または RRT* と組み合わせ、ポテンシャル場を使用して局所微調整を行います。
3.1.3 ボロノイ図法ボロノイ図は空間を複数のエリアに分割するために使用され、各ドローンはそのボロノイ ユニット内を飛行します。これにより、他のドローンまでの距離が常にボロノイ境界までの距離よりも大きくなります。
- 現時点のボロノイ図をリアルタイムで構築する
- 各ドローンは、ボロノイ ユニットの最遠点 (最適点) に近いウェイポイントを選択します。
- ウェイポイントの方向に移動し、競合が検出された場合は新しいボロノイ パスに切り替えます。
from scipy.spatial import Voronoi
import numpy as np
def voronoi_resolution(positions, v_i, p_i, v_max=20.0):
"""
基于 Voronoi 图的多机冲突消解
positions: 所有无人机位置 (N, 2)
"""
vor = Voronoi(positions)
# 找到当前无人机 i 的 Voronoi 单元
region_idx = vor.point_region[np.where(vor.point_region == vor.point_region[0])[0][0]]
# 简化的 Voronoi 路径选择：取 Voronoi 顶点的方向
vertices = [vor.vertices[v] for v in vor.regions[region_idx] if v >= 0]
if not vertices:
return v_i
# 选择最接近原始速度方向的安全顶点
best_dir = v_i / np.linalg.norm(v_i)
best_vertex = None
max_projection = -float('inf')
for v in vertices:
direction = v - p_i
if np.linalg.norm(direction) < 0.01:
continue
direction = direction / np.linalg.norm(direction)
projection = np.dot(direction, best_dir)
if projection > max_projection:
max_projection = projection
best_vertex = v
if best_vertex is None:
return v_i
return np.clip(best_vertex - p_i, -v_max, v_max)3.2 最適化手法
3.2.1 混合整数線形計画法 (MILP)
MILP は、複数の UAV の軌道計画を数学的最適化問題として形式化する古典的なフレームワークであり、Shouwenaars らによって UAV 分野で先駆的に開発されました。 (2001)。
核となるアイデア: 連続軌道は区分多項式または固定ウェイポイント シーケンスで表され、競合制約と安全制約は線形不等式で表され、飛行セグメントの切り替えロジックを表すために 2 進整数変数が導入されます。
制約:
- 運動学的制約:
, - 競合回避の制約:
の場合、対応するバイナリ変数 - OR 制約を導入します:
(すべての を強制的に 0、つまり競合しないようにします)
- タスク完了の制約:
# 概念性 MILP 冲突约束（伪代码）
"""
minimize: sum_i sum_k (v_i,k - v_pref_i,k)^2
subject to:
for each UAV i, segment k:
p_i,k+1 = p_i,k + v_i,k * dt # 运动学
norm(v_i,k) <= v_max # 速度限幅
norm(a_i,k) <= a_max # 加速度限幅
for each UAV pair (i,j), segment k:
norm(p_i,k - p_j,k)^2 >= d_safe^2 OR delta_ik = 0
M * delta_ik >= norm(p_i,k - p_j,k)^2 - d_safe^2
sum_j delta_jk <= 0 # 所有 delta 必须为 0
"""
```**利点**: 厳しい制約が確実に満たされる、グローバル最適ソリューション
**短所**: MILP ソルバー (CPLEX、Gurobi) の計算の複雑さはドローンの数に応じて指数関数的に増加するため、5 ～ 10 台を超えるドローンを使用したシナリオをリアルタイムで解決することが困難になります。
#### 3.2.2 動的ウィンドウアプローチ (DWA)
ロボットの動作計画から借用された DWA は、速度空間 $(v, \omega)$ をサンプリングし、各候補速度を評価します。
1. **ターゲットに向かう軌道**
2. **衝突安全性** (短期軌道のシミュレーションにより判断)
3. **速度到達性**
```python
def dwa_resolution(p_i, v_i, v_goal, obstacles,
v_max=3.0, v_min=0.0,
a_max=2.0, dt=0.1, predict_time=2.0,
safe_radius=1.5):
"""
Dynamic Window Approach 用于 UAV 冲突消解
"""
# 1. 构建动态窗口（当前可达速度集）
Vw = []
for v in np.arange(max(0, v_i[0] - a_max*dt), min(v_max, v_i[0] + a_max*dt), 0.1):
for w in np.arange(v_i[1] - a_max*dt, v_i[1] + a_max*dt, 0.1):
Vw.append((v, w))
best_score = -float('inf')
best_v = v_i
for (v, w) in Vw:
# 2. 预测轨迹
traj = []
p_pred = p_i.copy()
v_pred = np.array([v, w])
for t in np.arange(0, predict_time, dt):
traj.append(p_pred.copy())
p_pred = p_pred + v_pred * dt
# 3. 碰撞检测
collision = False
for p_obs in obstacles:
for p_t in traj:
if np.linalg.norm(p_t - p_obs) < safe_radius:
collision = True
break
if collision:
break
if collision:
continue
# 4. 评分函数
score_heading = np.linalg.norm(p_pred - v_goal) # 越小越好
score_velocity = v # 越大越好（偏好高速）
score_clearance = min([np.linalg.norm(p_t - p_obs)
for p_obs in obstacles for p_t in traj])
total_score = (
2.0 * (1.0 / (score_heading + 1e-6)) +
1.0 * score_velocity +
0.5 * score_clearance
)
if total_score > best_score:
best_score = total_score
best_v = np.array([v, w])
return best_v3.2.3 分散モデル予測制御 (DMPC)
DMPC は、大規模なマルチ UAV 群の主流の方法であり、各 UAV は次のことを行います。
- ローカル情報と近隣コミュニケーションに基づいてローカル予測モデルを構築する
- 有限時間領域での局所最適化問題を解く
- 制御の最初のステップのみを実行し、その後ローリング再最適化を実行します。
主要な一貫性制約:
ここで、
DMPC の主な利点は スケーラビリティ です。各ドローンは近隣のドローンと通信するだけで済み、グローバルなドローンの数に応じて計算量が急激に増加することはありません。
3.3 複数マシンの連携方法
3.3.1 グラフ理論に基づく一貫性アルゴリズム
マルチ UAV システムをグラフ
- ノード
はドローンを表します - エッジ
は通信リンクを表します
コンセンサスプロトコル:
\forall i, \quad \mathbf{s}i^* \in \arg\min{\mathbf{s}_i \in \mathcal{S}_i} \mathcal{J}_i(\mathbf{s}i^*, \mathbf{s}{-i}^*)
\mathcal{L} = -\mathbb{E}{(s,a) \sim d{\pi^*}}[\log \pi_\theta(a \mid s)]
\forall \omega \in \mathbb{W}: \quad \mathbf{x}[k+1] = A\mathbf{x}[k] + B\mathbf{u}[k] + E\omega[k]