特許 7456857 自己位置推定装置

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-03-18

(45)【発行日】2024-03-27

(54)【発明の名称】自己位置推定装置

(51)【国際特許分類】

   G01B 11/00 20060101AFI20240319BHJP

   G06T 7/70 20170101ALI20240319BHJP

【ＦＩ】

G01B11/00 H

G06T7/70 Z

【請求項の数】 3

(21)【出願番号】P 2020099244

(22)【出願日】2020-06-08

(65)【公開番号】P2021193340

(43)【公開日】2021-12-23

【審査請求日】2023-04-13

(73)【特許権者】

【識別番号】000001487

【氏名又は名称】フォルシアクラリオン・エレクトロニクス株式会社

(74)【代理人】

【識別番号】240000327

【弁護士】

【氏名又は名称】弁護士法人クレオ国際法律特許事務所

(72)【発明者】

【氏名】酒井 貴大

(72)【発明者】

【氏名】坂野 盛彦

【審査官】櫻井 仁

(56)【参考文献】

【文献】特開２０２０－０５２６８７（ＪＰ，Ａ）

【文献】特開２００５－１７３８９９（ＪＰ，Ａ）

【文献】特開２０１１－１４１２２１（ＪＰ，Ａ）

【文献】特開２０１３－１２５３０１（ＪＰ，Ａ）

【文献】米国特許出願公開第２０１８／００７３８９１（ＵＳ，Ａ１）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｂ １１／００

Ｇ０６Ｔ ７／００

(57)【特許請求の範囲】

【請求項1】

目標とする地点までの経路を移動する移動体の、前記経路における位置及び姿勢を推定する自己位置推定装置であって、
前記移動体に搭載された撮像手段により所定の周期で撮像された画像を順次取得して、前記画像から特徴点を抽出するとともに、一周期前に取得した画像における特徴点との対応付けを行う画像情報計算部と、
前記移動体の移動距離の算出に必要な車両情報を取得して、前記画像のうち、２つの画像の取得時刻間における前記移動距離を推定するスケール推定部と、
前記スケール推定部により推定した前記移動距離と、前記画像情報計算部において対応付けを行った特徴点の、前記画像における位置関係とに基づいて、前記移動体の、前記経路における位置及び姿勢を推定する位置姿勢推定部と、を備え、
前記画像と前記車両情報の取得時刻に差がある場合、前記画像の取得時刻に応じて、前記車両情報に基づいて推定した移動距離を補正する、自己位置推定装置。

【請求項2】

前記移動体の移動距離の算出に必要な車両情報は、前記移動体のタイヤの回転に応じて発生するパルス又は前記タイヤの回転量の情報であり、
前記スケール推定部は、前記移動体の移動距離の算出に必要な車両情報を基に前記移動体の移動距離を推定する請求項１に記載の自己位置推定装置。

【請求項3】

前記位置姿勢推定部は、前記移動体の運動モデルを用いて算出される複数の経路のうち、前記スケール推定部が推定した移動距離と、前記画像情報計算部において対応付けされた特徴点の前記画像における位置関係に基づいて、最も整合性がとれる前記移動体の位置及び姿勢を推定する請求項１に記載の自己位置推定装置。

【発明の詳細な説明】

【技術分野】

【0001】

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

【背景技術】

【0002】

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

【0003】

現在知られている自動駐車システムにおいては、車両速度やハンドル転舵角等の、車両情報によるデッドレコニング（Dead Reckoning；自律航法）に加えて、カメラなどの外界認識センサを用いることにより、自己の位置（自己位置）を高精度に推定しようとする試みがある。

【0004】

しかし、カメラはLiDAR（light detection and ranging；光による検知と測距）等と比較して安価であるものの、その撮像画像からは、２次元的な情報しか取得することができない。このため、撮像画像中の物体の大きさが一定せず、撮像画像中の物体を追跡することにより行う自己位置の推定技術は、移動距離の推定結果に大きな誤差を生じる。

【0005】

撮像画像と車両情報を利用した自己位置の推定に関して、移動体の移動に伴って移動する撮像部により順次撮像された複数の撮像画像に基づいて、撮像画像中に存在する特徴点の、実空間での位置（実位置）を推定する３次元位置推定装置が知られている（例えば、特許文献１参照）。

【0006】

この３次元位置推定装置は、前述の複数の撮像画像中において、１個又は複数個の特徴点をそれぞれ抽出し、その特徴点について複数の撮像画像間での対応付けを行う特徴点対応付け手段と、移動体の挙動に関わる挙動情報を取得する挙動情報取得手段と、挙動情報に基づいて複数の撮像画像を撮像する間の移動体の移動量（移動体移動量）を算出する移動量算出手段と、複数の撮像画像間での特徴点の移動量と移動体移動量とに基づいて実位置を推定する実位置推定手段と、複数の撮像画像のいずれかの撮像画像に実位置を再投影した際の撮像画像上の位置を表す抽出位置との差異を検出し、その差異が小さくなるように移動体移動量を補正する移動量補正手段と、複数の撮像画像での特徴点の移動量と補正後の移動体移動量とに基づいて実位置を推定する補正後実位置推定手段と、を備えている。

【先行技術文献】

【特許文献】

【0007】

【文献】特開２０１４-１４２２４１号公報

【発明の概要】

【発明が解決しようとする課題】

【0008】

