JPH085028B2 - 移動体の衝突判定方法並びに衝突判定装置 - Google Patents

Description

【発明の詳細な説明】 〔産業上の利用分野〕 本発明は、ロボット（移動体）の制御技術に係り、特
に、２次元以上の動作自由度を有するロボットの自律動
作に好適な移動体の衝突判定方法並びに衝突判定装置に
関する。

〔従来の技術〕

従来の移動体の衝突判定方法においては、アイ・イー
・イー・イー、トランザクション オン システム、マ
ン アンド サイバネティックス、エス エム シー1
1、（1981年）第681頁〜第698頁（IEEE,Trans.on Syste
m,Man and Cydernetic,SMC−11,（1981）PP681−698）
で論じられている。本文献は、マニピュレータの動作空
間を障害物と衝突する空間、衝突しない空間及び衝突不
明空間の３種類に分けて調べている。

ロボットと障害物との衝突の有無の干渉チェックは、
ロボットと障害物の形状が複雑な場合、定式化が困難で
あり非常に難しい。このため通常、ロボットや障害物を
複数の多面体（以下、この多面体を物体要素と呼ぶ）で
近似し、ロボットの物体要素と障害物の物体要素との間
（以下、単に物体要素間という）の干渉チェックを行な
い、ロボットが障害物と衝突するかしないかを判定する
必要がある。

〔発明が解決しようとする課題〕

従来の移動体の衝突判定方法にあっては、前述の空間
の分類にあたって、物体要素間のすべての組み合わせに
対して干渉チェックを実行していた。このため衝突判定
に膨大な計算時間を必要としていた。したがってロボッ
トの周囲の環境が単純な場合に適用範囲が限られてい
た。また周囲の環境が変化した場合、再度最初から判定
し直す必要があった。

本発明の目的は、ロボット（移動体）と障害物との衝
突判定を高速化し、複雑な環境や環境の変化に柔軟に対
応できる移動体の衝突判定方法並びに衝突判定装置を提
供することにある。

〔課題を解決するための手段〕

前記の目的を達成するため、本発明に係る移動体の衝
突判定方法は、移動体と障害物のそれぞれの形状を複数
の物体要素で近似し、それぞれの物体要素の間を干渉チ
ェックして移動体と障害物との間の衝突の有無を判定す
る移動体の衝突判定方法において、干渉チェックを行う
今回の移動体の位置・姿勢で該移動体と障害物との間の
衝突判定をする際、前回の移動体の位置・姿勢で得られ
た衝突判定結果と、少なくとも距離を含む衝突判定に必
要なデータとにより衝突の可能性の高い順にそれぞれの
物体要素を組合せ、干渉チェックの実行手順を決定する
ように構成されている。

そして、干渉チェック実行手順は、干渉チェックに要
する時間と回数とを最小限ににして決定される構成でも
よい。

また、干渉チェック実行手順は、今回の移動体の位置
・姿勢におけるそれぞれの物体要素間の干渉確率を算出
し該干渉確率に基づいて決定される構成でもよい。

さらに、衝突判定に必要なデータは、今回の移動体の
マニピュレータ姿勢で得られたデータが含まれている構
成でもよい。

そして、干渉チェックの実行手順は、それぞれの物体
要素間を干渉チェックする実行順位と、複数の干渉チェ
ック法から選択される少くとも一つの干渉チェック法を
決定する構成でもよい。

また、今回の移動体の位置・姿勢は、前回干渉チェッ
クした移動体の位置・姿勢のうち直前に干渉チェックし
た移動体の位置・姿勢に隣接又は近似して決定される構
成でもよい。

さらに、前回の移動体の位置・姿勢を、干渉チェック
を行う今回の移動体の位置・姿勢の近傍とする構成でも
よい。

そして、干渉チェックの実行手順は、前回干渉チェッ
クした移動体の位置・姿勢のうち直前に干渉チェックし
た前回の移動体の位置・姿勢でかつ衝突と判定されたそ
れぞれの物体要素間の干渉チェックを最上位にして決定
される構成でもよい。

また、干渉チェックの実行手順は、最上位につぐ実行
順位を、前回の移動体の位置・姿勢でかつそれぞれの物
体要素間の距離が小さい順に決定する構成でもよい。

さらに、干渉チェックの実行手順は、それぞれの物体
要素間の衝突判定結果が衝突の際は衝突判定に適した干
渉チェック法を選定し、衝突判定結果が非衝突判定の際
は非衝突判定に適した干渉チェック法を選定する構成で
もよい。

そして、干渉チェックの実行手順は、干渉チェックの
実行順位の決定と干渉チェック法の選択とを並列して実
行させる構成でもよい。

また、それぞれの物体要素を、移動体及び障害物のそ
れぞれの属性に応じて少くとも一つの物体要素群に階層
的に分類し、干渉チェックの実行順位の決定を階層ごと
に行う構成でもよい。

さらに、移動体の軌道検索方法においては、前記いず
れか一つの移動体の衝突判定方法を用いて得られた衝突
判定結果を利用し、移動体の少くとも２次元空間の移動
軌跡を探索する構成とする。

そして、マイクロコンピュータにおいては、前記いず
れか一つの移動体の衝突判定方法を書き込んだROMを有
する構成とである。

