JP5107154B2 - 運動推定装置 - Google Patents

Description

本発明は、例えば自動運転車両や移動ロボット等の自律移動機械の運動状態を推定する運動推定装置に関するものである。

自律移動機械の運動状態を推定する運動推定装置としては、例えば特許文献１に記載されているように、例えばビデオカメラにより周囲環境の画像シーケンスを取得し、その画像シーケンスを例えばビデオ処理技法に従って処理し、周囲環境に対する運動推定を行うものが知られている。
特表２００８−５０３７５７号公報

上記従来技術のように周囲環境の画像を用いて運動推定を行う場合には、例えば各画像において抽出された特徴対応点に基づいて自律移動機械の運動状態を推定する。しかし、各画像の特徴対応点は静止物体上にあるものと仮定するため、抽出された特徴対応点が移動物体（車両や歩行者等）上にある場合には、自律移動機械の運動推定を正しく行うことができなくなる。

本発明の目的は、画像中に移動物体が存在している場合でも、自律移動機械の運動推定を精度良く実施することができる運動推定装置を提供することである。

本発明は、撮像器により自律移動機械の周辺環境を撮像し、周辺環境の各撮像画像の変化に基づいて自律移動機械の運動状態を推定する運動推定装置において、周辺環境の各撮像画像に存在する移動物体を検出し、移動物体に対応する画像領域を除去する画像領域除去手段と、移動物体に対応する画像領域を除去した後の各撮像画像から特徴対応点を抽出する特徴対応点抽出手段と、各撮像画像の特徴対応点の位置関係に基づいて、自律移動機械の運動状態を推定する第１推定手段と、特徴対応点の数が所定値よりも多いかどうかを判断する判断手段と、撮像器に対する移動物体の位置を求める位置検出手段と、移動物体の速度を求める速度検出手段と、判断手段により特徴対応点の数が所定値よりも多くないと判断されたときに、自律移動機械の過去の位置に基づいて、自律移動機械の仮の運動状態を推定する仮推定手段と、撮像器に対する移動物体の位置と移動物体の速度と自律移動機械の仮の運動状態とに基づいて、自律移動機械の運動状態を推定する第２推定手段とを備え、第１推定手段は、判断手段により特徴対応点の数が所定値よりも多いと判断されたときに、各撮像画像の特徴対応点の位置関係に基づいて、自律移動機械の運動状態を推定することを特徴とするものである。

このように本発明の運動推定装置においては、撮像器により取得された自律移動機械の周辺環境の各撮像画像に存在する移動物体を検出し、その移動物体に対応する画像領域を除去する。そして、移動物体に対応する画像領域を除去した後の各撮像画像から特徴対応点を抽出することにより、実際には移動物体であるにも拘わらず、撮像画像において静止物体上にあるものとして特徴対応点が抽出されるといった不具合が生じることは無い。これにより、各撮像画像に移動物体が存在している場合でも、自律移動機械の運動推定を精度良く行うことができる。

また、運動推定にとって十分な数の特徴対応点を使用するので、自律移動機械の運動推定をより精度良く行うことができる。

また、各撮像画像に存在する静止物体が少なくても、各撮像画像に必要数の移動物体が存在していれば、自律移動機械の運動推定を行うことができる。

本発明によれば、画像中に移動物体が存在している場合でも、自律移動機械の運動推定を精度良く実施することができる。これにより、例えばその後の自律移動機械の運動制御が行いやすくなる。

以下、本発明に係わる運動推定装置の好適な実施形態について、図面を参照して詳細に説明する。

図１は、本発明に係わる運動推定装置の一実施形態の概略構成を示すブロック図である。同図において、本実施形態の運動推定装置１は、自動運転車両や移動ロボット等の自律移動機械に搭載され、自律移動機械の自己運動（移動や回転等）の状態を推定する装置である。