ところで、バレーパーキングのように長距離走行を含む自動駐車システムでは、駐車位置と周囲の障害物との位置関係によっては、複数回の旋回や切り返しを行う必要があるため、自車を駐車位置に正確に移動させるために、自車の向きと位置を精度よく求める必要がある。

【0009】

しかし、特許文献１に開示された技術をそのような自動駐車システムに適用した場合、３次元位置に復元する際に、徐々にスケール誤差が累積するスケールドリフトが生じてしまう恐れがあった。これは、特許文献１の技術では、画像情報を利用して最終的な車両位置や姿勢を算出していることに起因している。

【0010】

つまり、特許文献１による技術は、移動体から得られる車両情報に基づいて算出される並進ベクトル及び回転行列を初期位置として処理を行うが、最終的には画像から得られる情報を利用した最適化処理を行うため、スケールドリフトの発生を回避できない。

【0011】

本発明は上記事情に鑑みなされたものであり、移動体から得られる情報に基づいて算出した移動距離を固定値として扱うことで、自己位置を精度よく推定することができる自己位置推定装置を提供することを目的とする。

【課題を解決するための手段】

【0012】

本発明は、目標とする地点までの経路を移動する移動体の、前記経路における位置及び姿勢を推定する自己位置推定装置であって、前記移動体に搭載された撮像手段により所定の周期で撮像された画像を順次取得して、前記画像から特徴点を抽出するとともに、一周期前に取得した画像における特徴点との対応付けを行う画像情報計算部と、前記移動体の移動距離の算出に必要な車両情報を取得して、前記画像のうち、２つの画像の取得時刻間における前記移動距離を推定するスケール推定部と、前記スケール推定部により推定した前記移動距離と、前記画像情報計算部において対応付けを行った特徴点の、前記画像における位置関係とに基づいて、前記移動体の、前記経路における位置及び姿勢を推定する位置姿勢推定部と、を備えた自己位置推定装置である。

【発明の効果】

【0013】

本発明に係る自己位置推定装置によれば、移動体から得られる情報に基づいて算出した移動距離を固定値として扱うことで、自己位置を精度よく推定できる。

【図面の簡単な説明】

【0014】

【図1】本発明の一実施形態である自己位置推定装置の構成を示すブロック図である。

【図2】図１に示した実施形態１の自己位置推定装置による処理全体の流れを示すフローチャートである。

【図3】画像特徴点とエピポーラ線の関係を示した図である。

【図4】実施形態１の自己位置推定装置により解消される、対応する画像特徴点の推定位置における誤差の一例を示した図である。

【図5】自己位置推定装置が自車両の位置と姿勢を表すために使用する車両座標系を示す図である。

【図6】自己位置推定装置がセンサとしてのカメラの取付け位置と姿勢を表すためのセンサ座標系を示す図である。

【図7】実施形態２の自己位置推定装置の構成を示した図である。

【図8】処理時間による位置姿勢を反映するタイミングの遅れを示す図である。

【図9】実施形態２の自己位置推定装置の処理全体の流れを示したフローチャートである。

【図10】データ取得周期が原因となって発生する移動距離の誤差を示した図である。

【図11】実施形態による、車両情報の取得タイミングと画像の取得タイミングと位置及び姿勢の推定処理のタイミングとの関係を示す図である。

【発明を実施するための形態】

【0015】

以下、本発明に係る自己位置推定装置の実施形態について、図面を用いて説明する。

【0016】

［実施形態１］
図１は本発明の一実施形態である自己位置推定装置１０の構成を示すブロック図である。図示の自己位置推定装置１０は、乗用車などの移動体に搭載されており、自動駐車システムと組み合わされて使用されるものである。

【0017】

ただし、本発明に係る自己位置推定装置は、自動駐車システムと組み合わされるものに限定されず、自動駐車システムと組み合わされないものであってもよい。

【0018】

また、自己位置推定装置１０は、移動体として乗用車に限定されず、乗用車以外の車両や、車両以外の移動体、例えば、走行可能な走行体を有する建設機械等に適用することもできる。

【0019】

以下、自己位置推定装置１０が搭載される移動体を「自車両」と称して説明する。また、自己位置推定装置１０は予め決められた周期、例えば、時間１００［ｍｓ］（ミリ秒）ごとに周期的に動作する。そして、自己位置推定装置１０は、自車両から、速度、ハンドル角度（ステアリングホイールの中立位置からの回転角度）、シフト位置及びブレーキ（制動装置）の状態などの車両情報３０を取得する機能、自車両に設置されたカメラにより撮像された画像２０を取得する機能を有する。

【0020】

図１に示すように、自己位置推定装置１０はその機能として、画像情報計算部１１と、スケール推定部１２と、位置姿勢計算部１３と、を備えている。自己位置推定装置１０は、例えばＣＰＵ、ＧＰＵ、ＲＡＭ、ＲＯＭ等を有しており、ＲＯＭに格納された所定のプログラムをＲＡＭに展開してＣＰＵの演算処理を実行することにより、これらの機能を実現する。なお、自己位置推定装置１０は、その一部又は全部の機能を、ＦＰＧＡ、ＡＳＩＣなどのハードウェアで実現する構成であってもよい。

【0021】

