はじめに

ロボットの動作計画において、目的地までの最適な経路を求めることは重要な要素です。例えば、私達が取り組んでいるつくばチャレンジでは、以下のような経路で指定されたチェックポイントをすべて通過し、経路封鎖が発生した場合に適切に回避するという課題があります。本記事ではこのような課題に向けて、グラフ理論により効率的に最短経路を求める方法を紹介します。

チェックポイント間の経路をネットワークとして扱う

グラフ理論のダイクストラ法は、ノードとエッジで構成されるネットワークを基に最短経路を解く手法です。この手法を用いるため、つくばチャレンジのチェックポイントをノード、経路をエッジとして以下のようなネットワークを作成します。エッジの重みとして、距離情報を設定しています。

import pandas as pd
import networkx as nx
import matplotlib.pyplot as plt
from pathlib import Path
import math

# CSVファイルのパス
csv_path = Path.home() / "network" / "src" / "node_positions.csv"
edges_path = Path.home() / "network" / "src" / "tsukuba_edges.xlsx"
# CSVを読み込む
df = pd.read_csv(csv_path)
df_edges = pd.read_excel(edges_path)

# NetworkXのグラフを作成
G = nx.Graph()

# ノードを追加（座標付き）
pos = {}  # 座標情報を保存する辞書
for _, row in df.iterrows():
    node, x, y = row["Node"], row["X"], row["Y"]
    G.add_node(node, pos=(x, -y))  # y座標を反転（matplotlibの座標系調整）
    pos[node] = (x, -y)

for _, row in df_edges.iterrows():
    node1, node2 = row["node1"], row["node2"]
    #print('edges:', node1, node2)
    # ノード1とノード2の座標を取得
    x1, y1 = pos[node1]
    x2, y2 = pos[node2]
    # ユークリッド距離を計算
    distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)

    G.add_edge(node1, node2, weight = distance)

# ネットワークを描画
plt.figure(figsize=(8, 6))
# エッジのラベル（重み）を取得
edge_labels = nx.get_edge_attributes(G, 'weight')

# グラフを描画
nx.draw(G, pos, with_labels=True, node_size=300, node_color="DeepSkyBlue", edge_color="black", font_size=10)

# 重みを整数に変換してラベルに表示
edge_labels_int = {k: f"{int(v)}" for k, v in edge_labels.items()}

# エッジの重みを整数で描画
nx.draw_networkx_edge_labels(G, pos, edge_labels=edge_labels_int)
plt.show()

ノードとその座標位置をnode_positions.csvに保存し、ノード間の接続関係をtsukuba_edges.xlsxに保存しています。networkxを用いてネットワークを描画しました。

ダイクストラ法

ダイクストラ法は、ネットワーク上のあるノードを始点として、効率的に最短経路を求める手法です。以下にその手順を記述します。

1. 初期設定

始点ノードを設定し、始点から各ノードへの距離を無限大に設定します。ただし、始点ノードの距離は0とします。
訪れていないノードをすべてリストに保持します。

2. 最短距離の決定

訪れていないノードの中で、最も距離が短いノードを選び、そのノードを確定ノードとしてリストに追加します。
確定ノードを通ることで、他のノードへの最短距離が更新される場合は、その距離を更新します。

3. 反復

訪れていないノードの中から、最も距離が短いノードを選び、距離を更新するプロセスを繰り返します。
すべてのノードが確定した時点で、最短経路が求められます。

下図はダイクストラ法で最短経路を求める過程を描画してます。A地点からスタートし、新しいノードへ訪れるごとに最短距離を更新します。AからDまでの最短な経路は、A→B→C→Dの順で距離は４と求められます。

指定された地点を通過するという制約

つくばチャレンジの課題では、指定された地点を通過しなければなりません。そこで、その地点を効率的に巡回しながら目的地まで走行する経路を探索します。これは、巡回セールスマン問題にかなり似ています。巡回セールスマン問題とは、1つの都市から出発し、すべての都市をちょうど1回ずつ訪れて、最初の都市に戻るための最短ルートはどれかを求める問題です。これは「NP困難」と呼ばれる問題の1つで、都市の数が増えると可能なルートの組み合わせが爆発的に増えるため、厳密な解を求めるのが難しくなります。

