JP7212486B2

JP7212486B2 - 位置推定装置

Info

Publication number: JP7212486B2
Application number: JP2018180913A
Authority: JP
Inventors: 貴大酒井; 盛彦坂野; 大輔福田; 純哉村山
Original assignee: Clarion Co Ltd; Faurecia Clarion Electronics Co Ltd
Current assignee: Faurecia Clarion Electronics Co Ltd
Priority date: 2018-09-26
Filing date: 2018-09-26
Publication date: 2023-01-25
Anticipated expiration: 2038-09-26
Also published as: EP3859476A1; EP3859476A4; WO2020066069A1; JP2020052687A; US20220009552A1

Description

本発明は、位置推定装置に関する。

自動車の駐車位置を指定し、その駐車位置までの経路を設定して運転者による運転操作なしで自動車を自律移動させる自動駐車システムが提案されている。こうした自動駐車システムにおいては、駐車位置までの経路に従って自動車を正確に自律移動させるために、走行中の自動車の位置を高精度に推定することが求められる。

走行中の自動車の位置推定に関して、下記の特許文献１に記載の技術が知られている。特許文献１には、移動体の移動に伴って移動する撮像部によって順次撮像された複数の撮像画像に基づいて撮像画像中に存在する特徴点の実空間での位置を表す実位置を推定する３次元位置推定装置であって、前記複数の撮像画像中において１または複数の特徴点をそれぞれ抽出し、該特徴点について前記複数の撮像画像間での対応付けを行う特徴点対応付手段と、前記移動体の挙動に関わる挙動情報を取得する挙動情報取得手段と、前記挙動情報に基づいて、前記複数の撮像画像を撮像する間での前記移動体の移動量を表す移動体移動量を算出する移動量算出手段と、前記複数の撮像画像間での特徴点の移動量と前記移動体移動量とに基づいて前記実位置を推定する実位置推定手段と、を備えたものが開示されている。

特開２０１４－１４２２４１号公報

自動駐車システムでは、駐車位置と周囲の障害物との位置関係等に応じて、複数回の切り返しを含む複雑な経路が設定されることがある。しかしながら、特許文献１に開示された技術を自動駐車システムに適用した場合には、自動車が経路に従って正しく移動しているか否かを確認することができないため、複雑な経路の走行中に位置の推定誤差が蓄積してしまい、正確な自律移動が困難となる。したがって、経路に従って自律移動する移動体の位置推定精度に関して、改善の余地がある。

本発明による位置推定装置は、予め設定された経路に従って移動する移動体の位置を推定するものであって、前記移動体に搭載された撮像部によって所定地点から順次撮像された複数の画像から画像特徴点をそれぞれ抽出し、前記複数の画像間で前記画像特徴点同士を対応付ける画像情報計算部と、前記移動体の移動に関する情報に基づいて前記所定地点からの前記移動体の移動量を計算して前記移動体の位置を推定する移動量計算部と、前記複数の画像間での前記画像特徴点同士の対応付け結果と、前記移動量計算部により推定された前記移動体の位置とに基づいて、前記画像特徴点の実空間上の３次元位置を推定する３次元画像特徴点計算部と、前記移動量計算部が推定した前記移動体の位置が前記経路上に予め設定された地点に到達したか否かを判定する経路上位置判定部と、前記経路上位置判定部により前記地点に到達したと判定された場合、前記撮像部にて撮影された画像から推定した前記３次元位置を再投影したときの当該画像上での位置と、当該画像から抽出された前記画像特徴点との間の位置誤差を計算し、前記位置誤差が低減するように、前記移動体の位置および姿勢を補正する位置補正部と、前記位置補正部による補正後の前記移動体の位置および姿勢と、前記地点に対して予め設定された前記移動体の位置および姿勢との差分をそれぞれ計算し、計算した各差分の少なくとも一方が閾値以上であった場合に前記経路を再生成すると判定する経路再生成判定部と、を備え、前記地点は、前記経路上で前記移動体が前進から後退又は後退から前進に切り返す切り返し地点であり、前記経路は、指定された駐車位置までの駐車経路である。

本発明によれば、経路に従って自律移動する移動体の位置推定精度を向上させることができる。

本発明の一実施形態に係る位置推定装置の構成を示す図である。本発明の一実施形態に係る位置推定装置および自動駐車システムによる処理の全体を示すフローチャートである。自律移動処理のフローチャートである。画像情報蓄積処理のフローチャートである。自己位置補正処理のフローチャートである。駐車枠検知を行う駐車シーンを示す図である。駐車枠選択を行う駐車シーンを示す図である。駐車経路の生成を説明する駐車シーンを示す図である。中継地点を設定する駐車シーンを示す図である。中継地点での自車両の想定位置を設定する駐車シーンを示す図である。自車両が中継地点に到達した駐車シーンを示す図である。中継地点での誤差が閾値以内の駐車シーンを示す図である。中継地点での誤差が閾値以上の駐車シーンを示す図である。駐車経路が再生成された駐車シーンを示す図である。車両座標系を示す図である。カメラ座標系を示す図である。画像特徴点と推定済みの３次元画像特徴点との関連付けを説明する図である。画像特徴点に対する３次元画像特徴点の推定を説明する図である。３次元画像特徴点による再投影誤差の算出を説明する図である。

図１は、本発明の一実施形態に係る位置推定装置１０の構成を示す図である。図１に示す位置推定装置１０は、乗用車等の移動体に搭載されており、自動駐車システム５０と組み合わされて使用されるものである。以下では、位置推定装置１０が搭載される移動体を「自車両」と称して説明する。位置推定装置１０は、あらかじめ決められた周期、例えば100ms毎に周期的に動作する。

図１に示すように、位置推定装置１０は、その機能として、画像情報計算部１１と、移動量計算部１２と、経路上位置判定部１３と、３次元画像特徴点計算部１４と、位置補正部１５と、経路再生成判定部１６とを備えている。位置推定装置１０は、例えばＣＰＵ、ＲＡＭ、ＲＯＭ等を有しており、ＲＯＭに格納された所定のプログラムをＲＡＭに展開してＣＰＵで実行することにより、これらの機能を実現することができる。なお、位置推定装置１０が有する機能の一部または全部を、ＦＰＧＡ等のハードウェアを用いて実現してもよい。

位置推定装置１０には、撮像画像２０と、車両情報３０と、経路情報４０とが入力される。撮像画像２０は、自車両に搭載された不図示の撮像部によって所定のフレームレートごとに順次撮像された画像である。この撮像部は、一般的な画角のカメラよりも広角に撮像可能な魚眼カメラなどで構成されることが好ましい。車両情報３０は、自車両から得られる自車両の移動に関する情報、例えば車両速度、ハンドル角度、ブレーキの状態、シフトブレーキの状態などを表す情報である。経路情報４０は、自動駐車システム５０によって予め設定された駐車経路の情報であり、自車両が駐車時に走行すべき駐車経路の位置や形状を表す情報と、駐車経路上に設定された中継地点の位置を表す情報とを含む。

