JPH10143245A - 移動体の障害物衝突防止装置 - Google Patents

Abstract

(57)【要約】 （修正有） 【課題】障害物の数が多くても、相対速度が大きくて
も、また移動体の走行時の揺れが大きくても、逐次移動
する各障害物の対応を正確にとり、各障害物の相対速度
を正確に演算することによって、衝突回避動作を正確に
行えるようにする。 【解決手段】移動体１の外部に存在する各障害物３１お
よび３２の相対位置Ｐ1-2、Ｐ1-1、Ｐ10…およびＰ2-
2、Ｐ2-1、Ｐ20…が位置計測手段で一定時間以下の間隔
（ｔ-2、ｔ-1、ｔ0…）で逐次、計測されることによっ
て、当該逐次の相対位置データが、いずれの障害物のデ
ータであるかの対応付けが逐次、行われる。そして、上
記対応付けられた各障害物と逐次の相対位置データとの
関係に基づき、各障害物それぞれの相対速度が逐次、演
算される。そして、移動体に最も接近するまでの近接時
間Ｔおよびその最も近接したときの近接距離Ｌが逐次、
演算され、移動体１が障害物３１、３２に衝突すること
を回避する動作、処理が行われる。

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、無人ダンプトラッ
ク、移動ロボット等の移動体にとっての障害物がこの移
動体に最も近接するまでの近接時間と、この最も近接し
たときの障害物との近接距離とを予測し、この予測結果
に基づき移動体が障害物に衝突することを回避するよう
にした移動体の障害物衝突防止装置に関する。

【０００２】

【従来の技術および発明が解決しようとする課題】無人
ダンプトラック、移動ロボット等の移動体において、そ
の走行路上に存在する複数の障害物を検出する装置とし
て、レーザレンジファインダをスキャンさせることによ
って障害物を検出するレーザレーダ、ステレオカメラに
よる３次元視覚センサを用いて障害物を検出するものな
どがある。

【０００３】これら従来の障害物検出装置では、一定の
時間（サンプリングタイム）ごとに走査画像、撮像画像
が取得されて、各画像の中から複数の障害物が判別され
る。

【０００４】しかし、一つの障害物であるならば問題は
ないものの、複数の障害物を、異なる時刻で取得された
画像の中から、検出しようとするときには、たとえ、あ
る時刻の画像から移動体と複数の障害物との相対位置が
わかったとしても、時間間隔の大きいつぎの時刻で取得
された画像の中では、前回の位置から相対位置が相当変
化しているために、前回の画像の中の各障害物との対応
がつかないことがあった。

【０００５】このように、前回取得された画像と今回取
得された画像の中で、複数の障害物の対応付けがつかな
いために、個々の障害物の相対移動速度を演算しようと
しても精度よく求めることができないことがあった。

【０００６】さらには、無人ダンプトラックたる移動体
は、不整地等を走行することが多く、走行中の車体の揺
れは大きいため、移動体搭載の画像は、その揺れの影響
を受ける。このため、前回の画像と今回の画像とを比較
して複数の障害物の対応をとる作業は、さらに困難なも
のとなっていた。

【０００７】以上のように、従来の障害物検出装置で
は、移動体と各障害物との相対速度を精度よく演算する
ことができないため、この相対速度から障害物が移動体
に衝突するまでの時間等を予測して衝突を回避する処理
を行うことは、精度よく行うことができないこととなっ
ていた。すなわち、従来の障害物検出装置で、移動体と
各障害物との衝突が予測できるのは、障害物の数が少な
く、かつ移動体と障害物との相対速度が小さく、かつ移
動体の走行時の揺れが小さい場合に限られていた。

【０００８】本発明はこうした実状に鑑みてなされたも
のであり、障害物の数が多くても、また、移動体と各障
害物との相対速度が大きくても、また移動体の走行時の
揺れが大きくても、逐次移動する各障害物の対応を正確
にとり、これにより各障害物の相対速度を正確に演算す
ることによって、衝突回避動作を正確に行えるようにす
ることを目的とするものである。

【０００９】

