JP4811805B2 - 動的衝突予測方法 - Google Patents

Description

この発明は動的衝突予測方法に関するものであり、より詳しくは自己および自己以外の対象物（障害物）の動静に関係なく２つのカメラの各対応画素の速度対を用いることにより自己と対象物との衝突の有無と、衝突が予測された場合にはその位置とその時刻を予測する方法に関するものである。

この明細書において「自己」とは対象物との衝突を回避したい主体、例えばロボットや車両などを言う。また「眼」とは自己上に設けられた対象物の視覚的観測手段で、一般的にはロボットや車両に搭載されたカメラなどを言うものである。

自己と対象物との衝突を動的に予測して衝突を回避する方法としては、本出願人による特開２００５−１８２１２１号公報に開示された同名称の発明（以下従来技術を言う）がある。

この従来技術においては、（１）水平方向に離間配置された２個の眼（左眼、右眼）を用い、（２）左眼と右眼とで個別に対象物の速度（自己に対する相対速度）を観測し、（３）この観測された速度から３通りの条件式の値を求めて、それらの条件式がすべて満足された場合に「衝突する」と予測するものである。

この従来技術の場合、両眼間のどこかに衝突することのみ判定するが、自己全体のどこかに衝突することまでは判定できなかった。また、正確にどの位置に衝突するかを計算できていなかった。また、衝突時刻を知るために視野内の各点の３次元位置計測を時刻（ｔ）と（ｔ＋Δｔ）の２回行う必要があったが、今回は、時刻（ｔ）のみ１回で正確な予測ができている。
特開２００５−１８２１２１号公報

しかし複数回数の３次元位置観測を行うことはそれだけ時間と手間とを要することになり、条件式に関連する演算処理回数も多くなるという欠点がある。移動体にとっては、すばやく正確に判断することは最も重要な機能である。

かかる従来技術の現状に鑑みてこの発明の目的は、動的衝突予測を従来技術より短い処理時間と少ない手間で、かつ演算処理回数を低減することのみならず、衝突時刻や自己のどこに衝突するかの正確な位置予測をすることおよび自己からどのくらい離れて対象物が通過するかを予測することにある。

このためこの発明においては、移動平面に平行な基準平面上に２個の眼（カメラ）を光軸を平行にして離間配置し、時間軸上に所定の時間間隔（Δｔ）で選ばれた複数の時刻の内の１つの時刻（ｔ）における右カメラの各画素の両眼視差法による３次元位置と各カメラの各画素における時刻（ｔ）から時刻（ｔ＋Δｔ）間の左右方向の速度対（δ_R，δ_Ｌ）とを観測し、次の式から衝突位置Ｘ _Ｔ を算出し、

ここでαは１画素当りの視角であり、δ _Ｌ は対象物の左眼上での対象画素の移動画素数（左眼速度）であり、δ _Ｒ は対象物の右眼上での対象画素の移動画素数（右眼速度）であり、次いで、次の条件式が成立したときに「衝突する」ことを予測し、

ここでａは左右の眼の間の距離であり、ｂは右眼と自己の右端との距離および左眼と自己の左端との距離であり、Ｚ _０ は時刻（ｔ＋Δｔ）における対象画素のＺ方向の距離であることを要旨とする。

この発明によれば１回の両眼視差による距離測定と１回の速度対測定で衝突の完全な予測が可能となるので、動的衝突予測を時間的に短縮しかつ演算処理を簡単にすることができるとともに対象物が両眼を結ぶ直線上のどこを通過するかを正確に予測できるので自己のどこに何時に衝突するかを正確に予測できる。

以下の事例の説明においては、例えば右に向かう速度を正、左に向かう速度を負と符号を定めるが、勿論この逆であってもよい。また模型図においてはＸ方向とは両眼を結ぶ直線方向を言い、Ｚ方向とはカメラの光軸方向（奥行方向）を言う。

図１は、場面の平面図を示し、図１に示す場合において各符号はつぎの意味をもつものである。下部の四角形は自己を示し、Ｐは対象物を示す。Ｐ₀は対象物の時刻ｔにおける位置であり座標（Ｘ₀、Ｚ₀）で表わされる。Ｐは対象物の時刻（ｔ＋Δｔ）における位置であり座標（Ｘ、Ｚ）で表わされる。Ｐ_Lは点Ｐ₀を通りＸ軸と平行な直線と点Ｐと左眼（Ｌ）を結ぶ直線との交点である。同様にＰ_Rは点Ｐ₀を通りＸ軸と平行な直線と点Ｐと右眼（Ｒ）を結ぶ直線との交点である。

Ｚ₀は時刻ｔにおける対象物のＺ方向の距離であり、Ｚは時刻（ｔ＋Δｔ）における対象物のＺ方向の距離である。