画像情報計算部１１は、撮像部により順次撮像されて位置推定装置１０に入力される複数の撮像画像２０から画像特徴点をそれぞれ抽出し、複数の撮像画像２０間で画像特徴点同士の対応付けを行う。そして、抽出した画像特徴点および画像特徴点間の対応付け結果を３次元画像特徴点計算部１４へ出力する。

移動量計算部１２は、位置推定装置１０に入力される車両情報３０に基づいて、デッドレコニングにより自車両の移動量を計算し、得られた移動量の計算結果から自車両の位置を推定する。そして、自車両の位置の推定結果を、経路上位置判定部１３および３次元画像特徴点計算部１４へ出力する。

経路上位置判定部１３は、位置推定装置１０に入力される経路情報４０と、移動量計算部１２から入力される自車両の位置の推定結果とに基づいて、自車両が駐車経路上の特定の位置に到達したか否かを判定する。本実施形態では、経路上位置判定部１３は、経路情報が表す駐車経路上の中継地点や、駐車経路の終点である駐車位置を判定対象として、自車両がこれらの位置に到達したか否かを判定する。そして、判定結果を３次元画像特徴点計算部１４、位置補正部１５および経路再生成判定部１６へ出力する。

３次元画像特徴点計算部１４は、画像情報計算部１１から入力される複数の撮像画像２０間での画像特徴点同士の対応付け結果と、移動量計算部１２から入力される自車両の位置の推定結果とに基づいて、画像特徴点の実空間上の３次元位置を推定する。そして、推定した３次元位置を表す３次元画像特徴点を設定し、対応する画像特徴点の情報とともに位置補正部１５へ出力する。

位置補正部１５は、経路上位置判定部１３から入力される判定結果と、３次元画像特徴点計算部１４から入力される３次元画像特徴点の情報、すなわち画像特徴点に対する３次元位置の推定結果とに基づいて、移動量計算部１２により推定された自車両の位置を補正する。具体的には、位置補正部１５は、経路上位置判定部１３により自車両が中継地点に到達したと判定されると、そのときの撮像画像２０に対して、３次元画像特徴点計算部１４により推定された３次元画像特徴点が表す３次元位置を再投影したときの撮像画像２０上での位置と、撮像画像２０から抽出された画像特徴点との間の位置誤差を計算する。そして、計算した位置誤差が低減するように３次元画像特徴点および自車両の位置補正を繰り返し行い、最終的に得られた補正後の自車両の位置を経路再生成判定部１６および自動駐車システム５０へ出力する。ただし、自車両の位置補正をまだ行っていない場合、位置補正部１５は補正前の自車両の位置、すなわち移動量計算部１２から入力される自車両の位置の推定結果をそのまま出力する。なお、位置補正部１５が行う処理の内容については、後で詳細に説明する。

経路再生成判定部１６は、経路上位置判定部１３から入力される判定結果に基づいて、位置補正部１５から入力される補正後の自車両の位置と、経路情報４０が表す駐車経路との比較を行い、その比較の結果に基づいて駐車経路を再生成するか否かを判定する。そして、駐車経路を再生成すると判定した場合には、自動駐車システム５０に対して駐車経路の再生成を指示する。

自動駐車システム５０は、撮像部から入力される撮像画像２０に基づいて駐車候補位置を認識し、いずれかの駐車候補位置を駐車位置に設定する。そして、自車両の現在位置から設定した駐車位置までの駐車経路を生成し、その駐車経路に従って自車両を駐車位置まで自律移動させるように制御する。このとき自動駐車システム５０は、位置推定装置１０から出力される補正後の自車両の位置を用いて、自車両の制御を行う。また、位置推定装置１０から駐車経路の再生成が指示された場合には、その指示に応じて駐車経路を再生成し、以降の制御では再生成した駐車経路に従って自車両を自律移動させる。

次に、位置推定装置１０および自動駐車システム５０がそれぞれ実施する処理について図２～５のフローチャートを参照して説明する。図２は、本発明の一実施形態に係る位置推定装置１０および自動駐車システム５０による処理の全体を示すフローチャートである。図３は、図２の処理における自律移動処理のフローチャートである。図４は、図３の自律移動処理において位置推定装置１０が実施する画像情報蓄積処理のフローチャートである。図５は、図３の自律移動処理において位置推定装置１０が実施する自己位置補正処理のフローチャートである。なお、以下では図６～１４に示すような自車両の駐車シーンを例として想定し、図２～５の各フローチャートを説明する。

ここで、図２～５の説明に先立って、位置推定装置１０の処理で用いられる座標系について以下に説明する。図１５は、位置推定装置１０が自車両の位置および姿勢を表すために使用する車両座標系を示す図であり、図１６は、位置推定装置１０が撮像部の位置および姿勢を表すために使用するカメラ座標系を示す図である。

図１５に示す車両座標系では、自車両の向きに対して前後方向をｘ軸、左右方向をｙ軸、上下方向をｚ軸とそれぞれ定義している。ここで、ｘ軸は自車両の進行方向（正面方向）を正とし、ｙ軸は自車両の進行方向に対して左手方向を正とし、ｚ軸は自車両の真上方向を正としている。また、各軸周りの回転（roll、pitch、yaw）は、時計回り方向をそれぞれ正としている。本実施形態では、図１５の車両座標系を用いて、自車両の回転量と並進量を以下の式（１）のように表現し、これらを用いて自車両の位置と姿勢を表すこととする。式（１）において、θ_rollはｘ軸周りの回転量、θ_pitchはｙ軸周りの回転量、θ_yawはｚ軸周りの回転量をそれぞれ表し、x_carはｘ軸方向の並進量、y_carはｙ軸方向の並進量、z_carはｚ軸方向の並進量をそれぞれ表している。なお、回転量は自車両の姿勢に相当し、並進量は自車両の位置に相当する。
自車両の回転量＝（θ_roll，θ_pitch，θ_yaw）
自車両の並進量＝（x_car，y_car，z_car）・・・（１）