【課題を解決するための手段および効果】そこで、上記
目的を達成するために本発明の主たる発明では、移動体
にとっての障害物が当該移動体に最も近接するまでの近
接時間と、当該最も近接したときの前記障害物との近接
距離とを予測し、この予測結果に基づき前記移動体が前
記障害物に衝突することを回避するようにした移動体の
障害物衝突防止装置において、移動体の外部に存在する
各障害物の相対位置を位置計測手段で一定時間以下の間
隔で逐次、計測することによって、当該逐次の相対位置
データがいずれの障害物のデータであるかの対応付けを
逐次、行う対応付け手段と、前記対応付け手段によって
対応付けられた各障害物と逐次の相対位置データとの関
係に基づき、各障害物それぞれの相対速度を逐次、演算
する速度演算手段と、前記対応付け手段によって対応付
けられた各障害物と逐次の相対位置データとの関係と、
前記速度演算手段の演算結果から得られる各障害物と逐
次の相対速度データとの関係とに基づき、各障害物それ
ぞれについて、前記近接時間および前記近接距離を逐
次、演算する近接時間・距離演算手段とを具えるように
している。

【００１０】かかる構成によれば、図３に示すように、
移動体１の外部に存在する各障害物３１および３２の相
対位置Ｐ1-2、Ｐ1-1、Ｐ10…およびＰ2-2、Ｐ2-1、Ｐ20
…が位置計測手段で一定時間以下の間隔（ｔ-2、ｔ-1、
ｔ0…）で逐次、計測されることによって、図４のテー
ブル１に示すように、当該逐次の相対位置データが、い
ずれの障害物のデータであるかの対応付けが逐次、行わ
れる。

【００１１】そして、図５のテーブル２に示すように、
上記対応付けられた各障害物と逐次の相対位置データと
の関係に基づき、各障害物それぞれの相対速度が逐次、
演算される。

【００１２】そして、この対応付けられた各障害物と逐
次の相対位置データとの関係と、各障害物と逐次の相対
速度データとの関係とに基づき、図７に示すように、各
障害物３１、３２それぞれについて、移動体に最も接近
するまでの近接時間Ｔおよびその最も近接したときの近
接距離Ｌが逐次、演算される。この演算結果に基づき、
移動体１が障害物３１、３２に衝突することを回避する
動作、処理がなされる。

【００１３】以上のように、位置計測手段で一定時間以
下の間隔で逐次、位置計測を行うようにしているので、
位置計測データがいずれの障害物のものであるかの対応
づけを正確に行うことができる。つまり、障害物の数が
多くても、また移動体と各障害物との相対速度が大きく
ても、また移動体の走行時の揺れが大きくても、各障害
物の相対速度が正確に演算され、衝突回避動作を正確に
行うことができる。

【００１４】また、本発明の別の発明では、対応付け手
段が、移動体からみた３次元座標系における移動体外部
の各障害物の現在の３次元画像を一定時間以下の間隔で
逐次、生成する３次元画像生成手段と、３次元画像生成
手段で逐次生成される３次元画像の中の各障害物の座標
位置を逐次検出することによって、当該逐次の座標位置
データがいずれの障害物のデータであるかの対応付けを
逐次、行う手段とで構成される。さらに、上記生成され
た３次元画像の中の各障害物の特徴を表す画像情報が抽
出され、この抽出された画像情報に基づき、逐次計測さ
れる座標位置データがいずれの障害物のデータであるか
の対応付けが行われる。

【００１５】以上のように各障害物の３次元画像を生成
して、その３次元画像の中から各障害物の特徴を表す画
像情報を抽出するようにしているので、共通する内容の
画像情報の位置計測データが同一の障害物のものである
という知見のもとに、対応づけが更に正確に行われる。

【００１６】

【発明の実施の形態】以下、図面を参照して本発明の実
施の形態について説明する。

【００１７】本実施の形態では、図１に示すように無人
ダンプトラック、移動ロボット等の移動体１が走行路３
３を走行する場合に、この走行路３３上に存在する岩等
の障害物３１、他の車両等の障害物３２等の各障害物
を、移動体搭載の衝突予測装置２で検出し、衝突の危険
度の判定を行う場合を想定している。なお、説明の便宜
のため２つの障害物を想定しているが、本発明として
は、３以上の障害物がある場合でも当然適用することが
できる。

【００１８】図１において、Ｘ−Ｙ−Ｚは、移動体１と
ともに移動する車体座標系を示している。Ｘは移動体１
の車幅方向に対応する座標軸であり、Ｚは移動体１の進
行方向に対応する座標軸であり、Ｙは鉛直方向の座標軸
である。この車体座標系の原点は、衝突予測装置２の障
害物検出部２のカメラの原点に対応している。

