パーシステントホモロジーによる標高差を考慮した軌道生成アルゴリズムの作成○山田大智 伊丹琢 米山淳 （青山学院大学）

Path Planning Algorithm Considering Altitude Diﬀerence by PersistentHomology∗Daichi Yamada, Taku Itami, Jun Yoneyama(Aoyama Gakuin University)
Abstract– This study discusses a method for extending the path planning algorithm using persistent homology from a 2-dimensional plane to a 2.5-dimensional environment. In previous studies, a path planningalgorithm using persistent homology in 2-dimensional plane has been proposed. However, since the environment in which the vehicle actually travels is not a 2-dimensional plane but a 2.5-dimensional environment
with slopes and irregularities, there is a problem that has diﬃculty to apply in the actual environment. In
this study, we propose a trajectory generation algorithm by applying persistent homology taking into accountthe altitude of the terrain.
Key Words: Path planning, Persistent homology, mobile robots

1

はじめに

近年における自動運転技術の成長は著しく, 着実に完全自動運転に向けての技術が確立されつつある. 車両のみならず, 自立移動ロボットを初期位置から目的位置まで制御する問題に運動計画問題が存在する. 運動計画問題における最適解は, ある初期位置 A から目的位置 Bまで障害物に衝突せず, 短い時間と軌道で到達することにあり, 走行環境に依存しないためにロバスト性の高さも重要な要素の 1 つとなっている. 運動計画問題を解決するアルゴリズムとして, 最適経路検索アルゴリズムが存在する. 最適経路検索アルゴリズムを実装する環境
は, 観測した情報を基に各座標を小さなセルにグリッド化して行われる 1) . 今日までにダイクストラ法 2) をはじめ,A∗3) ,RRT (Rapidly exploring Random T ree)4)などが基礎として確立されており, またそれらを応用したアルゴリズムも多く提案されている. 特に近年では, パーシステントホモロジーを用いたロバストな軌道生成アルゴリズムが提案されており 5) , 高いロバス
ト性のため, 惑星探査車両の走行経路を決定する方法として有用である. しかしながら, この手法は 2 次元環境でしか実現されておらず, 実際に車両が走行する起伏のある環境には適用できないという課題点が存在する. 本研究は, 移動モデルを車両と想定した上で, パーシステントホモロジーを用いた軌道生成アルゴリズムを 2 次元平面から起伏のある立体環境に適用する手法を提案する. しかしながら, 車両は空中を移動することができないため, 計算の簡略化として 3 次元ではなく,2.5 次元への適用を目指した.

2問題設定

本研究では,Fig. 1 のような標高が存在する環境を考える.2.5 次元環境とは,1 つの xy 座標に対して単一の z座標をもつ座標系を表す. このような環境において, 制御対象である車両ロボットを初期位置から目的位置まで障害物に接触することなく移動するための運動計画に関して議論する. 各座標をセルに離散化し, 各セルにおける標高を観測する. 本研究において, 初期位置と目的位置は明確に定めるものとしていることから最適経路検索アルゴリズムとして,A∗ アルゴリズムを適用する 3) .

Fig. 1: 2.5D environment3階層構造A∗ アルゴリズムに代表される最適経路検索アルゴリ
ズムでは, 評価指標に基づき初期位置から目的位置までの軌道を 1 つのみ生成する. しかしながら, 障害物を回避する軌道は一般に一意でない.2 次元平面の場合, 障害物 1 つにつき右または左の回避軌道が存在し,2.5 次元環境においても同様である. 更に, 障害物が n 個存在する場合,2n 通りの回避軌道が想定される. これらの軌道を区別するために H2 階層構造 6)7) を用いる.
3.1 H 階層構造
複数の障害物が存在する環境下で複数の軌道を生成するこの手法は, 仮想的な 3 次元の螺旋構造を Fig. 3のように環境に適用し, 障害物の重心を基準に移動モデ
ルが各障害物の周りをどれだけ回転したかによって区別を行う. 回転量の式は Fig. 2 をもとにdθj =⃗ − (y − yj )dx⃗(x − xj )dy22(x − xj ) + (y − yj )(1)と表され,H 階層構造では (2) 式のように変換することで階層の移動量とする. ここで,(xs , ys ) は初期位置,(xg , yg )は目的位置である.

H(τ ) =12π

∫ (xg ,yg )dθ(xs ,ys )(2)

目的位置の階層を変化させることによって A∗ アルゴリズムで求められる軌道が変化する.Fig. 3 を例にとると, 次のように複数軌道を作成できる.

