JP6932058B2 - 移動体の位置推定装置及び位置推定方法 - Google Patents

Description

本発明は、ロボットや自動車などの移動体の位置推定装置及び位置推定方法に関する。

ロボット及び自動車などの移動体がその周囲の情報を収集し、移動体の現在位置及び走行状態を推定し、移動体の走行を制御する自律走行技術や運転支援技術の開発が進められている。

移動体の周囲の情報や位置の情報を収集する手段としては、様々な種類のセンサが用いられている。周囲の情報を計測するためのセンサとしては、カメラなどの撮像装置の他、レーザセンサやミリ波レーダなどがある。移動体の位置を測定するためのセンサとしては、ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）またはＩＭＵ（Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ）が用いられる。

自律走行制御においては、移動体に搭載された制御装置が、例えばＩＭＵで算出した移動体の速度または角速度を積算したり、ＧＰＳ測位を用いたりして、移動体自身の位置（自己位置）を推定している。また、地図情報やランドマークがなく、ＧＰＳも使えない場合には、移動体の周囲に存在する物体との相対位置を推定しながら走行中環境の地図を作成するＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）法が用いられている。ただし、ＳＬＡＭ法で推定した相対位置の誤差は、時系列に蓄積するため、位置修正が必須となる。この位置修正には、例えば、レーザセンサあるいはカメラなどを用いて周囲の情報を収集し、位置推定の際の基準となる路面ペイントまたは標識のようなランドマークを検出し、そして、制御装置により、検出されたランドマークの位置を地図情報と比較することにより、移動体の現在位置を修正している。従って、検出されたランドマークに位置誤差がある場合、移動体の位置を正確に修正できない場合がある。

特に、単眼カメラでランドマークを認識する場合、認識されたランドマークまでの距離を幾何学的に算出するため、カメラの画像上でのランドマークと実際のランドマークの位置を正確に変換する必要がある。ここで、高精度にランドマークの位置を推定するためには、カメラの内部パラメータキャリブレーションと外部パラメータキャリブレーションを実施する必要がある。内部パラメータキャリブレーションは、カメラのレンズ歪みを修正し、焦点距離を算出する。一方、外部パラメータキャリブレーションは、カメラの現在の設置高さと角度を求める。

例えば、車両に搭載されるカメラは、カメラの所定の設計値に準じた位置や角度で車両に取り付けられるが、この際誤差が生じてしまうことがあり、カメラによる周囲の認識精度が低下する。この誤差を補正するには、一般的に、紙や板などに印字された校正用指標を決められた位置に精密に設置して撮影し、所定の位置から撮影した場合の映像と一致するようにカメラのパラメータを修正する。工場などで車両の出荷前にキャリブレーションを行うことが一般的であるが、乗車人数の違いや座る場所の違い、荷物の積み方の違いなどによって車両の姿勢が変化するため、工場出荷後であってもキャリブレーションを行うことが必須となる場合がある。

ここで、例えば、特許文献１は、車両に搭載されるカメラのキャリブレーションを実施するキャリブレーション装置に関するものであって、前記車両の外部の画像を取得する画像取得部と、前記車両の姿勢変動前の画像と姿勢変動後の画像との間で対応する特徴点を用いて、前記カメラのロール角又はピッチ角の少なくとも一方のカメラパラメータをキャリブレーションするキャリブレーション部と、を備えた発明が記載されている。

特開２０１７−７８９２３号公報

特許文献１では、姿勢変動前後の画像を用いて単眼カメラのキャリブレーションを実施しているが、走行中の振動の影響やキャリブレーション実施時の誤差の影響については考慮がされておらず、カメラの内部パラメータやカメラの外部パラメータに反映がされていないため、カメラで認識したランドマークの位置の認識精度が低下する問題が残っている。ランドマークの位置の認識精度が低下すると、移動体自身の位置の推定精度も低下する。

そこで、本発明の目的は、走行中やキャリブレーション実施時の誤差があっても、移動体の位置推定精度を改善することを目的とする。

上記課題を解決するために、本発明は、移動体と、前記移動体に設けられた撮像装置と、前記撮像装置で取得した第１画像及び第２画像から同一対象である検知点の移動した第１移動量と、前記第１画像と前記２画像を取得した間に前記移動体が移動した第２移動量と、を求め、前記第１移動量と前記第２移動量から前記撮像装置で取得した検知点の認識精度を求め、前記認識精度と前記検知点の位置情報から前記移動体の位置推定をする情報処理装置と、を備えたことを特徴とする。

本発明によれば、走行中やキャリブレーション実施時の誤差があっても、移動体の位置推定精度を改善することができる。

実施例に係る移動体の位置推定装置の構成図 画像処理手順を示すフローチャート 画像上での移動点を説明する図 撮像装置による移動点の距離推定を説明する図 撮像装置による移動点の距離推定を説明する図 撮像装置による移動点の距離推定を説明する図 位置推定に関する原理を説明する図 位置推定に関する原理を説明する図 位置推定に関する原理を説明する図 適用例を説明する図 実施例におけるキャリブレーションの説明図 キャリブレーションのフローチャート 画像変換ステップの詳細を説明する図

以下、本発明の実施例について、図面を用いて説明する。なお、下記はあくまでも実施の例であり、発明の内容を下記具体的態様に限定することを意図する趣旨ではない。発明自体は、特許請求の範囲に記載された内容に即した限りにおいて種々の態様で実施することが可能である。

図１は、実施例に係る移動体の位置推定装置１の構成図である。位置推定装置１は、自動車またはロボットなどの移動体１００に搭載されている。位置推定装置１は、一台以上の撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎと、情報処理装置１３を有する。撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、例えばスチルカメラまたはビデオカメラである。また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは単眼カメラまたは複眼カメラでもよい。

情報処理装置１３は、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎで撮像された画像を処理して移動体１００の位置または移動量を算出する。情報処理装置１３は、算出された位置または移動量に応じた表示を行ってもよく、または移動体１００の制御に関する信号を出力してもよい。