運動推定装置１は、自律移動機械の周辺環境を撮像するカメラ２と、ＣＰＵ、ＲＯＭやＲＡＭ等のメモリ、入出力回路等により構成されたＥＣＵ（Electronic Control Unit）３とを備えている。カメラ２の数としては、１つでも良いし、２つ以上あっても良い。

ＥＣＵ３は、画像処理部４と、記憶部５と、運動推定部６と、駆動制御部７とを有している。画像処理部４は、カメラ２により取得された自律移動機械の周辺環境の撮像画像に対してフィルタ処理、２値化処理、特徴抽出処理等の画像処理を施し、画像フレームを生成する。

記憶部５には、自動車等の車両、二輪車、自転車及び歩行者等といった自律移動機械とは独立して移動する移動物体の形状や姿勢等に関するデータ（移動物体データ）が予め記憶されている。移動物体データとしては、移動物体の種類毎に数多くのデータが登録されている。

運動推定部６は、画像処理部４で得られた画像フレームを入力し、記憶部５に記憶された移動物体データを用いて所定の処理を行い、自律移動機械がどのように動いているかを推定する。

駆動制御部７は、運動推定部６で推定された運動状態に応じて、自律移動機械の駆動系（自動運転車両の場合には操舵系も含む）を制御する。

図２は、運動推定部６により実行される運動推定処理の手順の詳細を示すフローチャートである。

同図において、まず最新の画像フレームＴ’及びその１つ前の画像フレームＴを用意し、これらの画像フレームＴ，Ｔ’に存在する移動物体Ｏｉ，Ｏｉ’を検出し、移動物体Ｏｉ，Ｏｉ’に対応する画像領域Ｒｉ，Ｒｉ’を取得する（手順Ｓ１０１）。なお、ｉ＝１〜Ｎ（Ｎは検出された移動物体の数）である。

具体的には、画像フレームＴ，Ｔ’に存在する移動物体候補と記憶部５に記憶された移動物体データとを比較してパターン認識を行い、移動物体候補と何れかの移動物体データとの類似度が所定値以上のときに、当該移動物体候補を移動物体Ｏｉ，Ｏｉ’とする。

図３は、画像フレームＴ，Ｔ’の一例を示したものである。画像フレームＴ，Ｔ’には、移動物体（歩行者）Ｏ_１，Ｏ_１’に対応する画像領域Ｒ_１，Ｒ_１’と、移動物体（車両）Ｏ_２，Ｏ_２’ に対応する画像領域Ｒ_２，Ｒ_２’とが存在している。また、画像フレームＴ，Ｔ’には、３つの静止物体（建物等）も存在している。

続いて、既知の手法を用いて、カメラ２から見た場合の移動物体Ｏｉ，Ｏｉ’の位置Ｐｉ(Ｘｉ，Ｙｉ，Ｚｉ)，Ｐｉ’(Ｘｉ’，Ｙｉ’，Ｚｉ’)を計算する（手順Ｓ１０２）。

そして、カメラ２から見た場合の各移動物体の位置座標から各移動物体の移動距離を求め、この移動距離と撮像画像の取得時間間隔（例えば１００ｍｓ）とから各移動物体の速度Ｖｉを計算する（手順Ｓ１０３）。このとき、速度Ｖｉの算出精度を上げるためには、最新２枚の画像フレームＴ，Ｔ’だけでなく、過去何枚もの画像フレームの情報を用いて各移動物体の速度Ｖｉを求めるのが望ましい。

続いて、画像フレームＴ，Ｔ’において移動物体Ｏｉ，Ｏｉ’に対応する画像領域Ｒｉ，Ｒｉ’を除去する（手順Ｓ１０４）。例えば図３に示す画像フレームＴ，Ｔ’に対して本処理を実行すると、図４に示すように、移動物体Ｏ_１，Ｏ_１’に対応する画像領域Ｒ_１，Ｒ_１’と移動物体Ｏ_２，Ｏ_２’ に対応する画像領域Ｒ_２，Ｒ_２’とが取り除かれ、３つの静止物体が残った画像となる。