指定された地点の訪問順を全探索すると膨大な計算コストがかかるため、本記事では計算コストを抑えつつ、比較的良い解を得られる2-opt法を紹介します。

2-opt法

2-optは、ある巡回経路から2つのエッジを削除して繋ぎ直し、改善ができなくなるまで繰り返すアルゴリズムです。以下がその手順です。

現在の巡回経路から 2本の異なる辺を選ぶ
それらの辺を入れ替えて経路を逆順に接続し、新しい経路を作成
変更後の経路が元の経路より短くなれば、更新
これを、もう改善できなくなるまで繰り返す

実装

以下が指定されたチェックポイントを通過しつつ、スタートからゴール地点まで通る最短経路を求めるコードです。

import random
import networkx as nx
import matplotlib.pyplot as plt
import pandas as pd
import numpy as np
import math
from pathlib import Path

def total_distance(route, graph):
    """ルート全体の距離を計算"""
    
    return sum(nx.shortest_path_length(graph, source=route[i], target=route[i+1], weight='weight') for i in range(len(route)-1))

def two_opt(route, graph, max_iter=1000):
    """2-opt 法でルートを改善"""
    best_route = route[:]
    best_distance = total_distance(route, graph)
    
    for _ in range(max_iter):
        improved = False
        for i, j in itertools.combinations(range(1, len(route) - 1), 2):

            new_route = best_route[:i] + best_route[i:j+1][::-1] + best_route[j+1:]
            new_distance = total_distance(new_route, graph)
            
            if new_distance < best_distance:
                best_route, best_distance = new_route, new_distance
                improved = True
        
        if not improved:
            break

    return best_route, best_distance

# CSVファイルのパス
csv_path = Path.home() / "network" / "src" / "node_positions.csv"
edges_path = Path.home() / "network" / "src" / "tsukuba_edges.xlsx"

# CSVを読み込む
df = pd.read_csv(csv_path)
df_edges = pd.read_excel(edges_path)

# NetworkXのグラフを作成
G = nx.Graph()

# ノードを追加（座標付き）
pos = {}  # 座標情報を保存する辞書
for _, row in df.iterrows():
    node, x, y = row["Node"], row["X"], row["Y"]
    G.add_node(node, pos=(x, -y))  # y座標を反転（matplotlibの座標系調整）
    pos[node] = (x, -y)

# エッジを追加し、重みを設定（距離を計算）
for _, row in df_edges.iterrows():
    node1, node2 = row["node1"], row["node2"]
    x1, y1 = pos[node1]
    x2, y2 = pos[node2]
    distance = math.sqrt((x2 - x1)**2 + (y2 - y1)**2)
    G.add_edge(node1, node2, weight=distance)

# ** 入力データ ** #
start_node = 4
goal_node = 7
mandatory_nodes = [10, 2, 5, 19, 11, 20, 15, 30, 22, 8, 33]  # 必須で通過すべきノードをリストとして定義

# ** 初期ルート作成（ランダムな順序）** #
route = [start_node] + random.sample(mandatory_nodes, len(mandatory_nodes)) + [goal_node]

# ** 2-opt の適用 ** #
optimized_route, min_distance = two_opt(route, G)

# ** 結果の表示 ** #
best_path = optimized_route

# 最短経路を構築
full_path = []
for i in range(len(best_path) - 1):
    try:
        sub_path = nx.shortest_path(G, source=best_path[i], target=best_path[i + 1], weight='weight')
    except nx.NetworkXNoPath:
        continue
    full_path += sub_path[:-1]
full_path.append(goal_node)

# 最短経路に含まれるエッジを赤く描画
edges_to_highlight = set(zip(full_path[:-1], full_path[1:]))