自己位置推定装置１０には、自車両に取り付けられたカメラにより撮像された画像２０、及び自車両からＣＡＮなどを経由して取得した車両情報３０が入力される。画像２０は、カメラによって所定のフレームレートで順次撮像される。車両情報３０は自車両から得られる自車両の移動に関する情報、及び移動に関する情報に準ずる情報であり、例えば車両速度、ハンドル角度、車両旋回半径、アクセルやブレーキの状態、シフトレバーの位置（状態）などである。車両情報３０も画像２０と同様に、所定のフレームレートで順次取得される。

【0022】

画像情報計算部１１は、入力された画像２０に基づいて、その画像２０に写った建物の角（コーナー部）といった画像として特徴的な点（特徴点）の座標を抽出し、時系列的に過去に入力された別の画像２０から抽出した特徴点との座標の対応付けを行う。画像情報計算部１１は、抽出した特徴点と、対応つけられた過去の特徴点との位置（座標）の情報を、位置姿勢計算部１３に出力する。

【0023】

スケール推定部１２は、車両情報３０を基に自車両の移動した実空間上での距離を推定する。移動距離は、車両速度及びタイヤの回転量などから推定することができる。スケール推定部１２は、推定した移動距離を位置姿勢計算部１３に出力する。

【0024】

位置姿勢計算部１３は、スケール推定部１２が推定した移動距離を基に、自車両の運動特性（予め記憶されている）から、移動経路の候補を算出する。そして、位置姿勢計算部１３は、画像情報計算部１１が算出した特徴点と対応する過去の特徴点の情報を利用して、移動経路の候補上で最も整合性のとれる位置と姿勢を、自車両の位置と姿勢として設定する。設定した自車両の位置と姿勢は、後段のシステム、例えば、自動駐車システムへ出力される。なお、位置姿勢計算部１３の処理内容については、後に、詳細に説明する。

【0025】

次に、自己位置推定装置１０が実施する処理についてフローチャートを参照して説明する。図２は図１に示した実施形態１の自己位置推定装置１０による処理全体の流れを示すフローチャート、図３は画像特徴点とエピポーラ線の関係を示した図、図４は実施形態１の自己位置推定装置１０により解消される、対応する画像特徴点の推定位置における誤差の一例を示した図である。

【0026】

ここで、図２の説明に先立って、自己位置推定装置１０の処理で用いられる座標系について説明する。図５は自己位置推定装置１０が自車両の位置と姿勢を表すために使用する車両座標系を示す図、図６は自己位置推定装置１０がセンサとしてのカメラの取付け位置と姿勢を表すためのセンサ座標系を示す図である。

【0027】

図５に示した車両座標系では、自車両の向きに対して前後方向をｘ軸、左右方向をｙ軸、上下方向をそれぞれｚ軸としてそれぞれ定義する。また、ｘ軸は自車両の進行方向の前方を正とし、ｙ軸は自車両の進行方向に対して左側を正とし、ｚ軸は自車両の高さ方向の上方向を正とする。ｘ，ｙ，ｚの各軸回りの回転であるroll、pitch、yawは、時計回り方向をそれぞれ正とする。

【0028】

本実施形態において、図５の車両座標系を用いて、自車両の回転量と並進量を以下のように表現する。すなわち、θ_rollをｘ軸回りの回転量、θ_pitchをｙ軸回りの回転量、θ_yawをｚ軸回りの回転量、ｘ_carをｘ軸方向の並進量、ｙ_carをｙ軸方向の並進量、ｚ_carをｚ軸方向の並進量とする。なお、回転量は自車両の姿勢の変化量に相当し、並進量は自車両の位置の変化量に相当する。

【0029】

図６に示すセンサ座標系では、カメラを真上方向から見たときの左右方向をｘ軸、上下方向をｙ軸とそれぞれ定義し、カメラの撮像部の光軸方向をｚ軸と定義している。ここで、ｘ軸は撮像部の右方向を正とし、ｙ軸は撮像部の下方向を正とし、ｚ軸は撮像部の撮像方向を正としている。また、ｘ，ｙ，ｚの各軸回りの回転は、時計回り方向をそれぞれ正としている。

【0030】

本実施形態では、図６のセンサ座標系を用いて、カメラの回転量と並進量を以下のように表現し、これらを用いてカメラの位置と姿勢を表すこととする。すなわち、θ_xをｘ軸回りの回転量、θ_yをｙ軸回りの回転量、θ_zをｚ軸回りの回転量、ｘ_cameraをｘ軸方向の並進量、ｙ_cameraをｙ軸方向の並進量、ｚ_cameraをｚ軸方向の並進量とする。なお、回転量はカメラの姿勢に相当し、並進量はカメラの位置に相当する。

【0031】

以下の説明では、車両座標系における自車両の回転量θ_roll，θ_pitch，θ_yaw及び並進量ｘ_car，ｙ_car，ｚ_carは自己位置推定装置１０における車両情報３０を基にして、また、センサ座標系におけるセンサの回転量θ_x，θ_y，θ_z及び並進量ｘ_camera，ｙ_camera，ｚ_cameraは自己位置推定装置１０における位置姿勢計算部１３により、それぞれの任意の周期で計算されるものとする。

【0032】

次に、図２に示した自己位置推定装置１０の全体処理を示すフローチャートについて説明する。なお、図２に示す処理は、ユーザが自動駐車システム４０を立ち上げたタイミングに同期して開始される。

【0033】