続いて、移動物体Ｏｉ，Ｏｉ’に対応する画像領域Ｒｉ，Ｒｉ’が除去された後の画像領域の中から、画像フレームＴ，Ｔ’間の特徴点の対応点対Ｐｊ（ｊ＝１〜Ｍ：Ｍは対応点対の数）を取得する（手順Ｓ１０５）。特徴点は、画像フレームＴ，Ｔ’に残った静止物体における特徴的な部位を表す点のことである。

続いて、特徴点の対応点対（特徴対応点）の数Ｍが予め決められた閾値よりも多いかどうかを判断する（手順Ｓ１０６）。特徴点の対応点対Ｐｊの数Ｍが閾値よりも多いと判断されたときは、特徴点の対応点対Ｐｊの位置関係に基づいて、画像フレームＴ，Ｔ’間における自律移動機械の運動推定を実施する（手順Ｓ１０７）。このとき、運動推定を行う手法としては、因子分解法（例えばC.Tomasi and Kanade,Shape and Motion from Image Streams under Orthography:aFactorization Method,International Journal of Computer Vision,pp.137-154,1992参照）等が用いられる。

一方、特徴点の対応点対Ｐｊの数Ｍが閾値よりも多くないと判断されたときは、自律移動機械の過去の運動推定結果を用いて、画像フレームＴ，Ｔ’間における自律移動機械の運動推定を実施する（手順Ｓ１０８）。このとき、例えば自律移動機械の過去の動きを線形に外挿して、運動推定を行う。

なお、手順Ｓ１０８においては、自律移動機械の過去の運動推定結果に加えて、画像フレームＴ，Ｔ’に存在する特徴点の対応点対Ｐｊの位置関係も用いて、自律移動機械の運動推定を実施しても良い。

以上において、ＥＣＵ３における記憶部５と運動推定部６の手順Ｓ１０１，Ｓ１０４とは、周辺環境の各撮像画像に存在する移動物体を検出し、移動物体に対応する画像領域を除去する画像領域除去手段を構成する。運動推定部６の手順Ｓ１０５は、移動物体に対応する画像領域を除去した後の各撮像画像から特徴対応点を抽出する特徴対応点抽出手段を構成する。運動推定部６の手順Ｓ１０７は、各撮像画像の特徴対応点の位置関係に基づいて、自律移動機械の運動状態を推定する第１推定手段を構成する。

また、運動推定部６の手順Ｓ１０６は、特徴対応点の数が所定値よりも多いかどうかを判断する判断手段を構成する。運動推定部６の手順Ｓ１０８は、判断手段により特徴対応点の数が所定値よりも多くないと判断されたときに、自律移動機械の過去の位置に基づいて、自律移動機械の運動状態を推定する第２推定手段を構成する。

図５は、本実施形態の運動推定装置１により自律移動機械の運動状態を推定する時の動作を模式的に示したものである。ここでは、図５（ａ）に示すように、走行中の自律移動機械の前方に既知物体Ａ，Ｂが存在しているものとする。この場合には、既知物体Ａ，Ｂがカメラ２により撮像されることで、既知物体Ａ，Ｂが写っている画像フレームＴが取得される。

既知物体Ａ，Ｂが静止物体であり、その静止物体上の特徴点数が十分多い場合には、図５（ｂ）に示すように、自律移動機械の走行によってカメラ２が静止物体に近づくようになる。このため、その時の静止物体をカメラ２で撮像して画像フレームＴ’を取得し、画像フレームＴ，Ｔ’間の特徴点の対応点対を求めることにより、自律移動機械の運動状態を正確に推定することができる。

ところで、既知物体Ａ，Ｂが本当は移動物体であるのに静止物体であると仮定して、運動推定を行った場合には、図５（ｃ）に示すように、実際には自律移動機械が前進しているにも拘わらず、自律移動機械が停止または後退していると誤推定されてしまう。