情報処理装置１３は、例えば一般的なコンピュータであって、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎによって撮像された画像を処理する画像処理部１４と、画像処理部の結果に基づき制御を行う制御部１５（ＣＰＵ）と、メモリ１６と、ディスプレイなどの表示部１７と、これら構成要素を相互に接続するバス１８とを有する。情報処理装置１３は、画像処理部１４及び制御部１５が所定のコンピュータプログラムを実行することにより、以下の処理を行う。

撮像装置１２ａは、例えば、移動体１００の前方に設置されている。撮像装置１２ａのレンズは移動体１００の前方に向けられている。撮像装置１２ａは、例えば、移動体１００の前方の遠景を撮像する。他の撮像装置１２ｂ、・・・撮像装置１２ｎは、撮像装置１２ａと異なる位置に設置され、撮像装置１２ａと異なる撮像方向または領域を撮像する。撮像装置１２ｂは、例えば、移動体１００の後方で下方に向けて設置されていてもよい。撮像装置１２ｂは、移動体１００後方の近景を撮像するものでよい。

撮像装置１２ａが単眼カメラの場合、路面が平らであれば、画像上のピクセル位置と実際の地上位置関係（ｘ、ｙ）が一定になるため、撮像装置１２ａから特徴点までの距離を幾何学的に計算できる。撮像装置１２ａがステレオカメラの場合、画像上の特徴点までの距離をより正確に計測できる。以下の説明では、単眼の標準レンズを有するカメラを採用した事例について説明するが、これ以外のカメラ（広角レンズを有するカメラまたはステレオカメラなど）でもよい。また、撮像装置１２ａ、撮像装置１２ｂ・・・撮像装置１２ｎが、ある時刻で撮像する対象物は、それぞれ互いに異なるものでよい。例えば、撮像装置１２ａは、移動体１００の前方の遠景を撮像するものでよい。この場合、遠景を撮像した画像からは、立体物、または位置推定のためのランドマークなどの特徴点が抽出されるようにしてもよい。撮像装置１２ｂは、移動体１００周辺の路面などの近景を撮像するようにしてもよい。この場合、近景を撮像した画像からは、移動体１００の周囲の白線、または路面ペイントなどが検出されるようにしてもよい。

また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、同時に雨や日差しなどの環境外乱の影響を受けないような条件で移動体１００に設置されてもよい。例えば、撮像装置１２ａは移動体１００の前方で前向きに設置されるのに対して、撮像装置１２ｂは移動体１００の後方で後ろ向きまたは下向きに設置されてもよい。これにより、例えば、降雨時に撮像装置１２ａのレンズに雨滴が付着した場合でも、進行方向の逆向きまたは下向きの撮像装置１２ｂのレンズには雨滴が付着しにくい。このため、撮像装置１２ａが撮像した画像が雨滴の影響で不鮮明であっても、撮像装置１２ｂが撮像した画像は雨滴の影響を受けにくい。あるいは、日差しの影響で撮像装置１２ａの画像が不鮮明であっても、撮像装置１２ｂが撮像した画像は鮮明である場合がある。

また、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、互いに異なる撮像条件（絞り値、ホワイトバランス、等）で撮影してもよい。例えば、明るい場所用にパラメータを調整した撮像装置と、暗い場所用にパラメータを調整した撮像装置とを搭載することで、環境の明暗によらず撮像可能としてもよい。

撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎは、制御部１５から撮影開始の指令を受けたとき、または一定の時間間隔で画像を撮像してもよい。撮像された画像のデータ及び撮像時刻は、メモリ１６に格納される。なお、メモリ１６は、情報処理装置１３の主記憶装置（メインメモリ）およびストレージなどの補助記憶装置を含む。

画像処理部１４がメモリ１６に格納された画像データ及び撮像時刻に基づいて、様々な画像処理を行う。この画像処理では、例えば、中間画像が作成されてメモリ１６に保存される。中間画像は、画像処理部１４による処理の他、制御部１５などの判断や処理に利用されてもよい。

バス１８は、ＩＥＢＵＳ（Ｉｎｔｅｒ Ｅｑｕｉｐｍｅｎｔ Ｂｕｓ）やＬＩＮ（Ｌｏｃａｌ Ｉｎｔｅｒｃｏｎｎｅｃｔ Ｎｅｔｗｏｒｋ）やＣＡＮ（Ｃｏｎｔロールｅｒ Ａｒｅａ Ｎｅｔｗｏｒｋ）などで構成できる。

画像処理部１４は、撮像装置１２で撮像された画像に基づいて複数の移動体の位置候補を特定し、その複数の位置候補と移動体１００の移動速度とに基づいて移動体１００の位置を推定する。

その他、画像処理部１４は、例えば、移動体１００の走行中に撮像装置１２が撮像した画像を処理して、移動体１００の位置を推定したり、撮像装置１２が撮像したビデオ画像で移動体１００の移動量を算出し、スタート地点に移動量を加算して現在位置を推定してもよい。

画像処理部１４は、ビデオ画像の各フレーム画像で特徴点を抽出してもよい。画像処理部１４は、さらに、次以降のフレーム画像で同じ特徴点を抽出する。そして、画像処理部１４は、特徴点のトラッキングにより移動体１００の移動量を算出してもよい。

制御部１５は、画像処理部１４の画像処理の結果に基づいて、移動体１００に対して移動速度に関する指令を出力してもよい。例えば、制御部１５は、画像内の立体物の画素数、画像内の特徴点のうちの外れ値の数または画像処理の種類等に応じて、移動体１００の移動速度を増加させる指令、減少させる指令または維持させる指令を出力してもよい。

図２は、画像処理部１４が行う画像処理手順を示すフローチャートである。

画像処理部１４は、メモリ１６から撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像データを取得する（Ｓ２１）。ステップＳ２１で取得する画像データは撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが撮像した画像のうち１枚のみ、または、複数の画像データでよい。また、スッテプＳ２１は、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎがそれぞれ撮像した最新の画像のみならず、過去撮像した画像でもよい。