図２のフローチャートに示す実施形態では、センサとして、自車両に取り付けた広角な単眼カメラを適用するものとして説明する。また、カメラ及び自車両から取得するデータ（画像２０、車両情報３０）は、例えば３３［ｍｓ］の周期で取得される。また、画像２０の画像サイズは、例えば幅６４０［ピクセル］×縦４００［ピクセル］のものを想定する。

【0034】

車両情報３０は、自車両がどのように移動したかを計算するために必要となる情報を指す。まず、画像情報計算部１１は、単眼カメラで撮像された画像２０を取得して（Ｓ１００）、その画像から画像特徴点を抽出する（Ｓ１０１）。画像特徴点とは、画像中に写る建物の角（コーナー部）など、一意に定まる特徴的な点の座標である。

【0035】

本実施形態では、例えばＯＲＢ（Oriented FAST and Rotated BRIEF）と呼ばれる周知の処理手法により、画像特徴点を抽出する。なお、ＯＲＢに代えて、他の処理手法、例えばHarrisのコーナー検出やTomasi、FAST（Features from Accelerated Segment Test）などの特徴量の記述がないコーナー点抽出手法や、SURFやSIFTなどの特徴点抽出手法などを用いてもよく、処理手法はＯＲＢに限定されない。

【0036】

次いで、画像情報計算部１１は、最新の画像から抽出された画像特徴点と、時系列的に１つ以前の処理周期で取得した画像から抽出した画像特徴点との対応付けを行う（Ｓ１０２）。ここでは、時系列に異なるタイミングで取得された撮像画像からそれぞれ抽出された画像特徴点のうち、同一の被写体の同じ部分を撮像した画像特徴点同士を互いに対応付ける。

【0037】

この対応付けは、画像特徴点の特徴量同士の比較や、画像特徴点の周囲の画素値の比較などにより実施し、類似している特徴点同士を対応付ける。例えば、本実施形態で画像特徴点の抽出に使用されるＯＲＢの場合、バイナリで記述された特徴量の排他的論理和をとることで画像特徴点の特徴量の比較を行うことが可能である。

【0038】

そして、比較の結果、差が小さいほど、同一被写体の同じ部分を撮像した特徴点の可能性が高くなる。また、Harrisなどの特徴量のない方法で検出したコーナー点の場合は、例えばＳＳＤ（Sum of Squared Difference）やＫＬＴ（Kanade-Lucas-Tomasi Feature Tracker）法などの画像特徴点の周辺画素の比較により、画像特徴点同士の対応付けを行うことができる。

【0039】

また、画像特徴点の対応付け（Ｓ１０２）を行う際には、誤って対応付けされた画像特徴点に対する対応付け結果の除去を行うようにしてもよい。例えば、クロスチェックと呼ばれる公知の手法を用いて、画像特徴点の対応付け結果が正しいか否かを確認することができる。

【0040】

詳細な説明は省略するが、この手法は、画像特徴点同士が対応付けされた二つの撮像画像を画像Ａ、Ｂとすると、一方の画像Ａから他方の画像Ｂへと対応付けを行った結果と、画像Ｂから画像Ａへと対応付けを行った結果とを比較し、共通の結果が得られた場合はこれを画像特徴点の対応付け結果として採用する手法である。

【0041】

次に、画像情報計算部１１は、入力された画像から抽出され、かつ時系列的で１つ以前の処理周期で取得した画像で対応付けられた画像特徴点の歪み補正を行う（Ｓ１０３）。歪み補正とは、カメラのレンズの歪みによって生じる画像の歪みを補正するものである。一般的な方法としては、Zhangの手法がある。Zhangの手法では、最小二乗法によりパラメータを推定する。

【0042】

具体的には、キャリブレーションボードなどの既知の３次元座標と画像中のキャリブレーションボードの画像座標の組を集めて、既知の３次元座標をカメラモデルによって画像中に投影した際の画像座標と、画像中の対応するキャリブレーションボードの画像座標の差が小さくなるようなパラメータを推定する。このパラメータを利用して画像特徴点を補正することでカメラレンズの歪みを除去し、より精度の高い画像特徴点の座標を得る。

【0043】

次に、スケール推定部１２は、自車両から、例えばＣＡＮ(Control Area Network)を通じて、車両情報３０を取得する（Ｓ１０４）。車両情報３０とは、移動距離を推定（計算）する（Ｓ１０５）ために必要となる情報を指す。例えば、自車両の速度や、タイヤの回転数などである。

【0044】

移動距離を推定する処理は、取得した車両情報３０を利用することで行う。例えば、タイヤの回転数を利用した方法であれば、自車両のタイヤ径に処理周期中にタイヤが回転した量を掛ければよい。また、車両速度を利用する場合には例えば、１秒当たりの車両速度を基にして、前回の処理からの移動距離を求めればよい。

【0045】

次に、位置姿勢計算部１３は、自車量の位置及び姿勢の設定・更新処理（Ｓ１０６）と、自車量の移動後の位置及び姿勢の計算処理（Ｓ１０７）と、画像情報１１を利用した誤差の計算処理（Ｓ１０８）とを繰り返し演算する（Ｓ１０６～Ｓ１０９）。

【0046】

まず、位置姿勢計算部１３は、最適化処理のために推定する姿勢の初期値の設定及びその更新を行う（Ｓ１０６）。今回、姿勢の初期位置の推定は、例えば画像情報から推定可能な５点法という公知の手法を用いる。この５点法は、２枚の画像中に５つ以上の対応点がある場合に利用可能な姿勢推定手法である。