また、移動体の衝突判定装置においては、移動体と障
害物のそれぞれを複数の物体要素で近似し、それぞれの
物体要素間を干渉チェックして移動体と障害物との間の
衝突の有無を判定する手段を備えた移動体の衝突判定装
置において、前回の移動体の位置・姿勢でそれぞれの物
体要素間を干渉チェックし得られた衝突判定結果と少な
くとも距離を含む衝突判定に必要なデータとを記憶しか
つその記憶内容を出力する記憶手段と、記憶手段を利用
して干渉チェックを行う今回の移動体の位置・姿勢で衝
突の可能性の高い順にそれぞれの物体要素を組合せ干渉
チェックの実行手順を決定する手段とを備えた構成とす
る。

さらに、衝突判定に必要なデータに、今回の移動体の
位置・姿勢で得られた衝突判定に必要なデータを含む構
成でもよい。

そして、衝突判定結果と衝突判定に必要なデータとに
より、今回の移動体の位置・姿勢で移動体と障害物との
間の衝突の有無の干渉チェックと並列に、次回の移動体
の位置・姿勢における干渉チェックの実行手順を決定す
る手段を備えた構成でもよい。

〔作用〕

本発明の移動体の衝突判定方法によれば、干渉チェッ
ク用記憶手段に、前回のロボット（移動体）位置・姿勢
におけるロボットと障害物の衝突判定結果と、ロボット
と障害物の物体要素間の距離（以下単に距離と略する）
とが記憶されている。

順位選定手段は、この記憶された衝突判定結果と距離
に基づき、今回干渉チェックしようとするロボット位置
・姿勢での、ロボットと障害物の物体要素間の組み合わ
せに対する順位を設定し、記憶する。この順位は衝突の
可能性の高い順に設定される。

また、方法選定手段は、この物体要素の組合せに対し
て、前回のロボット位置・姿勢での衝突判定結果を干渉
チェック用記憶手段から読み出し、この衝突判定結果で
ある衝突の有無に応じて、複数個ある干渉チェック法の
うち、最適な干渉チェック法を選び記憶する。

ロボットと障害物の衝突判定は、順位選定手段が指定
する順番に物体要素間の組合せを選び、その組合せに対
して方法選定手段が指定する干渉チェック法を用いて実
行し、衝突を判定するとともに物体要素間の距離を計算
する。計算後は干渉チェック用記憶手段の情報を更新す
る。

この手順によって、衝突の可能性が高い物体要素間の
組合せから優先的に衝突判定が行なわれ、且つそれぞれ
の組合せに最適な干渉チェック法が実行されるため、ロ
ボットと障害物の衝突判定に要する干渉チェックの実行
回数が削減される。

〔実施例〕

本発明の一実施例を図面を参照しながら説明する。第
１図は本発明の一実施例の全体構成を示したもので、ロ
ボット（移動体）１と、ロボット１の駆動手段３と、ロ
ボット１及び障害物２の幾何データ（物体要素の点や面
の座標値）を格納したデータベース８と、この幾何デー
タを用いてロボット１と障害物２との衝突を判定する干
渉チェック手段６と、この判定結果を記憶する地図記憶
手段７と、この地図記憶手段７が記憶する地図を参照し
て障害物２と衝突しないようにロボット１の経路４を求
める経路探索手段５と、求めた経路４を記憶する経路デ
ータ格納手段９とから構成される。

更に、干渉チェック手段６は、干渉チェックを実行す
る際に更新が必要な計算データを記憶する干渉チェック
用記憶手段10と、この計算データを用いて干渉チェック
すべき物体要素の要素順位を定める順位選定手段11と、
複数の相違なる干渉チェック法14a〜14dを格納する方法
格納手段14と、干渉チェック用記憶手段（記憶手段）10
のデータによって方法格納手段14の中から最適な干渉チ
ェック法を選ぶ方法選定手段12と、これらの手段10,11,
12及び14を用いて障害物との衝突の有無の判定を実行す
る干渉チェック実行手段（実行手順を決定する手段）15
とから構成される。

干渉チェック用記憶手段10は第２図に示すように、あ
る位置・姿勢に対するロボット１を多面体（物体要素）
等の幾何データで近似しそのデータを一時的に記憶する
幾何データ一時記憶手段17と、ロボット１と障害物２の
それぞれの物体要素間の衝突判定の実行順位を表す実行
順位表18と、ロボット１と障害物２との距離を表す距離
マップ19と、衝突判定結果を記憶する衝突判定表20とか
ら構成される。

先ず、第１図に示す一実施例の概略動作を説明する。
本実施例は大別して次の２つの動作からなる。第１の動
作は、ロボット１が動作する予定の空間に対して、干渉
チェック手段６がデータベース８のデータを用いて、ロ
ボット１と障害物２とが衝突する空間を調べ、動作する
予定の空間と衝突する空間とを第３図に示す地図16に記
述し、地図記憶手段７に記憶するものである。第２の動
作は、地図16、データベース８及び経路検索手段５によ
り、ロボット１が障害物２と衝突しない経路を探索し、
その結果の経路データP_iを経路データ格納手段９に格納
し、経路データP_iを用いて駆動手段３がロボット１を現
在位置から目標位置に動作を制御するものである。

以下、第１図に示す一実施例を、地図16、第２の動作
及び第１の動作の順に説明する。

最初に、地図記憶手段７に記憶される地図16の構造に
ついて説明する。