【００１９】図２は、本発明の実施形態である衝突予測
装置２の構成を示している。

【００２０】同図２に示すように、この衝突予測装置２
は、移動体１の外部に存在する各障害物３１および３２
の移動体１に対する相対位置Ｐ1-2、Ｐ1-1、Ｐ10…およ
びＰ2-2、Ｐ2-1、Ｐ20…を、一定時間以下の間隔（ｔ-
2、ｔ-1、ｔ0…）で高速に逐次、位置計測し、この逐次
の相対位置データが、いずれの障害物のデータであるか
の対応付けを逐次、行う障害物検出部４と、上記対応付
けられた各障害物３１、３２と逐次の相対位置データと
の関係に基づき、各障害物３１、３２それぞれの移動体
１に対する相対速度を逐次、演算する相対速度算出部５
と、この対応付けられた各障害物３１、３２と逐次の相
対位置データとの関係と、各障害物３１、３２と逐次の
相対速度データとの関係とに基づき、各障害物３１、３
２それぞれについて、移動体１に最も接近するまでの近
接時間Ｔおよびその最も近接したときの近接距離Ｌを逐
次、予測演算する予測部６と、衝突の危険度を判断する
ためのテーブル、式が予め記憶されている記憶部８と、
予測部６で予測された近接時間Ｔと近接距離Ｌを、記憶
部８のテーブル、式の内容に突き合わせて障害物が移動
体１に衝突する危険の度合いを判定する判定部７とから
構成されている。

【００２１】図１に示す移動体搭載の制御コントローラ
３では、上記判定部７から出力される衝突危険度に応じ
て、移動体１が障害物に衝突しないように、移動体１が
駆動制御される。このようにして、移動体１が障害物３
１、３２に衝突することが回避される。

【００２２】以下、衝突予測装置２で実行される処理に
ついて図３〜図１２を参照して説明する。

【００２３】さて、本発明者らは、移動体進行方向の距
離画像を、各画素の３次元分布に変換し、この画素の３
次元分布状態から、走行路に対応する画素群を特定し、
この走行路を平面とみて、この平面の高さを基準として
これより所定の高さ以上にある物体を障害物として検出
するという発明を提案しており、この発明を実施する試
みがなされている。

【００２４】衝突予測装置２の障害物検出部４は、上記
本発明者らの提案に係る障害物検出方法を適用して構成
することができる。

【００２５】すなわち、障害物検出部４では、多眼レン
ズ（多眼のカメラ）によって、例えば図１１に示すよう
な走行路面３３と、走行路面３３上に存在する障害物３
１、３２からなる距離画像３０が生成される。距離画像
３０の各画素５０には、ｉ−ｊ２次元座標系における２
次元座標位置（ｉ、ｊ）、移動体１の基準位置（基準
面）からの距離ｄを示す３次元のデータ（ｉ、ｊ、ｄ）
が対応づけられており、距離画像３０の各位置ｉ、ｊの
画素は、距離ｄに応じた明度を有している。こうした３
次元の距離画像を生成するための距離計測の方法として
は、例えば本発明者らに係る特願平７−２００９９９号
に示される多眼レンズ（多眼カメラ）を使用した方法を
用いることができる。

【００２６】ここで、複数のカメラの撮像画像中の画素
をパターンマッチングによって対応させているため、距
離画像３０の中には、誤対応を起こしている画素が存在
している可能性がある。そこで、得られた距離画像３０
の画素のうち、パターンマッチング時のエラーの大きさ
が所定のしきい値以上のものについては、誤対応点であ
るとして距離画像３０の中から除去しておくことができ
る。

【００２７】このようにして距離画像３０が取得される
と、この距離画像３０を座標変換することによって、図
１２に示すように、車体座標系Ｘ−Ｙ−Ｚにおける３次
元画像４０が生成される。

【００２８】すなわち、距離画像３０の各画素５０に
は、上述したように（ｉ、ｊ、ｄ）の３次元の情報が対
応づけられているので、この距離画像データ（ｉ、ｊ、
ｄ）で示される各画素５０を、図１２に示すように、移
動体１とともに移動し、移動体１のカメラ原点を原点と
するカメラ座標系たる車体座標系Ｘ−Ｙ−Ｚ上の３次元
座標位置データ（Ｘ、Ｙ、Ｚ）に対応づけられた各画素
６０に変換することができる。このような変換を施すこ
とによって、画素６０の３次元座標位置の分布図として
３次元分布画像４０を取得することができる。