【0047】

ただし、５点法は、姿勢を精度よく推定できる一方で、スケールが不定となることが起こりうる。このため、位置姿勢計算部１３は、５点法で推定した姿勢とスケール推定部１２で推定したスケール（移動距離）と車両運動のモデルを利用して初期値となる位置及び姿勢を、以下の式（１）～(４)にしたがって算出する。つまり、位置姿勢計算部１３は、ｘ－ｙ平面上の位置と向きを推定する。以下に、自車両の運動モデルを想定する。

【0048】

【数1】

【0049】

ここで，Ｒはハンドル舵角から計算された旋回半径、ρは曲率、θ_yawは自車両の向き、Sは自車両の移動距離、ｘ_car，ｙ_carは車両座標上の座標、ｔは時刻である。また、ここで推定するのは、自車両の向きの時間的な変化量に相当するΔθ_yawtである。

【0050】

また、本実施例では、自車両の向きの時間的な変化量に相当するΔθ_yawtの更新を反復法の一種であるレーベンバーグ・マーカート法により行う。なお、値の更新には、他の反復法であるガウスニュートン法などを用いてもよい。

【0051】

位置姿勢計算部１３は、式(３)，(４)の結果を、カメラ座標系上での位置姿勢を表す行列形式に変換する（Ｓ１０７）。まず、式(５)を利用して、式(３)，(４)の結果を車両座標系上の位置及び姿勢を表現する行列形式に変換する。次に、式(６)を利用して車両座標系上の位置及び姿勢から、カメラ座標系上での位置及び姿勢に変換する。

【0052】

ここで、Pose_cartは時刻tでの車両座標上の位置と姿勢を表す行列、Pose_cameratは時刻tでのカメラ座標上での位置と姿勢を表す行列、Ａ^car _cameraは車両座標系からカメラ座標系へと値を変換する変換行列を示している。

【0053】

【数2】

【0054】

位置姿勢計算部１３は、ステップ１０７（Ｓ１０７）の結果とステップ１０３（Ｓ１０３）の結果を利用して、ステップ１０７（Ｓ１０７）における誤差を計算する（Ｓ１０８）。本実施形態では、後述の式（９）に示した基礎行列を利用して計算したエピポーラ線と画像特徴点の距離を誤差とする方法を示す。

【0055】

自車量の移動に伴うカメラの移動により、画像特徴点とエピポーラ線との間には、図３，４に示す関係がある。図３は、自車両の向きの時間的な変化量に相当するΔθ_yawtが正確に求められている場合における特徴点とエピポーラ線との関係を、また、図４は、自車両の向きの時間的な変化量に相当するΔθ_yawtに誤差が含まれる場合の特徴点とエピポーラ線との関係をそれぞれ示している。

【0056】

自車両の向きの時間的な変化量に相当するΔθ_yawtが正確に求められている場合、図３に示すように、時系列的に１つ以前（時刻ｔ－１）のカメラの位置Pose_camerat-1での画像における画像特徴点とカメラ原点を結んだ直線(光線)を、もう一方（時刻ｔにおけるカメラの位置Pose_cameratでの）画像上へ投影した場合、カメラ間の移動量Pose_refに対応する画像特徴点は、投影された直線(エピポーラ線)上に存在する。

【0057】

一方、自車両の向きの時間的な変化量に相当するΔθ_yawtに誤差がある場合には、図４に示すように、カメラ間の移動量Pose_refにも誤差が生じる（誤差が生じたカメラ間の移動量Pose′_ref）ため、図４のように、時刻ｔの誤差を含むカメラの位置Pose′_cameratでの画像に投影されたエピポーラ線と対応する画像特徴点との間で誤差が生じる。このため、このエピポーラ線と画像特徴点との誤差を解消するような自車両の向きの時間的な変化量に相当するΔθ_yawtを求めることにより、自車両の向きの時間的な変化量に相当するΔθ_yawtの誤差を解消することができる。

【0058】

位置姿勢計算部１３は、まず、時系列で１つ以前の時刻ｔ－１におけるカメラ座標上での位置及び姿勢間との移動量を式(９)により計算する。次に、計算した時刻ｔ－１，ｔ間の移動量の並進成分を利用してスキュー行列を計算する。そして、時刻ｔ－１，ｔ間のカメラ移動量とカメラの内部パラメータ及びスキュー行列を利用して基礎行列を計算する。

【0059】

ここで、SkewはPose_refの並進成分から計算したスキュー行列を、C_intrinsicは焦点距離や画像中心といったカメラの内部パラメータを、F_t-1,tは時刻ｔ－１，ｔ間のカメラ移動量から計算した基礎行列を示す。

【0060】

【数3】

【0061】

次に、位置姿勢計算部１３は、式(９)で算出した基礎行列と特徴点を利用してエピポーラ線を計算し、対応する特徴点との距離を以下に示す式(１０)～(１４)で計算する。エピポーラ線と特徴点の距離は、時刻ｔから時刻ｔ－１の方向におけるものと、時刻ｔ－１から時刻ｔの方向におけるものとの両方を計算する。

【0062】

