動的衝突予測方法
【課題】動的衝突予測を従来技術より短い時間と少ない手間で、かつ演算処理回数を低減し、対象物の移動コースすなわち衝突位置を正確に予測する。
【解決手段】基準平面上に自己の２個の眼を離間配置し、時間軸上に所定の時間間隔（Δｔ）で選ばれた複数の時刻のうちのひとつの時刻（ｔ）において視野内にある対象物の３次元位置と時刻（ｔ）から（ｔ＋Δｔ）間における速度対を観測し、つぎの条件式が成立したときに「衝突する」と予測する。
−（ａ＋ｂ）≦ＸＴ≦ｂ （条件式１）
Ｚ０−Ｚ＞０ （条件式２）
ここでａ，ｂ，Ｚ０，Ｚは図参照
ＸＴ＝ａ ｔａｎ δＲ／（ｔａｎ α δＬ−ｔａｎ α δＲ）
ここでαは１画素当りの視角であり、δLは対象物の左眼上での移動画素数であり、δRは対象物の右眼上での移動画素数である。
【解決手段】基準平面上に自己の２個の眼を離間配置し、時間軸上に所定の時間間隔（Δｔ）で選ばれた複数の時刻のうちのひとつの時刻（ｔ）において視野内にある対象物の３次元位置と時刻（ｔ）から（ｔ＋Δｔ）間における速度対を観測し、つぎの条件式が成立したときに「衝突する」と予測する。
−（ａ＋ｂ）≦ＸＴ≦ｂ （条件式１）
Ｚ０−Ｚ＞０ （条件式２）
ここでａ，ｂ，Ｚ０，Ｚは図参照
ＸＴ＝ａ ｔａｎ δＲ／（ｔａｎ α δＬ−ｔａｎ α δＲ）
ここでαは１画素当りの視角であり、δLは対象物の左眼上での移動画素数であり、δRは対象物の右眼上での移動画素数である。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は動的衝突予測方法に関するものであり、より詳しくは自己および自己以外の対象物（障害物）の動静に関係なく２つのカメラの各対応画素の速度対を用いることにより自己と対象物との衝突の有無と、衝突が予測された場合にはその位置とその時刻を予測する方法に関するものである。
【背景技術】
【０００２】
この明細書において「自己」とは対象物との衝突を回避したい主体、例えばロボットや車両などを言う。また「眼」とは自己上に設けられた対象物の視覚的観測手段で、一般的にはロボットや車両に搭載されたカメラなどを言うものである。
【０００３】
自己と対象物との衝突を動的に予測して衝突を回避する方法としては、本出願人による特開２００５−１８２１２１号公報に開示された同名称の発明（以下従来技術を言う）がある。
【０００４】
この従来技術においては、（１）水平方向に離間配置された２個の眼（左眼、右眼）を用い、（２）左眼と右眼とで個別に対象物の速度（自己に対する相対速度）を観測し、（３）この観測された速度から３通りの条件式の値を求めて、それらの条件式がすべて満足された場合に「衝突する」と予測するものである。
【０００５】
この従来技術の場合、両眼間のどこかに衝突することのみ判定するが、自己全体のどこかに衝突することまでは判定できなかった。また、正確にどの位置に衝突するかを計算できていなかった。また、衝突時刻を知るために視野内の各点の３次元位置計測を時刻（ｔ）と（ｔ＋Δｔ）の２回行う必要があったが、今回は、時刻（ｔ）のみ１回で正確な予測ができている。
【特許文献１】特開２００５−１８２１２１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかし複数回数の３次元位置観測を行うことはそれだけ時間と手間とを要することになり、条件式に関連する演算処理回数も多くなるという欠点がある。移動体にとっては、すばやく正確に判断することは最も重要な機能である。
【０００７】
かかる従来技術の現状に鑑みてこの発明の目的は、動的衝突予測を従来技術より短い処理時間と少ない手間で、かつ演算処理回数を低減することのみならず、衝突時刻や自己のどこに衝突するかの正確な位置予測をすることおよび自己からどのくらい離れて対象物が通過するかを予測することにある。
【課題を解決するための手段】
【０００８】
このためこの発明においては、移動平面に平行な基準平面上に２個の眼（カメラ）を光軸を平行にして離間配置し、時間軸上に所定の時間間隔（Δｔ）で選ばれた複数の時刻の内の１つの時刻（ｔ）における右カメラの各画素の両眼視差法による３次元位置（Ｘ0，Ｙ0，Ｚ0）と各カメラの各画素における時刻（ｔ）から時刻（ｔ＋Δｔ）間の左右方向の速度対（δR，δL）を観測し、つぎの条件式が成立したときに「衝突する」と予測するものであり、
【数４】
ここでａは左右の眼の間の距離であり、ｂは右眼と自己の右端間および左眼と自己の左端間の距離であり、Ｚ0は時刻ｔにおける対象画素の奥行方向（Ｚ方向）の距離であり、Ｚは時刻（ｔ＋Δｔ）における対象画素のＺ方向の距離であり、次の式で得られる位置ＸTに衝突するものとすると、
【数５】
ここでαは１画素当りの視角であり、δLは対象物の左眼上での対象画素の移動画素数（左眼速度）であり、δRは対象画素の右眼上での移動画素数（右眼速度）であることを要旨とする。
【発明の効果】
【０００９】
この発明によれば１回の両眼視差による距離測定と１回の速度対測定で衝突の完全な予測が可能となるので、動的衝突予測を時間的に短縮しかつ演算処理を簡単にすることができるとともに対象物が両眼を結ぶ直線上のどこを通過するかを正確に予測できるので自己のどこに何時に衝突するかを正確に予測できる。
【実施例】
【００１０】
以下の事例の説明においては、例えば右に向かう速度を正、左に向かう速度を負と符号を定めるが、勿論この逆であってもよい。また模型図においてはＸ方向とは両眼を結ぶ直線方向を言い、Ｚ方向とはカメラの光軸方向（奥行方向）を言う。
【００１１】
図１は、場面の平面図を示し、図１に示す場合において各符号はつぎの意味をもつものである。下部の四角形は自己を示し、Ｐは対象物を示す。Ｐ0は対象物の時刻ｔにおける位置であり座標（Ｘ0、Ｚ0）で表わされる。Ｐは対象物の時刻（ｔ＋Δｔ）における位置であり座標（Ｘ、Ｚ）で表わされる。ＰLは点Ｐ0を通りＸ軸と平行な直線と点Ｐと左眼（Ｌ）を結ぶ直線との交点である。同様にＰRは点Ｐ0を通りＸ軸と平行な直線と点Ｐと右眼（Ｒ）を結ぶ直線との交点である。
【００１２】
Ｚ0は時刻ｔにおける対象物のＺ方向の距離であり、Ｚは時刻（ｔ＋Δｔ）における対象物のＺ方向の距離である。
【００１３】
ａは左右の眼の間の距離（一例を挙げると１２ｃｍ）である。ｂは右眼と右端間および左眼と左端間の距離である。Ｒは右眼でありＬは左眼である。αは１画素当りの視角（一例を挙げると目視角７０度が３２０画素に相当する。単位はラジアンで示す。）である。δLは点Ｐの左眼上での移動画素数（左眼速度）であり、δRは右眼上での移動画素数（右眼速度）である。
【００１４】
さらにＸTとは衝突予測点であり、直線ＬＲと直線Ｐ0Ｐとの交点であり、次の式で計算される。
【数６】
【００１５】
衝突するための条件は次の式で定義される。
【数７】
【００１６】
また衝突時間Ｔは次の式で計算される。
【数８】
【００１７】
上記の条件式（１）、（２）が成立したときには衝突が予測され、自己は最近に衝突する対象物（Ｔが最も小さい対象物）を避ける方向に移動する。つまり右視野にある対象物なら左前方へ、左視野にある対象物なら右前方に移動するのである。ちなみに、距離が近づいてくる対象物が衝突しない場合の離れ具合は、ｍｉｎ［｜ＸT−ｂ｜，｜−（ａ+ｂ）−ＸT｜］で求まる。左項は右側通過、右項は左側通過に対応する。
【００１８】
以下図２〜５に示す種々の事例について衝突の有無を予測する。
【００１９】
図２に示す事例１の場合には、対象物は右方に移動する。この場合にはｔａｎαδR＞０でｔａｎαδL＞０となる。しかもδR＝δLであるから、ＸT＝∞となり、条件式（１）が成立しない。すなわち「衝突しない」と予測されるのである。
【００２０】
図３に示す事例２の場合には、対象物は左方に移動する。この場合にはｔａｎαδR＜０でｔａｎαδL＜０となる。しかもδR＝δLであるから、ＸT＝−∞となり、条件式（１）が成立しない。すなわち「衝突しない」と予測されるのである。
【００２１】
図４に示す事例３の場合には対象物が自己に向かってＺ方向に接近してくる。この場合にはｔａｎαδR＜０でｔａｎαδL＞０となる。しかもδR＝−δLであるから、ＸT＝−ａ／２となり条件式（１）、（２）ともに成立する。すなわち「衝突する」と予測されるのである。
【００２２】
図５に示す事例４の場合には対象物が自己からＺ方向に離間する。この場合にはＺ0−Ｚ＜０となり、条件式（２）が成立しない。すなわち「衝突しない」と予測されるのである。
【００２３】
つぎに実際の実験例を示す。実験には東京精機（株）製作の移動プラットフォームシステムとＶｉｅｗＰｌｕｓ（株）製作の２眼カメラシステムを用いた。前方１００ｃｍにある対象物がΔｔ後に１０ｃｍ右方、左方、前方および後方に移動したとき（前記の事例１〜４に相当する）、対象物が右眼（カメラ）および左眼（カメラ）上を何画素移動したかを測定した結果を表１に示す。
【表１】
【産業上の利用可能性】
【００２４】
この発明の応用はロボットや車両に限らず、航空機や船舶など三次元方向に移動するあらゆるタイプの主体の衝突予測に応用できるものである。
【図面の簡単な説明】
【００２５】
【図１】この発明による動的衝突予測の基本的メカニズムを説明するための模型図である。
【図２】事例１の場合の状態を示す模型図である。
【図３】事例２の場合の状態を示す模型図である。
【図４】事例３の場合の状態を示す模型図である。
【図５】事例４の場合の状態を示す模型図である。
【技術分野】
【０００１】
この発明は動的衝突予測方法に関するものであり、より詳しくは自己および自己以外の対象物（障害物）の動静に関係なく２つのカメラの各対応画素の速度対を用いることにより自己と対象物との衝突の有無と、衝突が予測された場合にはその位置とその時刻を予測する方法に関するものである。
【背景技術】
【０００２】
この明細書において「自己」とは対象物との衝突を回避したい主体、例えばロボットや車両などを言う。また「眼」とは自己上に設けられた対象物の視覚的観測手段で、一般的にはロボットや車両に搭載されたカメラなどを言うものである。
【０００３】
自己と対象物との衝突を動的に予測して衝突を回避する方法としては、本出願人による特開２００５−１８２１２１号公報に開示された同名称の発明（以下従来技術を言う）がある。
【０００４】
この従来技術においては、（１）水平方向に離間配置された２個の眼（左眼、右眼）を用い、（２）左眼と右眼とで個別に対象物の速度（自己に対する相対速度）を観測し、（３）この観測された速度から３通りの条件式の値を求めて、それらの条件式がすべて満足された場合に「衝突する」と予測するものである。
【０００５】
この従来技術の場合、両眼間のどこかに衝突することのみ判定するが、自己全体のどこかに衝突することまでは判定できなかった。また、正確にどの位置に衝突するかを計算できていなかった。また、衝突時刻を知るために視野内の各点の３次元位置計測を時刻（ｔ）と（ｔ＋Δｔ）の２回行う必要があったが、今回は、時刻（ｔ）のみ１回で正確な予測ができている。
【特許文献１】特開２００５−１８２１２１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
しかし複数回数の３次元位置観測を行うことはそれだけ時間と手間とを要することになり、条件式に関連する演算処理回数も多くなるという欠点がある。移動体にとっては、すばやく正確に判断することは最も重要な機能である。
【０００７】
かかる従来技術の現状に鑑みてこの発明の目的は、動的衝突予測を従来技術より短い処理時間と少ない手間で、かつ演算処理回数を低減することのみならず、衝突時刻や自己のどこに衝突するかの正確な位置予測をすることおよび自己からどのくらい離れて対象物が通過するかを予測することにある。
【課題を解決するための手段】
【０００８】
このためこの発明においては、移動平面に平行な基準平面上に２個の眼（カメラ）を光軸を平行にして離間配置し、時間軸上に所定の時間間隔（Δｔ）で選ばれた複数の時刻の内の１つの時刻（ｔ）における右カメラの各画素の両眼視差法による３次元位置（Ｘ0，Ｙ0，Ｚ0）と各カメラの各画素における時刻（ｔ）から時刻（ｔ＋Δｔ）間の左右方向の速度対（δR，δL）を観測し、つぎの条件式が成立したときに「衝突する」と予測するものであり、
【数４】
ここでａは左右の眼の間の距離であり、ｂは右眼と自己の右端間および左眼と自己の左端間の距離であり、Ｚ0は時刻ｔにおける対象画素の奥行方向（Ｚ方向）の距離であり、Ｚは時刻（ｔ＋Δｔ）における対象画素のＺ方向の距離であり、次の式で得られる位置ＸTに衝突するものとすると、
【数５】
ここでαは１画素当りの視角であり、δLは対象物の左眼上での対象画素の移動画素数（左眼速度）であり、δRは対象画素の右眼上での移動画素数（右眼速度）であることを要旨とする。
【発明の効果】
【０００９】
この発明によれば１回の両眼視差による距離測定と１回の速度対測定で衝突の完全な予測が可能となるので、動的衝突予測を時間的に短縮しかつ演算処理を簡単にすることができるとともに対象物が両眼を結ぶ直線上のどこを通過するかを正確に予測できるので自己のどこに何時に衝突するかを正確に予測できる。
【実施例】
【００１０】
以下の事例の説明においては、例えば右に向かう速度を正、左に向かう速度を負と符号を定めるが、勿論この逆であってもよい。また模型図においてはＸ方向とは両眼を結ぶ直線方向を言い、Ｚ方向とはカメラの光軸方向（奥行方向）を言う。
【００１１】
図１は、場面の平面図を示し、図１に示す場合において各符号はつぎの意味をもつものである。下部の四角形は自己を示し、Ｐは対象物を示す。Ｐ0は対象物の時刻ｔにおける位置であり座標（Ｘ0、Ｚ0）で表わされる。Ｐは対象物の時刻（ｔ＋Δｔ）における位置であり座標（Ｘ、Ｚ）で表わされる。ＰLは点Ｐ0を通りＸ軸と平行な直線と点Ｐと左眼（Ｌ）を結ぶ直線との交点である。同様にＰRは点Ｐ0を通りＸ軸と平行な直線と点Ｐと右眼（Ｒ）を結ぶ直線との交点である。
【００１２】
Ｚ0は時刻ｔにおける対象物のＺ方向の距離であり、Ｚは時刻（ｔ＋Δｔ）における対象物のＺ方向の距離である。
【００１３】
ａは左右の眼の間の距離（一例を挙げると１２ｃｍ）である。ｂは右眼と右端間および左眼と左端間の距離である。Ｒは右眼でありＬは左眼である。αは１画素当りの視角（一例を挙げると目視角７０度が３２０画素に相当する。単位はラジアンで示す。）である。δLは点Ｐの左眼上での移動画素数（左眼速度）であり、δRは右眼上での移動画素数（右眼速度）である。
【００１４】
さらにＸTとは衝突予測点であり、直線ＬＲと直線Ｐ0Ｐとの交点であり、次の式で計算される。
【数６】
【００１５】
衝突するための条件は次の式で定義される。
【数７】
【００１６】
また衝突時間Ｔは次の式で計算される。
【数８】
【００１７】
上記の条件式（１）、（２）が成立したときには衝突が予測され、自己は最近に衝突する対象物（Ｔが最も小さい対象物）を避ける方向に移動する。つまり右視野にある対象物なら左前方へ、左視野にある対象物なら右前方に移動するのである。ちなみに、距離が近づいてくる対象物が衝突しない場合の離れ具合は、ｍｉｎ［｜ＸT−ｂ｜，｜−（ａ+ｂ）−ＸT｜］で求まる。左項は右側通過、右項は左側通過に対応する。
【００１８】
以下図２〜５に示す種々の事例について衝突の有無を予測する。
【００１９】
図２に示す事例１の場合には、対象物は右方に移動する。この場合にはｔａｎαδR＞０でｔａｎαδL＞０となる。しかもδR＝δLであるから、ＸT＝∞となり、条件式（１）が成立しない。すなわち「衝突しない」と予測されるのである。
【００２０】
図３に示す事例２の場合には、対象物は左方に移動する。この場合にはｔａｎαδR＜０でｔａｎαδL＜０となる。しかもδR＝δLであるから、ＸT＝−∞となり、条件式（１）が成立しない。すなわち「衝突しない」と予測されるのである。
【００２１】
図４に示す事例３の場合には対象物が自己に向かってＺ方向に接近してくる。この場合にはｔａｎαδR＜０でｔａｎαδL＞０となる。しかもδR＝−δLであるから、ＸT＝−ａ／２となり条件式（１）、（２）ともに成立する。すなわち「衝突する」と予測されるのである。
【００２２】
図５に示す事例４の場合には対象物が自己からＺ方向に離間する。この場合にはＺ0−Ｚ＜０となり、条件式（２）が成立しない。すなわち「衝突しない」と予測されるのである。
【００２３】
つぎに実際の実験例を示す。実験には東京精機（株）製作の移動プラットフォームシステムとＶｉｅｗＰｌｕｓ（株）製作の２眼カメラシステムを用いた。前方１００ｃｍにある対象物がΔｔ後に１０ｃｍ右方、左方、前方および後方に移動したとき（前記の事例１〜４に相当する）、対象物が右眼（カメラ）および左眼（カメラ）上を何画素移動したかを測定した結果を表１に示す。
【表１】
【産業上の利用可能性】
【００２４】
この発明の応用はロボットや車両に限らず、航空機や船舶など三次元方向に移動するあらゆるタイプの主体の衝突予測に応用できるものである。
【図面の簡単な説明】
【００２５】
【図１】この発明による動的衝突予測の基本的メカニズムを説明するための模型図である。
【図２】事例１の場合の状態を示す模型図である。
【図３】事例２の場合の状態を示す模型図である。
【図４】事例３の場合の状態を示す模型図である。
【図５】事例４の場合の状態を示す模型図である。
【特許請求の範囲】
【請求項１】
移動平面に平行な基準平面上に２個の眼（カメラ）を光軸を平行にして離間配置し、時間軸上に所定の時間間隔（Δｔ）で選ばれた複数の時刻の内に１つの時刻（ｔ）における右カメラの各画素の両眼視差法による３次元位置（Ｘ0，Ｙ0，Ｚ0）と各カメラの各画素における時刻（ｔ）から時刻（ｔ＋Δｔ）間の左右方向の速度対（δR，δL）を観測し、つぎの条件式が成立したときに「衝突する」と予測するものであり、
【数１】
ここでａは左右の眼の間の距離であり、ｂは右眼と自己の右端間および左眼と自己の左端間の距離であり、Ｚ0は時刻ｔにおける対象画素の奥行方向（Ｚ方向）の距離であり、Ｚは時刻（ｔ＋Δｔ）における対象画素のＺ方向の距離であり、次の式で得られる時間ＸT後に衝突するものとすると、
【数２】
ここでαは１画素当りの視角であり、δLは対象物の左眼上での対象画素の移動画素数（左眼速度）であり、δRは対象画素の右眼上での移動画素数（右眼速度）であることを特徴とする動的衝突予測方法。
【請求項２】
さらに次の式により衝突予測時刻（Ｔ）を求める
【数３】
ことを特徴とする請求項１に記載の方法。
【請求項１】
移動平面に平行な基準平面上に２個の眼（カメラ）を光軸を平行にして離間配置し、時間軸上に所定の時間間隔（Δｔ）で選ばれた複数の時刻の内に１つの時刻（ｔ）における右カメラの各画素の両眼視差法による３次元位置（Ｘ0，Ｙ0，Ｚ0）と各カメラの各画素における時刻（ｔ）から時刻（ｔ＋Δｔ）間の左右方向の速度対（δR，δL）を観測し、つぎの条件式が成立したときに「衝突する」と予測するものであり、
【数１】
ここでａは左右の眼の間の距離であり、ｂは右眼と自己の右端間および左眼と自己の左端間の距離であり、Ｚ0は時刻ｔにおける対象画素の奥行方向（Ｚ方向）の距離であり、Ｚは時刻（ｔ＋Δｔ）における対象画素のＺ方向の距離であり、次の式で得られる時間ＸT後に衝突するものとすると、
【数２】
ここでαは１画素当りの視角であり、δLは対象物の左眼上での対象画素の移動画素数（左眼速度）であり、δRは対象画素の右眼上での移動画素数（右眼速度）であることを特徴とする動的衝突予測方法。
【請求項２】
さらに次の式により衝突予測時刻（Ｔ）を求める
【数３】
ことを特徴とする請求項１に記載の方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図２】
【図３】
【図４】
【図５】
【公開番号】特開２００７−２３３９５６（Ｐ２００７−２３３９５６Ａ）
【公開日】平成１９年９月１３日（２００７．９．１３）
【国際特許分類】
【出願番号】特願２００６−５８１３８（Ｐ２００６−５８１３８）
【出願日】平成１８年３月３日（２００６．３．３）
【出願人】（５９３１７１５９２）学校法人玉川学園 (38)
【Ｆターム（参考）】
【公開日】平成１９年９月１３日（２００７．９．１３）
【国際特許分類】
【出願日】平成１８年３月３日（２００６．３．３）
【出願人】（５９３１７１５９２）学校法人玉川学園 (38)
【Ｆターム（参考）】
[ Back to top ]