【００２９】そして、この３次元分布画像４０の全画素
６０の中から鉛直方向（Ｙ軸方向）最下点にある画素群
を選択し、これらを平面近似することによって、走行路
面３３に相当する平面（Ｘ−Ｚ平面）を検出することが
できる。

【００３０】つぎに、走行路面３３に対応する平面を基
準とする所定のしきい値以上の高さの物体があるか否か
が検出される。この場合、平面３３を基準として所定の
しきい値以上の物体３１、３２があるので、これが「障
害物」であると検出される。

【００３１】この障害物の検出は、以下のように画素分
布の連続性を考慮して行うことができる。

【００３２】すなわち、障害物３１、３２は、ひとまと
まりの物体であるので、Ｘ−Ｙの２次元分布において、
ひとかたまりの画素群としてそれぞれ観察されるはずで
あり、ひとまとまりの画素群ではないもの（障害物以外
の物体）と区別することができるはずである。

【００３３】そこで、こうした知見に基づき、この２次
元分布を、幅Ｘ方向に、所定の間隔毎に区分して各領域
７１、７１…毎に（図１２参照）、障害物候補点たる画
素が存在しているか否かを探索していく。そして、障害
物候補点が存在している領域を障害物候補領域とする。
そして、この障害物候補領域が、幅方向Ｘで連続してい
るか否かが探索される。こうして、幅方向Ｘで連続して
いる障害物候補領域の全体が、障害物領域とされる。本
実施の形態の場合、障害物３１に対応する一の障害物領
域と、障害物３２に対応する他の一の障害物領域の２つ
の障害物領域が探索されるはずである。

【００３４】つぎに、今度は、上記各領域７１、７１…
毎に、Ｙ−Ｚの２次元分布が求められる。

【００３５】そして、この各領域７１毎の２次元分布に
ついて、同様にして障害物領域を探索する処理を実行し
ていく。

【００３６】こうして、最終的に、３次元的に画素群が
連続している２つの障害物領域が取得され、これら２つ
の障害物領域に含まれる画素群がそれぞれ障害物３１、
３２であると特定、検出される。

【００３７】つぎに、こうして検出された障害物３１を
構成する画素６０の中で、この障害物３１を代表する位
置（例えば重心位置）に対応する画素の３次元座標位置
Ｐ1が求められる。そして、この障害物３１の横方向の
最大長さが「幅ａ1」として求められる。そして、この
障害物３１の幅ａ1に直角方向の最大長さが「奥行きｂ
1」として求められる。さらに、この障害物３１の最大
高さが「高さｃ1」として求められる。さらに、障害物
３１を特徴づける色ｄ1（障害物３１を構成する画素６
０の明度を平均等することによって得られる）が求めら
れる。他方の障害物３２についても同様の処理が実行さ
れ、代表位置Ｐ2、幅ａ2、奥行きｂ2、高さｃ2、色ｄ2
が求められる。なお、障害物３１、３２の代表位置Ｐ
1、Ｐ2としては、中心位置でもよい。

【００３８】こうして、平面近似された画素群として走
行路面３３が検出され、ひとまとまりの連続した分布の
画素群として障害物３１、３２が検出されると、最終的
に図１３に示すような３次元画像４０′が取得される。

【００３９】ここで、上述した障害物検出処理は、高速
にリアルタイムに行うことが可能であり、障害物３１、
３２の位置Ｐ1、Ｐ2、つまり移動体１（移動体１のカメ
ラ）に対する障害物３１、３２の相対位置Ｐ1、Ｐ2をき
わめて短いサンプリング間隔で計測することができる。

【００４０】こうして、図３に示すように、障害物３１
および３２の相対位置Ｐ1-2、Ｐ1-1、Ｐ10…およびＰ2-
2、Ｐ2-1、Ｐ20…が、きわめて短いサンプリングタイム
（ｔ-2、ｔ-1、ｔ0…）で逐次、計測される。

【００４１】ただし、各時刻の相対位置データＰ1-2、
Ｐ2-2（時刻ｔ-2）、Ｐ1-1、Ｐ2-1（時刻ｔ-1）、Ｐ1
0、Ｐ20（時刻ｔ0）…を求めただけでは、各時刻のいず
れの位置データが、いずれの障害物のデータであるかの
対応づけは得られない。