式(１４)のＳは特徴点の数を表しており、位置姿勢計算部１３は、全ての画像特徴点とエピポーラ線の距離の２乗を総和したものを誤差とする（Ｓ１０８）。

【0063】

上述したステップ１０６（Ｓ１０６）からステップ１０９（Ｓ１０６）までの繰り返しの処理では、この誤差が最小となる、自車両の向きの時間的な変化量に相当するΔθ_yawtを推定することを目的としている。ここで、Kはエピポーラ線、Dは特徴点とエピポーラ線Kとの距離、Ptは画像中の特徴点の位置、R_esidualsは画像特徴点とエピポーラ線Kから計算した誤差である。

【0064】

【数4】

【0065】

位置姿勢計算部１３は、ステップ１０９（Ｓ１０９）において、繰り返し処理の終了判定を行う。本実施形態では、繰り返し処理の終了条件を３つ設定した。１つ目の条件は誤差の大きさに応じた条件である。特徴点とエピポーラ線の距離から計算される誤差の大きさが予め設定した閾値を下回った場合、つまり、誤差が閾値よりも小さい場合に、繰り返し処理を終了する。

【0066】

２つ目の条件は、自車両の向きの時間的な変化量に相当するΔθ_yawtの変化量に関する条件である。すなわち、繰り返し処理の１つ以前の周期のΔθ_yawtと比較したとき、Δθ_yawtの値の変化量が閾値よりも予め設定した閾値を下回った場合に繰り返し処理を終了する。

【0067】

３つ目の条件は、繰り返し処理の上限回数に関する条件である。これは、繰り返し処理の回数をカウントし、回数が予め設定した閾値に到達した場合は繰り返し処理を終了する。

【0068】

これら３つの条件のうち、いずれか１つの条件でも満たした場合には、繰り返し処理（Ｓ１０６～Ｓ１０９）の次の処理であるステップ１１０（Ｓ１１０）に進む。３つの条件のうち１つの条件も満たさなかった場合には、繰り返し処理を継続する。

【0069】

繰り返し処理を終了した位置姿勢計算部１３は、繰り返し処理で算出した自車両の移動量を他のシステムへ出力する（Ｓ１１０）。本実施形態では、カメラ座標上での自車両の移動を推定しているため、他のシステムへ出力する際には、以下の式(１５)により、車両座標上での位置及び姿勢に変換する。ここで、Ａ^car _cameraはカメラ座標から車両座標への変換行列である。

【0070】

【数5】

【0071】

［実施形態１の変形例］
上述した実施形態１の自己位置推定装置をＧＰＳ（Global Positioning System）（又はＧＮＳＳ（Global Navigation Satellite System））と組み合わせて利用する構成としてもよい。すなわち、自車両の上空が開けた環境であれば、自己位置推定装置が自車両の位置及び姿勢を計算するのではなく、ＧＰＳによって自車両の位置及び姿勢を計算することができ、これにより、位置及び姿勢を精度よく計算することができる。

【0072】

ただし、立体駐車場や地下駐車場のようにＧＰＳ信号を受信することができない環境では、ＧＰＳを利用することができないため、ＧＰＳ信号を受信することができない環境では、本発明に係る自己位置推定装置を利用することにより、自車両の位置や姿勢を算出すればよい.

【0073】

つまり、上述した実施形態１の自己位置推定装置１０により推定した自己位置推定結果と、受信したＧＰＳ信号に基づいて推定した自己位置推定結果とのうち、どちらの自己位置推定結果を利用するかを自己位置推定装置１０が判定し、判定により選択した、より適切な自己位置推定結果を自動駐車システム４０へ出力する。

【0074】

自己位置推定装置１０によるＧＰＳ信号を受信できているか否かの判断は、受信できているＧＰＳ信号の数、すなわちＧＰＳ信号に対応した衛星の数と、計算される位置及び姿勢の標準偏差とに基づいて行うことができる。

【0075】

判断の１つ目の条件である衛星の数とは、各衛星から送信されているＧＰＳ信号を基に自己位置を計算する場合、４基以上の衛星からＧＰＳ信号を受信できている必要があるため、４個以上の衛星からＧＰＳ信号を受信できていないときは、この条件を満たさず、したがって、本実施形態１の自己位置推定装置１０により推定した位置及び姿勢を自動駐車システム４０に出力する。

【0076】

判断の２つ目の条件である位置及び姿勢の標準偏差とは、ＧＰＳ信号を利用して計算した位置及び姿勢のばらつきを示したものである。この条件は、後段の処理によって必要とされる精度が異なるため、一義的に定めることはできない。自動駐車を行うための条件として、例えば、自車両を駐車場の白線内に収めるように駐車させることを考慮して、位置の標準偏差σ_x，σ_yを例えば１０［ｃｍ］以下、姿勢の標準偏差σ_yawを例えば１［°］以下に設定することができる。

【0077】

［実施形態２］
図７は、自動駐車システムにおけるカメラＥＣＵ４０の機能の一部として本発明の自己位置推定装置を組み込む場合の実施形態（以下、実施形態２という）を示した図である。このように他の車載システムの一部として実行する場合、同システムの要求によっては、車両情報３０および画像２０の取得周期やタイミングが異なる場合が考えられる。

【0078】

このような場合、各データの取得周期の差が原因となり、移動距離に誤差が生じる恐れがある。移動距離に誤差が生じると、移動距離を正確に推定できるという前提を失うため、自車両の姿勢を精度よく推定できない。