地図16は第３図に示すように、ロボット１の動作量を
軸に取った空間である。この動作量は、ロボット１の３
次元空間の位置及び姿勢を表す座標値や、ロボット１の
足及び腕の関節角などで表される。この地図16は、ロボ
ット１の動作自由度がｎであれば、ｎ次元の軸を持つ空
間となる。以下、原理説明を分かり易くするため、ロボ
ット１の位置を表す３次元量x,y,zの場合を例にして説
明するが、ｎ次元の場合も同様の考え方を適用でき３次
元に限定するものではない。以下にその原理を説明す
る。

地図16は第３図に示されるように、ロボット１の動作
空間を複数のセルに分割し、それぞれのセルに対してロ
ボット１を代表する点（重心点又は腕の先端）がそのセ
ル内の空間に入った場合、ロボット１と障害物２とが衝
突するかしないかの判定をすることによって作成され
る。この時ロボット１の代表点が各セルの中心Ｐ＝（x,
y,z）に一致するロボット位置で、ロボット１と障害物
２とが衝突するかしないかを判定することで、各セル内
の空間におけるロボット１と障害物２との衝突判定を近
似する。

次に第２の動作について説明する。地図16が作成され
ると、経路探索手段５は、ロボット１の初期位置P_I、目
標市P_Gをデータベース８から読み出し、これらの位置に
ロボット１の先端を位置決めするために必要なロボット
１に固有の座標系を持つ動作量（腕の場合は関節角）、 P_I＝（θ₁₁,θ₁₂,θ₁₃） …（１） P_G＝（θ_G1,θ_G2,θ_G3） …（２） を求める。次に、地図記憶手段７のデータを読み出し、
障害物と衝突しないロボット１の軌道を探索する。この
探索法は、例えば従来例の文献に記憶されている方法を
用いる。すなわち、ロボット１の現在位置を表す点P_iか
ら障害物の接点方向に進んで、目標にもっとも近くなる
点P_i+1を選ぶ。その点について同じ操作を再帰的に繰り
返すことで、初期位置から目標位置に到る障害物２と衝
突しないロボット１の経路データP₁P₂,……P_Gが求ま
る。この経路データを経路データ格納手段９に収納す
る。

駆動手段３は、特定の時間間隔で経路データ格納手段
９から経路データ例 P_i＝（θ_i1,θ_i2,θ_i3） …（３） を読み出し、ロボット１の動作量がθ_i1,θ_i2,θ_i3に等
しくなるようにロボット１を動作させる。

このようにして、ロボット１は周囲の障害物を自律的
に回避して目標に到達する。

次に、本実施例の主要部をなし、地図16を作成する第
１の動作について説明する。

ロボット１と障害物２との衝突の有無の干渉チェック
は、ロボット１と障害物２の形状が複雑な場合、安定化
が困難であり非常に難しい。このため通常、ロボット１
や障害物２を複数の物体要素と呼ぶ複数の多面体で近似
し、ロボット１の物体要素と障害物２の物体要素との間
の干渉チェックを行ない、ロボット１が障害物２と衝突
するかしないかを判定する必要がある。

また、ロボット１と障害物２の一物体要素間の干渉チ
ェックだけでは、必ずしもロボット１と障害物２が衝突
するかしないかを確定できない場合がある。そこでロボ
ット１と障害物２の一物体要素間の衝突判定結果をミク
ロ判定、ロボット１と障害物２のすべての物体要素に体
する衝突判定結果をマクロ判定結果を呼ぶことにする。

地図16を得るため、各セルに対してすべての物体要素
間で干渉チェックをしていたのでは、衝突判定に膨大な
計算時間が必要になる。

本実施例は、この問題点を解決するため次の２つの方
法を提案する。以下、その基本的な考え方を説明する。
第一の方法は、第３図のセル１においてマクロ判定結果
が『衝突』である場合、次のセルを如何に選び、かつマ
クロ判定を如何に高速で引き出すかに関するものであ
る。多数のミクロ判定のうち１つでも『衝突』があれ
ば、マクロ判定を『衝突』と決定できるので、早く『衝
突』のマクロ判定を得るため、早く『衝突』のミクロ判
定を持つ物体要素間の干渉チェックをすることが重要に
なる。このため、次に干渉チェックするセルとして、直
前に干渉チェックを完了したロボット位置に近いロボッ
ト位置を示すセルを選ぶ。その理由は両セルのロボット
の位置は近似しているため、今干渉チェックしたセルで
ある物体要素間で衝突すれば、次のセルでも同じ物体要
素間で衝突する確率が高いからである。したがって、次
のセルとしてはセル１の近傍のセルを選び、衝突という
ミクロ判定結果をもたらした物体要素間から干渉チェッ
クを行なう。如何、前記近傍のセルに、前記した衝突確
率が最も高い隣接セルを選んだ場合で説明する。

第２の方法は、干渉チェック方法に関するものであ
る。第１の方法はセル１の判定結果が『衝突』の場合に
有効である。セル１の判定結果が『非衝突』の場合、す
べての物体要素間の干渉チェックをする必要がある。既
存の干渉チェック法の中には、大局的に衝突かつ非衝突
かを判定するもの、局部的に判定するもの及び物体要素
が特定の位置関係にある場合のみに有効なものなどがあ
る。従って、これらの干渉チェック法としてロボット１
と障害物２の干渉状態を指標に、その干渉状態に適した
干渉チェック法を用いることにより、全体として効率の
良い干渉チェックを実行できる。