【００４２】そこで、同図３に示すように、今回の計測
時刻の２つの位置データのうち、前回の計測時刻の位置
に最も近い方の位置データが、同一の障害物の位置デー
タであると順次判断することによって、各時刻の相対位
置データと各障害物３１、３２との対応づけが行われ
る。ここで、たとえ、障害物３１、３２が移動体１に対
して高速に移動していたとしても、位置計測はきわめて
短い間隔で行われているので、前回の計測時刻の位置の
近傍に存在する障害物を同一の障害物とみなす上述の判
断を正確に行うことができ、逐次計測されるある時刻に
おける２つの相対位置データがいずれの障害物に属する
ものかの対応付けを、正確かつ確実に行うことができ
る。

【００４３】また、障害物の数が多かったり、移動体１
の走行時の揺れが大きくて３次元画像４０′にその揺れ
の影響が加わったとしても、上記対応付けを正確かつ確
実に行うことができる。

【００４４】さらに、本実施の形態では、相対位置デー
タＰ1と一緒に、幅ａ1、奥行きｂ1、高さｃ1、色ｄ1と
いった特徴情報が取得され、相対位置データＰ2と一緒
に、幅ａ2、奥行きｂ2、高さｃ2、色ｄ2といった特徴情
報が取得されているので、特徴情報の内容が共通してい
る相対位置データが、同一の障害物のデータであると判
断することができ、ある時刻における２つの相対位置デ
ータがいずれの障害物に属するものかの対応付けを、よ
り正確に、より確実に行うことができる。なお、特徴情
報としては、幅ａ、奥行きｂ、高さｃ、色ｄのうちのい
ずれか一つあるいは任意の２つ以上の組合わせを使用す
ることができる。また、特徴情報を使用しないで対応付
けを行う実施も可能である。

【００４５】また、本発明としては、距離画像から３次
元画像に変換する上述した方法に限定されるものではな
い。

【００４６】移動体１からみた３次元座標系Ｘ−Ｙ−Ｚ
における移動体外部の各障害物の現在の３次元画像を一
定時間以下の間隔で逐次、生成することができるものあ
れば、３次元画像を生成する方法は任意である。

【００４７】さらには、本発明としては、必ずしも３次
元画像を生成するものに限定されるものではない。

【００４８】要は、移動体１の外部に存在する各障害物
の相対位置を一定時間以下の間隔で逐次、計測すること
ができるものであればよい。

【００４９】このようにして、図３に示すように、３次
元画像４０′における位置の軌跡Ｍ1が障害物３１のも
のであり、軌跡Ｍ2が障害物３２のものであるとの対応
付けが行われ、その結果が、図４に示すように、テーブ
ル１として記憶、格納される。相対速度算出部５では、
図４のテーブル１に格納された各障害物３１、３２と逐
次の相対位置データとの対応関係に基づき、各障害物３
１、３２それぞれの相対速度が逐次、演算され、その演
算結果が図５に示すように、テーブル２として記憶、格
納される。

【００５０】相対位置データから相対速度を算出する方
法は、任意の方法を適用することができ、例えば以下の
ような方法が挙げられる。

【００５１】（速度算出方法１）最終の時刻ｔ0と最終
の時刻の一つ前の時刻ｔ-1の相対位置の各成分の差分を
とる方法である。

【００５２】たとえば、障害物３１の最終時刻ｔ0にお
ける相対速度Ｖ10（Ｖx10、Ｖy10、Ｖz10）は、以下の
ような演算によって求められる。

【００５３】 相対速度のＸ成分：Ｖx10＝Ｘ10−Ｘ1-1 相対速度のＹ成分：Ｖy10＝Ｙ10−Ｙ1-1 相対速度のＺ成分：Ｖz10＝Ｚ10−Ｚ1-1 （速度算出方法２）加速度を考慮した相対速度を算出す
る方法である。

【００５４】これは、最終の時刻ｔ0と最終の時刻の一
つ前の時刻ｔ-1における相対位置だけでなく、それ以前
の時刻ｔ-2…（少なくとも時刻ｔ-2）における相対位置
を用いて、これら少なくとも３つの相対位置間を補間し
た曲線を、図６（ａ）に示すように求め、この曲線の時
刻ｔ0における接線を、最終時刻ｔ0における相対速度Ｖ
0とするものである。

