MyEnigma

とある自律移動システムエンジニアのブログです。#Robotics #Programing #C++ #Python #MATLAB #Vim #Mathematics #Book #Movie #Traveling #Mac #iPhone

ロボティクスのための衝突チェック(Collision Check)技術

 

目次

はじめに

ロボティクスの技術の中で、

Collision checkは一見地味ですが、

かなり重要な技術です。

 

主にパスプランニングの際に、

静的物体や他の移動物体に

衝突しないかどうか判断するのに使われます。

myenigma.hatenablog.com

 

一般的に、

周辺環境の障害物の表現には

Grid Mapを使うことが多いようですが、

myenigma.hatenablog.com

周辺環境の変化に対応するために、

比較的高周期でパスを生成&衝突チェックをしないといけないことを考えると、

衝突チェックはかなり計算量の負荷が大きい処理です。

 

これまで高速に衝突チェックを実施する技術は、

色々提案されてきたのですが、

今回はその中でも代表的な技術について概要を説明したいと思います。

 

円の内外判定

最も簡単なCollision Checkの方法は、

ロボットを半径rの円で近似し、

障害物の端までの距離を距離を計算して、

その距離がr以下の時は衝突すると判断する方法です。

 

二次元平面上では、xo,yoを障害物の位置、

x,yをロボットの円中心とすると下記で計算できます。

 

生成した軌跡に対して衝突検知をする場合は、

下記の図のように

パスの上に円を半径づつ重ねて

上記の衝突計算を繰り返す方法がよく使われます。

 

ちなみに、ロボットの形状を円近似をする場合は、

後述のGrid MapのDistance Transformを使う方法や、

階層チェックを使うことで上記の処理を高速化することが出来る場合があります。

 

Grid MapとDistanse Transformによる円の内外判定の高速化

前述の円の内外判定の方法は

非常にシンプルで、障害物の数が少ない場合は高速に実行可能です。

しかし、衝突チェックの対象になる障害物の数が大きい場合や、

チェックするパスの長さが長い場合、

衝突チェックの計算時間が非常に大きくなってしまうという問題があります。

 

そこで、円の内外判定の高速化する方法として、

以前紹介したGrid MapとDistance Transformという手法を使う方法が

よく使用されます。

myenigma.hatenablog.com

この手法は事前に各障害物の位置の情報から、

Grid Mapの各グリッドに最も近い障害物までの距離を格納しておきます。

この手法を距離画像(Distance Transform)と呼ぶようです。

例えば下記の図は各グリッドの色を

最も近い障害物までの距離を元にして表したものになります。

(青:近い、赤:遠い)

 

上記のように、

事前にグリッドマップに障害物までの距離を格納しておくことで、

衝突チェックの際は、車両中心の位置から対応するグリッドを探索し、

そのグリッド内の距離データが車両半径以内であるかを確認すれば

衝突チェックを実施することができます。

Grid Mapの特性として、対応するGridの探索は、

O(1)で計算できるので非常に高速に探索可能です。

 

しかし、この手法も問題点として、

動的に障害物の位置が変わるような環境では、

毎回、距離情報を更新する必要があるため、

その部分で計算コストが増大してしまうという問題があります。

 

ロボットの複数円近似

前述の通り、

ロボットの形状を円近似することで、

比較的高速に衝突チェックを実施することができますが、

細長い形状をしたロボットだと、

円で近似すると狭い場所を走行できなくなります。

 

そのような場合は、

下記の図のように、複数の円を使って

ロボットの外周を網羅するように

衝突チェックを実施する方法がよく使用されます。

 

多角形の内外判定

前述の複数円で近似することが難しい場合、

ロボットを多角形で近似し、

障害物がその多角形の中に存在するかを確認する方法もよく使用されます。

 

これはコンピュータ幾何学で使用される、

点の内外判定(英語では Point in polygon)という技術です。

 

点の内外判定では、2つ代表的な手法があり、

  • Ray casting

  • Winding number algorithm

があります。

Ray castingは、内外判定をする点から線を伸ばし、

その線が多角形の辺に何回当たるかで判定する手法です。

 

一方、Winding number algorithmは下記の図のように

内外判定する点と、多角形の各頂点の成す角度を

それぞれ足し合わせ、360度の場合は内部、

0度の場合は外部であると判定する方法です。

 

ポリゴン同士の衝突チェック

前述の点のポリゴンの内外判定で、

点とポリゴンの衝突は判定できますが、

ポリゴンとポリゴンの衝突判定の場合は、

また、いくつか代表的な手法があります。

  • ポリゴンとポリゴンの頂点の内外判定を使う方法

  • ポリゴンの辺同士の交差判定を使う方法

  • ポリゴンの辺の垂線を使って、ポリゴンの点を垂線に投影して処理する方法

kyndinfo.notion.site

 

階層チェックによるパス衝突チェックの高速化

パスプランニングでは、

生成したパスを評価するために、

パスの各点での衝突チェックを実施する必要がありますが、

大量のパス上のサンプリング点と

大量の障害物の衝突チェックをする必要があるため、

計算コストが大きくなることがあります。

そこで、階層的なチェック手法を用いて

このパスの衝突チェックを高速化する手法が提案されています。

 

これは下記の3つのステップで衝突チェックを実施します。

1. Global Rectangle Collision Check

まず初めにパスのx-yの値の最大最小値と、

ロボットの長さを使った

グローバル座標系での長方形領域を設定し、

その長方形領域を使って、前述の多角形内外判定を実施します。

 

この長方形領域は衝突チェックに使うには、

かなりコンサバですが、

すべてのパスのサンプリング点を評価するよりは

非常に計算を早く実施できます。

この長方形領域に障害物が入っていない場合は、

衝突無しと非常に早い段階で判断できます。

 

また、この階層的衝突チェックの手法では、

一つ前の衝突チェックで、衝突確認された障害物のみを

次の衝突チェックに利用するため、

本当にパスに衝突の可能性がある障害物のみを

早い段階で選択し、後述の詳細な衝突チェックに利用します。

2. Circle Collision Check

2つ目の衝突チェックでは、

前述の円の内外判定で衝突チェックを実施します。

 

前述の通り、一つ目のGlobal Rectangle Collision Checkで

衝突の可能性がある障害物のみが、

二つ目の衝突チェックの対象になるため、

非常に高速にチェックを実施できます。

 

この時点で衝突する障害物が無い場合は、

このパスは安全と判断できます。

 

3. Polygon Collision Check

最後に最も計算コストが必要ですが、

最も精密な衝突チェックができる

パスの各サンプリング点における多角形の内外判定を実施します。

すでに衝突の可能性のある障害物はかなり絞り込まれているので、

計算は比較的高速に実現できます。

 

この時点で衝突判定された場合は、

このパスは危険であるという判断ができます。

 

アルゴリズムによっては、

2と3のいずれかを省略するアルゴリズムもあるようです。

 

参考資料

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

myenigma.hatenablog.com

 

MyEnigma Supporters

もしこの記事が参考になり、

ブログをサポートしたいと思われた方は、

こちらからよろしくお願いします。

myenigma.hatenablog.com