次に、画像処理部１４は、取得した各フレーム画像内の移動点を抽出する（Ｓ２２）。移動点は、例えば、画像内のエッジやコーナーやピクセル強度の最大値や最小値などのような特徴点でよい。特徴点の抽出には、Ｃａｎｎｙ、Ｓｏｂｅｌ、ＦＡＳＴ、Ｈｅｓｓｉａｎ、Ｇａｕｓｓｉａｎなどの技術を用いればよい。具体的なアルゴリズムは、画像の特徴に応じて適宜選択される。また、移動点は、認識されたランドマークの代表点（ランドマークの中心、コーナーなど）でよい。ランドマーク認識と代表点抽出はＤｅｅｐ ｌｅａｒｎｉｎｇ、ｔｅｍｐｌａｔｅ ｍａｔｃｈｉｎｇなどの従来の画像認識技術を用いればよい。移動点の詳細は後述する。

画像処理部１４は、フレーム画像の時系列に従って、各フレーム画像でそれぞれ抽出された移動点をトラッキングする（Ｓ２３）。トラッキングにはＬｕｃａｓ−Ｋａｎａｄｅ法やＳｈｉ−Ｔｏｍａｓｉ法やＤｉｒｅｃｔ Ｍａｔｃｈｉｎｇ法などの技術を用いればよい。また、ステップＳ２３のトラッキングは、直前や直後に取得したフレームの移動点に限らず、数フレーム間隔でもよい。具体的なアルゴリズムは、画像の移動点に応じて適宜選択される。

次に、スッテプＳ２３でそれぞれトラッキングした移動点を実世界上での移動量に換算する計算を実施する（Ｓ２４）。ステップＳ２３のトラッキングで得られた画像上でのピクセル位置とそれ以前の前フレームのピクセル位置の差分を算出し、その差分をメートル単位に変換する。

スッテプＳ２５では、撮像装置か搭載されている移動体の移動量を推定する。このステップでは、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎが今回撮像した画像と前回撮像した画像の間の移動体１００の実際の移動量を推定する。移動体１００の実際の移動量推定には、ＧＰＳ情報、オドメトリ、画像オドメトリ、ＳＬＡＭ法などの技術を適応すればよい。また、過去の移動量に基づいて今回の移動量を推定する時系列フィルタを用いてもよい。また、前述のセンサ情報とフィルタを組み合わせて、移動体１００の移動量も推定してもよい。最終的に、移動体１００の前フレームと今回のフレームの間の移動量を推定できる手法であれば、どのセンサや組み合わせでもよい。スッテプＳ２５を実施するタイミングは、ステップＳ２４の直後でもよいが、スッテプＳ２１からステップＳ２４と平行に実施してもよい。ステップＳ２６の処理が始まる前であれば、ステップＳ２５をいつでも実施してもよい。

ステップＳ２６では、ステップＳ２４で得られた移動点の移動量情報とステップＳ２５で得られた移動体１００の移動量情報を用いて、ステップＳ２３でトラッキングした移動点の精度を推定する。ステップＳ２６の詳細は後述する。

ステップＳ２６で推定した精度に基づいて、必要に応じて、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎのキャリブレーションを実施する（Ｓ２７）。ステップＳ２７の実施は任意であり、詳細は後述する。

図３は、画像上での移動点を説明する図である。画像２１０は、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎによってスッテプＳ２１で取得した画像である。（ｕ、ｖ）で示される座標２１１は、画像２１０における座標系である。道路２１２は、移動体１００が走行中で画像２１０に写る道路である。

移動体１００が道路２１２を走行し、ある時に道路２１２の路面上のランドマーク２１３が画像２１０に写るとする。この時、ステップＳ２２で移動点を抽出する場合、ランドマーク２１３から特徴点２１４を抽出したとする。なお、この特徴点は画像内のエッジやコーナーに限らず、ピクセル強度の最大値や最小値でもよい。また、ランドマークをマップとマッチングする場合、簡単のため、認識したランドマークを代表点（ランドマークの中心、コーナーなど）で表す場合があり、ランドマーク２１４の代表として代表点２１６として表す。ここで、画像２１０上でステップＳ２２で抽出した特徴点２１４と特徴点２１５と代表点２１６は実世界での静止点であるが、移動体１００に固定された撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎに対して、特徴点２１４と特徴点２１５と代表点２１６が移動するため、特徴点２１４と特徴点２１５と代表点２１６を全て「移動点」と定義つける。

図４から図６を用いて撮像装置による検出した移動点の距離推定を説明する。図４から図６は撮像装置による移動点の距離推定を説明する図である。

図４の設置角度α_Ｎ３０ａは移動体１００が走行中の路面に対する撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎの設置角度である。高さＨ_Ｎ３１ａは移動体１００が走行中の路面に対する撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎの設置高さである。簡単のため、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎを１台の撮像装置１２を代表として説明する。座標３２ａは、移動体１００に固定したメートル単位の座標であり、点３３ａは路面上の一点である。この時、画像３４ａ上で点３３ａはステップＳ２２で移動点として抽出され、ピクセル３５ａとして表される。この場合、撮像装置のキャリブレーションができているとし、設置角度α_Ｎ３０ａと設置高さＨ_Ｎ３１ａを用いて、点３３ａの実際の位置（メートル）と画像上での移動点３５ａ（ピクセル）の関係が求められる。従って、撮像装置１２の設置角度α_Ｎ３０ａと設置高さＨ_Ｎ３１ａが一定の場合、座標３２ａ（メートル）と画像（ピクセル）の関係が一定であり、メートルからピクセルへ簡単に変換できる。また、撮像装置１２の設置角度α_Ｎ３０ａと設置高さＨ_Ｎ３１ａが一定であれば、画像３４ａを俯瞰画像などに変換しても、変換パラメータが既知であれば、前述の座標３２ａ（メートル）と画像（ピクセル）の関係が変わらない。