以下に、説明する本実施例の具体例では、ロボット１
と障害物２の干渉状態を検出するのに、物体要素間や物
体要素を形成する点・面の間の距離と衝突判定結果とを
用いる。

まず、データベース８に格納するロボット１と障害物
２の幾何データの構造を説明する。通常、ロボット１や
障害物２の幾何データは、これらを複数の多面体に分割
し記述する。

以下、説明を簡単にするため、第４図に示すようなロ
ボット１が９個の多面体31〜39で、障害物２が３個の多
面体41〜43で形成されている場合の衝突判定について説
明するが、衝突判定する対象はこれに限定されるもので
はない。これらの多面体31〜39及び41〜43を第５図に示
すように、これらを形成する面23、頂点24と辺25のデー
タを記述し、それらの面23、頂点24や辺25が所属する多
面体のアドレスデータと共に、幾何データとしてデータ
ベース８に格納する。このとき、障害物２は互いに隣接
して接触する障害物同士を一つの群とみなし障害物群21
に分類する。この例では41と42とを一つの群とし、43を
もう一つの群とする。これらの多面体を形成する面23と
頂点24などのデータは、それぞれが所属する障害物群21
と多面体31〜39及び41〜43のアドレスデータと共に、幾
何データとしてデータベース８に記憶する。

データベース８に格納する頂点24の幾何データｐ及び
面23の幾何データｓは、４次元ベクトルで次式のように
表す。

ｐ＝（x,y,z,1） …（４） ｓ＝（a,b,c,d） …（５） ここに、x,y,zは、頂点ｐのx,y,z座標値であり、a,b,
cは単位ベクトルであり、 a²＋b²＋b²＝１ …（６） を満足する。ｄは原点と面23との距離であり、面23を表
す方程式 ax＋by＋cz＋ｄ＝０ …（７） を満足する。

次に、物体要素間の距離について説明する。物体要
素、すなわち多面体を形成する面23と頂点24との距離Ｌ
は、式（４），（５）を用いて次式で表される。

Ｌ＝ｓ・ｐ …（８） ここに、式（８）はベクトルの内積を表す。本実施例
では、頂点24が多面体の内部に存在する時は距離Ｌが負
になるように、すなわち面23を規定する法線ベクトルが
外側を向くように定義する。また、多面体間の距離及び
多面体群間の距離は、例えばそれらの幾何学的重心間の
距離として表す。

次に、本発明の主要部である第２動作の機能と動作に
ついて、地図16の作成手順を使って説明する。第６図は
地図作成手順を示すフローチャートである。

（Ａ）初期設定 （ｉ）初期値設定 干渉チェック実行手段15は、最初に初期状態を設定す
る。すなわち、地図記憶手段７に第３図に示すような干
渉判定が未処理の地図を作成させる。第７図は本実施例
と従来技術との干渉判定の実行順位表の一例を示したも
のである。干渉判定の実行順位の初期値は、第７図の本
発明の欄のセル１の欄に示すように、ロボット（マニピ
ュレータ）１と障害物の物体要素間の組み合わせに対し
て、それらの物体要素がデータベースに登録されている
順に設定され、第２図に示す実行順位表18に記憶され
る。

また、ロボットの多面体群と障害物の多面体群間の組
み合わせ、同じく多面体間の組み合わせ、同じく点と面
との間の組み合わせのそれぞれについても、第２図に示
す衝突判定表20と距離マップ19とを作成する。更に、そ
れらの表の初期値として、衝突判定表20には非干渉、距
離マップ18には∞を設定し、干渉チェック用記憶手段10
に記憶する。

（ii）幾何データの一時記憶 次に、干渉チェック実行手段15は、この判定未処理の
地図16から最初のセルを選び、このセルの中心の位置Ｐ
（θ₁,θ₂,θ_３）を地図16から読み出す。また、干渉チ
ェック実行手段15は、データベース８から、ロボット１
の幾何データであるロボット11を形成する多面体31〜39
の頂点24の位置ベクトルｐと面23の法線ベクトルｓとを
読み出す。これらのデータは、ロボット固有の座標系で
記述されており、障害物は世界座標系で記述されている
ため、両者の干渉測定には、どちらかの座標系に一致さ
せる必要がある。干渉チェック実行手段15は、ロボット
のデータp,sを世界座表系の値p,^wsに直す。この処理は
次式で表される。^w p＝ｐ＊Ｔ（θ₁,θ₂,θ_３） ……（９）^w s＝ｓ＊Ｔ（θ₁,θ₂,θ_３）^-1 ……（10） 但し、Ｔ（θ₁,θ₂,θ_３）は、ロボット固有の座標系
から世界座標系への座標変換行列であり、ロボットの位
置を表す３変数θ₁,θ₂,θ_３の関数である。

この座標変換されたロボットの幾何データは、干渉チ
ェック用記憶手段10内の幾何データ一時記憶手段17に記
憶する。

（Ｂ）判定方法の選択 方法選定手段12は、干渉チェック記憶手段10内にある
衝突判定表20から、前回のマクロ判定結果を読み、その
値に応じて、方法格納手段14にある干渉チェック法14a
〜14dのいずれかを選び、選んだ方法を干渉チェック実
行手段15の指定する。選択方法の一例を、第６図の
（ａ）判定方法の優先順位に示す。この例では、前回の
マクロ判定が干渉の場合、 （ａ）包含点チェック法14a （ｂ）境界球チェック法14b （ｃ）分離面チェック法14c （ｄ）辺・面チェック法14d の順に干渉チェック法を選択する。前回のマクロ判定が
非干渉の場合、 （ｂ）→（ｃ）→（ａ）→（ｄ） の順に干渉チェック法を選択する。以下にこれらの干渉
チェック法を詳細に説明する。