3 において, 初期位置と目的位置, 障害物の重心が同一直線状にあるものと仮定し, 目的位置を 3F にしたときの軌道を τ3 とすると次のように計算できる.

• 目的位置を 1 階に設定する→ τ1 の軌道を生成する

H2 (τ1 ) = −• 目的位置を 2 階に設定する→ τ2 の軌道を生成する

123H2 (τ3 ) =212H2 (τ2 ) =• 目的位置を 3 階に設定する→障害物を反時計回りに 1 周して目的位置に到達する

(4)
(5)
(6)

ここで, 各軌道での階層移動量の差を求めると

H2 (|τ1 − τ2 |) = |H(τ1 ) − H(τ2 )| (mod 2) = 1(7)H2 (|τ1 − τ3 |) = |H(τ1 ) − H(τ3 )| (mod 2) = 0(8)H2 (|τ2 − τ3 |) = |H(τ2 ) − H(τ3 )| (mod 2) = 1(9)となり, これらの軌道は H2 によって (τ1 ̸= τ2 ),(τ1 = τ3 )であることを意味する. これにより,τ3 は τ1 と同じ軌道であるとみなし, 軌道を削除した上でアルゴリズムを終了する.

4

パーシステントホモロジー

本章では, 離散化されたセルにより, 車両が走行する
幾何的な環境を観測するパーシステントホモロジー 8)9)に関する説明を行う.

Fig. 2: Calculation of rotation amount in hierarchicalstructure

4.1 ホモロジー群
N 次元のユークリッド空間 RN を考え, この空間内に点群を配置する. 各点にある半径 r を設定して点を中心に膨らませると, 点の距離によって球同士で重複する部分が現れる. 重複した頂点同士を線で結ぶことで線分(1 単体や) 三角形 (2 単体), 正四面体 (3 単体) などの図形が構築される. このような点群によって構成される幾何学的特性を代数的に抽出したものがホモロジー群である. このとき,r は定数である.
4.2 パーシステントホモロジー群
ホモロジー群では各点に設定する半径は定数であった. パーシステントホモロジー群は, 半径 r を変数として徐々に膨張させ, 半径の変化と同時に発生する点群の幾何学的特性を観測する理論である. 本研究では,Fig. 4のように, 離散化された各セルに点群を設置することで地形を分析し,1 単体で接続された部分を移動可能な経路とし,1 単体が生成する「穴」を障害物として扱う.

Fig. 3: Relationship between obstacle avoidance andhierarchical structure
3.2 H2 への拡張
H 階層構造によって, 軌道を複数作成することができる. しかしながら, 障害物を複数にわたって回転する軌
道は最適経路であるとはいえない. また, 目的位置の階層変更を何回行えばすべての軌道が出揃うか判別ができない. そこで,H2 階層構造を導入する 6) .H によって回転量を計測するまでは同様であるが,(3) 式のように階層を移動した量を 2 で割った余りを計算する.
H2 (τ ) = H(τ ) (mod 2)(3)

各軌道の回転量の差を求め, 余りが 1 の場合は別の軌
道, 余りが 0 の場合は同じ軌道として区別を行う. Fig.

Fig. 4: Topographic analysis example

4.3

標高差を考慮したパーシステントホモロジー

パーシステントホモロジー群により, 障害物の観測が可能であることを確認した. 半径同士が接続されている部分を通過可能としていたが, 環境が 2.5 次元であるので, 隣接する頂点間の標高が異なる場合は車両は坂を移動することになる. 車両は上り下りに関わらず, 急な坂道を移動する際に転倒の危険性を伴う. 標高の高い平坦な道を走行すること自体はあまり危険ではないが, 高低差の激しい道は車種によっては移動が不可能である. そこで, 最適経路検索アルゴリズムを実行する上で急斜面を通過しないよう制限する変数として, 許容標高差 (ϵ)
を定義する. 許容標高差とは, 車両が移動可能な標高差を制限する数値であり, 以下のような規則に従う.

Fig. 6: Observation results of obstacles by connecting
vertices(r = 1.5[m])• (隣接する座標との標高差)> ϵ の場合, その座標は障害物とする• (隣接する座標との標高差)≤ ϵ の場合は通過可能とする
上記のような規則を定めることで急斜面の移動を避け,車両の安全を担保する.

5

また,r = 2.54[m] のとき,Fig. 7 のように障害物が存在しない状態を観測できたことから, 隣接する頂点との距離のうち最大のものが r = 2.54[m] であることが確認できた.