【0079】

このため、この問題を解消する必要があるが、実施形態１の場合、図８に示すように、自己（自車両）の位置及び姿勢を算出するまでに時間を要するため、この要求を満たせないことがある。

【0080】

そこで、基になる自己位置姿勢は高速に計算可能なデッドレコニングという手法で計算しておき、実施形態の自己位置推定装置により、デッドレコニングを補正して位置姿勢推定を高精度化する。

【0081】

図７は、図１に示した自己位置推定装置１０と同様に、乗用車などの移動体に搭載されており、自動駐車システム４０の一部として機能する実施形態２の自己位置推定装置１０ａを示すブロック図である。

【0082】

実施形態２は、図８に示すように、車両情報３０の取得周期を２０［ｍｓ］、画像２０の取得周期を３３［ｍｓ］、自己位置推定装置による自己位置推定の実行周期を９９［ｍｓ］として説明する。そして、自車両から速度、ハンドル角度、シフト及びブレーキの状態など車両情報３０を取得する機能、及びカメラから撮像した画像２０を取得する機能を有する。

【0083】

図７に示すように、自己位置推定装置１０ａはその機能として、画像情報計算部１１と、スケール推定部１２と、位置姿勢計算部１３と、時刻差計算部１４と、スケール補正部１５および、位置姿勢推定結果検証部１６とを備えている。自己位置推定装置１０ａは、例えば、ＣＰＵ、ＧＰＵ、ＲＡＭ、ＲＯＭ等を有しており、ROMに格納された所定のプログラムをＲＡＭに展開してＣＰＵが実行することにより、これらの機能を実現することができる。

【0084】

なお、自己位置推定装置１０が有する一部又は全ての機能を、ＦＰＧＡ，ＡＳＩＣなどのハードウェアを用いて実現してもよい。

【0085】

自己位置推定装置１０ａには、自車両に取り付けられたカメラにより取得された画像２０及び自車両からＣＡＮなどを経由して取得した車両情報３０が入力される。画像２０は、自車両に搭載されたカメラによって所定のフレームレートで順次取得される。

【0086】

車両情報３０は自車両から得られる自車両の移動に関する情報及びこれに準ずるものを指しており、例えば、自車両の速度、ハンドルの操舵量、自車両の旋回半径、アクセルやブレーキの状態、シフトレバーの状態（ポジション位置）などである。車両情報３０も画像２０と同様に所定のフレームレートで順次取得される。

【0087】

画像情報計算部１１、スケール推定部１２、及び位置姿勢計算部１３の処理内容は、実施形態１と同じであるため省略し、時刻差計算部１４の処理内容から説明する。

【0088】

時刻差計算部１４では、入力される画像２０と車両情報３０との取得する周期の差から、スケール補正部１５へ入力する補正量を計算する。取得周期のずれにより移動距離に生じる誤差を、図１０を参照して説明する。

【0089】

本実施形態２のシステムは、車両情報３０を例えば２０［ｍｓ］、画像２０を例えば３３［ｍｓ］の周期で取得し、位置及び姿勢の推定処理を例えば９９［ｍｓ］の周期で行う。例えば、位置及び姿勢の推定は、時刻tのタイミングで動作したとする。

【0090】

この場合、位置及び姿勢の推定処理で推定するのは、時刻ｔ－１から時刻ｔまでの移動量である。この場合、位置及び姿勢の推定処理に利用できる画像２０は、時刻ｔ－１で取得したものと、時刻ｔ－１から例えば９９［ｍｓ］後に取得した画像である。

【0091】

一方、車両情報３０については、時刻ｔ－１で取得した車両情報３０と時刻ｔ－１から例えば８０［ｍｓ］後に取得した車両情報３０となる。ここで、車両情報３０と画像２０とには、１９［ｍｓ］の時間差が生じる。位置及び姿勢の推定処理に必要なスケール情報が９９［ｍｓ］の間に移動した距離に対して、システム内では８０［ｍｓ］分の移動距離しかないため、スケール(移動距離)に誤差が生じる。

【0092】

時刻差計算部１５は、この誤差を補間によって解消するために、各データを取得した時刻差を計算し、以下の式(１６)により補正量を計算する。

【0093】

ここで、timestamp_carinfoは車両情報３０の取得時刻を、timestamp_imageは画像２０の取得時刻を、c_scaleは補正量を、それぞれ表す。

【0094】

【数6】

【0095】

スケール補正部１５では、スケール推定部１２で推定した移動距離に対して、時刻計算部１４で計算した車両情報３０と画像２０との取得時刻差を基に計算した補正量を乗じる演算により、画像２０を取得した時間間隔に一致するように線形補間の演算により移動距離を補正する。補正した移動距離の情報は、位置姿勢計算部１３へ出力する。

【0096】

位置姿勢推定結果検証部１６は、位置姿勢計算１３で推定した位置と姿勢の検証を行う。検証は、２つの条件を満たしているかを確認することで行う。

【0097】

１つ目の条件は、位置姿勢計算部１３で算出した画像特徴点とエピポーラ線との距離に基づいて計算した誤差が予め設定した誤差以下で、かつ収束しているか否かである。

【0098】

他の１つの条件は、車両情報３０から得られるハンドル操舵量と比較して、同じ方向に車両向きが変化しているか、また変化量に大きな差がないかを確認する。