【００５５】予測部６では、テーブル１に格納された各
障害物３１、３２と逐次の相対位置データとの対応関係
と、テーブル２に格納された各障害物３１、３２と逐次
の相対速度データとの対応関係とに基づき、各障害物３
１、３２それぞれについて、最終時刻たる現在の時刻ｔ
0より障害物が移動体１に最も接近するまでの近接時間
Ｔが演算されるとともに、その最も近接したときの移動
体１と障害物との近接距離Ｌが演算される。

【００５６】相対位置と相対速度から近接時間Ｔ、近接
距離Ｌを算出する方法は、任意の方法を適用することが
でき、例えば以下のような方法が挙げられる。

【００５７】（近接時間・距離算出方法１）最終の時刻
ｔ0における相対位置から最終時刻ｔ0における相対速度
のままで障害物が移動するものと仮定して、その予測移
動軌跡を直線（最終時刻の相対速度ベクトルの方向成
分）とみなし、この直線と車体座標系Ｘ−Ｙ−Ｚとの幾
何的関係から、算出する方法である。

【００５８】すなわち、障害物３１を例にとると、図７
に示すように、最終の時刻ｔ0における相対位置Ｐから
最終時刻ｔ0における相対速度Ｖのままで障害物３１が
移動するものと仮定する。この結果、最終時刻の相対位
置Ｐ（Ｘ10、Ｙ10、Ｚ10）を起点として相対速度ベクト
ルＶ（Ｖx10、Ｖy10、Ｖz10）の方向成分方向に延びる
直線が予測移動軌跡Ｍとされる。

【００５９】つぎに、車体座標系Ｘ−Ｙ−Ｚの原点Ｏ
（０、０、０）から直線Ｍに下ろした垂線が求められ、
この垂線が直線Ｍと交差する座標位置Ｑ（ＸQ、ＹQ、Ｚ
Q）が演算される。この交差点Ｑが、障害物３１が移動
体１に最接近するときの相対位置に対応している。そし
て、原点Ｏ（０、０、０）と交差点Ｑ（ＸQ、ＹQ、Ｚ
Q）との距離が、近接距離Ｌとされる。

【００６０】つぎに、最終時刻の相対位置Ｐ（Ｘ10、Ｙ
10、Ｚ10）と最接近するときの予測相対位置Ｑ（ＸQ、
ＹQ、ＺQ）との距離が演算され、さらにこの距離を相対
速度Ｖで移動したときの時間が演算され、これが近接時
間Ｔとされる。

【００６１】（近接時間・距離算出方法２）この方法
は、上記算出方法１において、鉛直方向Ｙ軸の成分を無
視して演算処理を行う方法である。

【００６２】走行路が坂道ではない平坦な路面であり、
障害物が空中を飛んだり、坂道を走行していない状況下
では、この方法を適用することができ、演算を簡易にす
ることによって演算処理時間を短縮することができる。

【００６３】すなわち、図８に示すように、最終の時刻
ｔ0における相対位置Ｐから最終時刻ｔ0における相対速
度Ｖのままで障害物３１が移動するものと仮定する。こ
の結果、最終時刻の相対位置Ｐ（Ｘ10、Ｚ10）を起点と
して相対速度ベクトルＶ（Ｖx10、Ｖz10）の方向成分方
向に延びる直線が予測移動軌跡Ｍとされる。

【００６４】この直線Ｍは、相対速度Ｖ（Ｖx10、Ｖz1
0）の方向成分に応じた傾き（Ｖz10/Ｖx10）を有し、Ｐ
点（Ｘ10、Ｚ10）、障害物３１が移動体１に最接近する
ときの相対位置たるＱ点（ＸQ、ＺQ）を通るので、下記
（１）式が得られる。

【００６５】 ＺQ＝（Ｖz10/Ｖx10）・ＸQ−（Ｖz10/Ｖx10）・Ｘ10＋Ｚ10 …（１） つぎに、直線Ｍたる相対速度ベクトルＶ（Ｖx10、Ｖz1
0）と直線ＯＱとは垂直になるという条件の下に、下記
（２）式が得られる。

【００６６】ＸQ・Ｖx10＋ＺQ・Ｖz10＝０ …（２） 上記（１）、（２）式から最接近点Ｑの座標位置（Ｘ
Q、ＺQ）は、以下のようにして求められる。