図１６に示すカメラ座標系では、撮像部を真上方向から見たときの左右方向をｘ軸、上下方向をｙ軸とそれぞれ定義し、撮像部の光軸方向をｚ軸と定義している。ここで、ｘ軸は撮像部の右手方向を正とし、ｙ軸は撮像部の真下方向を正とし、ｚ軸は撮像部の撮像方向を正としている。また、各軸周りの回転は、時計回り方向をそれぞれ正としている。本実施形態では、図１６のカメラ座標系を用いて、撮像部の回転量と並進量を以下の式（２）のように表現し、これらを用いて撮像部の位置と姿勢を表すこととする。式（２）において、θ_xはｘ軸周りの回転量、θ_yはｙ軸周りの回転量、θ_zはｚ軸周りの回転量をそれぞれ表し、x_cameraはｘ軸方向の並進量、y_cameraはｙ軸方向の並進量、z_cameraはｚ軸方向の並進量をそれぞれ表している。なお、回転量は撮像部の姿勢に相当し、並進量は撮像部の位置に相当する。
撮像部の回転量＝（θ_x，θ_y，θ_z）
撮像部の並進量＝（x_camera，y_camera，z_camera）・・・（２）

以下の説明では、上記の式（１）で表される車両座標系における自車両の回転量および並進量と、上記の式（２）で表されるカメラ座標系における撮像部の回転量および並進量とは、位置推定装置１０において移動量計算部１２により、車両情報３０に基づいて所定の処理周期ごとに計算されるものとする。

続いて、図２のフローチャートについて説明する。図２のフローチャートに示す全体処理は、ユーザが自動駐車システム５０を立ち上げることで開始される。

ステップ１００において、自動駐車システム５０は駐車枠検知を行う。ここでは、例えば図６の駐車シーンの例に示すように、自車両が駐車開始位置５００から矢印５０１の方向に移動しているときに、白線５０２に囲まれた各領域のうち他の駐車が駐車済みでない領域を、駐車候補位置５０３、５０４として検出する。

ステップ１１０において、自動駐車システム５０は駐車枠選択を行う。ここでは、例えば図７の駐車シーンの例に示すように、ステップ１００で検出された駐車候補位置の中でいずれか一つを選択し、駐車位置５１０として設定する。なお、選択されなかった駐車候補位置は、未選択位置５１１として設定され、以降の処理対象から除外される。あるいは、駐車位置５１０への駐車が不可能になったときに、未選択位置５１１を新たに駐車位置として設定するようにしてもよい。図７に示す駐車シーンの例では、図６の駐車候補位置５０３が駐車位置５１０に設定され、駐車候補位置５０４が未選択位置５１１に設定された様子を示している。

ステップ１１０における駐車枠選択では、例えば、ステップ１００で検出した各駐車候補位置をディスプレイ等によりユーザに提示し、その中でいずれかを選択させる。あるいは、自動駐車システム５０が自動的に駐車候補位置を選択してもよい。例えば、複数の駐車候補位置のうち領域が広い方や、駐車までの切り返し回数が少なくなる方などを、駐車候補位置として自動的に選択することができる。ただし、駐車候補位置の選択方法はこれに限定するものではなく、他の選択方法としてもよい。

ステップ１１０の駐車枠選択によって駐車位置を決定した後、ステップ１２０では、決定した駐車位置まで自車両を自律移動させる自律移動処理を実行する。この自律移動処理では、図３に示すフローチャートに従って、位置推定装置１０および自動駐車システム５０を協調動作させる。ステップ１２０の自律移動処理によって自車両が駐車位置に到着したら、図２に示すフローチャートを終了する。

次に、図２のステップ１２０で実施される自律移動処理について、図３を参照して以下に説明する。

ステップ４００において、自動駐車システム５０は、駐車位置までの駐車経路を生成する。ここでは、自車両の位置と駐車位置との関係や、自車両の周囲に存在する障害物等の位置に基づいて駐車経路を生成し、その駐車経路上に一つまたは複数の中継地点を設定する。そして、中継地点に到達したときの自車両の位置および姿勢を計算し、これらの計算結果に基づいて中継地点での自車両の想定位置を設定する。これにより、例えば図８の駐車シーンの例に示すように、自車両の現在位置から駐車位置５１０の間に駐車経路５２０が設定される。また、例えば図９の駐車シーンの例に示すように、駐車経路５２０上で自車両が前進から後退に、または後退から前進に切り返す切り返し地点に中継地点５３０が設定され、この中継地点５３０に対して、図１０の駐車シーンの例に示すように自車両の想定位置５４０が設定される。なお、駐車経路の生成には任意の手法を用いることができるが、その具体的な説明は省略する。また、中継地点は、例えば数メートル毎に設定したり、切り返し地点毎に設定したりするなど、任意の間隔や位置に設定することができる。

次にステップ４１０において、自動駐車システム５０は、位置推定装置１０の位置補正部１５から出力される自車両の位置を取得し、この位置情報に基づいて、ステップ４００で生成した駐車経路に従って自車両を自律移動させるように制御する。

次にステップ４２０において、位置推定装置１０は、経路上位置判定部１３により、自車両が駐車位置に到着したか否かを判定する。ここでは、例えば駐車位置到達時に想定される自車両の位置および姿勢と、現在の自車両の位置および姿勢との差分をそれぞれ求め、求めた各差分が予め設定された閾値以下であるか否かにより、自車両が駐車位置に到着したか否かを判定する。その結果、自車両が駐車位置にまだ到着していないと判定した場合はステップ４３０に進み、到着したと判定した場合は図３のフローチャートに示す自律移動処理を終了する。なお、駐車位置到達時に想定される自車両の位置および姿勢は、ステップ４００で計算される中継地点での自車両の想定位置と同様の方法により、経路情報４０に基づいて計算可能である。

ステップ４２０で経路上位置判定部１３は、例えば以下の計算式（３）～（７）を用いて、駐車位置到達時に想定される自車両の位置および姿勢と、現在の自車両の位置および姿勢との差分を計算する。

P_error＝P_car ^-1P_current ・・・（４）
diff＝0.5×（r_{11_error}＋r_{22_error}＋r_{33_error}－1.0）・・・（５）
E_rotation＝acos（max（min（diff,1.0),-1.0））・・・（６）
E_translation＝√（t_{1_error} ²＋t_{2_error} ²＋t_{3_error} ²）・・・（７）

式（３）は、各地点での自車両の位置および姿勢を表すアフィン行列であり、式（４）のP_error、P_car ^-1およびP_currentはこのアフィン行列を用いてそれぞれ表される。なお、P_car ^-1は駐車位置での自車両の位置および姿勢を表すアフィン行列の逆行列であり、P_currentは現在位置での自車両の位置および姿勢を表すアフィン行列である。

式（３）のアフィン行列における各行列成分は、以下の式（８）、（９）により表される。

式（８）において、行列R_roll、R_pitch、R_yawは、式（１）における自車両の回転量の各成分θ_roll、θ_pitch、θ_yawを用いて、それぞれ以下の式（１０）～（１２）により表される。

一方、式（９）におけるx_car、y_car、z_caは、式（１）における自車両の並進量の各成分である。

なお、自車両の回転量および並進量は、前述のように移動量計算部１２によって計算される。ただし、位置補正部１５が後述のステップ４５０で自己位置補正処理を実行済みであり、これによって自車両の位置が補正されている場合には、その補正後の位置に対応する回転量および並進量を用いて、ステップ４２０の判定を行うことが好ましい。