【0099】

大きな変化がないことの確認は、例えば、自動駐車を行う速度であれば、スリップによる誤差が極めて小さいと仮定できることから、タイヤ摩耗による誤差と空気圧変化による誤差を考慮して、約５％とすることができるため、ハンドル操舵量から計算した車両向きの変化との差が５％以内であれば、運動モデルを考慮した位置姿勢計算部１３で精度よく位置姿勢が計算できたものとする。

【0100】

ハンドル操舵量から計算する自車両の向きの変化は、ハンドル操舵量と移動距離とに基づいて算出することができる。検証の結果、上述した条件を満たしている場合には、自己位置推定結果を、位置姿勢推定結果統合部１８に出力する。

【0101】

位置姿勢推定部１７では、２０［ｍｓ］の周期で取得される車両情報３０であるハンドル操舵量とタイヤの回転数とに基づいて計算した自車両の向きと位置（移動量）との変化量を基に、自車両の位置及び姿勢を計算する。

【0102】

ハンドル操舵量とタイヤの回転数とに基づいて算出した移動量を基に、自車両の位置及び姿勢を計算する方法には、以下のような自車両の運動モデルを想定する。つまり、自動車について、ｘ－ｙ平面上の位置と向きとを推定する。

【0103】

ここで、Ｒはハンドルの操舵角度から計算された旋回半径、ρは曲率、θ_yawは自車両の向き、sは自車両の移動距離、ｘ_car及びｙ_carは車両座標での座標、ｔは時刻である。なお、実施形態２では上述した車両運動モデルを利用して自車両の位置及び姿勢を計算したが、自車両の位置及び姿勢の算出は上述した車両運動モデルを利用した手法に限定されない。

【0104】

位置姿勢推定結果統合部１８は、位置姿勢推定部１７により２０［ｍｓ］周期で算出される自己（自車両）の位置及び姿勢（自己位置姿勢）と、位置姿勢推定結果検証部１６により３３［ｍｓ］周期で取得される画像情報とを利用して、推定した自己位置姿勢を統合する。

【0105】

なお、自己位置姿勢の統合は、位置姿勢推定部１７により２０［ｍｓ］周期で計算される自己位置姿勢を基準として、２０［ｍｓ］周期で計算されている自己位置姿勢を基準とした統合の方法を、図１１に示す。

【0106】

画像２０を基に算出される位置及び姿勢の変化は、画像取得区間（図１１における画像１～画像２の間、及び画像２～画像３の間）のものとなるが、画像２０を利用して算出する方法は、車両情報３０だけを利用して推定する方法と比較して処理に時間がかかるため、その処理時間分の遅延が生じる。

【0107】

したがって、これらを考慮して自己位置姿勢を統合する必要がある。そのため、位置姿勢推定に利用した車両情報３０及び画像２０を取得した時刻のタイムスタンプをそれぞれ取得しておく。

【0108】

そして、各範囲での位置姿勢の推定結果が出力され、自己位置姿勢を統合するときに、この各データを取得した時刻を表すタイムスタンプを基準として、図１１に示した統合範囲１（画像１～画像２）、統合範囲２（画像２～画像３）のように、対応する画像間で推定した自己位置姿勢を利用し、過去に車両情報３０だけを基に計算した自己位置姿勢と統合する。

【0109】

図９に実施形態２の自己位置推定装置１０ａにおける全体処理を示したフローチャートを示す。ここで、Ｓ２００～Ｓ２０４，Ｓ２０６，Ｓ２０９～Ｓ２１３は、実施形態１のＳ１００～Ｓ１０４，Ｓ１０５，Ｓ１０６～Ｓ１１０と同じ処理であるため、説明を省略する。

【0110】

Ｓ２０５（初期位置姿勢計算）は、自己位置推定装置１０ａを搭載した自動駐車システム４０が起動した後に得られた車両情報３０に基づいて、自己位置推定装置１０ａの処理が開始され、画像２０を取得し始めた際の位置と姿勢（初期位置姿勢）を算出するという処理であり、図２に示した実施形態１のフローチャートにはないが、実施形態１のフローチャートに加えることは可能である。

【0111】

また、Ｓ２０７（取得時刻差計算）とＳ２０８（取得時刻差による移動距離の補正）は、スケール推定部１２が移動距離を推定（計算）した（Ｓ２０６）後に、時刻差計算部１４とスケール補正部１５とがそれぞれが行う。

【0112】

以上、本発明に係る自己位置推定装置の実施形態について説明したが、本発明に係る自己位置推定装置は、上述した実施形態や変形例に限定されるものではない。例えば、本発明に係る自己位置推定装置は、実施形態や変形例の自己位置推定装置の全ての構成を備える必要はない。また、実施形態や変形例としては、説明した構成に他の実施形態の構成を付加することも可能であり、また、構成の一部について、追加、削除、置き換えることも可能である。

【符号の説明】

【0113】

１０，１０ａ 自己位置推定装置
１１ 画像情報計算部
１２ スケール推定部
１３ 位置姿勢計算部
１４ 時刻差計算部
１５ スケール補正部
１６ 位置姿勢推定結果検証部
１７ 位置姿勢推定部
１８ 位置姿勢推定結果統合部
２０ 画像
３０ 車両情報
４０ カメラＥＣＵ（自動駐車システム用）

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】

【図10】

【図11】