一方、図５は図４の状態で撮像装置のキャリブレーションを実施した時と異なる場合を説明する。振動や乗車人数の違いで撮像装置１２の設置角度α_Ｎ３０ａが設置角度α’_Ｎ３０ｂとなり、高さＨ_Ｎ３１ａが高さＨ’_Ｎ３１ｂとなったとする。この状態で点３３ａは、画像３４ｂ上で移動点３５ｂとして抽出されるが、座標３２ａに基づいてキャリブレーションが行われたため、座標３２ａに対する仮想点３３ｂへの距離が算出されてしまう。しかし、この場合は、座標３２ｂに対する点３３ａへの距離が正確のため、座標３２ａに対する距離を算出する際誤差が生じる。簡単のため、移動体１００のピッチ角が変換した場合を、図５にて説明したが、移動体１００のロール角やヨー角が変化した場合でも前述の原理は変わらない。

図６は図４と同じ設置角度α_Ｎ３０ａ、高さＨ_Ｎ３１ａ、座標３２ａと点３３ａを示す。撮像装置１２でスッテプＳ２１で取得する画像３４ｃは、キャリブレーションを行っても歪みの修正ができなかった画像を表す。歪みの修正ができていない画像３４ｃで点３３ａはピクセル３５ｃとして写り、歪みの修正ができた画像３４ａのピクセル３５ａとは違う位置に写る。従って、歪みの修正ができていない画像３４ｃで移動点３３ａを抽出しても、ピクセル３５ｃからメートルに変換し、実際の位置を推定する場合、誤差が大きい。

次に、図７から図９を用いて本実施例の原理を説明する。図７から図９は位置推定に関する原理を説明する図である。

図７は、移動体１００がキャリブレーションした時と同じ設置角度α_Ｎ３０ａと高さＨ_Ｎ３１ａで走行している状態である。撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎの歪みの修正ができているとする。点４０は、移動体１００が走行中の路面上の特徴点である。簡単のため、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎを１台の撮像装置１２と代表して、説明する。画像４１ａは撮像装置１２の撮像した俯瞰画像であり、座標を（ｕ’、ｖ’）とする。原理の説明を簡単にするため、画像４１ａを俯瞰画像としたが、ピクセルとメートルの関係が既知であれば、どんな画像でもよい。ステップＳ２２で各点４０を移動点４２ａとして画像４１ａ上で抽出する。移動体１００が移動量Ｄ_Ｎ４３を走行した後、抽出した各移動点４２ａをステップＳ２３でトラッキングし、ステップＳ２４で移動量を算出すると、俯瞰画像４１ａ上での各移動量４４ａは全て一定である。また、俯瞰画像４１ａ上でのそれぞれの移動量４４ａをピクセルからメートルに変換すると、それぞれの移動量は移動体１００の移動量Ｄ_Ｎ４３と同じ移動量となる。

一方、図８は、移動体１００がキャリブレーションした時と異なる設置角度α’_Ｎ３０ｂと高さＨ’_Ｎ３１ｂで走行している状態である。撮像装置１２の歪みの修正ができているとする。移動体１００が移動量Ｄ_Ｎ４３を走行した後、抽出した各移動点４２ｂをステップＳ２３でトラッキングし、ステップＳ２４で移動量を算出すると、俯瞰画像４１ｂ上での各移動点４２ｂの移動量４４ｂは画像の位置によって異なり、撮像装置に近い点の移動量は画像装置から遠い点の移動量より大きくみえる。この状態の設置角度α’_Ｎ３０ｂと高さＨ’_Ｎ３１ｂで移動点４０を写す時、平面４５と間違えて画像４１ｂ上の移動量４４ｂを算出するため、移動体１００に近い移動点４２ｂの移動量は大きく、移動体１００から遠い移動点４２ｂの移動量４４ｂは小さく見える。従って、俯瞰画像４１ｂ上でのそれぞれの移動量４４ｂをピクセルからメートルに変換すると、それぞれの移動量４４ｂは移動体１００の移動量Ｄ_Ｎ４３とは異なった移動量となる。従って、スッテプＳ２１とステップＳ２２で抽出した移動点の移動量が一定でない場合、キャリブレーションができていなく、この状態で路面上の点へ距離を算出する場合、距離誤差が大きい。

図９は、図７と同じ設置角度α_Ｎ３０ａ、高さＨ_Ｎ３１ａ、点４０、移動点４２ａと移動体１００の移動量Ｄ_Ｎ４３を表す。俯瞰画像４１ｃの歪みの修正ができていないとする。この状態で移動体１００が移動量Ｄ_Ｎ４３を走行する場合、各移動点４２ａの俯瞰画像４１ｃ上での移動量は移動量４４ｃとなる。歪みのため、移動点４２ａの俯瞰画像４１ｃの領域によって、移動量４４ｃが異なり、その領域内の移動点への距離を算出すると、距離誤差が大きい。

図１０を用いて本実施例での適用例を説明する。図１０は、適用例を説明する図である。走行環境５０は、移動体１００が走行する環境である。簡単のため、走行環境５０を駐車場とする。地図情報５１は、走行環境５０の地図である。走行環境５０は、走行環境の車線、駐車枠、標識などの静止ランドマークで構成し、それぞれの正確な位置情報を有する。地図情報５１の座標系は、世界の絶対位置かある領域の相対位置を表してもよい。最終的に、移動体１００の現在位置を正確に表示できれば、どんな座標でもよい。

撮像範囲５２は撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎの撮像範囲である。簡単のため、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎを２台とし、移動体１００の右側と左側に設置され、移動体１００の右側と左側を向いているとする。この適用例では、上述の精度推定を用いて、２台の撮像装置で認識したランドマークの位置を地図情報５１と比較しながら、移動体１００の自己位置推定を行う。