ａは左右の眼の間の距離（一例を挙げると１２ｃｍ）である。ｂは右眼と右端間および左眼と左端間の距離である。Ｒは右眼でありＬは左眼である。αは１画素当りの視角（一例を挙げると目視角７０度が３２０画素に相当する。単位はラジアンで示す。）である。δ_Lは点Ｐの左眼上での移動画素数（左眼速度）であり、δ_Rは右眼上での移動画素数（右眼速度）である。

さらにＸ_Ｔとは衝突予測点であり、直線ＬＲと直線Ｐ_０Ｐとの交点であり、次の式で計算される。

衝突するための条件は次の式で定義される。

また衝突時間Ｔは次の式で計算される。

上記の条件式（１）、（２）が成立したときには衝突が予測され、自己は最近に衝突する対象物（Ｔが最も小さい対象物）を避ける方向に移動する。つまり右視野にある対象物なら左前方へ、左視野にある対象物なら右前方に移動するのである。ちなみに、距離が近づいてくる対象物が衝突しない場合の離れ具合は、ｍｉｎ［｜Ｘ_T−ｂ｜，｜−（ａ+ｂ）−Ｘ_T｜］で求まる。左項は右側通過、右項は左側通過に対応する。

以下図２〜５に示す種々の事例について衝突の有無を予測する。

図２に示す事例１の場合には、対象物は右方に移動する。この場合にはｔａｎαδ_R＞０でｔａｎαδ_L＞０となる。しかもδ_R＝δ_Lであるから、Ｘ_T＝∞となり、条件式（１）が成立しない。すなわち「衝突しない」と予測されるのである。

図３に示す事例２の場合には、対象物は左方に移動する。この場合にはｔａｎαδ_R＜０でｔａｎαδ_L＜０となる。しかもδ_R＝δ_Lであるから、Ｘ_T＝−∞となり、条件式（１）が成立しない。すなわち「衝突しない」と予測されるのである。

図４に示す事例３の場合には対象物が自己に向かってＺ方向に接近してくる。この場合にはｔａｎαδ_R＜０でｔａｎαδ_L＞０となる。しかもδ_R＝−δ_Lであるから、Ｘ_T＝−ａ／２となり条件式（１）、（２）ともに成立する。すなわち「衝突する」と予測されるのである。

図５に示す事例４の場合には対象物が自己からＺ方向に離間する。この場合にはＺ₀−Ｚ＜０となり、条件式（２）が成立しない。すなわち「衝突しない」と予測されるのである。

つぎに実際の実験例を示す。実験には東京精機（株）製作の移動プラットフォームシステムとＶｉｅｗＰｌｕｓ（株）製作の２眼カメラシステムを用いた。前方１００ｃｍにある対象物がΔｔ後に１０ｃｍ右方、左方、前方および後方に移動したとき（前記の事例１〜４に相当する）、対象物が右眼（カメラ）および左眼（カメラ）上を何画素移動したかを測定した結果を表１に示す。

この発明の応用はロボットや車両に限らず、航空機や船舶など三次元方向に移動するあらゆるタイプの主体の衝突予測に応用できるものである。

この発明による動的衝突予測の基本的メカニズムを説明するための模型図である。 事例１の場合の状態を示す模型図である。 事例２の場合の状態を示す模型図である。 事例３の場合の状態を示す模型図である。 事例４の場合の状態を示す模型図である。

Claims (2)

1. 移動平面に平行な基準平面上に２個の眼（カメラ）を光軸を平行にして離間配置し、
   時間軸上に所定の時間間隔（Δｔ）で選ばれた複数の時刻の内の１つの時刻（ｔ）における右カメラの各画素の両眼視差法による３次元位置と各カメラの各画素における時刻（ｔ）から時刻（ｔ＋Δｔ）間の左右方向の速度対（δ_R，δ_Ｌ）とを観測し、
   次の式から衝突位置Ｘ _Ｔ を算出し、
   

   

   ここでαは１画素当りの視角であり、
   δ _Ｌ は対象物の左眼上での対象画素の移動画素数（左眼速度）であり、
   δ _Ｒ は対象物の右眼上での対象画素の移動画素数（右眼速度）であり、
   次いで、次の条件式が成立したときに「衝突する」ことを予測し、
   

   

   ここでａは左右の眼の間の距離であり、
   ｂは右眼と自己の右端との距離および左眼と自己の左端との距離であり、
   Ｚ _０ は時刻（ｔ＋Δｔ）における対象画素のＺ方向の距離であることを特徴とする
   動的衝突予測方法。
2. さらに次の式により衝突予測時刻（Ｔ）を求める
   

   

   ことを特徴とする請求項１に記載の方法。

Images