(57)【要約】      （修正有）【課題】単眼カメラで特徴点の画像上の位置に誤差が有る場合でも、誤差の影響を低減し、高精度に物体までの距離を推定することができる距離推定装置を提供する。【解決手段】移動体から画像上の特徴点までの距離を推定する距離推定装置であって、距離推定装置は、第１のタイミングにおいて撮像装置１４から取得した画像に対して、画像上の特徴点を１つもしくは複数設定する第１の手段と、第２のタイミングにおいて撮像装置から取得した画像に対して、第１の手段で設定した画像上の特徴点を検知する第２の手段と、第１のタイミングと、第２のタイミングの間での画像上の特徴点の移動量を求める第４の手段と、第１のタイミングと、第２のタイミングの間での移動体の移動量を求める第５の手段と、第１のタイミングと、第２のタイミングの間での、画像上の移動量と、移動体の移動量に基づいて、移動体から特徴点までの距離を推定する第６の手段を備える。【選択図】図２
2019056629.tif 000002
【特許請求の範囲】
【請求項１】
移動体に搭載された撮像装置からの画像を用いて、移動体から画像上の特徴点までの距離を推定する距離推定装置であって、
距離推定装置は、
第１のタイミングにおいて前記撮像装置から取得した画像に対して、画像上の特徴点を１つもしくは複数設定する第１の手段と、
第２のタイミングにおいて前記撮像装置から取得した画像に対して、前記第１の手段で設定した画像上の特徴点を検知する第２の手段と、
前記第１のタイミングと、前記第２のタイミングの間での前記画像上の特徴点の移動量を求める第４の手段と、
前記第１のタイミングと、前記第２のタイミングの間での前記移動体の移動量を求める第５の手段と、
前記第１のタイミングと、前記第２のタイミングの間での、前記画像上の移動量と、前記移動体の移動量に基づいて、移動体から特徴点までの距離を推定する第６の手段を備えることを特徴とする距離推定装置。
【請求項２】
請求項１に記載の距離推定装置であって、
前記第６の手段は、前記画像上の移動量を複数の方向に分け、画像上の各方向の特徴点の移動量に基づいて特徴点までの距離を推定することを特徴とする距離推定装置。
【請求項３】
請求項２に記載の距離推定装置であって、
前記第６の手段は、複数方向のそれぞれについて特徴点までの距離を推定し、前記複数の方向で推定した複数の推定距離を組み合わせて特徴点までの距離を推定することを特徴とする距離推定装置。
【請求項４】
請求項２または請求項３に記載の距離推定装置であって、
前記第６の手段は、前記距離の推定に際し、前記複数の方向の特徴点の移動量に基づいた重み付けにより求めることを特徴とする距離推定装置。
【請求項５】
請求項３に距離推定装置であって、
前記第６の手段は、前記距離の推定に際し、特徴点の画像上の位置誤差を推定し、推定した誤差に基づいて、組み合わせパラメータを調整し、前記複数の方向で推定した距離と調整したパラメータに基づいて距離を推定することを特徴とする距離推定装置。
【請求項６】
請求項３記載の距離推定装置であって、
前記複数の推定距離の組み合わせは、移動体の移動量および／または特徴点の画像上の位置に基づいて決定することを特徴とする距離推定装置。
【請求項７】
請求項１から請求項６のいずれか１項に記載の距離推定装置であって、
前記移動体は、前記撮像装置を複数備え、設置条件の異なる前記複数の撮像装置が移動体周囲の同一の物を撮像することで、移動体から周囲の物までの距離を推定することを特徴とする距離推定装置。
【請求項８】
請求項２から請求項６のいずれか１項に記載の距離推定装置であって、
前記第６の手段は、複数の方向に分けた、各方向の特徴点の移動量のうち、その方向への移動量が大きい側の移動量を主たる要因として、特徴点までの距離を推定することを特徴とする距離推定装置。
【請求項９】
移動体に搭載された撮像装置からの画像を用いて、移動体から画像上の特徴点までの距離を推定する距離推定方法であって、
第１のタイミングにおいて前記撮像装置から取得した画像に対して、画像上の特徴点を１つもしくは複数設定し、
第２のタイミングにおいて前記撮像装置から取得した画像に対して、前記画像上の特徴点を検知し、
前記第１のタイミングと、前記第２のタイミングの間での前記画像上の特徴点の移動量を求め、
前記第１のタイミングと、前記第２のタイミングの間での前記移動体の移動量を求め、
前記第１のタイミングと、前記第２のタイミングの間での、前記画像上の移動量と、前記移動体の移動量に基づいて、移動体から特徴点までの距離を推定することを特徴とする距離推定方法。
【請求項１０】
請求項９に記載の距離推定方法であって、
前記画像上の移動量を複数の方向に分け、画像上の各方向の特徴点の移動量に基づいて特徴点までの距離を推定することを特徴とする距離推定方法。
【請求項１１】
請求項９または請求項１０に記載の距離推定方法であって、
複数の方向に分けた、各方向の特徴点の移動量のうち、その方向への移動量が大きい側の移動量を主たる要因として、特徴点までの距離を推定することを特徴とする距離推定方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットや自動車などの移動体から周囲の物体までの距離を推定する距離推定装置及び方法に関する。
【背景技術】
【０００２】
ロボットや自動車などの移動体が、収集した周囲の情報を用い、現在の自己位置及び走行状態を推定し、移動体の走行を制御する自律走行技術及び運転支援技術が開発されている。
【０００３】
この場合に、移動体の周囲の情報検出のためには、カメラなどの撮像装置、レーザセンサ、ミリ波レーダなどの様々なセンサが用いられる。また、移動体の位置を直接的に測定するためのセンサとしては、ＧＰＳ（Ｇｌｏｂａｌ  Ｐｏｓｉｔｉｏｎｉｎｇ  Ｓｙｓｔｅｍ）またはＩＭＵ（Ｉｎｅｒｔｉａｌ  Ｍｅａｓｕｒｅｍｅｎｔ  Ｕｎｉｔ）が用いられる。
【０００４】
また自律走行を実現する制御処理としては、移動体の自己位置推定処理があり、この処理は一般に幾つかの手法を組み合わせて実現する。その手法には、例えばＩＭＵで計測した移動体の速度または角速度の積算による相対移動量の算出手法（オドメトリ）、ＧＰＳ測位手法などがある。また移動体の現在位置を修正する手法には、レーザセンサあるいはカメラなどで、位置推定の基準となる路面ペイントまたは標識のようなランドマークを検出し、検出されたランドマークの位置を地図情報と比較するマップマッチング手法がある。
【０００５】
また、地図情報とランドマークがなく、かつＧＰＳも使えない場合でも有効な自己位置推定手法として、移動体の周囲に存在する物体との相対位置を推定しながら、同時に環境の地図を作成するＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ  Ｌｏｃａｌｉｚａｔｉｏｎ  ａｎｄ  Ｍａｐｐｉｎｇ）手法がある。
【０００６】
画像を用いたＳＬＡＭ処理を以下に簡単に説明する。まず第１タイミングで移動体の周囲の画像（フレーム）を取得し、特徴抽出画像処理技術で取得した画像の特徴点を抽出する。次に第２タイミングで取得した画像（フレーム）で同様に特徴点を抽出し、第１タイミングの画像で抽出した特徴点と同一の点を第２タイミングで取得した画像上で追跡する。そして、追跡した特徴点の画像上での移動量を用い、カメラ／自己位置の移動量を推定すると同時に周囲環境の地図を作成する。
【０００７】
このとき、移動体と特徴点との距離が必要になる。周囲の情報検出にステレオカメラやレーザセンサを用いる場合は、特徴点との距離を直接算出できる。しかし、コストや処理の複雑性、処理速度、メンテナンス性などの観点から、周囲の情報検出に単眼カメラ（距離を直接算出できない）を用いる場合も多く、この場合に、検出した特徴点を複数の画像（フレーム）で追跡し、その時系列情報から粒子フィルタやカルマンフィルタ等により、移動体を特徴点との距離を収束計算する、単眼ステレオ手法が用いられる。
【０００８】
この様な手法として、例えば、特許文献１は、移動体からの単眼ステレオ距離推定方法において、画像を撮影する撮影ステップと、時系列に撮影された複数の画像を基に画像中心からのオプティカルフローを算出するオプティカルフロー算出ステップと、移動体の速度を計測する速度計測ステップと、画像中心からのオプティカルフローと速度とから画像中の物体までの距離を推定する距離推定ステップとを備えたものである。
【先行技術文献】
【特許文献】
【０００９】
【特許文献１】特開２０１６−１４８５１２号公報
【発明の概要】
【発明が解決しようとする課題】
【００１０】
しかしながら、特許文献１の例のように単眼カメラで物体までの距離推定を行う場合、ノイズや演算処理誤差、画像取得タイミングなどにより特徴点の画像上の位置に誤差が生じた時に、その特徴点までの距離誤差が大きくなり、演算が収束しない、収束までの時間が長くなる、収束した場合でも距離精度が低下する、という課題がある。
【００１１】
そこで、本発明の目的は、単眼カメラで物体までの距離推定を行うに際し、特徴点の画像上の位置に誤差が有る場合でも、誤差の影響を低減し、高精度に物体までの距離を推定することができる距離推定装置及び方法を提供することにある。
【課題を解決するための手段】
【００１２】
上記課題を解決するために、本発明の距離推定装置は、「移動体に搭載された撮像装置からの画像を用いて、移動体から画像上の特徴点までの距離を推定する距離推定装置であって、距離推定装置１は、第１のタイミングにおいて撮像装置から取得した画像に対して、画像上の特徴点を１つもしくは複数設定する第１の手段と、第２のタイミングにおいて撮像装置から取得した画像に対して、第１の手段で設定した画像上の特徴点を検知する第２の手段と、第１のタイミングと、第２のタイミングの間での画像上の特徴点の移動量を求める第４の手段と、第１のタイミングと、第２のタイミングの間での移動体の移動量を求める第５の手段と、第１のタイミングと、第２のタイミングの間での、画像上の移動量と、移動体の移動量に基づいて、移動体から特徴点までの距離を推定する第６の手段を備える」ものである。
【００１３】
また上記課題を解決するために、本発明の距離推定方法は、「移動体に搭載された撮像装置からの画像を用いて、移動体から画像上の特徴点までの距離を推定する距離推定方法であって、第１のタイミングにおいて撮像装置から取得した画像に対して、画像上の特徴点を１つもしくは複数設定し、第２のタイミングにおいて撮像装置から取得した画像に対して、第１の手段で設定した画像上の特徴点を検知し、第１のタイミングと、第２のタイミングの間での画像上の特徴点の移動量を求め、第１のタイミングと、第２のタイミングの間での移動体の移動量を求め、第１のタイミングと、第２のタイミングの間での、画像上の移動量と、移動体の移動量に基づいて、移動体から特徴点までの距離を推定する」ものである。
【発明の効果】
【００１４】
本発明の距離推定装置及び方法によれば、天候などの様々な外乱、時刻や演算の誤差、ノイズの影響があった場合でも、安定して演算が行え、誤差の影響を低減し、高精度に物体までの距離を推定することができる。
【図面の簡単な説明】
【００１５】
【図１】本発明の実施例に係る距離推定装置１の構成例を示す図。
【図２】画像処理部が行う処理手順を示すフローチャート。
【図３ａ】移動体１００が直進した場合の、実際の現場（移動体走行現場）と、撮像したフレーム画像の関係を示す図。
【図３ｂ】移動体１００が旋回した場合の、実際の現場（移動体走行現場）と、撮像したフレーム画像の関係を示す図。
【図４】物体までの距離と画像上の誤差による影響を説明する図。
【図５】推定した距離と特徴点の画像上での移動量の関係を説明する図。
【図６ａ】特徴点の移動量を２方向に分けて移動体の移動量を推定する方法として、ｕ方向の移動量のみを用いた距離推定方法を説明する図。
【図６ｂ】特徴点の移動量を２方向に分けて移動体の移動量を推定する方法として、ｖ方向の移動量のみを用いた距離推定方法を説明する図。
【図７】画像上の特徴点の例を説明する図。
【発明を実施するための形態】
【００１６】
以下、本発明の実施例に係る距離推定装置について、図面を用いて説明する。
【実施例】
【００１７】
図１は、本発明の実施例に係る距離推定装置１の構成例を示す図である。
【００１８】
距離推定装置１は、自動車またはロボットなどの移動体１００に搭載されている。距離推定装置１は、一台もしくはそれ以上の撮像装置１２（１２ａ、１２ｂ、・・・１２ｎ）と、情報処理装置１３とを有する。撮像装置１２は、例えばスチルカメラまたはビデオカメラでよいし、単眼カメラでもステレオカメラでもよい。
【００１９】
距離推定装置１内の情報処理装置１３は、撮像装置１２で撮像された画像を処理して、周囲の物体までの距離や移動体１００の位置または移動量を算出する。算出された位置または移動量は、表示や、移動体１００の制御に用いられる。
【００２０】
情報処理装置１３は、例えば一般的なコンピュータであって、撮像装置１２によって撮像された画像を処理する画像処理部１４と、画像処理結果に基づく処理を行う制御部（ＣＰＵ）１５と、メモリ１６と、ディスプレイなどの表示部１７と、これら構成要素を相互に接続するバス１８とを有する。計算機で構成された情報処理装置１３は、画像処理部１４及び制御部１５が所定のコンピュータプログラムを実行することにより、様々な演算処理を行う。
【００２１】
撮像装置１２は、移動体１００に設置され、その周囲の画像を連続的に取得できれば良く、一台であっても複数台であっても良い。撮像装置１２ａの設置方法は、例えば、移動体１００の前方、後方、側方のどこに設置しても良いし、前方に設置する場合でも、移動体１００の前方の遠景を撮像しても良いし、移動体１００の下方の路面（白線、または路面ペイント）などの近景を撮像しても良い。撮像装置１２を複数設置する場合の他の撮像装置１２ｂ、・・・撮像装置１２ｎは、撮像装置１２ａと同じ撮像方向または領域を撮像しても良いし、異なる撮像方向または領域を撮像しても良い。このとき、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、様々な外乱、ノイズ、誤差の影響を同時に受けないような条件で設置するのがよい。
【００２２】
例えば、雨や日差しなどの環境外乱（ノイズ）の影響を低減するため、撮像装置１２ａは移動体１００の前方で前向きに設置する場合、撮像装置１２ｂは移動体１００の後方で後ろ向きまたは下向きに設置する。これにより、例えば、降雨時に撮像装置１２ａが撮像した画像が雨滴の影響で不鮮明であっても、撮像装置１２ｂが撮像した画像は雨滴の影響を受けにくく、日差し（上からの強い光線）の影響で撮像装置１２ａの画像が不鮮明であっても、撮像装置１２ｂが撮像した画像は影響を受けないものとすることができる。
【００２３】
また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、互いに異なる撮像条件（絞り値、ホワイトバランス、等）で撮影してもよい。例えば、明るい場所用にパラメータを調整した撮像装置と、暗い場所用にパラメータを調整した撮像装置とを搭載することで、環境の明暗によらず撮像可能としてもよいし、様々特性の撮像素子やレンズを組み合わせても良い。
【００２４】
また、撮像装置１２ａ、撮像装置１２ｂ・・・撮像装置１２ｎは、それぞれ互いに異なる撮像方向または領域を撮影してもよい。例えば、撮像装置１２ａは、移動体１００の前方の遠景を撮像し、建物や木、標識などの立体物のドマークなどの特徴点が抽出されるようにし、撮像装置１２ｂは、移動体１００周辺の路面などの近景を撮像し、移動体１００の周囲の白線、または路面ペイントなどが検出されるようにしてもよい。このようにすることで、同時刻帯に多くの特徴点が抽出でき、移動体の速さや環境外乱に影響されにくい特徴点を撮像できる。
【００２５】
さらに、撮像装置１２は、複数の撮像装置がセットになったステレオカメラ／複眼カメラでもよいし、単眼カメラでもよい。撮像装置１２が複眼カメラの場合、撮像装置１２から特徴点までの距離を直接計測できるが、撮像装置１２が単眼カメラの場合でも、移動体が平らな路面を走行する場合は、画像上の画素（ピクセル）位置と実際の位置（ｘ、ｙ、ｚ）との関係が一定になるため、撮像装置１２（移動体１００）から特徴点までの距離を幾何学的に計算できる。
【００２６】
撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、制御部１５から撮影指令を受けたとき、または一定の時間間隔で自動的／連続的に画像を撮像する。取得時刻の誤差は、物体までの距離や移動体１００の位置の誤差に影響するので、一定の時間間隔で自動的／連続的に画像を撮像した方が撮像時刻の誤差は低減される。
【００２７】
メモリ１６は、情報処理装置１３の主記憶装置（メインメモリ）およびストレージなどの補助記憶装置で構成され、撮像装置１２により撮像された画像のデータ及び撮像時刻を補助記憶装置に格納する。また、メモリ１６には、画像処理部１４、制御部１５、表示部１７の計算途中や計算結果の情報も格納される。例えば画像処理部１４は、メモリ１６に格納された画像データ及び撮像時刻に基づいて、様々な画像処理を行い、計算途中の中間画像をメモリ１６に保存する。これにより中間画像は、画像処理部１４による他の処理、制御部１５、表示部１７でも利用できる。
【００２８】
バス１８は、画像の情報伝達に用いることが出来るものであれば良く、例えばＩＥＢＵＳ（登録商標：Ｉｎｔｅｒ  Ｅｑｕｉｐｍｅｎｔ  Ｂｕｓ）やＬＩＮ（Ｌｏｃａｌ  Ｉｎｔｅｒｃｏｎｎｅｃｔ  Ｎｅｔｗｏｒｋ）やＣＡＮ（Ｃｏｎｔｒｏｌｌｅｒ  Ａｒｅａ  Ｎｅｔｗｏｒｋ）などで構成できる。
【００２９】
画像処理部１４は、異なるタイミングで取得された画像（フレーム）について特徴点を抽出し、同一特徴点を追跡（トラッキング）する。そして、特徴点の画像上の移動量により移動体１００の移動量を算出し、所定の地点（既知の移動体の位置）に移動体の移動量を加算して現在位置を推定する。このとき、多数の特徴点から算出される移動体の複数の位置候補から、特徴点の移動量や移動体１００の移動速度等に基づいて高精度に移動体１００の位置を推定する。なお本発明において、位置を推定することは距離を推定することと同義であり、位置が推定できたということはロボットや自動車などの移動体から周囲の物体までの距離を推定したことをも意味している。
【００３０】
制御部１５は、画像処理部１４での画像処理の結果に基づいて、移動体１００の駆動部や操舵部（図示せず）に対して移動速度や方向に関する指令を出力する。
【００３１】
図２は、画像処理部１４が行う画像処理手順を示すフローチャートである。
【００３２】
画像処理部１４では、最初に処理ステップＳ２１において、メモリ１６から撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した１フレーム、または複数フレームの画像データを取得する。各画像データには取得時間が記録されているため、処理する順番に拘束条件はなく、この画像データは、どの時刻に撮像したものでもよいし、複数フレームの場合、連続していなくともよい。
【００３３】
なお以下の説明においては前方を撮像する撮像装置１２ａが撮像した異なる時刻での２つのフレームに着目して本発明の処理についての説明を行うものとする。従って、同様の処理は他の撮像装置から取得したフレームについて行われてもよい。
【００３４】
次に、画像処理部１４は、処理ステップＳ２２において、取得したフレーム画像内の特徴点を抽出する。特徴点には、例えば、画像内のエッジやコーナーやピクセル強度の最大値や最小値などがある。特徴点の抽出には、Ｃａｎｎｙ、Ｓｏｂｅｌ、ＦＡＳＴ、Ｈｅｓｓｉａｎ、Ｇａｕｓｓｉａｎなどの画像処理技術を用いることができる。具体的なアルゴリズムは、画像の特徴に応じて適宜選択されるのがよい。
【００３５】
次に画像処理部１４は、フレーム画像の時系列に従って、各フレーム画像でそれぞれ抽出された同一特徴点を別のフレームの画像上でトラッキングする。トラッキングにはＬｕｃａｓ−Ｋａｎａｄｅ法やＳｈｉ−Ｔｏｍａｓｉ法やＤｉｒｅｃｔ  Ｍａｔｃｈｉｎｇ法などの技術を用いることができる。具体的なアルゴリズムは、画像の特徴に応じて適宜選択されるのがよい。また、処理ステップＳ２３のトラッキングは、直前や直後に取得した連続フレームの特徴点に限らず、数フレーム間隔でもよい。
【００３６】
次に処理ステップＳ２４において、それぞれ処理ステップＳ２３でトラッキングした特徴点の移動量を算出する。なお本発明において、「移動量」には、特徴点の移動量と、移動体の移動量があるので、用語的に区別して使用している。特徴点の移動量は、例えば、処理ステップＳ２３のトラッキングで得られた、先行する時刻でのフレームの第１の画像上での特徴点のピクセル位置と、後行する時刻での別のフレームの第２の画像上での、同一特徴点のピクセル位置の差分を算出して求められる。
【００３７】
処理ステップＳ２５では、撮像装置１２の移動量、つまり移動体の移動量を推定する。この処理ステップＳ２５では、撮像装置１２が、あるフレームを撮像した時刻と別のフレームを撮像した時刻間の移動体１００の実際の移動量を推定する。但し、最終的に移動体１００のフレーム上における移動量を推定できるものであれば、他の手法を用いてもよい。例えばＧＰＳ情報、オドメトリ、画像オドメトリ、ＳＬＡＭなどの技術を用いてもよく、また過去の移動体の移動量に基づいて今回の移動量を推定する時系列フィルタ（粒子フィルタやカルマンフィルタ）、前記センサ情報と各種フィルタの組み合わせでもよい。
【００３８】
処理ステップＳ２５を実施するタイミングは、処理ステップＳ２４の直後でもよいが、処理ステップＳ２１から処理ステップＳ２４と平行に実施してもよい。処理ステップＳ２６の処理が始まる前であれば、処理ステップＳ２５をいつでも実施してもよい。
【００３９】
処理ステップＳ２６では、処理ステップＳ２４で得られた特徴点の移動量の情報と処理ステップＳ２５で得られた移動体１００の移動量の情報を用いて、処理ステップＳ２３でトラッキングした特徴点と撮像装置１２（移動体）との量を推定する。処理ステップＳ２６の詳細は後述する。
【００４０】
本発明の考え方について図３ａ、図３ｂを用いて説明する。図３ａ、図３ｂには、実際の現場（移動体走行現場）と、移動体に搭載された撮像装置１２が撮像したフレーム画像が対比して示されている。なお実際の現場（移動体走行現場）における位置は、移動体１００の地上空間上にある固定座標（ｘ、ｙ）で表しており、３０ａとして図示表記している。また移動体に搭載された撮像装置１２が撮像したフレーム画像における位置は、座標（ｕ、ｖ）で表しており、３０ｂとして図示表記している。
【００４１】
図３ａは、移動体１００が直進した場合の、実際の現場（移動体走行現場）と、撮像したフレーム画像の関係を示している。実際の現場（移動体走行現場）において、時刻ｔ０と時刻ｔ１における移動体位置が１００ｔ０と１００ｔ１であり、この間の移動体１００の移動量が３１ｂで示されている。他方、撮像したフレーム３３ｂの画像についてみると、道路側方に設定した特徴点（遠方の特徴点３６ａと、近傍の特徴点３６ｂを表示）の時刻ｔ０における特徴点位置が３６ａｔ０、３６ｂｔ０であり、時刻ｔ１における特徴点位置が３６ａｔ１、３６ｂｔ１で示されており、この間の特徴点の移動量が３７ａ、３７ｂで示されている。
【００４２】
図３ａにおいて、実際の現場（移動体走行現場）の座標３０ａは、移動体１００の地上空間上にある固定座標（ｘ、ｙ）である。ここでは説明の都合上、簡単のため、移動体１００は２次元平面上にいると仮定する。移動量３１ｂは移動体１００が走行した移動量（並進）である。ここでは簡単のため、移動体１００の移動量３１ｂを（０、ｙ）とする。
【００４３】
他方フレーム３３ｂは、移動体１００が移動量３１ｂを走行した後の撮像装置１２が撮像した画像のうちの１枚を表す。処理ステップＳ２１から処理ステップＳ２３で抽出した特徴点３６ａ、３６ｂの画像上での移動量は、それぞれ特徴点移動量３７ａ、３７ｂである。図３ａにおいて、画像の上方にある特徴点３６ａは、移動体の遠方に存在し、画像の下方にある特徴点３６ｂは、移動体の近傍に存在する。このときの特徴点移動量と画像の位置の関係は、移動量３７ａ（遠方）＜移動量３７ｂ（近傍）となる。
【００４４】
図３ｂは、移動体１００が大きく旋回した場合の、実際の現場（移動体走行現場）と、撮像したフレーム画像の関係を示している。実際の現場（移動体走行現場）において、時刻ｔ０と時刻ｔ１における移動体位置が１００ｔ０と１００ｔ１であり、この間の移動体１００の移動量が３１ａで示されている。他方、撮像したフレーム３３ａの画像についてみると、道路遠方に設定した特徴点３４ｂ、３４ｃと、道路上に設定した近傍の特徴点３４ａの時刻ｔ０における特徴点位置が３４ｂｔ０、３４ｃｔ０、３４ａｔ０であり、時刻ｔ１における特徴点位置が３４ｂｔ１、３４ｃｔ１、３４ａｔ１で示されており、この間の特徴点の移動量が３５ｂ、３５ｃ、３５ａで示されている。
【００４５】
図３ｂにおいて、実際の現場（移動体走行現場）における移動体の移動量３１ａは（ｘ、ｙ、ｙａｗ）となる。なお移動体１００の旋回成分をｙａｗとして表している。ここで、距離３２は、移動体１００が移動量３１ａを走行した後の位置から遠方にある立体物までの距離であり、フレーム３３ａは、移動体１００が移動量３１ａを走行した後の撮像装置１２が撮像した画像のうちの１枚を表す。座標３０ｂはフレーム３３ａ上の座標（ｕ、ｖ）であり、処理ステップＳ２１から処理ステップＳ２３で抽出した特徴点３４ａ、３４ｂ、３４ｃの画像上での移動量は、それぞれ特徴点の移動量３５ａ、３５ｂ、３５ｃとして示されている。
【００４６】
移動体１００が移動量３１ａを走行した後、移動体の近傍にある特徴点３４ａの画像上での移動量３５ａはｕ軸成分とｖ軸成分がともに大きいが、遠方にある特徴点３４ｂ、特徴点３４ｃの移動量３５ｂ、移動量３５ｃはｕ軸成分が大きく、ｖ軸成分はそれに比べ小さな値となる。また、特徴点３４ａが抽出された立体物は、特徴点３４ｂと特徴点３４ｃが抽出された立体物に比べて移動体１００に近いため、画像上での移動量３５ａ（ｖ軸成分）＞移動量３５ｂ（ｖ軸成分）、移動量３５ａ（ｖ軸成分）＞移動量３５ｃ（ｖ軸成分）となる。
【００４７】
以上、図３ａでは直進の場合について、図３ｂでは旋回の場合について、実際の現場（移動体走行現場）と、撮像したフレーム画像の関係を説明したが、このことから以下のようなことが導き出すことができる。まず、遠方特徴点と近傍特徴点が受ける移動体１００の移動量３１にａよる影響は異なる。遠方特徴点３４ｂ、特徴点３４ｃの画像上での移動量３５ｂ（ｕ軸成分）、移動量３５ｃ（ｕ軸成分）は、移動体１００の旋回成分ｙａｗの依存が大きく、近傍特徴点３４ａの画像上での移動量３５ａは移動体１００の（ｘ、ｙ、ｙａｗ）に依存する。
【００４８】
従って、移動体１００の移動量、画像上の特徴点位置（移動体からの距離：遠方・近傍）によって、概略の特徴点の移動量が推定でき、画像上でのノイズの影響が少ない成分が分かる。
【００４９】
図４は、物体までの距離が画像上の誤差（ノイズ等）により受ける影響について説明する図である。図４の上側には、遠方にある物体４１ａとこのときに撮像したフレーム４４ａを表しており、図４の下側には、近傍にある物体４１ｂとこのときに撮像したフレーム４４ｂを表している。なお実際の現場（移動体走行現場）の座標３０ａでは、移動体の高さ方向をＺで表している。
【００５０】
図４において、フレーム４４ａ、４４ｂでは、処理ステップＳ２２で物体４１ａ、４１ｂから抽出した特徴点について、誤差が無い場合の特徴点を４２ａ、４２ｂとして表している。ここで、処理ステップＳ２３で特徴点をトラッキングした時、特徴点の位置に誤差４３ａ、４３ｂが発生し、特徴点４２ａ、４２ｂは点４５ａ、４５ｂになったものとする。但し、ここでは簡単のため、誤差４３の影響を縦方向のみで説明するが、他の方向の移動も可能である。
【００５１】
画像上での誤差４３ａ、４３ｂを、移動体１００の地上空間上にある固定座標３０ａの高さ方向に変換すると、特徴点４２ａ、４２ｂの実際の位置から、誤差分として４６ａ、４６ｂだけ離れた位置となる。
【００５２】
これらの図によれば、実際の現場（移動体走行現場）における測定上の誤差４６ａおよび誤差４６ｂは、移動体からの距離に比例して大きくなり、誤差４６ａ＞誤差４６ｂとなる。よって誤差は、移動体からの距離が遠いほど影響を受けることがわかる。
【００５３】
図５は、推定した移動体から物体までの距離と特徴点の画像上での移動量の関係を示す図である。図５において、画像５０は処理ステップＳ２１で取得した画像を示し、特徴点５１は処理ステップＳ２２で抽出した特徴点を示す。処理ステップＳ２３で特徴点５１をトラッキングした後、画像５０上での移動量をｄｐとすると、移動量ｄｐは座標３０ｂについて、ｕ方向のｄｕとｖ方向のｄｖに分けて把握することができる。
【００５４】
また、処理ステップＳ２３で特徴点５１をトラッキングした後の誤差をｅとする。ここで、誤差ｅは確率的にｕ方向とｖ方向に分散し、それぞれの方向が受ける影響は未知となる。誤差ｅがｕ方向に影響すると、ｕ方向のｄｕに誤差ｅが追加され、移動量ｄｐが（１）式の通り移動量ｄｐＡとなり、誤差ｅがｖ方向に影響すると、ｖ方向のｄｖに誤差ｅが追加され、移動量ｄｐが（２）式の通り移動量ｄｐＢとなる。
［数１］
ｄｐＡ＝ｓｑｒｔ（ｄｖ＾２＋（ｄｕ＋ｅ）＾２）・・・（１）
［数２］
ｄｐＢ＝ｓｑｒｔ（ｄｕ＾２＋（ｄｖ＋ｅ）＾２）・・・（２）
図４に示した通り、誤差ｅが一定の場合でも、物体の距離によって座標３０に対する誤差が大きくなる。また図３ａ、図３ｂの説明によると、移動体が並進移動すると、近傍物体は画像上での移動量は大きいが、遠方物体は画像上での移動量は小さい。従って、画像上での移動量が小さい場合、誤差ｅに近づくため、その移動量で推定した距離の誤差は大きい。例えば、図５の場合、ｄｕ＞ｄｖのため、（ｅ＋ｄｕ）に対して、誤差ｅの割合は小さい。一方、（ｅ＋ｄｖ）に対して、誤差ｅの割合は大きい。従って、移動量が大きい特徴点の移動量が大きい方向で距離を推定すれば誤差が小さいということが理解できる。
【００５５】
図６ａ、図６ｂは、特徴点の移動量をｕ方向とｖ方向に分けて距離を推定する場合の処理ステップＳ２６の詳細を説明する図である。図６ａ、図６ｂでは、説明を簡単にするため、撮像装置１２の設置角度を水平にし、画像上のｕ軸方向と座標３０ａのｘ方向を一致させ、画像上のｖ軸方向と座標３０ａのｚ方向を一致させたが、実際は複数の撮像装置１２によってそれぞれ異なる設置位置、角度、方向となる。すなわち、移動量を分ける方向および数は、状況により様々なケースが存在する。
【００５６】
図６ａを用いて、ｕ方向の移動量のみを用いた距離推定方法を説明する。移動体１００は、時刻ｔ＝ｔｎ−１の位置から時刻ｔ＝ｔｎの位置まで移動量Ｄａを走行し、処理ステップＳ２３において、処理ステップＳ２２で物体６１ａから抽出した特徴点をトラッキングする。このとき、時刻ｔ＝ｔｎ−１における物体６１ａに対する角度を角度ｎ−１とし、時刻ｔ＝ｔｎにおける物体６１ａに対する角度を角度ｎとする。時刻ｔ＝ｔｎの位置から物体６１ａまでの距離Ｋｕは、幾何学的な関係から（３）式で算出できる。ここで移動量Ｄａは、処理ステップＳ２５で推定する移動量である。
［数３］
Ｋｕ＝Ｄａ×ｔａｎ（ｎ−１）／（ｔａｎ（ｎ）−ｔａｎ（ｎ−１））・・・（３）
図６ｂを用いて、ｖ方向の移動量のみを用いた距離推定方法を説明する。移動体１００は、時刻ｔ＝ｔＮ−１の位置から時刻ｔ＝ｔＮの位置まで移動量Ｄｂを走行し、処理ステップＳ２３において、処理ステップＳ２２で物体６１ｂから抽出した特徴点をトラッキングする。時刻ｔ＝ｔＮ−１の物体６１ｂに対するｖ方向の角度を角度Ｎ−１とし、時刻ｔ＝ｔＮの物体６１ｂに対するｖ方向の角度を角度Ｎとする。時刻ｔ＝ｔＮの位置から物体６１ｂまでの距離Ｋｖは、幾何学的関係から（４）式で算出できる。ここで移動量Ｄｂは、処理ステップＳ２５で推定する移動体の移動量である。
［数４］
Ｋｖ＝Ｄｂ×ｔａｎ（Ｎ−１）／（ｔａｎ（Ｎ）−ｔａｎ（Ｎ−１））・・・（４）
式（３）式と（４）式の角度ｎ、ｎ−１、Ｎ、Ｎ−１は、例えば、（５）式と（６）式の方法で算出できる。ここでＷは撮像装置１２が撮像する画像のｕ方向のサイズであり、Ｖはｖ方向の最大サイズ、ＦＯＶｕとＦＯＶｖは撮像装置１２が撮像できるｕ方向とｖ方向の最大角度である。
［数５］
ｎ＝ａｔａｎ［｛（Ｗ−２×ｕ）×ｔａｎ（ＦＯＶｕ／２）｝／Ｗ］・・・（５）
［数６］
Ｎ＝ａｔａｎ［｛（Ｖ−２×ｖ）×ｔａｎ（ＦＯＶｖ／２）｝／Ｖ］・・・（６）
ここで算出した距離Ｋｕと距離Ｋｖは、それぞれに重みをつけて補正した距離Ｋを算出する。図５で説明した通り、推定した距離の精度は特徴点の画像上での位置、移動量、移動方向に依存する。単純に移動量の大きさを考慮した場合は、（７）式に示すように、ｕ方向とｖ方向の移動量ｄｕとｄｖを重み付けすればよい。
［数７］
Ｋ＝（Ｋｕ×ｄｕ＋Ｋｖ×ｄｖ）／（ｄｕ＋ｄｖ）・・・（７）
また、（８）式の通り、移動量ｄｕとｄｖの重みをパラメータｍ＝１、２、３．．．で調整してもよい。
［数８］
Ｋ＝（Ｋｕ×ｄｕ＾ｍ＋Ｋｖ×ｄｖ＾ｍ）／（ｄｕ＾ｍ＋ｄｖ＾ｍ）・・・（８）
また、移動量ｄｕとｄｖを重みにせず、ＫｕとＫｖの平均でＫを算出してもよいし、（９）、式に示す通り、移動量の短い方向の重みを０にし、ＫｕとＫｖに切り替えてもよい。
［数９］
Ｋ＝Ｋｕ、ｉｆ  ｄｕ＞ｄｖ
Ｋｖ、ｉｆ  ｄｕ＜ｄｖ・・・・・・（９）
また、処理ステップＳ２５で推定した移動体１００の移動量と時刻ｔ＝ｔｎ−１のピクセル（ｕ、ｖ）ｎ−１と推定した距離を用いて、時刻ｔ＝ｔｎのピクセル（Ｕ、Ｖ）ｎを推定し、実際の時刻ｔ＝ｔｎのピクセル（ｕ、ｖ）ｎと比較することにより、ＫｕとＫｖの選択、もしくは重み付けを行っても良い。ピクセル（Ｕ、Ｖ）ｎは（１０）式で得られる。ここでＲとＴは処理ステップＳ２５で推定した移動体１００の移動量（並進と旋回）を表す行列であり、Ｐｎ−１とＰｎはそれぞれの時刻ｔ＝ｔｎ−１と時刻ｔ＝ｔｎのピクセルからメートルに変換する行列である。
［数１０］
（Ｕ、Ｖ）ｎ＝Ｐｎ−１×Ｒ×Ｔ×Ｐｎ−１×（ｕ、ｖ）ｎ−１・・・（１０）
推定したピクセル（Ｕ、Ｖ）ｎを求め、時刻ｔ＝ｔｎの実際のピクセル（ｕ、ｖ）ｎと比較し、ｕ方向とｖ方向の誤差を求め、ｕ方向とｖ方向の距離ＫｕとＫｖの重み付けをする。
【００５７】
（１１）式は、誤差の小さい方向で推定した距離を選択する場合を示しているが、「選択」は、距離ＫｕとＫｖ重みを、それぞれ１、０もしくは０、１にするのと同等ケースである。
［数１１］
Ｋ＝Ｋｕ、ｉｆ  ａｂｓ（Ｖｎ−ｖｎ）＞ａｂｓ（Ｕｎ−ｕｎ）
Ｋｖ、ｉｆ  ａｂｓ（Ｖｎ−ｖｎ）＜ａｂｓ（Ｕｎ−ｕｎ）・・・・（１１）
また、（１２）式で示すように、処理ステップＳ２３でトラッキングした特徴点の予測誤差を表すパラメータＧで距離Ｋｕと距離Ｋｖの有効判定をしてもよい。特徴点の予測誤差は、例えば、移動体１００の移動量に基づいて設定でき、例えば移動体１００が高速で移動する場合は処理ステップＳ２３で特徴点のトラッキングが失敗する可能性が高いため、速度の高さに応じてＧを高く設定すればよい。また、特徴点の画像上の位置に基づいても設定でき、画像上の位置が下方では、撮像装置１２に近い特徴点の移動量が大きくなり、相対的に誤差が小さくなるため、位置が下方に行くほどＧを小さく設定すればよい。
［数１２］
Ｋ  ＝  Ｋｕ、ｉｆ  Ｇ＜ｄｖ
Ｋｖ、ｉｆ  Ｇ＜ｄｕ・・・・・（１２）
以上の複数の方法の組み合わせや、異なる撮像装置１２の画像および異なる画像取得タイミングで同一特徴点をトラッキングした時系列情報の組み合わせにより、状況に応じて誤差の影響が小さい組み合わせパターンを用いて高精度に物体までの距離が推定できる。ここで、異なる撮像装置１２で同一の物体の距離を測定するためには、移動体が移動する過程において、異なる複数の撮像装置１２が同じ領域を撮像できるように、位置、方向、画角等のセッティングをすればよい。
【００５８】
図７は特徴点の例とその処理例を説明する図である。図において、移動量Ｄは移動体１００が走行した移動量、画像７１は、移動体１００が移動量Ｄを走行する前の処理ステップＳ２１で取得した画像である。特徴点７２はビルなどの遠方の高い箇所の例、特徴点７３は標識などの近傍の立体物の例、特徴点７４は路面ペイントなどの近傍の路面上の特徴点の例を示す。特徴点はあらゆる場所に存在するので、本発明の距離を測定する「物体」とは、移動体の「周囲の物」を表し、標識などの構造物そのものに限らず、路面のひびや模様といった、構造物や自然物の一部分も含まれる。特徴点７２、７３、７４は処理ステップＳ２２で抽出し、処理ステップＳ２３で別のタイミングで取得した画像にトラッキングされ、処理ステップＳ２４で移動量が算出される。移動量７２ａ、７３ａ、７４ａは、それぞれ特徴点７２、７３、７４の処理ステップＳ２４算出した画像上での移動量である。移動量７２ａ、７３ａ、７４ａのｕ方向とｖ方向の移動量は、それぞれ、移動量７２ｂ、７３ｂ、７４ｂと、移動量７２ｃ、７３ｃ、７４ｃである。
【００５９】
処理ステップＳ２５の移動体の移動量推定は、撮像装置１２の設置高さと角度が既知のため、路面から抽出した特徴点との距離から推定できる。従って図７において、路面から抽出した特徴点７４を用いて、画像オドメトリ技術で移動体１００の移動量Ｄが推定できる。
【００６０】
処理ステップＳ２６で特徴点７２と特徴点７３の距離を推定する場合、例えば、特徴点７２と特徴点７３のｕ方向とｖ方向に分けた移動量７２ｂ、移動量７３ｂ、移動量７２ｃ、移動量７３ｃを用いて推定する。例として、ここでは（３）式と（４）式でそれぞれの特徴点のＫｕとＫｖを算出し、それぞれの特徴点で算出したＫｕとＫｖを例えば（７）式で組み合わせ、処理ステップＳ２２で抽出した其々の特徴点のｕ方向とｖ方向の移動量を重み付けにする。特徴点７３と特徴点７２は、画像７１の位置が異なるため、ｕ方向とｖ方向の移動量の割合が異なる。例えば、図７の例では移動量７３ｃ＞移動量７３ｂ、移動量７２ｃ移動量７２ｂとなる。
【００６１】
従ってこのとき、移動量７３ｃで算出するＫｕの重みは移動量７３ｂで算出するＫｖの重みより大きいので、特徴点７３への距離ＫはＫｕに近い値となる。一方、移動量７２ｃで算出するＫｕの重みは移動量７２ｂで算出するＫｖの重みとあまり差がないので、特徴点７２への距離ＫはＫｖとＫｕの平均に近い値となる。また、特徴点７３は特徴点７２より移動体１００に近いため、移動量７３ａ＞移動量７２ａであり、特徴点７３の方が高精度に距離が推定できる。
【００６２】
以上に説明した本発明の距離推定装置は、「移動体１００に搭載された撮像装置１４からの画像を用いて、移動体１００から画像３３上の特徴点３４、３６までの距離を推定する距離推定装置１であって、距離推定装置１は、第１のタイミングにおいて撮像装置１４から取得した画像に対して、画像上の特徴点を１つもしくは複数設定する第１の手段Ｓ２２と、第２のタイミングにおいて撮像装置から取得した画像に対して、第１の手段で設定した画像上の特徴点を検知する第２の手段Ｓ２３と、第１のタイミングと、第２のタイミングの間での画像上の特徴点の移動量を求める第４の手段Ｓ２４と、第１のタイミングと、第２のタイミングの間での移動体の移動量を求める第５の手段Ｓ２５と、第１のタイミングと、第２のタイミングの間での、画像上の移動量と、移動体の移動量に基づいて、移動体から特徴点までの距離を推定する第６の手段Ｓ２６を備える」ものである。
【００６３】
また本発明においては、画像上の移動量を複数の方向に分け、画像上の各方向の特徴点の移動量に基づいて特徴点までの距離を推定するが、その際に複数の方向に分けた、各方向の特徴点の移動量のうち、その方向への移動量が大きい側の移動量を主たる要因として、特徴点までの距離を推定するものである。
【００６４】
ここで主たる要因とするための具体手法は、大きい側の推定距離を採用する、重みづけして求める、誤差を考慮するなどである。
【符号の説明】
【００６５】
１：位置推定装置
１２：撮像装置
１３：情報処理装置
１４：画像処理部
１５：制御部
１６：メモリ
１７：表示部
１００：移動体
３０ａ、３０ｂ：座標
３３ａ、３３ｂ、４４ａ、４４ｂ：フレーム
３４ａ、３４ｂ、３６ａ、３６ｂ：特徴点
３１ａ、３１ｂ、３５ａ、３５ｂ、３５ｃ、３７ａ、３７ｂ：移動量
４３ａ、４３ｂ：誤差
【図１】
2019056629.tif 000003
【図２】
2019056629.tif 000004
【図３ａ】
2019056629.tif 000005
【図３ｂ】
2019056629.tif 000006
【図４】
2019056629.tif 000007
【図５】
2019056629.tif 000008
【図６ａ】
2019056629.tif 000009
【図６ｂ】
2019056629.tif 000010
【図７】
2019056629.tif 000011