移動点５３は、移動体１００が走行環境５０に入った時の点である。簡単のため、本適用例での移動点は全て駐車枠の角とする。移動体１００が移動量５４を走行した後の撮像装置で取得した画像を俯瞰画像５５ａと俯瞰画像５５ｂとする。移動体１００が移動量５４を走行した後の画像５５ａと５５ｂ上での移動点５３の移動量を移動量５６ａとする。ここで、移動量５６ａは、撮像装置を用いてステップＳ２２で抽出した移動点と、その抽出した移動点をステップＳ２３で追跡した際の移動点の移動量である。

移動量５６ａは、スッテプＳ２５で求めた移動体１００の実際の移動量５４と同じ移動量になったため、移動量５６ａの周りの画像上での領域が高精度（認識誤差が少ない）と判定する。従って、算出した移動点５３の位置に基づいて、地図情報５１から移動体１００の自己位置を推定する場合、高精度に推定できる。

移動点５７ｂと移動点５７ｃは、移動体１００が開始位置を出た後の移動点である。移動体１００が移動量ｄ_Ｎ５８を走行した後の撮像装置で取得した画像を俯瞰画像５５ｃとする。移動体１００が移動量ｄ_Ｎ５８を走行した後の画像５５ｃ上での移動点５７ｂの移動量を移動量５６ｂとし、移動点５７ｃの移動量を移動量５６ｃとする。ここで、移動量５６ｂ、５６ｃは、撮像装置を用いてスッテプＳ２２で移動点５７ｂと移動点５７ｃから移動点を抽出し、その抽出した移動点をスッテプＳ２３で追跡した時の移動点の移動量である。撮像装置の設置高さと角度に基づいて、移動量５６ｂをメートルに変換した時に、移動体１００の移動量ｄ_Ｎ５８と同じ移動量になったとする。従って、移動体１００が移動量ｄ_Ｎ５８を走行した時、移動量５６ｂの画像５５ｃ上での周りの領域は高精度であるため、検知された移動点５７ｂの精度が高いと判定し、検出された移動点５７ｂの位置を地図情報５１とマッチングし、位置推定を行う。一方、移動量５６ｃは移動量ｄ_Ｎ５８と異なったため、移動量５６ｃの周りの画像上での領域の精度が低いと判定するため、地図情報５１とマッチングせず、撮像装置の数フレームをかけて、精度がよくなるまでスッテプＳ２３で追跡する。精度の低い領域を高くする方法は、例えば、Ｋａｌｍａｎ Ｆｉｌｔｅｒのような時系列フィルタをかける。詳細は後述する。

従って、算出した移動点５７ｂと移動点５７ｃの位置と判定した精度に基づいて、地図情報５１から移動体１００の自己位置を高精度に推定できる。

前述判定した精度は、例えば、重みｗ_Ｎ、ｐにして位置推定に活用する。式（１）に示すように、移動体１００の実際の移動量ｄ_Ｎ５８と移動点の移動量Ｉ_Ｎ、ｐ（ｐ＝１、２、・・、移動点数）の差ｅｒｒｏｒ_Ｎ、ｐとする。
１／ｗ_Ｎ、ｐ ＝ ｅｒｒｏｒ_Ｎ、ｐ ＝ ｜ｄ_Ｎ − Ｉ_Ｎ、ｐ｜ ・・・式（１）
また、式（１）の誤差をメートルでなく、式（２）に示すように、移動体１００の移動量ｄ_Ｎ５８の割合で算出してもよい。
１／ｗ_Ｎ、ｐ ＝ ｅｒｒｏｒ_Ｎ、ｐ ＝ ｜ｄ_Ｎ − Ｉ_Ｎ、ｐ｜／ｄ_Ｎ×１００ ・・・式（２）
式（１）と式（２）の重みを、例えば、Ｋａｌｍａｎ Ｆｉｌｔｅｒのような時系列フィルタのパラメータに代入してもよい。Ｋａｌｍａｎ Ｆｉｌｔｅｒを用いる場合、センサやシステムの誤差を偏差σ_Ｎとして設定する必要があり、式（３）に示すように、前述誤差ｅｒｒｏｒ_Ｎ、ｐを偏差σ_Ｎに代入する。

図１０の場合、前述の偏差σ_Ｎを偏差５９ｂと偏差５９ｃで示す。移動量５６ｂの重みは高いので、偏差５９ｂが小さくなり、移動量５６ｃの重みは低いため、偏差５９ｃが大きくなる。
σ_Ｎ ＝ ｅｒｒｏｒ_Ｎ、ｐ ・・・式（３）
また、撮像装置の設置高さと角度に基づいて、幾何学的にピクセルからメートルに変換するため、撮像装置からランドマークが遠ければ移動体１００の振動などに影響されやすいため、誤差が大きくなる可能性が高い。従って、前述の誤差ｅｒｒｏｒ_Ｎ、ｐを算出せず、距離・ピクセルの位置に対する偏差σ_Ｎを設定してもよい。例えば、撮像装置が撮像する俯瞰画像５５ａと俯瞰画像５５ｂの幅（ｕ’方向）をＷとし、高さ（ｖ’方向）をＶとすると、（ｕ’、ｖ’）＝（Ｗ／２、Ｖ）が移動体１００に最も近いピクセル位置のため、誤差が最も小さい領域となる。一方、（ｕ’、ｖ’）＝（０、０）か（ｕ’、ｖ’）＝（Ｗ、０）が移動体１００に最も遠いピクセル位置のため、誤差が最も大きい領域となる。従って、ｕ’方向の最大偏差をσ_{ｕ、ｍａｘ}とすれば、ピクセルｕに対する偏差σを式（４）から求められる。
σ_ｕ ＝ σ_{ｕ、ｍａｘ}｜Ｗ／２ − ｕ’｜／（Ｗ／２） ・・・式（４）
ｕ方向と同じく、ｖ’方向の最大偏差をσ_{ｖ、ｍａｘ}とすれば、ピクセルｖ’に対する偏差σ_ｖを式（５）から求められる。
σ_ｖ ＝ σ_{ｖ、ｍａｘ}｜Ｖ − ｖ'｜／（Ｖ） ・・・式（５）
σ_ｕとσ_ｖを組み合わせて、偏差σ_Ｎを例えば、式（６）から得られる。
σ_Ｎ ＝ σ_ｕ ＋ σ_ｖ ・・・式（６）
また、σ_ｕとσ_ｖを組み合わせて、偏差σ_Ｎを式（７）から算出してもよい。
σ_Ｎ ＝ σ_ｕσ_ｖ ・・・式（７）
また、σ_ｕとσ_ｖに重みｍ（１、２、・・・、ｍ）をつけて、偏差σ_Ｎを式（８）から算出してもよい。
σ_Ｎ ＝ （σ_ｕ ^ｍ ＋ σ_ｖ ^ｍ）^１／ｍ ・・・式（８）
また、前述のｅｒｒｏｒ_Ｎ、ｐを算出した場合、ｅｒｒｏｒ_Ｎ、ｐをσ_ｕとσ_ｖを組み合わせて、偏差σ_Ｎを式（９）に示すように算出してもよい。
σ_Ｎ ＝ ｅｒｒｏｒ_Ｎ、ｐ（σ_ｕ ^ｍ ＋ σ_ｖ ^ｍ）^１／ｍ ・・・式（９）
偏差σＮの算出は、ｄ_ＮとＩ_Ｎ、ｐとｅｒｒｏｒ_Ｎ、ｐとσ_ｕとσ_ｖとｕとｖの組み合わせであれば、式（１）から式（９）の何でもよい。また、前述のσ_{ｕ、ｍａｘ}とσ_{ｖ、ｍａｘ}の設定は、固定値か経験的に設定してもよい。また、σ_{ｕ、ｍａｘ}とσ_{ｖ、ｍａｘ}は、必ずしもσ_{ｕ、ｍａｘ}＝σ_{ｖ、ｍａｘ}でないため、それぞれ異なったパラメータを設定してもよい。