これに対し本実施形態では、既知物体Ａ，Ｂが移動物体である場合には、その事が認識可能であるため、移動後の移動物体をカメラ２で撮像して画像フレームＴ’を取得し、自律移動機械の過去の運動推定結果を用いることで、自律移動機械の運動状態を推定することができる。

以上のように本実施形態にあっては、連続する画像フレームＴ，Ｔ’に存在する移動物体Ｏｉ，Ｏｉ’を検出し、この移動物体Ｏｉ，Ｏｉ’に対応する画像領域Ｒｉ，Ｒｉ’を除去し、その後の画像フレームＴ，Ｔ’において特徴点の対応点対Ｐｊを抽出し、対応点対Ｐｊの数が閾値よりも多いときに、画像フレームＴ，Ｔ’間の特徴点の対応点対Ｐｊの位置関係に基づいて自律移動機械の運動状態を推定する。このように画像フレームに移動物体が存在している場合には、その移動物体に対応する画像領域を画像フレームから除去するので、図５（ｃ）に示すように移動物体を静止物体と仮定して運動推定を行うといったことが防止される。このとき、移動物体が停止している場合や、移動物体の速度変化がゼロに近い場合でも、移動物体に対応する画像領域を除去するので、移動物体の微妙な動きによる推定誤差を無くすことができる。これにより、自律移動機械の周囲に多数の移動物体が存在している場合でも、自律移動機械の運動推定を高精度に行うことができる。

また、特徴点の対応点対Ｐｊの数が閾値よりも少ないときは、自律移動機械の過去の運動推定結果を運動推定に用いることで、画像フレームに存在する静止物体が少なくても、自律移動機械の運動状態を推定することができる。

さらに、ＧＰＳ（全地球測位システム）を利用して運動推定を行う場合には、ＧＰＳ衛星からの電波が建物等で遮られて運動推定が困難になることがあり、また車輪の回転速度を検出して運動推定を行う場合には、路面でスリップすると運動推定が困難になることがあるが、本実施形態の運動推定装置１によれば、そのような不具合を回避することが可能となる。

図６は、運動推定部６により実行される運動推定処理の他の手順を示すフローチャートである。図中、図２に示すフローチャートと同一の手順には同じ符号を付し、その説明を省略する。

同図において、手順Ｓ１０６において特徴点の対応点対Ｐｊの数Ｍが閾値よりも多くないと判断されたときは、自律移動機械の過去の運動推定結果を用いて、画像フレームＴ，Ｔ’間における自律移動機械の仮の運動推定を実施する（手順Ｓ１１１）。

続いて、上述した移動物体Ｏｉの位置Ｐｉ及び移動物体の速度Ｖｉを用いて、画像フレームＴ’の時間におけるカメラ２から見た場合の移動物体Ｏｉ’の位置Ｐｉ’(Ｘｉ’，Ｙｉ’，Ｚｉ’)を補正する（手順Ｓ１１２）。このとき、移動物体が速度Ｖｉで等速度運動を行っていると仮定して、補正計算を行う。

そして、上記の移動物体Ｏｉの位置Ｐｉと補正された移動物体Ｏｉ’の位置Ｐｉ’との位置関係と、移動物体の速度Ｖｉと、自律移動機械の仮の運動推定結果とに基づいて、画像フレームＴ，Ｔ’間における自律移動機械の運動推定を実施する（手順Ｓ１１３）。この時の運動推定法としては、例えば上記の手順Ｓ１０７と同様に因子分解法等を採用することができる。

なお、手順Ｓ１１３においては、画像フレームＴ，Ｔ’に存在する特徴点の対応点対Ｐｊの位置関係も用いて、自律移動機械の運動推定を実施しても良い。