（ａ）包含点チェック法14a これは、いずれか一方の多面体を形成するすべての面
の内側に、もう一方の多面体を形成する点が存在するか
否かを判定する方法である。具体的な処理は、次の通り
である。

∃Pj:Si・Pj＜０（∀Si） ……（11） この方法では、包含点が存在すれば干渉と判定される
が、包含点が存在しない場合、干渉の有無を判定できな
いため、他の方法でチェックする必要がある。

（ｂ）境界球チェック法14b この方法は、各々の多面体を各々すべて含む境界球で
各々近似し、その境界球の中心間距離が、両境界球の半
径の和より、大きいか否かを判定する。もし、大きい場
合は、両多面体は干渉していないと言える。この判定法
は、境界球が干渉している場合、両多面体間の干渉の有
無を判定できないため、他の方法で干渉チェックする必
要がある。

（ｃ）分離面チェック法14c この方法は、いずれか一方の多面体を形成するすべて
の点が、もう一方の多面体を形成する面に対して、外側
（＋の側）になるような面（分離面と呼ぶ）が存在して
いるか否かを判定する。具体的な処理は次の通りであ
る。

∃Sj:Sj・Pi＞０（∀Pi） ……（12） が成立すれば、両多面体は非干渉である。成立しなけれ
ば、両多面体の干渉の有無を判定できないため、他の方
法で干渉チェックする必要がある。

（ｄ）辺・面チェック法14d 一方の多面体を形成する辺と他の一方の多面体を形成
する面との干渉チェックをする辺・面チェック法14dを
選択する。この計算方法は種々考えられるが、（ａ）〜
（ｃ）の方法に比べて計算が複雑になるため、ここでは
詳細な説明は省略する。

（ｃ）物体要素ペアの選択 干渉チェック実行手段15は、実行順位表18内にある物
体要素ペアの優先順位表18bから、まだ選択されていな
いロボット及び障害物の物体要素を組み合わせた物体要
素ペアの中から最上位のロボットと障害物の物体要素ペ
アの組み合わせを選ぶ。ここで、「まだ選択されていな
い物体要素ペア」とは、今回、干渉チェックしようとす
るロボットの位置・姿勢で、指定された干渉チェック法
に対してまだ選択されていない物体要素ペアをいう。こ
の「まだ選択されていない物体要素ペア」を捜すため、
現在選択されている優先順位を記憶し、次回選択時にそ
の次の順位を選ぶ。この物体要素ペアの選択は、多面体
群間、多面体間及び多面体を形成する辺・面の順に階層
的に作成する。

尚、この物体要素ペアの優先順位の修正は、第６図の
（ｂ）に示すように、後述の（Ｈ）優先順位修正におい
て、距離マップ19と前回のマクロ判定の値に応じて実行
される。

（Ｄ）面・頂点の選択 干渉チェック手段６は、（Ｂ）判定方法の選択で選択
された干渉チェック法が「b.境界球チェック法」でない
場合（ｃ）物体要素ペアの選択で指定された物再要素ペ
アを構成する面・頂点を優先順位表18cから選択する。
ここで選択される面・頂点は、それぞれ（ｃ）の干渉チ
ェック法で選ばれる分離面Sj,（ａ）の干渉チェック法
で選ばれる包含点Pjである。

この融点順位表18は、第６図の（ｃ）に示すように、
後述の（Ｈ）優先順位修正において、前回のマクロ判定
と、距離マップ19の値に応じて修正される。

（Ｅ）干渉チェック 干渉チェック実行手段15は、（Ｃ）で選択された物体
要素ペア（さらには（Ｄ）で選択された面・頂点）に対
して、（Ｂ）で指定された干渉チェック方法によって、
干渉チェックを実行する。判定結果はミクロ判定とし
て、衝突判定表20に記憶する。また次回の干渉チェック
や、優先順位の修正に利用するために、面Siと頂点Pjの
距離Si・Pjを干渉チェック用記憶手段10の距離マップ19
に記憶する。

（Ｆ）完了判定 今回チェックしたミクロ判定が干渉の場合、ロボット
と障害物が干渉しているため、マクロ判定を干渉に設定
し、後述の（Ｇ）地図に登録へ進む。

ミクロ判定が非干渉の場合、すべての物体要素ベアの
ミクロ判定が非干渉の場合にだけ、マクロ判定を非干渉
に設定し、後述の（Ｇ）に進む。具体的には、今干渉チ
ェックしようとするロボットの位置・姿勢で、ロボット
と障害物を構成する物体要素ペアの中に、干渉チェック
されていない物体要素ペアがあるか否かを判定し、もし
あれば（Ｂ）に戻り、なければマクロ判定を非干渉に設
定し、（Ｇ）に進む。

ミクロ判定が干渉か非干渉か分からない場合、次の判
定方法を選ぶため（Ｂ）に戻る。

（Ｇ）地図に登録 地図記憶手段７は、今回干渉チェックしたロボット位
置・姿勢に対応する座標に関するマクロ判定結果を地図
16に記述する。