撮像装置内に移動点が１点のみの場合（ｐ＝１）、移動体１００の自己位置（Ｘ、Ｙ）_Ｎと方位（θ）_Ｎを式（１０）が示すように、移動点の位置（Ｘ、Ｙ、θ）ｐ＝１に基づいて算出する。
（Ｘ、Ｙ、θ）_Ｎ ＝ ｗ_{Ｎ、ｐ＝１}（Ｘ、Ｙ、θ）_ｐ＝１ ・・・式（１０）
また、撮像装置内に移動点が複数の場合、移動体１００の自己位置（Ｘ、Ｙ、θ）_Ｎを式（１１）から求められる。
（Ｘ、Ｙ、θ）_Ｎ ＝ ［ｗ_{Ｎ、１（Ｘ、Ｙ、θ）１}＋．．．＋ｗ_{Ｎ、ｐ（Ｘ、Ｙ、θ）ｐ} ］／（ｗ_Ｎ、１＋．．．＋ｗ_Ｎ、ｐ） ・・・式（１１）
移動体１００の自己位置を推定する場合、前述の式（１０）と式（１１）以外、移動点の算出した位置（Ｘ、Ｙ、θ）１、．．．、（Ｘ、Ｙ、θ）ｐと重みｗ_Ｎ、１、．．．、ｗ_Ｎ、ｐの組み合わせであれば、なんでもよい。

また、本実施例では、移動点を駐車枠の角とし、問題なく画像処理技術を用いて撮像装置１２で移動点の認識ができるとした。一方、実際の駐車所や道路では、歩行者や他車のような移動物が存在するため、駐車枠の角の認識が困難な場合がある。ただし、このような障害物は路面より高いため、ステップＳ２１からステップＳ２４で移動点の移動量を算出しても、移動量Ｉ_Ｎ、ｐが大きくなり、誤差ｅｒｒｏｒ_Ｎ、ｐも大きくなる。従って、誤って障害物を移動点として認識しても、重みｗ_Ｎ、ｐが低くなるため、位置推定結果に影響しない。

図１１及び図１２を用いて、本実施例によるキャリブレーションの説明をする。図１１は、本実施例におけるキャリブレーションの説明図、図１２はキャリブレーションのフローチャートである。

図１１の画像６０ａ、画像６０ｂ、・・・画像６０Ｎは、スッテプＳ２１で撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎで時間ｔ０、ｔ１・・・ｔＮに時系列に撮像した画像である。移動点６１ａ、移動点６１ｂ、・・・移動点６１Ｎは、ステップＳ２２で画像６０ａ、画像６０ｂ、・・・画像６０Ｎから抽出した移動点である。移動量６２ａ、移動量６２ｂ、・・・移動量６２Ｎは、スッテプＳ２３とスッテプＳ２４で移動点６１ａ、移動点６１ｂ、・・・移動点６１Ｎの算出した移動量である。ここで、キャリブレーションできていないとし、移動量６２ａ、移動量６２ｂ、・・・移動量６２Ｎは必ずしも一定ではない。

画像６３はキャリブレーションＳ２７を実施した後の画像である。移動量６４は、スッテプＳ２１からスッテプＳ２４で算出したキャリブレーションスッテプＳ２７を実施した後の画像６３上での移動点の移動量である。画像６３はキャリブレーションを実施した後の画像のため、スッテプＳ２１からスッテプＳ２４で算出した移動点の移動量６４は一定である。

図１２のスッテプＳ６５からスッテプＳ６８はキャリブレーションＳ２７の中の処理である。ステップＳ６５は、ステップＳ２１からステップＳ２５で算出した情報を保存するステップである。画像６０ａ、画像６０ｂ、・・・画像６０Ｎ、移動点６１ａ、移動点６１ｂ、・・・移動点６１Ｎ、移動量６２ａ、移動量６２ｂ、・・・移動量６２Ｎ、移動体１００の移動量などをメモリ１６に保存する。