式（５）におけるr_{11_error}、r_{22_error}、r_{33_error}は、式（４）で計算される位置姿勢誤差P_errorのアフィン行列において式（３）の各行列成分r₁₁、r₂₂、r₃₃にそれぞれ対応する行列成分の値を表す。式（６）は、式（５）で計算される角度差diffまたは１のうち小さい方の値と－１とを比較して、いずれか大きい方の値の逆余弦から回転誤差E_rotationが計算されることを表している。また、式（７）におけるt_{1_error}、t_{2_error}、t_{3_error}は、式（４）で計算される位置姿勢誤差P_errorのアフィン行列において式（３）の各行列成分t₁、t₂、t₃にそれぞれ対応する行列成分の値を表す。式（７）は、これらの値により位置誤差E_translationが計算されることを表している。

ステップ４２０では、上記の回転誤差E_rotationおよび位置誤差E_translationを計算することで、駐車位置到達時に想定される自車両の姿勢と現在の自車両の姿勢との差分、および駐車位置到達時に想定される自車両の位置と現在の自車両の位置との差分を、それぞれ計算することができる。そして、これらの差分をそれぞれの閾値と比較することで、自車両が駐車位置に到着したか否かを判定することができる。

図１１は、自車両が中継地点に到達した駐車シーンの例を示している。図１１の駐車シーンでは、ステップ４００で設定された想定位置５４０と中継地点での実際の自車両の位置５５１との間にずれが生じている。これは、ステップ４１０の制御において車両情報３０から求められる自車両の移動量には、誤差が含まれているためである。

ステップ４２０からステップ４３０に進んだ場合、ステップ４３０において位置推定装置１０は、画像情報蓄積処理を行う。ここでは、画像情報計算部１１、移動量計算部１２および３次元画像特徴点計算部１４により、図４に示すフローチャートに従って画像情報蓄積処理を実行する。これにより、撮像部によって所定のフレームレートごとに順次撮像された撮像画像２０からそれぞれ抽出された画像特徴点の情報や、撮像画像２０間での画像特徴点同士の対応付け情報、実空間上で推定された画像特徴点の３次元位置の情報などが、位置推定装置１０において蓄積される。なお、ステップ４２０の画像情報蓄積処理の詳細は後述する。

ステップ４４０において、位置推定装置１０は、自車両の位置を補正するか否かを判定する。ここでは、経路上位置判定部１３により自車両が次の中継地点に到達したか否かを判定することで、自車両の位置を補正するか否かを判定する。すなわち、自車両が次の中継地点にまだ到達していない場合は、自車両の位置を補正しないと判定してステップ４１０に戻り、前述の処理を繰り返す。一方、自車両が次の中継地点に到達した場合は、自車両の位置を補正すると判定してステップ４５０に進む。なお、ステップ４４０における自車両が次の中継地点に到達したか否かの判定は、ステップ４００で計算した中継地点での自車両の想定位置を表す位置および姿勢に基づき、ステップ４２０で行った駐車位置への到着判定と同様の方法により行うことが可能である。

ステップ４５０において、位置推定装置１０は、自己位置補正処理を行う。ここでは、位置補正部１５により、図５に示すフローチャートに従って自己位置補正処理を実行する。これにより、ステップ４３０の画像情報蓄積処理によって蓄積された情報を用いて、自車両の位置が補正される。なお、ステップ４５０の自己位置補正処理の詳細は後述する。

ステップ４６０において、位置推定装置１０は、経路再生成判定部１６により、ステップ４５０の自己位置補正処理によって補正された自車両の位置と駐車経路とを比較する。ここでは、補正後の自車両の位置および姿勢と、駐車経路上に設定した中継地点に対して予め設定された自車両の位置および姿勢との間の差分をそれぞれ計算することにより、補正後の自車両の位置と駐車経路との比較を行う。なお、このときの計算には、ステップ４４０で行った中継地点への到着判定と同様の方法を用いることができる。すなわち、ステップ４００で計算した中継地点での自車両の想定位置を表す位置および姿勢に基づき、ステップ４２０で行った駐車位置への到着判定と同様の方法により、補正後の自車両の位置および姿勢と、中継地点において想定される自車両の位置および姿勢との間の差分をそれぞれ計算することができる。

ステップ４７０において、位置推定装置１０は、経路再生成判定部１６により、ステップ４６０で計算した位置および姿勢の差分、すなわち補正後の自車両の位置および姿勢に対する中継地点での誤差が、それぞれ所定の閾値以内であるか否かを判定する。その結果、これらの計算結果がいずれも閾値以内であれば、補正後の自車両の位置が適切であり、そのため駐車経路を再生成しないと判断してステップ４１０に戻る。この場合、自動駐車システム５０では補正後の自車両の位置を用いた制御が行われることにより、自車両の自律移動が継続される。

図１２は、ステップ４７０で補正後の自車両の位置および姿勢に対する中継地点での誤差がいずれも閾値以内と判定される駐車シーンの例を示している。図１２の駐車シーンでは、ステップ４００で設定された想定位置５４０と中継地点での実際の自車両の位置５５１との間のずれが小さい。そのため、ステップ４７０では、補正後の自車両の位置および姿勢に対する中継地点での誤差がいずれも閾値以内と判定される。

一方、ステップ４６０で計算した補正後の自車両の位置および姿勢に対する中継地点での誤差の少なくとも一方が閾値以上であれば、ステップ４７０において経路再生成判定部１６は、補正後の自車両の位置が適切ではなく、このままでは自動駐車に失敗する可能性が高いため、駐車経路を再生成すると判断してステップ４００に戻る。この場合、自動駐車システム５０ではステップ４００において駐車経路が再生成され、以降は再生成後の駐車経路に従って自車両の自律移動が行われる。

図１３は、ステップ４７０で補正後の自車両の位置および姿勢に対する中継地点での誤差の少なくとも一方が閾値以上と判定される駐車シーンの例を示している。図１３の駐車シーンでは、ステップ４００で設定された想定位置５４０と中継地点での実際の自車両の位置５５１との間のずれが大きい。そのため、ステップ４７０では、補正後の自車両の位置および姿勢に対する中継地点での誤差が閾値以上と判定される。

図１４は、駐車経路が再生成された駐車シーンの例を示している。図１４の駐車シーンでは、図１３で想定位置５４０との誤差が閾値以上と判定された自車両の位置５５１と駐車位置５１０の間に、新たな駐車経路５７０が再生成されている。

次に、図３のステップ４３０で実行される画像情報蓄積処理について、図４のフローチャートを参照して以下に説明する。