（Ｈ）優先順位修正 順位選定手段11は、実行順位表18にある判定方法、物
体要素ペア及び面・頂点の優先順位を修正する。

順位選定手段11は、最初に判定方法の優先順位を修正
する。その手順は既に（Ｂ）判定方法の選択で述べたよ
うに、順位選定手段11がマクロ判定を読み、その値に応
じて第６図の（ａ）に示すように修正する。

すなわち、 マクロ判定＝干渉の場合 ａ→ｂ→ｃ→ｄ→ マクロ判定＝非干渉の場合 ｂ→ａ→ｃ→ｄ ここに、a:包含点チェック法 b:境界球チェック法 c:分離面チェック法 d:辺・チェック法 次に、順位選定手段11は、物体要素ペアの優先順位を
修正する。順位選定手段11は、物体要素ペアのミクロ判
定結果を衝突判定表20から読み出し、その値に応じて、
第６図の（ｂ）に示す手順で物体要素ペアの優先順位を
修正する。すなわち、 マクロ判定＝干渉の場合 ａ′→ｂ′→ｃ′→ｄ′→ｅ′→ｆ′ マクロ判定＝非干渉の場合 ｆ′→ｅ′→ｄ′→ｃ′→ｂ′→ａ′ ここに、ａ′〜ｆ′は以下に示す物体要素ペアの干渉状
態を表す。

ａ′，物体要素包含 一方の物体要素が他方の物体要素を包含している状態
で物体要素ペアが最も近接している状態を指す。

ｂ′，包含点存在 一方の物体要素の少なくとも一頂点が、他方の物体要
素に包含されて干渉している状態を指す。

ｃ′，辺・面干渉 ２つの物体要素のどの頂点も他方の物体要素に含まれ
ていないが、一方の物体要素を形成する辺が他方の物体
要素を形成する面と交差して干渉している状態を指す。

ｄ′，辺・面非干渉 互いの物体要素を分離する平面は存在しない状態にあ
るが、互いの物体要素を形成するどの辺も他方の物体要
素を形成する面に交差しないで非干渉な状態を指す。

ｅ′，分離面存在 ２つの物体要素を分離する平面が少なくとも一つ存在
し、非干渉な状態を指す。

ｆ′，境界球分離 物体要素を包含する球同士が干渉していない非干渉状
態を指す。

次に順位選定手段11は面・頂点の融点順位を修正す
る。順位選定手段11は距離マップ19から面・頂点間の距
離を読み出し、且つ衝突判定表20からその面・頂点が所
属する物体要素ペアのミクロ判定を読み出し、これらの
値を用いて面・頂点の優先順位を第６図の（ｃ）に示す
手順で修正する。

すなわち、 ミクロ判定＝干渉の場合： 包含点である頂点を最上位に、以下、距離の短い頂点
から優先順位を付け最も遠いものを最下位にする。

ミクロ判定＝不干渉の場合： 分離面があれば、その面が最上位なるように修正し、
次に距離の遠い面から順に優先順位を付け最も近い面を
最下位する。

（Ｊ）次回のロボットの位置・姿勢 次回干渉チェックするセルを以下の手順で選ぶ。今回
選んだロボットの位置・姿勢に対応する地図16上のセル
に最隣接で且つ未処理のセルを選ぶ。もし、該当するセ
ルがなければ、地図は完成していることを経路探索手段
５に知らせた後、干渉チェックを終了する。

該当するセルがある場合、（Ａ）（ii）と同様に、セ
ルの中心となるロボット位置・姿勢に対するロボット１
全物体要素の幾何データを、式（９）〜（10）によって
座標変換し、幾何データ一時記憶手段17に記憶する。次
に（Ｂ）判定方法の選択に戻る。

以上述べた優先順位の修正処理により、地図16内のセ
ルに対し、干渉チェックを順番に実行していくときの効
果を、ロボット１と障害物２とが第４図の位置関係にあ
る例を用いて説明する。本発明の実施例の実行順位表18
の変化を従来例と共に第７図に示す。従来例及び本実施
例とも、セル１、すなわち、前回のロボット位置・姿勢
での実行順位は同じである。セル１の干渉チェックにお
いて、実行順位18、すなわち、ロボットの多面体39と障
害物の多面体42とが衝突すると判定されたと仮定する。
この場合、セル２、すなわち今回のロボット位置・姿勢
における干渉チェック実行順位は、従来例では不変であ
るが、本実施例では、ロボットの多面体39と障害物の多
面体42との組合せの実行順位が第１位になっている。本
実施例では第２位以下の実行順位は、セル１において干
渉チェックが完了している組合せのうち距離の小さい順
になっている。例えば第７図のセル２の41と42の頃のよ
うに修正される。またセル２の43は、干渉チェックが未
処理であるため、セル１と同じ実行順位のままである。
もし、干渉がセル２でも起るとすれば、最もその可能性
の高いものは、前回衝突した組合せ、すなわち、ロボッ
トの物体要素39と障害物の物体要素43である。実施例
は、前回干渉した多面体の干渉チェックが第１番目に実
行されるため、もし干渉が再びこの組合せで実行される
とすると、第１回目の物体要素ペアの選択でマクロ判定
が定まり、次のセルを選択できる。一方、従来例では18
回の物体要素ペアを選択、干渉チェック後にマクロ判定
が定まる。次に干渉チェックを実行するセルは、今回の
セルの最隣接であるため、もし干渉しなかった場合も、
距離データはほぼ同じであると近似できるので、前回計
算した距離が近いものから次の多面体の組合せを選択で
きるため、極めて効率良く干渉チェックが実行できる。