ステップＳ６６は、画像６０ａ、画像６０ｂ、・・・画像６０ＮとステップＳ２３でトラッキングした移動点６１ａ、移動点６１ｂ、・・・移動点６１Ｎを画像変換するステップである。Ｓ６６の画像変換は、例えば、アフィン変換や透視変換などであり、画像６０ａ、画像６０ｂ、・・・画像６０ＮとステップＳ２３でトラッキングした移動点６１ａ、移動点６１ｂ、・・・移動点６１Ｎの回転と並進を変換する。ステップＳ６６の詳細は後述する。

ステップＳ６７は、ステップＳ６６で変換された画像６０ａ、画像６０ｂ、・・・画像６０ＮとステップＳ２３でトラッキングした移動点６１ａ、移動点６１ｂ、・・・移動点６１Ｎから、移動点６１ａ、移動点６１ｂ、・・・移動点６１Ｎのそれぞれの新たな移動量Ｉ_{Ｎ、ｐ、ｉ}（ｉ＝１、．．．、キャリブレーション回数）を算出する。式（１２）に示すように、改めて算出したＩ_{Ｎ、ｐ、ｉ}とステップＳ６５で時系列に保存した移動体１００の移動量ｄ_０、・・・、ｄ_Ｎの誤差Ｅ_{Ｎ、ｐ、ｉ}を算出する。
Ｅ_{Ｎ、ｐ、ｉ} ＝ ｜ｄ_Ｎ − Ｉ_{Ｎ、ｐ、ｉ}｜ ・・・式（１２）
ステップＳ６８は、ステップＳ６７で算出した誤差Ｅ_{Ｎ、ｐ、ｉ}を予めに設定された閾値ｍｉｎ_{ｅｒｒｏｒ}と比較するステップである。ステップＳ６７で算出した誤差Ｅ_{Ｎ、ｐ、ｉ}がｍｉｎ_{ｅｒｒｏｒ}より小さければステップＳ２７が終了し、ステップＳ６７で算出した誤差Ｅ_{Ｎ、ｐ、ｉ}がｍｉｎ_{ｅｒｒｏｒ}より大きければステップＳ６６に戻る。

フレーム数Ｎは、少なくても２枚が必須であるが、Ｎの最大値は、例えば、ステップＳ２１からステップＳ２４で時系列に得られた移動点数によって設定してもよい。基本的に、移動点数が多ければキャリブレーション誤差が小さいが、処理負荷が大きい。従って、例えば、ステップＳ２１からステップＳ２４で時系列に得られた移動点数が予めに設定された閾値より大きくなれば、その時間までのステップＳ２１からステップＳ２４で時系列に得られたフレーム数と移動点を全て用いてキャリブレーションを行う。また、移動体１００の走行速度によって前回行ったキャリブレーションパラメータとは違うパラメータになる可能性が高いため、Ｎを移動体１００の速度に応じて設定してもよい。例えば、移動体１００の速度が遅ければ、キャリブレーションパラメータが大きく変わらないため、Ｎを高く設定し、移動体１００の速度が速ければ、キャリブレーションパラメータが大きく変わるためＮを低く設定し、高い頻度でキャリブレーションを実施する。また、走行時間に基づいて、Ｎを設定してもよい。例えば、処理負荷に余裕があれば数ｍｓや数十ｍｓごとにキャリブレーションを実施し、処理負荷に余裕がなければ数百ｍｓや数秒ごとにキャリブレーションを実施する。

図１３を用いて、ステップＳ６６の詳細を説明する。図１３は画像変換ステップの詳細を説明する図である。

俯瞰画像７０ａは、撮像装置で撮像した画像で、俯瞰画像に変換されたものである。ステップＳ２２で抽出した移動点を移動点７１とし、ステップＳ２１からステップＳ２４で得られた移動点７１のそれぞれの移動量は移動量７２ａとする。ステップＳ２１からステップＳ２４で移動点７１の移動量７２ａが得られた時、撮像装置のキャリブレーションができていたとし、ステップＳ２１からステップＳ２４で得られた移動点７１のそれぞれの移動量７２ａは一定となった。ステップＳ２１からステップＳ２４で算出した移動点７１の移動量７２ａは全て一定のため、撮像装置のロール、ピッチ、ヨーはキャリブレーションした時と変わっていない。一方、ステップＳ２１からステップＳ２４で算出した移動点７１の移動量７２ａは全て一定であっても、撮像装置の高さがキャリブレーションを行った時と異なる場合がある。ここで、撮像装置の高さが変わる場合、画像７０ａの移動量７２ａが全て変わるため、移動体１００の実際の移動量と比較し、撮像装置の高さのキャリブレーションを行う。撮像装置の高さのキャリブレーションができている場合、移動量７２ａは全て移動体１００の実際の移動量と同じになるため、式（１）のｅｒｒｏｒ_Ｎ、ｐが０に近くなるまで撮像装置の高さを修正する。修正は、例えば、試行錯誤で新たな高さを設定し、ｅｒｒｏｒ_Ｎ、ｐを改めて算出し、ｅｒｒｏｒ_Ｎ、ｐが０に近くなるまで修正を繰り返す。また、移動量７２ａは移動体１００の実際の移動量ｄ_Ｎより大きくなった場合、撮像装置の実際の高さはキャリブレーションした時より低いという意味を持っているため、ｅｒｒｏｒ_Ｎ、ｐが０に近くなるまで撮像装置の高さパラメータを低く設定する。ステップＳ２５で推定する移動体の移動量は必ずしも正確でなく、ステップＳ２３で移動点の追跡に誤差があるため、ｅｒｒｏｒ_Ｎ、ｐが０にならないが、０に近くなれば画像の全体の領域のキャリブレーションができている。