【００６７】 ＸQ＝１/（Ｖx10・Ｖx10＋Ｖz10・Ｖz10）・｛（Ｖz10・Ｖz10）・Ｘ10− （Ｖx10・Ｖx10）・Ｚ10｝ ＺQ＝１/（Ｖx10・Ｖx10＋Ｖz10・Ｖz10）・｛−（Ｖz10・Ｖz10）・Ｘ10＋ （Ｖx10・Ｖx10）・Ｚ10｝ …（３） よって、近接距離Ｌは、 Ｌ＝ｓｑｒｔ（ＸQ・ＸQ＋ＺQ・ＺQ） …（４） と求められる。

【００６８】また、近接時間Ｔは、 Ｔ＝（Ｚ10−ＺQ）/Ｖz10あるいは、（Ｘ10−ＸQ）/Ｖx10 …（５） として求められる。

【００６９】（近接時間・距離算出方法３）この方法
は、上記算出方法２における演算をより簡易に行うこと
によって演算処理時間をさらに短縮する方法である。

【００７０】すなわち、図９に示すように、障害物３１
がＺ＝０の面を横切るときの位置を、障害物３１が移動
体１に最接近するときの位置Ｑ（ＸQ、０）と仮定する
ものである。

【００７１】この場合、近接時間Ｔは、 Ｔ＝Ｚ10/Ｖz10 …（６） と、近接距離Ｌは、 Ｌ＝ＸQ＝Ｘ10＋Ｖx10・Ｔ＝Ｘ10＋Ｖx10・Ｚ10/Ｖz10 …（７） と、きわめて簡易に算出することができる。

【００７２】（近接時間・距離算出方法４）この方法
は、加速度を考慮して障害物の予測移動軌跡を正確に予
測する方法である。

【００７３】すなわち、最終の時刻ｔ0における相対位
置Ｐ0、相対速度Ｖ0だけでなく、それ以前の時刻ｔ-1、
ｔ-2…（少なくとも時刻ｔ-1）における相対位置、相対
速度を用いて、これら少なくとも２つの相対位置間を補
間した曲線Ｍ′を、図６（ｂ）に示すように求め、この
曲線Ｍ′上の点であってＸ−Ｙ−Ｚ座標の原点Ｏに最も
近接するときの点Ｑを求め、近接時間Ｔ、近接距離Ｌを
求めるものである。

【００７４】なお、障害物３１の場合を想定して説明し
たが、他の障害物３２についても同様の処理が実行さ
れ、近接時間Ｔ、近接距離Ｌが演算される。

【００７５】記憶部８には、図１０に示すように、近接
時間Ｔの大きさと、近接距離Ｌの大きさの組合わせに応
じて、障害物が移動体１に衝突する危険の度合いを判定
するためのテーブル３が記憶、格納されている。

【００７６】判定部７では、上記記憶部８に格納された
テーブル３の内容と、各障害物３１、３２それぞれにつ
いて演算された近接時間Ｔ、近接距離Ｌとが、障害物ご
とに突き合わされることによって、衝突の危険の度合い
が判定される。

【００７７】たとえば、障害物３１の近接時間Ｔがしき
い値Ｔh1からしきい値Ｔh2の間の範囲にあり、同障害物
３１の近接距離Ｌがしきい値Ｌh1からしきい値Ｌh2の間
の範囲にある場合には、「危険度小」と判断される。各
障害物３１、３２の危険度の判定結果が判定部７から出
力される。また、記憶部８に、衝突の危険度を数値とし
て演算する演算式を記憶させておき、この演算式に近接
時間Ｔ、近接距離Ｌを代入することによって、衝突の危
険度を求めてもよい。

【００７８】たとえば、移動体１の車幅をＷとして、下
記（８）式に示す演算式から衝突の危険度Ｄを演算する
ことができる。

【００７９】 Ｄ＝（１/Ｔ）・（１/Ｔ）・（１/Ｔ）・（Ｗ/Ｌ）・（Ｗ/Ｌ） …（８） 各障害物３１、３２の危険度Ｄが判定部７から出力され
る。

【００８０】制御コントローラ３は、判定部７から出力
された各障害物３１、３２の危険度と、各障害物３１、
３２の現在位置とに基づき、急ブレーキをかけるべき
か、ゆっくりブレーキをかけるべきか、あるいはハンド
ルをきって回避すべきか等を判断して、障害物を回避す
るよう移動体１を駆動制御する。

【００８１】このようにして衝突回避動作が的確に行わ
れ、移動体の安全性を飛躍的に向上させることができ
る。