# ネットワークを描画
plt.figure(figsize=(8, 6))
edge_colors = ['red' if (u, v) in edges_to_highlight or (v, u) in edges_to_highlight else 'black' 
               for u, v in G.edges()]
nx.draw(G, pos, with_labels=True, node_size=300, node_color="DeepSkyBlue", font_size=10)

nx.draw_networkx_nodes(G, pos, nodelist=mandatory_nodes, node_size=500,
                       node_color="white", edgecolors="DarkMagenta", linewidths=2)

nx.draw_networkx_nodes(G, pos, nodelist=[start_node], node_size=500,
                       node_color="white", edgecolors="red", linewidths=2)

nx.draw_networkx_nodes(G, pos, nodelist=[goal_node], node_size=500,
                       node_color="white", edgecolors="blue", linewidths=2)


# エッジの重みを整数で描画
nx.draw_networkx_edge_labels(G, pos, edge_labels=edge_labels_int)

nx.draw_networkx_edges(G, pos, edgelist=edges_to_highlight, edge_color='MediumSeaGreen',
                       width=2, alpha=1, arrows=True, arrowstyle="-|>", arrowsize=10)
plt.show()

# 結果を出力
print(f"最適な訪問順序: {best_path}")
print(f"最適な訪問順序(全て): {full_path}")
print(f"総移動距離: {min_distance}")

def total_distance(route, graph):
    """ルート全体の距離を計算"""
    return sum(nx.shortest_path_length(graph, source=route[i], target=route[i+1], weight='weight')
               for i in range(len(route)-1))

与えられた route の総距離を計算します。各隣接ノード間の最短距離を NetworkX の shortest_path_length を用いて取得し、合計しています。

def two_opt(route, graph, max_iter=1000):
    """2-opt 法でルートを改善"""
    best_route = route[:]
    best_distance = total_distance(route, graph)
    
    for _ in range(max_iter):
        improved = False
        for i, j in itertools.combinations(range(1, len(route) - 1), 2):
            new_route = best_route[:i] + best_route[i:j+1][::-1] + best_route[j+1:]
            new_distance = total_distance(new_route, graph)
            
            if new_distance < best_distance:
                best_route, best_distance = new_route, new_distance
                improved = True
        
        if not improved:
            break
    return best_route, best_distance

2-opt法のアルゴリズムです。
経路内の2点 (i, j) を選び、2点間のエッジを繋ぎ直します。経路が短縮されるなら更新
繰り返し適用し、最適な経路を求めます。

start_node = 4
goal_node = 7
mandatory_nodes = [10, 2, 5, 19, 11, 20, 15, 30, 22, 8, 33]
route = [start_node] + random.sample(mandatory_nodes, len(mandatory_nodes)) + [goal_node]

スタート地点(start_node)、ゴール地点(goal_node)、指定された地点(mandatory_nodes)を定義します。

optimized_route, min_distance = two_opt(route, G)

full_path = []
for i in range(len(optimized_route) - 1):
    try:
        sub_path = nx.shortest_path(G, source=optimized_route[i], target=optimized_route[i + 1], weight='weight')
    except nx.NetworkXNoPath:
        continue
    full_path += sub_path[:-1]
full_path.append(goal_node)

two_opt法で求めた最適な訪問順のなかでの最短パスを求めます。

このコードの出力結果です。緑色の線が求められた最短経路を示しています。実行時間は約0.4秒でした。ちなみに、同じ問題を全探索で解くと計算に約2分かかりました。

つくばチャレンジの課題では、ランダムに選ばれた地点に黄色い看板が設置され、その地点が通行不可になります。課題をクリアするには、看板を検知して経路を再探索する必要があります。0.4秒程度で経路を算出できれば、リアルタイム性を確保しながらロボットを制御することが可能です。

ソースコード

参考文献

謝辞

この取り組みは, GxP(グロースエクスパートナーズ)株式会社様のサポートを受けて実施しています. 貴重なアドバイスや, ロボットに必要な機材の支援をいただきました. 心より感謝申し上げます.