一方、俯瞰画像７０ｂは、撮像装置のロール角がキャリブレーションを行った時と違うロール角となっている場合を示す。この場合、ステップＳ２１からステップＳ２４で得られた移動点７１の移動量７２ｂは俯瞰画像７０ｂの領域によって異なる。例えば、俯瞰画像７０ｂの左側の移動点７１の移動量７２ｂは俯瞰画像７０ｂの右側の移動量７２ｂより大きい。また、俯瞰画像７０ｂ中心の移動点７１の移動量７２ｂは俯瞰画像７０ａの移動量７２ａとは変わらない。従って、この移動量７２ｂのパターンがある際、ロール角に誤りがあるという原因で、俯瞰画像７０ｂの移動点７１の移動量７２ｂのｅｒｒｏｒ_Ｎ、ｐが０になるまでロール角を修正する。

俯瞰画像７０ｃは、撮像装置のピッチ角がキャリブレーションを行った時と違うピッチ角となっている場合を示す。この場合、ステップＳ２１からステップＳ２４で得られた移動点７１の移動量７２ｃは俯瞰画像７０ｃの領域によって異なる。例えば、俯瞰画像７０ｃの上の移動点７１の移動量７２ｃは俯瞰画像７０ｃの下の移動量７２ｃより大きい。ｖ＝０に近ければ近いほど移動量７２ｃが大きくなり、ｖ＝０から離れると移動量７２ｃが小さくなる。従って、この移動量７２ｃのパターンがある際、ピッチ角に誤りがある原因であり、俯瞰画像７０ｃの移動点７１の移動量７２ｃのｅｒｒｏｒ_Ｎ、ｐが０になるまでピッチ角を修正する。

俯瞰画像７０ｄは、撮像装置１２のヨー角がキャリブレーションを行った時とは違うヨー角となっている場合を示す。この場合、ステップＳ２１からステップＳ２４で得られた移動点７１の移動量７２ｄは一定であるが、ｖ’方向とは違う方向に移動する。従って、この移動量７２ｃのパターンがある際、ヨー角に誤りがある原因で、俯瞰画像７０ｄの移動点７１の移動量７２ｄのｖ’方向と同じ方向に移動するまでヨー角を修正する。

俯瞰画像７０ｅは、撮像装置１２ａ、撮像装置１２ｂ、・・・撮像装置１２ｎの歪みの修正ができていない場合を示す。この場合、ステップＳ２１からステップＳ２４で得られた移動点７１の移動量７２ｅの方向は一定でない。従って、俯瞰画像７０ｅの移動点７１の移動量７２ｅの方向が一定になるまで歪みを修正する。

以上、本発明によれば、走行中やキャリブレーション実施時の誤差があっても、移動体の位置推定精度を改善することができる。

なお、本発明は上記した実施例に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施例は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。また、ある実施例の構成の一部を他の実施例の構成に置き換えることが可能であり、また、ある実施例の構成に他の実施例の構成を加えることも可能である。また、各実施例の構成の一部について、他の構成の追加・削除・置換をすることが可能である。また、上記の各構成、機能、処理部、処理手段等は、それらの一部又は全部を、例えば集積回路で設計する等によりハードウェアで実現してもよい。また、上記の各構成、機能等は、プロセッサがそれぞれの機能を実現するプログラムを解釈し、実行することによりソフトウェアで実現してもよい。各機能を実現するプログラム、テーブル、ファイル等の情報は、メモリや、ハードディスク、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ等の記録装置、または、ＩＣカード、ＳＤカード、ＤＶＤ等の記録媒体に置くことができる。

１：位置推定装置
１２：撮像装置
１３：情報処理装置
１４：画像処理部
１５：制御部
１６：メモリ
１７：表示部
１８：バス
５１：地図情報
１００：移動体
２１２：道路
２１３：ランドマーク

Claims (10)

1. 移動体と、
   前記移動体に設けられた撮像装置と、
   前記撮像装置で取得した第１画像及び第２画像から同一対象である検知点の移動した第１移動量と、前記第１画像と前記２画像を取得した間に前記移動体が移動した第２移動量と、を求め、前記第１移動量と前記第２移動量から前記撮像装置で取得した検知点の認識精度を求め、前記認識精度と前記検知点の位置情報から前記移動体の位置推定をする情報処理装置と、を備えた移動体の位置推定装置。
2. 請求項１における移動体の位置推定装置において、
   前記第１移動量は、取得した第１画像及び第２画像上における移動量を前記移動体の空間座標に変換した移動量であることを特徴とする移動体の位置推定装置。
3. 請求項２における移動体の位置推定装置において、
   前記認識精度は前記第１移動量と前記第２移動量の差分であることを特徴とする移動体の位置推定装置。
4. 請求項３における移動体の位置推定装置において、
   前記差分から検知点の重みを求め、
   前記重みと前記検知点の位置情報から前記移動体の位置推定をすることを特徴とする移動体の位置推定装置。
5. 請求項４における移動体の位置推定装置において、
   前記検知点の位置情報は、地図情報上における前記検知点の位置情報であることを特徴とする移動体の位置推定装置。
6. 請求項５における移動体の位置推定装置において、
   前記差分に基づいて撮像装置のキャリブレーションを行うことを特徴とする移動体の位置推定装置。
7. 請求項６における移動体の位置推定装置において、
   前記撮像装置は撮影方向が異なる複数の撮像装置を備えたことを特徴とする移動体の位置推定装置。
8. 移動体に設けられた撮像装置で取得した２フレーム以上の撮像画像のそれぞれから抽出した同一対象である検知点の移動した第１移動量を求め、
   前記２フレーム以上の撮像画像を取得した間に前記移動体が移動した第２移動量を求め、
   前記第１移動量と前記第２移動量から前記撮像装置で取得した検知点の精度を求め、
   前記精度と前記検知点の位置情報から移動体の位置を推定する移動体の位置推定方法。
9. 請求項８に記載の移動体の位置推定方法において、
   前記精度に基づいて、前記検知点の重みを求め、
   前記重みと前記検知点の位置情報から移動体の位置を推定することを特徴とする移動体の位置推定方法。
10. 請求項９に記載の移動体の位置推定方法において、
    前記精度に基づいて、撮像装置のキャリブレーションを行い、
    移動体の位置を推定することを特徴とする移動体の位置推定方法。

Images