【００８２】なお、判定部７から各障害物３１、３２の
うちで、危険度の最も大きい方の判定結果あるいは数値
Ｄを出力して、判断、制御するようにしてもよい。

【図面の簡単な説明】

【図１】図１は本発明に係る移動体の障害物衝突防止装
置の実施形態の全体構成を示す斜視図である。

【図２】図２は実施の形態における衝突予測装置の構成
を示すブロック図である。

【図３】図３は相対位置データと各障害物との対応関係
を説明するために用いた図である。

【図４】図４は図３に示す対応関係を示すテーブルであ
る。

【図５】図５は図４に示すテーブルから、各障害物と相
対速度データとの対応関係のテーブルが求められる様子
を説明する図である。

【図６】図６（ａ）は補間によって相対速度を求める方
法を説明する図で、同図（ｂ）は補間によって近接点を
求める方法を説明する図である。

【図７】図７は近接時間および近接距離を演算する第１
の方法を説明する図である。

【図８】図８は近接時間および近接距離を演算する第２
の方法を説明する図である。

【図９】図９は近接時間および近接距離を演算する第３
の方法を説明する図である。

【図１０】図１０は衝突の危険度を判定するためのテー
ブルである。

【図１１】図１１は距離画像を示す図である。

【図１２】図１２は距離画像から得られる画素の３次元
分布画像を示す図である。

【図１３】図１３は画素の３次元分布画像から、走行路
面および各障害物が特定された３次元画像を示す図であ
る。

【符号の説明】

１ 移動体 ２ 衝突予測装置 ４ 障害物検出部 ５ 相対速度算出部 ６ 予測部 ７ 判定部

───────────────────────────────────────────────────── フロントページの続き (72)発明者 水井 精一 神奈川県平塚市万田1200 株式会社小松製 作所研究所内

Claims (4)

【特許請求の範囲】
1. 【請求項１】 移動体にとっての障害物が当該移動
   体に最も近接するまでの近接時間と、当該最も近接した
   ときの前記障害物との近接距離とを予測し、この予測結
   果に基づき前記移動体が前記障害物に衝突することを回
   避するようにした移動体の障害物衝突防止装置におい
   て、 移動体の外部に存在する各障害物の相対位置を位置計測
   手段で一定時間以下の間隔で逐次、計測することによっ
   て、当該逐次の相対位置データがいずれの障害物のデー
   タであるかの対応付けを逐次、行う対応付け手段と、 前記対応付け手段によって対応付けられた各障害物と逐
   次の相対位置データとの関係に基づき、各障害物それぞ
   れの相対速度を逐次、演算する速度演算手段と、 前記対応付け手段によって対応付けられた各障害物と逐
   次の相対位置データとの関係と、前記速度演算手段の演
   算結果から得られる各障害物と逐次の相対速度データと
   の関係とに基づき、各障害物それぞれについて、前記近
   接時間および前記近接距離を逐次、演算する近接時間・
   距離演算手段とを具えた移動体の障害物衝突防止装置。
2. 【請求項２】 前記対応付け手段は、移動体からみ
   た３次元座標系における移動体外部の各障害物の現在の
   ３次元画像を一定時間以下の間隔で逐次、生成する３次
   元画像生成手段と、 前記３次元画像生成手段で逐次生成される３次元画像の
   中の各障害物の座標位置を逐次検出することによって、
   当該逐次の座標位置データがいずれの障害物のデータで
   あるかの対応付けを逐次、行う手段とからなるものであ
   る、 請求項１記載の移動体の障害物衝突防止装置。
3. 【請求項３】 前記３次元画像生成手段は、 移動体の基準位置から、移動体外部の各障害物までの距
   離を計測し、当該各障害物の距離画像を逐次、生成し、
   この距離画像の各画素毎の２次元座標位置データと、各
   画素毎の前記基準位置からの距離データとに基づき、前
   記距離画像の各画素毎に、移動体からみた３次元座標系
   における３次元座標位置データを演算し、当該移動体座
   標系における前記各障害物の現在の３次元画像を一定時
   間以下の間隔で逐次、生成するものである、 請求項２記載の移動体の障害物衝突防止装置。
4. 【請求項４】 前記対応付け手段は、 前記生成された３次元画像の中の各障害物の特徴を表す
   画像情報を抽出し、この抽出された画像情報に基づき、
   逐次計測される座標位置データがいずれの障害物のデー
   タであるかの対応付けを行うものである、 請求項２記載の移動体の障害物衝突防止装置。