以上において、運動推定部６の手順Ｓ１０２は、撮像器２に対する移動物体の位置を求める位置検出手段を構成する。運動推定部６の手順Ｓ１０３は、移動物体の速度を求める速度検出手段を構成する。運動推定部６の手順Ｓ１１１は、判断手段により特徴対応点の数が所定値よりも多くないと判断されたときに、自律移動機械の過去の位置に基づいて、自律移動機械の仮の運動状態を推定する仮推定手段を構成する。運動推定部６の手順Ｓ１１２，Ｓ１１３は、撮像器２に対する移動物体の位置と移動物体の速度と自律移動機械の仮の運動状態とに基づいて、自律移動機械の運動状態を推定する第２推定手段を構成する。

このような構成では、既知物体Ａ，Ｂが移動物体である場合には、その事が認識可能であるため、図５（ｄ）に示すように、移動後の移動物体をカメラ２で撮像して画像フレームＴ’を取得し、画像フレームＴ，Ｔ’間の移動物体の位置関係と移動物体の速度とを求め、移動物体の動きを予測することにより、自律移動機械の運動状態を正確に推定することができる。

なお、本発明は、上記実施形態に限定されるものではない。例えば上記実施形態では、移動物体の速度Ｖｉを求め、静止物体上の特徴点の対応点対の数が閾値よりも少ないときは、移動物体が速度Ｖｉで等速度運動を行っていると仮定して運動推定を行うようにしたが、移動物体が等速度運動を行っていると仮定せずに、移動物体の加速度やジャーク等の速度変化を考慮して、運動推定を行っても良い。

本発明に係わる運動推定装置の一実施形態の概略構成を示すブロック図である。 図１に示した運動推定部により実行される運動推定処理の手順の詳細を示すフローチャートである。 図１に示した画像処理部により得られる画像フレームの一例を示す概略図である。 図３に示した画像フレームから移動物体に対応する画像領域を除去した状態を示す概略図である。 図１に示した運動推定装置により自律移動機械の運動状態を推定する時の動作を模式的に示す図である。 図１に示した運動推定部により実行される運動推定処理の他の手順を示すフローチャートである。

符号の説明

１…運動推定装置、２…カメラ（撮像器）、３…ＥＣＵ、４…画像処理部、５…記憶部（画像領域除去手段）、６…運動推定部（画像領域除去手段、特徴対応点抽出手段、第１推定手段、判断手段、第２推定手段、位置検出手段、速度検出手段、仮推定手段）。

Claims (1)

1. 撮像器により自律移動機械の周辺環境を撮像し、前記周辺環境の各撮像画像の変化に基づいて前記自律移動機械の運動状態を推定する運動推定装置において、
   前記周辺環境の各撮像画像に存在する移動物体を検出し、前記移動物体に対応する画像領域を除去する画像領域除去手段と、
   前記移動物体に対応する画像領域を除去した後の前記各撮像画像から特徴対応点を抽出する特徴対応点抽出手段と、
   前記各撮像画像の前記特徴対応点の位置関係に基づいて、前記自律移動機械の運動状態を推定する第１推定手段と、
   前記特徴対応点の数が所定値よりも多いかどうかを判断する判断手段と、
   前記撮像器に対する前記移動物体の位置を求める位置検出手段と、
   前記移動物体の速度を求める速度検出手段と、
   前記判断手段により前記特徴対応点の数が前記所定値よりも多くないと判断されたときに、前記自律移動機械の過去の位置に基づいて、前記自律移動機械の仮の運動状態を推定する仮推定手段と、
   前記撮像器に対する前記移動物体の位置と前記移動物体の速度と前記自律移動機械の仮の運動状態とに基づいて、前記自律移動機械の運動状態を推定する第２推定手段と
   を備え、
   前記第１推定手段は、前記判断手段により前記特徴対応点の数が前記所定値よりも多いと判断されたときに、前記各撮像画像の特徴対応点の位置関係に基づいて、前記自律移動機械の運動状態を推定することを特徴とする運動推定装置。

Images