更に、物体要素ペアの前回、すなわちセル１における
干渉状態が包含点存在の場合、従来例ではあらかじめ決
められた順に方法選択、物体要素ペア及び面・頂点の選
択が行われるため実行順序は不変であるのに対し、本実
施例では包含点チェック法が最初に選ばれ、且つ前回の
干渉チェックで包含点と判定された頂点から包含線チェ
ックが実行される。したがって、セル２でもセル１と同
じ頂点が包含点になれば、本実施例は第１番目の干渉チ
ェックでマクロ判定が定まり、従来例に比べて干渉チェ
ックに到る手順が最小化する。セル１とセル２は最も近
接しているため、頂点が引き続き包含点である確率はき
わめて高い。たとえ、その頂点が包含点でなかった場合
にも、次善の策として頂点と面との距離の近い順に頂点
が選ばれるため、セル２において干渉する確率が高い順
に干渉チェックされる。

一方、ロボット１がセル１の姿勢で障害物２と非干渉
の位置関係にあり、これらの物体要素に対して分離面が
存在する場合を考える。このケースでは、従来例におけ
るセル１とセル２におけるロボット位置・姿勢での干渉
チェックは前記の干渉する例と同様に、干渉チェック
法、物体要素ペア、面・頂点の実行順序は不変である。
本実施例では、セル１におけるロボット位置・姿勢での
干渉チェック手順は従来例と同じである。セル２におけ
るロボット位置・姿勢での物体要素ペアを形成する面
は、セル１で分離面となった面が最初に選択されるた
め、引き続き同じ面が分離面になる場合、マクロ判定が
定まるために必要な干渉チェックは一回で済む。また、
選ばれた面が分離面でない場合、次善の策として頂点と
面との距離が最も大きいものを選ぶ。この選択では選ば
れた面が分離面になる確率は、他の面より高いため、こ
の判定でマクロ判定が確定する可能性が高い。以下、距
離の遠い順に面を選択していくため、従来例に比べて分
離面を速く見つけることができ、マクロ判定確定に要す
る干渉チェック回数が最小化する。

以上詳しく述べたように本実施例によれば、干渉チェ
ックに要する干渉チェック回数が最小化するため、効率
良く干渉チェックが実行でき、地図作成が高速化する。

〔発明の効果〕

本発明の移動体の衝突判定方法によれば、マニピュレ
ータ（移動体）と障害物を形成する多面体（物体要素）
の中から距離の最も近い組合せを優先して干渉チェック
を実行するため、衝突の可能性の高い組合せから干渉チ
ェックが実行でき、衝突判定に至る計算処理時間が半減
する効果ある。

【図面の簡単な説明】

第１図は本発明の一実施例を示す構成図、第２図は干渉
チェック記憶手段の内部を説明する図、第３図はマニピ
ュレータ（移動体）の関節空間を説明する図、第４図は
本発明の一実施例を説明するマニピュレータと障害物と
の配置を示す図、第５図は物体要素を説明する図、第６
図は本発明の主要部を説明するフローチャート、第７図
は本実施例と従来技術との比較を説明する図である。 １……マニピュレータ（移動体）、 ２……障害物、 ６……干渉チェック手段。

───────────────────────────────────────────────────── フロントページの続き (71)出願人 999999999 中国電力株式会社 広島県広島市中区小町４番33号 (71)出願人 999999999 日本原子力発電株式会社 東京都千代田区大手町１丁目６番１号 (71)出願人 999999999 株式会社東芝 神奈川県川崎市幸区堀川町72番地 (74)上記７名の代理人 弁理士 鵜沼 辰之 （外１名 ） (72)発明者 鈴木 正憲 茨城県日立市森山町1168番地 株式会社日 立製作所エネルギー研究所内 (72)発明者 山本 晋児 東京都千代田区内幸町１丁目１番３号 東 京電力株式会社内 (72)発明者 桂沢 昇 宮城県仙台市青葉区一番町３丁目７番１号 東北電力株式会社内 (72)発明者 吉田 博 愛知県名古屋市東区東新町１番地 中部電 力株式会社内 (72)発明者 森本 英光 富山県富山市牛島町15番１号 北陸電力株 式会社内 (72)発明者 河村 吉之助 東京都千代田区大手町１丁目６番１号 日 本原子力発電株式会社内 (72)発明者 阿部 朗 神奈川県横浜市磯子区新杉田８番地 株式 会社東芝横浜事業所内 審査官 中村 則夫 (56)参考文献 特開 平１−315802（ＪＰ，Ａ) 特開 平１−73205（ＪＰ，Ａ)

Claims (17)

【特許請求の範囲】
1. 【請求項１】移動体と障害物のそれぞれの形状を複数の
   物体要素で近似し、それぞれの物体要素の間を干渉チェ
   ックして前記移動体と前記障害物との間の衝突の有無を
   判定する移動体の衝突判定方法において、干渉チェック
   を行う今回の前記移動体の位置・姿勢で該移動体と前記
   障害物との間の衝突判定をする際、前回の前記移動体の
   位置・姿勢で得られた衝突判定結果と、少なくとも距離
   を含む衝突判定に必要なデータとにより衝突の可能性の
   高い順にそれぞれの物体要素を組合せ、干渉チェックの
   実行手順を決定することを特徴とする移動体の衝突判定
   方法。