ステップ２００において、画像情報計算部１１は、撮像部で撮像された撮像画像２０を読み込む。この撮像画像２０は、例えば魚眼カメラで撮像された広角画像などが望ましいが、これに限定しない。

ステップ２０１において、画像情報計算部１１は、ステップ２００で読み込んだ撮像画像２０から画像特徴点を抽出する。本実施形態では、例えばORB（Oriented FAST and Rotated BRIEF）と呼ばれる周知の処理手法により、画像特徴点を抽出する。なお、他の処理手法、例えばHarrisコーナー検出やTomasi、FAST（Features from Accelerated Segment Test）などの特徴量の記述がないコーナー点抽出手法や、SURFやSIFTなどの特徴点抽出手法などを用いてもよく、処理手法は限定しない。

ステップ２０２において、画像情報計算部１１は、ステップ２０１で抽出した画像特徴点の対応付けを行う。ここでは、時系列で取得された複数の撮像画像からそれぞれ抽出された画像特徴点のうち、同一の被写体の同じ部分を撮像した画像特徴点同士を互いに対応付ける。この対応付けは、画像特徴点の特徴量同士の比較や、画像特徴点の周囲の画素値の比較などにより実施される。例えば、本実施形態で画像特徴点の抽出に使用されるORBの場合、バイナリで記述された特徴量の排他的論理和をとることで画像特徴点の特徴量の比較を行うことが可能であり、その差が小さいほど、同一被写体の同じ部分を撮像した特徴点の可能性が高くなる。また、Harrisなどの特徴量のない方法で検出したコーナー点の場合は、例えばSSD（Sum of Squared Difference）やKLT（Kanade-Lucas-Tomasi Feature Tracker）法などの画像特徴点の周辺画素の比較により、画像特徴点同士の対応付けを行うことができる。

なお、本実施形態では、ステップ２０２で画像特徴点同士を対応付ける際に、ある撮像画像における一つの画像特徴点に対して、他の画像における二つの画像特徴点を対応付けることが好ましい。位置補正部１５において後述する自己位置補正処理の際に行われるバンドル調整では、３次元画像特徴点の数が多いほど安定した結果が得られることが知られている。そのため、一つの画像特徴点に対して二つの画像特徴点を対応付けることは、後段のステップ２０６で３次元画像特徴点への関連付けを行う際や、続くステップ２０７で３次元画像特徴点の推定を行う際に、３次元画像特徴点の数が増加することに繋がる。その結果、バンドル調整の精度や安定性の向上に寄与する。なお、ステップ２０２で一つの画像特徴点に対応付ける画像特徴点の数は二つに限らず、複数であれば任意の数としてもよい。あるいは、一つの画像特徴点に対して一つの画像特徴点を対応付けてもよい。

また、ステップ２０２で画像特徴点の対応付けを行う際には、誤って対応付けされた画像特徴点に対する対応付け結果の除去を行うようにしてもよい。例えば、クロスチェックと呼ばれる公知の手法を用いて、画像特徴点の対応付け結果が正しいか否かを確認することができる。詳細な説明は省略するが、この手法は、画像特徴点同士が対応付けされた二つの撮像画像を画像Ａ、Ｂとすると、一方の画像Ａから他方の画像Ｂへと対応付けを行った結果と、画像Ｂから画像Ａへと対応付けを行った結果とを比較し、共通の結果が得られた場合はこれを画像特徴点の対応付け結果として採用する手法である。

ステップ２０３において、画像情報計算部１１は、ステップ２０２で対応付けされた画像特徴点の歪みを補正する。カメラを用いて構成された撮像部で撮像された画像には、一般にカメラの光学系によるレンズ歪みが生じている。このため、ステップ２０３では、対応付けされた画像特徴点に対してレンズ歪みを取り除く処理を行う。なお、ここでは画像特徴点に対してレンズ歪みを取り除く処理を行ったが、ステップ２０１で画像特徴点を抽出しやすくなるように、ステップ２００で読み込んだ撮像画像２０に対してレンズ歪みを取り除く処理を実施しても構わない。また、レンズ歪みが小さい場合は、ステップ２０３の処理を省略してもよい。

ステップ２０４において、移動量計算部１２は、自車両から車両情報３０を読み込む。ここで読み込まれる車両情報３０は、前述したように、例えば車両速度やハンドル角度、シフトブレーキの状態などの自車両の移動に関する情報であり、自車両から取得可能なものである。

ステップ２０５において、移動量計算部１２は、ステップ２０４で読み込んだ車両情報３０から自車両の移動量を推定する。ここでは、前述の式（１）に示した自車両の回転量および並進量を表す６つの変数のうち、z_car、θ_rollおよびθ_pitchについては、車両情報３０が表す車両速度とハンドル角度からは推定できないため、これらの値を０とする。こうして推定した自車両の移動量を積算することで、自車両の位置を推定することができる。また、以降の処理に必要となる撮像部カメラ座標系での移動量は、車両へのカメラの取り付け位置を基に計算した前述の式（２）に示した撮像部の回転量および並進量は、車両座標系とカメラ座標系間の変換行列が既知であるとし、この変換行列を用いて、自車両の回転量および並進量から座標変換により算出する。

ステップ２０６において、３次元画像特徴点計算部１４は、ステップ２０２で行った画像特徴点の対応付け結果に基づいて、画像特徴点と推定済みの３次元画像特徴点との関連付けを行う。ここでは、ステップ２０１で抽出した画像特徴点の中に、過去に実行された後述のステップ２０７で既に３次元画像特徴点を推定済みの画像特徴点に対応付けられたものがある場合は、その３次元画像特徴点に当該画像特徴点を関連付ける。これにより、当該画像特徴点については、ステップ２０７で３次元画像特徴点の推定を行わずに、推定済みの３次元画像特徴点を当該画像特徴点の実空間上の３次元位置を表すものとして取り扱うようにする。この処理は、３次元画像特徴点毎の拘束条件を増やすことにつながり、後述するバンドル調整を利用した位置補正の精度を向上させる効果がある。

以上説明したステップ２０６の処理について、以下に図１７の具体例を参照してさらに説明を行う。図１７の例では、カメラ位置P_Aで取得した撮像画像上の画像特徴点（u_PA，v_PA）と、カメラ位置P_Bで取得した撮像画像上の画像特徴点（u_PB，v_PB）とが互いに対応付けされており、これらの画像特徴点から既に３次元画像特徴点（X_cp，Y_cp，Z_cp）が推定されているとする。ここで、３次元画像特徴点とは、画像特徴点の実空間上の位置のことを表している。この状態において、新たにカメラ位置P_Cで取得した撮像画像から画像特徴点（u_PC，v_PC）が抽出され、これと画像特徴点（u_PA，v_PA）または画像特徴点（u_PB，v_PB）とが対応付けられたと仮定する。このような場合、ステップ２０６では、画像特徴点（u_PC，v_PC）を３次元画像特徴点（X_cp，Y_cp，Z_cp）と関連付けることで、３次元画像特徴点（X_cp，Y_cp，Z_cp）が上記３つの画像特徴点から推定されたものとして扱うようにする。これにより、続くステップ２０７において画像特徴点（u_PC，v_PC）から３次元画像特徴点の計算を行わないようにする。なお、図１７の例では三つの画像特徴点を一つの３次元画像特徴点（X_cp，Y_cp，Z_cp）に関連付ける様子を示したが、３次元画像特徴点に関連付けされる画像特徴点の数は特に制限しない。