シミュレーション結果

シミュレーションにおける数値例として,Fig. 1 を用いた. 障害物回避の軌道を簡潔な形式で観測するために,Fig. 5 のように標高をグレースケールとした平面図において出力軌道を確認する. 初期位置と目的位置をそれぞれ (10, 10),(90, 90) と設定し, 標高 height(x, y) は(10) 式で表されるものとする.

Fig. 7: Observation results of obstacles by connectingvertices(r = 2.54[m])以上の結果を基に許容標高差 ϵ を設定し,A∗ アルゴリズムを実装した. 次の Figs. 8,9 に出力軌道を示す.

Fig. 5: Numerical example Top view of environment(Initial position: ●, Target position: ×)
また, 頂点座標は (1 ≤ x ≤ 99), (1 ≤ y ≤ 99) の範囲内の整数とする.

√height(x, y) = (5 cos(x−49.5)2 +(y−50.5)2+5 4
203)

(10)
(10) 式は頂上の xy 座標を (49.5, 50.5) としたもので(50, 50) より少しずれている. パーシステントホモロジーによって環境の障害物を観測した結果,r = 1.5[m]のとき,Fig. 6 のような結果が得られ, 障害物が 1 つ存在することが確認できた.

Fig. 8: Trajectory generated when the permissible
elevation diﬀerence ϵ = 2参考文献

Fig. 9: Trajectory generated when the permissible
elevation diﬀerence ϵ = 3Fig. 8 より, 障害物が存在する状況では 2 通りの軌道が確認できた. 障害物観測において頂点接続の最大値が 2.54[m] であったことから, 最大の標高差 hmax はhmax =√2.542 − 12 ≒ 2.33(11)である.ϵ = 3 のとき,Fig. 1 における山が通過可能であるので, 障害物と認識していないことから軌道が 1 つのみ出力となった. また, 各許容標高差 ϵ における軌道の長さを比較した結果 Fig. 10 により, 各軌道の長さを比較することができる.

Fig. 10: Comparison of Trajectory lengths for permissible elevation diﬀerence ϵ6まとめ

本研究では, 標高差を考慮した軌道生成アルゴリズムを検討した. 各セルに標高情報を設定し, 隣接するセルとの標高差を通過しにくさの指標としてアルゴリズムの作成を行った. また, 移動モデルが車両である場合を想定しているので近道であっても危険な斜面を通過しないよう, 許容標高差を用いた手法を提案した. 本研究の課題点として,3 次元環境での適用が挙げられる.2.5
次元では,1 つの平面座標 (x, y) に対して単一の z 座標しか定義できないために,Fig. 1 における山にトンネルが存在する場合に対応できない. また, トンネルを通過するような軌道の場合は障害物を左右に回避ではなく,下からくぐる形になるので階層構造を更に拡張した理論が必要である.

1) S.Thrun, D.Fox, W.Burgard:Intelligent Robotics andAutonomous Agents, A Bradford Book (2005)
2) E.W.Dijkstra:A Note on Two Problems in Connection
with Graphs, Numerische Mathematik 1, pp.269-271(1959)
3) Peter E.Hart, Bertman Raphael, Nils J.Nilson:A Formal Basis for the Heuristic Determination of Minimal
Coat Paths, IEEE Transactions on Systems Scienceand Cybernetics Vol.4, No.2, pp.100-107 (1968)
4) Steven M. Lavalle:Rapidly exploring random trees:
A new tool for path planning, Technical Report.
Computer Science Department, Iowa State University(TR 98-11) (1998)
5) S.Bhattacharya, R.Ghrist, V.Kumar:Persistent homology for path planning in uncertain environments, IEEE Transactions on Robotics Vol.31, No.3,
pp.578-590 (2015)6) S.Bhattacharya, M.Likhachev, V.Kumar:Topological
Constraints in Search-based Robot Path Planning,
Autonomous Robots 33, pp.273-290 (2012)7) S.Bhattacharya,
D.Lipsky,
R.Ghrist,
V.Kumar:Invariants for Homology Classes withApplication to Optimal Search and Planning Problem in Robotics, Annals of Mathematics and Artiﬁcial
Intelligence 67, pp.251-281 (2013)8) 平岡裕章:タンパク質構造とトポロジー -パーシステント
ホモロジー群入門- , 2/118, 共立出版 (2013)9) Afra Zomorodian, Gunnar Carlsson:Computing Persistent Homology, Discrete & Computational Geometry 33, pp.249-274 (2005)