2. 【請求項２】干渉チェックの実行手順は、干渉チェック
   に要する時間と回数とを最小限ににして決定されること
   を特徴とする請求項１記載の移動体の衝突判定方法。
3. 【請求項３】干渉チェックの実行手順は、今回の移動体
   の位置・姿勢におけるそれぞれの物体要素間の干渉確率
   を算出し該干渉確率に基づいて決定されることを特徴と
   する請求項１又は２記載の移動体の衝突判定方法。
4. 【請求項４】衝突判定に必要なデータは、今回の移動体
   のマニピュレータ姿勢で得られたデータが含まれている
   ことを特徴とする請求項1,2,3又は４記載の移動体の衝
   突判定方法。
5. 【請求項５】干渉チェックの実行手順は、それぞれの物
   体要素間を干渉チェックする実行順位と、複数の干渉チ
   ェック法から選択される少くとも一つの干渉チェック法
   を決定することを特徴とする請求項１〜４のいずれか１
   項記載の移動体の衝突判定方法。
6. 【請求項６】今回の移動体の位置・姿勢は、前回干渉チ
   ェックした前記移動体の位置・姿勢のうち直前に干渉チ
   ェックした前記移動体の位置・姿勢に隣接又は近似して
   決定されること特徴とする請求項１〜５いずれか１項記
   載の移動体の衝突判定方法。
7. 【請求項７】前回の移動体の位置・姿勢を、干渉チェッ
   クを行う今回の該移動体の位置・姿勢の近傍とすること
   を特徴とする請求項１〜５のいずれか１項記載の移動体
   の衝突判定方法。
8. 【請求項８】干渉チェックの実行手順は、前回干渉チェ
   ックした移動体の位置・姿勢のうち直前に干渉チェック
   した前回の前記移動体の位置・姿勢でかつ衝突と判定さ
   れたそれぞれの物体要素間の干渉チェックを最上位にし
   て決定されることを特徴とする請求項１〜５のいずれか
   １項記載の移動体の衝突判定方法。
9. 【請求項９】干渉チェックの実行手順は、最上位につぐ
   実行順位を、前回の移動体の位置・姿勢でかつそれぞれ
   の物体要素間の距離が小さい順に決定することを特徴と
   する請求項１〜５又は８記載のいずれか１項記載の移動
   体の衝突判定方法。
10. 【請求項１０】干渉チェックの実行手順は、それぞれの
    物体要素間の衝突判定結果が衝突の際は衝突判定に適し
    た干渉チェック法を選定し、前記衝突判定結果が非衝突
    判定の際は非衝突判定に適した干渉チェック法を選定す
    ることを特徴とする請求項１〜５のいずれか１項記載の
    移動体の衝突判定方法。
11. 【請求項１１】干渉チェックの実行手順は、干渉チェッ
    クの実行順位の決定と干渉チェック法の選択とを並列し
    て実行させることを特徴とする請求項１〜５のいずれか
    １項記載の移動体の衝突判定方法。
12. 【請求項１２】それぞれの物体要素を、移動体及び障害
    物のそれぞれの属性に応じて少くとも一つの物体要素群
    に階層的に分類し、干渉チェックの実行順位の決定を階
    層ごとに行うことを特徴とする請求項１〜５及び8,9の
    いずれか１項記載の移動体の衝突判定方法。
13. 【請求項１３】請求項１〜12のいずれか１項記載の移動
    体の衝突判定方法を用いて得られた衝突判定結果を利用
    し、移動体の少くとも２次元空間の移動軌跡を探索する
    ことを特徴とする移動体の軌道探索方法。
14. 【請求項１４】請求項１〜12のいずれか１項記載の移動
    体の衝突判定方法を書き込んだROMを有することを特徴
    とするマイクロコンピュータ。
15. 【請求項１５】移動体と障害物のそれぞれを複数の物体
    要素で近似し、それぞれの物体要素間を干渉チェックし
    て前記移動体と前記障害物との間の衝突の有無を判定す
    る手段を備えた移動体の衝突判定装置において、前回の
    前記移動体の位置・姿勢でそれぞれの物体要素間を干渉
    チェックし得られた衝突判定結果と少なくとも距離を含
    む衝突判定に必要なデータとを記憶しかつその記憶内容
    を出力する記憶手段と、該記憶手段を利用して干渉チェ
    ックを行う今回の前記移動体の位置・姿勢で衝突の可能
    性の高い順にそれぞれの物体要素を組合せ干渉チェック
    の実行手順を決定する手段とを備えたことを特徴とする
    移動体の衝突判定装置。
16. 【請求項１６】衝突判定に必要なデータに、今回の移動
    体の位置・姿勢で得られた衝突判定に必要なデータを含
    むことを特徴とする請求項15記載の移動体の衝突判定装
    置。
17. 【請求項１７】衝突判定結果と衝突判定に必要なデータ
    とにより、今回の移動体の位置・姿勢で該移動体と障害
    物との間の衝突の有無の干渉チェックと並列に、次回の
    前記移動体の位置・姿勢における干渉チェックの実行手
    順を決定する手段を備えたことを特徴とする請求項15記
    載の衝突判定装置。