ステップ２０７において、３次元画像特徴点計算部１４は、ステップ２０５で推定した自車両の移動量に基づいて、ステップ２０２で対応付けした画像特徴点に対する３次元画像特徴点の推定を行う。ここでは、互いに対応付けられた画像特徴点の各撮像画像２０上での位置と、各撮像画像２０を撮像したときのカメラ位置とに基づいて、三角測量の要領により実空間上の位置を求めることで、３次元画像特徴点推定する。なお、ここで対象となる画像特徴点は、ステップ２０６で３次元画像特徴点に関連付けされていないものに限られる。

以上説明したステップ２０７の処理について、以下に図１８の具体例を参照してさらに説明を行う。図１８の例では、カメラ位置P_Aで取得した撮像画像上の画像特徴点（u_PA，v_PA）と、カメラ位置P_Cで取得した撮像画像上の画像特徴点（u_PC，v_PC）とが互いに対応付けされており、これらのカメラ位置および画像特徴点から、３次元画像特徴点（X_cp，Y_cp，Z_cp）を推定する様子を示している。なお、カメラ位置P_Cは、ステップ２０５で推定した自車両の移動量を座標変換することで算出されるカメラ座標上での撮像部の移動量ΔP_cから求められる。

ステップ２０８において、３次元画像特徴点計算部１４は、ステップ２０７で推定した３次元画像特徴点のうち、信頼性が低いものを削除する。ここでは、推定した各３次元画像特徴点を元の撮像画像２０のいずれかに再投影したときの当該撮像画像上での位置と、当該撮像画像２０から抽出された元の画像特徴点との間の位置誤差（再投影誤差）を算出する。そして、算出した位置誤差に基づいて、位置誤差が大きいほどその３次元画像特徴点の信頼度が低いものとして各３次元画像特徴点の信頼度を判断し、信頼度が所定値以下の３次元画像特徴点を削除する。これにより、位置補正部１５において後述する自己位置補正処理の際に行われるバンドル調整では、信頼性が低い３次元画像特徴点を除外して自車両の位置補正を行うようにする。

以上説明したステップ２０８の処理について、以下に図１９の具体例を参照してさらに説明を行う。図１９の例では、カメラ位置P_A、P_Cでそれぞれ取得した撮像画像に３次元画像特徴点（X_cp，Y_cp，Z_cp）を再投影したときの各再投影点の位置と、これらの撮像画像における元の画像特徴点（u_PA，v_PA）、（u_PC，v_PC）との間の位置誤差（再投影誤差）から、３次元画像特徴点（X_cp，Y_cp，Z_cp）の信頼度を算出する様子を示している。なお、この３次元画像特徴点の信頼度は、カメラ位置P_Aでの撮像画像とカメラ位置P_Cでの撮像画像との両方から算出してもよいし、いずれか一方のみから算出してもよい。

図１９における位置誤差（再投影誤差）E_reprojectionは、以下の式（１３）により算出することができる。式（１３）において、U_pおよびV_pは各撮像画像上での元の画像特徴点の位置を表し、X_cp、Y_cpおよびZ_cpは３次元画像特徴点の位置、すなわち画像特徴点の実空間上での位置を表している。また、fは撮像部に用いたカメラの焦点距離を表している。ステップ２０８では、この式（１３）で求められた再投影誤差E_reprojectionが所定の閾値より大きいものを、信頼度の低い３次元画像特徴点として削除する。

なお、本実施形態では、上記のように再投影誤差に基づいて信頼度の低い３次元画像特徴点を除去するようにしたが、他の方法を利用して信頼度の低い３次元画像特徴点を除去してもよい。例えば、十分な視差が得られていない画像特徴点同士から生成された３次元画像特徴点は、一般的に位置精度が低くなるため、これを信頼度の低い３次元画像特徴点として削除することができる。また、これ以外にも公知の方法が存在するため、それらの方法を利用しても構わない。

ステップ２０９では、上記の各処理で得られた画像情報を位置推定装置１０において登録する。ここで登録される画像情報とは、ステップ２０１で抽出した画像特徴点や、ステップ２０２で行った画像特徴点の対応付け結果、ステップ２０３で行った歪み補正後の画像特徴点、ステップ２０６で行った画像特徴点と３次元画像特徴点との関連付け結果、ステップ２０７で推定した３次元画像特徴点のうちステップ２０８で除去されたものを除いた３次元画像特徴点の推定結果などである。ステップ２０９では、これらの情報を位置推定装置１０が有するＲＡＭやハードディスク等の記憶媒体に記憶させることで、後の処理において利用できるようにする。

ステップ２１０では、自己位置情報、すなわちステップ２０５で推定した移動量から求められた自車両の位置情報を位置推定装置１０において登録する。ここで登録される自己位置情報とは、前述の式（１）、（２）により６自由度でそれぞれ表される自車両の位置および姿勢と撮像部の位置および姿勢である。このステップ２１０でもステップ２０９と同様に、自己位置情報をＲＡＭやハードディスク等の記憶媒体に記憶させることで、後の処理において利用できるようにする。

図３のステップ４３０では、以上説明したような画像情報蓄積処理が実行される。

次に、図３のステップ４５０で実行される自己位置補正処理について、図５のフローチャートを参照して以下に説明する。

ステップ３００において、位置補正部１５は、図４のステップ２０９で登録された画像情報を読み込む。ここでは、撮像部によって順次撮像された複数の撮像画像２０からそれぞれ抽出された画像特徴点や、画像特徴点の実空間上の３次元位置を表す３次元画像特徴点などの情報を、画像情報として読み込む。なお、前述のように画像特徴点は一つの３次元画像特徴点に対して二つ以上存在する。

ステップ３１０において、位置補正部１５は、図４のステップ２１０で登録された自己位置情報を読み込む。ここでは、各３次元画像特徴点に対応する撮像部の位置および姿勢を自己位置情報として読み込む。

ステップ３２０において、位置補正部１５は、ステップ３００、３１０でそれぞれ読み込んだ情報を利用して、これまでに推定された自車両の位置を補正する自己位置補正処理を行う。ここでは、各３次元画像特徴点が表す画像特徴点の実空間上の３次元位置をいずれかの撮像画像２０に再投影したときの当該撮像画像２０上での各位置と、その撮像画像２０から抽出された各画像特徴点との間の位置誤差を、互いに関連するもの同士で計算する。そして、位置誤差の合計が最小となるように各３次元画像特徴点と自車両位置の補正を繰り返し行い、最終的に得られたこれらの補正結果を自己位置補正処理の結果として出力する。

上記のような自己位置補正処理は、例えばバンドル調整という公知の手法を用いて実現できる。詳細な説明は省略するが、バンドル調整とは、多画像間で抽出された画像特徴点の対応関係から幾何学的なモデルのパラメータを推定する問題を解く際に利用されるものであり、非線形最適化問題を数値的に解く方法である。非線形最適化問題を解く上では、初期値と評価関数が重要であることが知られている。本実施形態では、複数の３次元画像特徴点を撮像画像２０上に再投影した際の再投影誤差の合計値を評価関数として利用することで、ステップ３２０の自己位置補正処理を非線形最小二乗問題として扱っている。具体的には、以下の評価式（１４）で計算される再投影誤差の合計値E_sumが最小になるような撮像部の位置および姿勢を推定し、その推定結果から補正後の自車両の位置を計算することで、ステップ３２０の自己位置補正処理を行うこととしている。なお、ここで推定される撮像部の位置および姿勢は、前述のように６自由度で表されるものである。

評価式（１４）の初期値は、前回の処理までに求められた各３次元画像特徴点と各画像特徴点の位置であり、これらはステップ３００で読み込んだ画像情報から設定される。また、本実施形態の位置推定装置１０では、時間経過とともに６自由度で撮像部の位置および姿勢が変化し、撮像画像２０が新たに取得されてそこから３次元画像特徴点が求められる。したがって、ステップ３２０の自己位置補正処理において最適化すべきパラメータの数は、３次元画像特徴点数×３＋撮像画像数×６個となる。このため、自動駐車の開始時から取得した全ての画像情報と撮像部の位置および姿勢を利用して自己位置補正処理を行うのは、パラメータ数が際限なく増加するため現実的ではない。そこで本実施形態では、自己位置補正処理において利用する撮像画像２０の数を制限し、これに対応する画像情報と撮像部の位置および姿勢を利用することが好ましい。例えば、直近の２０画像分に相当する画像情報とこれに対応する各地点での撮像部の位置および姿勢を利用して、自己位置補正処理を行うことができる。

また、バンドル調整で最適解を一意に求めるためには、３次元画像特徴点の一部または撮像部の位置および姿勢の一部を定数として固定する必要がある。このため本実施形態では、所定数の撮像画像２０に対応する自車両の位置を固定した局所的なバンドル調整を行うことにより、３次元画像特徴点および自車両位置の補正を行うことが好ましい。例えば、画像１０枚分に相当する自車両の位置を固定することで、これに対応する撮像部の位置および姿勢を定数として固定し、局所的なバンドル調整を行うことができる。ここで、３次元画像特徴点ではなく撮像部の位置および姿勢の一部を固定値としたのは、３次元画像特徴点の一部を固定した場合と比べて、推定される位置および姿勢の分散が小さくなることが経験的に知られているためである。さらに加えて、本実施形態ではデッドレコニングにより自車両の移動量を推定した結果から撮像部の位置および姿勢を推定しているため、撮像部の位置および姿勢の一部を固定値とすることで、スケールドリフトを解消することも可能である。

ステップ３２０の自己位置補正処理によって補正後の自車両の位置を計算できたら、ステップ３３０、３４０において、位置補正部１５は、その計算結果と、当該計算結果が得られたときの３次元画像特徴点および撮像部の位置、姿勢とに基づいて、蓄積された画像情報と自己位置情報をそれぞれ更新する。

図３のステップ４５０では、以上説明したような自己位置補正処理が実行される。

以上説明した本発明の一実施形態によれば、以下の作用効果が得られる。

（１）位置推定装置１０は、予め設定された経路に従って移動する移動体である自車両の位置を推定する。位置推定装置１０は、画像情報計算部１１と、移動量計算部１２と、３次元画像特徴点計算部１４と、位置補正部１５と、経路再生成判定部１６とを備える。画像情報計算部１１は、自車両に搭載された撮像部によって順次撮像された複数の画像から画像特徴点をそれぞれ抽出し（ステップ２０１）、複数の画像間で画像特徴点同士を対応付ける（ステップ２０２）。移動量計算部１２は、自車両の移動量を計算して自車両の位置を推定する（ステップ２０５）。３次元画像特徴点計算部１４は、ステップ２０２での画像情報計算部１１の処理による複数の画像間での画像特徴点同士の対応付け結果と、ステップ２０５で移動量計算部１２により推定された自車両の位置とに基づいて、画像特徴点の実空間上の３次元位置を表す３次元画像特徴点を推定する（ステップ２０７）。位置補正部１５は、３次元画像特徴点を複数の画像のいずれかに再投影したときの当該画像上での位置と、当該画像から抽出された画像特徴点との間の位置誤差を計算し、この位置誤差が低減するように、３次元画像特徴点および自車両の位置を補正する（ステップ３２０）。経路再生成判定部１６は、位置補正部１５により補正された自車両の位置と経路との比較を行い（ステップ４６０）、この比較の結果に基づいて経路を再生成するか否かを判定する（ステップ４７０）。このようにしたので、経路に従って自律移動する移動体である自車両の位置推定精度を向上させることができる。

（２）位置推定装置１０は、経路上に予め設定された中継地点に自車両が到達したか否かを判定する経路上位置判定部１３を備える。位置補正部１５は、経路上位置判定部１３により自車両が中継地点に到達したと判定されたとき（ステップ４４０：Ｙｅｓ）に、ステップ４５０の自己位置補正処理を実行して自車両の位置の補正を行う。このようにしたので、自車両が経路に従って自律移動しているときに、適切なタイミングで自車両の位置の補正を行うことができる。

（３）経路再生成判定部１６は、ステップ４６０において、位置補正部１５による補正後の自車両の位置および姿勢と、中継地点に対して予め設定された自車両の位置および姿勢との間の差分をそれぞれ計算する。ステップ４７０では、ステップ４６０で計算した各差分に基づいて駐車経路を再生成するか否かの判定を行う。このようにしたので、補正後の自車両の位置が適切ではなく、このままでは自動駐車に失敗する可能性が高い場合には、駐車経路を再生成して自動駐車を確実に成功させることができる。

（４）３次元画像特徴点計算部１４は、推定した３次元画像特徴点を複数の画像のいずれかに再投影したときの当該画像上での位置と、当該画像から抽出された画像特徴点との間の位置誤差に基づいて、３次元画像特徴点の信頼度を判定する（ステップ２０８）。この信頼度が低い３次元画像特徴点はステップ２０８で除去されるため、位置補正部１５は、ステップ３２０では、３次元画像特徴点計算部１４により信頼度が低いと判定された３次元画像特徴点を除外して、位置誤差の計算を行う。このようにしたので、位置補正部１５が３次元画像特徴点および自車両の位置を補正する際には、位置誤差の計算を適切に行うことができる。

（５）画像情報計算部１１は、ステップ２０２において複数の画像間で画像特徴点同士を対応付ける際に、複数の画像に含まれる二つの画像のうち一方の画像における一つの画像特徴点に対して、他方の画像における複数の画像特徴点を対応付けることが好ましい。このようにすれば、位置補正部１５において、３次元画像特徴点および自車両の位置を精度良く補正することが可能となる。

（６）位置補正部１５は、ステップ３２０において、複数の画像のうち所定数の画像に対応する自車両の位置を固定した局所的なバンドル調整を行うことにより、３次元画像特徴点および自車両の位置の補正を行うことが好ましい。このようにすれば、３次元画像特徴点および自車両の位置の補正結果としての最適解を一意に求めることが可能となる。

（７）３次元画像特徴点計算部１４は、既に３次元画像特徴点を推定済みの画像特徴点と対応付けされた画像特徴点については、３次元画像特徴点の推定を行わずに、推定済みの３次元画像特徴点を当該画像特徴点の３次元位置とする（ステップ２０６）。このようにしたので、処理負荷を減らしつつ、位置補正部１５が３次元画像特徴点および自車両の位置を補正する際の補正精度を向上させることができる。

（８）自車両が自律移動する経路は、指定された駐車位置までの駐車経路であることとしたので、位置推定装置１０を用いて自動駐車を実現することができる。

（９）位置推定装置１０は、駐車経路を生成して自車両を駐車位置まで自律移動させる自動駐車システム５０と接続されている。ステップ４７０で経路再生成判定部１６が駐車経路を再生成すると判定した場合、位置推定装置１０は、駐車経路の再生成を自動駐車システム５０に指示する。このようにしたので、必要に応じて駐車経路を再生成し、自動駐車を確実に成功させることができる。

以上、本発明の実施形態について述べたが、本発明は前述の実施形態に限定されるものでなく、特許請求の範囲に記載された範囲を逸脱しない範囲で種々の変更を行うことができる。例えば、上記の実施形態では自動駐車システム５０と組み合わされて自動駐車を行う位置推定装置１０について説明したが、他のシステムで用いられる位置推定装置においても本発明を適用可能である。また、前述した実施の形態は本発明を詳細に説明したものであり、必ずしも説明した全ての構成を備える必要はない。さらに、構成に他の実施形態の構成を加えることも可能である。加えて、構成の一部について、追加、削除、置き換えも可能である。

以上説明した実施形態や変形例はあくまで一例であり、発明の特徴が損なわれない限り、本発明はこれらの内容に限定されるものではない。また、上記では種々の実施形態や変形例を説明したが、本発明はこれらの内容に限定されるものではない。本発明の技術的思想の範囲内で考えられるその他の態様も本発明の範囲内に含まれる。

１０位置推定装置
１１画像情報計算部
１２移動量計算部
１３経路上位置判定部
１４３次元画像特徴点計算部
１５位置補正部
１６経路再生成判定部
２０撮像画像
３０車両情報
４０経路情報
５０自動駐車システム

Claims

予め設定された経路に従って移動する移動体の位置を推定する位置推定装置であって、
前記移動体に搭載された撮像部によって所定地点から順次撮像された複数の画像から画像特徴点をそれぞれ抽出し、前記複数の画像間で前記画像特徴点同士を対応付ける画像情報計算部と、
前記移動体の移動に関する情報に基づいて前記所定地点からの前記移動体の移動量を計算して前記移動体の位置を推定する移動量計算部と、
前記複数の画像間での前記画像特徴点同士の対応付け結果と、前記移動量計算部により推定された前記移動体の位置とに基づいて、前記画像特徴点の実空間上の３次元位置を推定する３次元画像特徴点計算部と、
前記移動量計算部が推定した前記移動体の位置が前記経路上に予め設定された地点に到達したか否かを判定する経路上位置判定部と、
前記経路上位置判定部により前記地点に到達したと判定された場合、前記撮像部にて撮影された画像から推定した前記３次元位置を再投影したときの当該画像上での位置と、当該画像から抽出された前記画像特徴点との間の位置誤差を計算し、前記位置誤差が低減するように、前記移動体の位置および姿勢を補正する位置補正部と、
前記位置補正部による補正後の前記移動体の位置および姿勢と、前記地点に対して予め設定された前記移動体の位置および姿勢との差分をそれぞれ計算し、計算した各差分の少なくとも一方が閾値以上であった場合に前記経路を再生成すると判定する経路再生成判定部と、を備え、
前記地点は、前記経路上で前記移動体が前進から後退又は後退から前進に切り返す切り返し地点であり、前記経路は、指定された駐車位置までの駐車経路である位置推定装置。
請求項１に記載の位置推定装置において、
前記地点は、所定の間隔で設定された地点である位置推定装置。
請求項１または請求項２に記載の位置推定装置において、
前記３次元画像特徴点計算部は、推定した前記３次元位置を前記複数の画像のいずれかに再投影したときの当該画像上での位置と、当該画像から抽出された前記画像特徴点との間の位置誤差に基づいて、前記３次元位置の信頼度を判定し、
前記位置補正部は、前記３次元画像特徴点計算部により前記信頼度が低いと判定された前記３次元位置を除外して、前記位置誤差の計算を行う位置推定装置。
請求項１または請求項２に記載の位置推定装置において、
前記画像情報計算部は、前記複数の画像間で前記画像特徴点同士を対応付ける際に、前記複数の画像のうちの一つの画像における一つの前記画像特徴点に対して、他の画像における複数の前記画像特徴点を対応付ける位置推定装置。
請求項１または請求項２に記載の位置推定装置において、
前記位置補正部は、前記複数の画像のうち所定数の画像に対応する前記移動体の位置を固定した局所的なバンドル調整を行うことにより、前記３次元位置および前記移動体の位置の補正を行う位置推定装置。
請求項１または請求項２に記載の位置推定装置において、
前記３次元画像特徴点計算部は、既に前記３次元位置を推定済みの画像特徴点と対応付けされた画像特徴点については、前記３次元位置の推定を行わずに、推定済みの前記３次元位置を当該画像特徴点の３次元位置とする位置推定装置。
請求項１に記載の位置推定装置において、
前記駐車経路を生成して前記移動体を前記駐車位置まで自律移動させる自動駐車システムと接続されており、
前記経路再生成判定部が前記駐車経路を再生成すると判定した場合、前記駐車経路の再生成を前記自動駐車システムに指示する位置推定装置。