標定装置、標定方法および標定プログラム
【課題】車両の位置姿勢を標定できるようにする。
【解決手段】消失点抽出部１２０は、画像センサ１０４により車両から撮像された画像２０４に基づいて画像２０４に映っている白線の消失点実測値２０５を算出する。航法演算部１１０は、慣性センサ１０１とオドメトリ１０２とにより計測された角速度２０１と速度２０２とに基づいて車両の自己姿勢角２０７と自己位置２０６とを計測する。データベース検索部１３０は自己位置２０６と自己姿勢角２０７とに基づいて画像２０４に映っている白線の白線座標２０３を取得する。消失点計算部１４０は白線座標２０３と自己姿勢角２０７とに基づいて消失点予測値２０８を算出する。カルマンフィルタ部１６０は消失点実測値２０５と消失点予測値２０８との差分である消失点残差２０９に基づいて姿勢誤差推定量２１０を算出する。そして、航法演算部１１０は自己姿勢角２０７を姿勢誤差推定量２１０により補正する。
【解決手段】消失点抽出部１２０は、画像センサ１０４により車両から撮像された画像２０４に基づいて画像２０４に映っている白線の消失点実測値２０５を算出する。航法演算部１１０は、慣性センサ１０１とオドメトリ１０２とにより計測された角速度２０１と速度２０２とに基づいて車両の自己姿勢角２０７と自己位置２０６とを計測する。データベース検索部１３０は自己位置２０６と自己姿勢角２０７とに基づいて画像２０４に映っている白線の白線座標２０３を取得する。消失点計算部１４０は白線座標２０３と自己姿勢角２０７とに基づいて消失点予測値２０８を算出する。カルマンフィルタ部１６０は消失点実測値２０５と消失点予測値２０８との差分である消失点残差２０９に基づいて姿勢誤差推定量２１０を算出する。そして、航法演算部１１０は自己姿勢角２０７を姿勢誤差推定量２１０により補正する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、３次元白線データベースと画像センサとを用いて自己位置・姿勢を標定する標定装置、標定方法および標定プログラムに関するものである。
【背景技術】
【０００２】
従来、車両の位置を標定するために、変位センサ（慣性センサ、オドメトリ等）で得た変位量を積分して位置を算出し、算出値に含まれる誤差をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）により取得した位置を用いて補正するという方式が一般的である。
しかし、ＧＰＳは、高層ビルの林立する都市部、樹木の生い茂った場所、トンネルなど、ＧＰＳ衛星からの測位信号を受信できない場所においては使用できない。
このため、このような場所では、精度の高い位置を標定することが困難であった。
【０００３】
また、現在、カーナビゲーションシステムに搭載されている自己位置標定装置は、オドメータと１軸（ヨー角［方位角］）のレートジャイロ（慣性センサ、慣性装置）を組合せたものである。そして、自己位置標定装置は、車両が移動する向きとして方位角（ヨー角）のみを考慮する２次元の平面モデルを仮定し、車両位置を地図画像にマップマッチングさせてナビゲーション画像を表示している。
このため、一般道路が下方を通り、高速道路が上方を通っているような高架式の道路を車両が走行している場合、カーナビゲーションシステムは車両が一般道路と高速道路とのいずれを走行しているか正確には特定することができない。
【特許文献１】特開平８−１５９７１６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
通常、３次元の位置・姿勢を標定するにはヨー角およびピッチ角（仰角）のレート（変位量）を高精度に算出するジャイロが必要となるが、このようなジャイロは非常に高価なため車両に搭載することは考えにくい。
但し、将来、カーナビゲーションシステムを３次元に対応させる際には、車両の位置・姿勢を３次元で高精度に標定することが必須の要件となる。
【０００５】
本発明は、例えば、２次元の平面モデルを拡張するなどして、高価なジャイロを用いることなく車両の３次元の位置・姿勢を標定できるようにすることを目的とする。
【課題を解決するための手段】
【０００６】
本発明の標定装置は、標定物の姿勢角を標定する標定装置であり、前記標定物から撮像された画像であり平行な直線を成す２つの地物を示す平行直線地物が映っている画像である平行直線画像に基づいて前記平行直線地物の消失点を消失点の実測値としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する消失点実測値算出部と、平行直線地物の直線の向きを示す地物データが予め記憶されている記憶機器から前記平行直線画像に映っている前記平行直線地物の地物データを取得する地物データ取得部と、前記地物データ取得部により取得された前記地物データと前記平行直線画像が撮像されたときの前記標定物の姿勢角として予め測定された姿勢角の測定値とに基づいて前記平行直線地物の消失点を消失点の予測値としてＣＰＵを用いて算出する消失点予測値算出部と、前記消失点実測値算出部により算出された前記消失点の実測値と前記消失点予測値算出部により算出された前記消失点の予測値との差を消失点の残差としてＣＰＵを用いて算出する消失点残差算出部と、前記消失点残差算出部により算出された前記消失点の残差に基づいて前記姿勢角の測定値をＣＰＵを用いて補正する標定部とを備える。
【発明の効果】
【０００７】
本発明によれば、例えば、白線（平行直線地物の一例）の消失点に基づいて姿勢角を補正することにより、高精度なジャイロを用いなくても車両（標定物の一例）の姿勢角を高精度に標定することができる。
また例えば、標定した姿勢角を用いることにより、車両の位置を高精度に標定することができる。
また例えば、標定した車両の位置を用いることにより、カーナビゲーションシステムを３次元に対応させることができる。
【発明を実施するための最良の形態】
【０００８】
実施の形態１．
図１は、実施の形態１における自己位置姿勢標定装置１００を搭載した車両３００を示す図である。
実施の形態１における車両３００について、図１に基づいて以下に説明する。
【０００９】
車両３００（標定物の一例）は、慣性センサ１０１、画像センサ１０４、およびＧＰＳ受信機１０５を有する天板３０１が車体上部に設置されている。
また、車両３００は、オドメトリ１０２、カーナビゲーションシステム３０２、ディスプレイ装置３０３および自己位置姿勢標定装置１００を備える。
【００１０】
慣性センサ１０１は、天板３０１に取り付けられ、走行中および停止中の車両３００のヨー角（方位角）およびピッチ角（仰角）の角速度（微小変位量、レート）を計測する。例えば、慣性センサ１０１は２軸または３軸ジャイロである。慣性センサ１０１により計測される角速度は高精度でなくてもよいため、慣性センサ１０１として高価なジャイロが用いられなくても構わない。
【００１１】
オドメトリ１０２は、車輪の回転数を計数して車両３００の走行速度の微小変位量を計測する。
【００１２】
画像センサ１０４は、車両３００の前方または後方に向けて天板３０１に取り付けられ、車両３００が走行する路面を撮像する。例えば、画像センサ１０４はカメラである。
【００１３】
ＧＰＳ受信機１０５は、天板３０１に１台または複数台設置され、測位衛星（ＧＰＳ衛星）からの測位信号を受信する。車両３００の位置はＧＰＳ受信機１０５により受信された測位信号に基づいてＧＰＳ測位される。車両３００が、トンネルや高層ビルの林立する都市部や森林など、測位衛星への視線方向が遮蔽されてしまう場所を走行している場合、ＧＰＳ受信機１０５が測位信号を受信できないため、車両３００の位置はＧＰＳ測位されない。
以下、測位信号を用いて測位する測位方法を「ＧＰＳ測位」という。例えば、ＧＰＳ測位では、測位信号に基づいて複数の測位衛星とＧＰＳ受信機１０５との擬似距離を算出し、複数の測位衛星それぞれから擬似距離分離れた地点として共通する地点をＧＰＳ受信機１０５の位置として算出する。
【００１４】
自己位置姿勢標定装置１００は、ＧＰＳ受信機１０５により受信された測位信号に基づいて車両３００の位置をＧＰＳ測位する。さらに、自己位置姿勢標定装置１００は、慣性センサ１０１とオドメトリ１０２とにより得られた計測値および画像センサ１０４により撮像された画像に基づいて車両３００の位置および姿勢角を標定する。
【００１５】
カーナビゲーションシステム３０２は、自己位置姿勢標定装置１００により得られた車両３００の位置および姿勢角に基づいて、現在の走行位置を地図画像にマップマッチングさせたナビゲーション画面をディスプレイ装置３０３に表示し、運転者に道案内をする。
【００１６】
図２は、実施の形態１における自己位置姿勢標定装置１００の機能構成を示す図である。
実施の形態１における自己位置姿勢標定装置１００の機能構成について、図２に基づいて以下に説明する。
【００１７】
自己位置姿勢標定装置１００は、航法演算部１１０、消失点抽出部１２０、データベース検索部１３０、消失点計算部１４０、残差計算部１５０およびカルマンフィルタ部１６０を備える。
【００１８】
航法演算部１１０（標定部の一例）は、慣性センサ１０１により計測された車両３００（標定物の一例）の姿勢角（ヨー角、ピッチ角）の角速度２０１に基づいて車両３００の姿勢角を測定し、測定した姿勢角をカルマンフィルタ部１６０（後述）により算出された姿勢誤差推定量２１０に基づいて補正して車両３００の姿勢角を標定する。
以下、車両３００の姿勢角を自己姿勢角２０７という。
さらに、航法演算部１１０は、標定した自己姿勢角２０７とオドメトリ１０２により計測された車両３００の走行速度の変位量（速度２０２と記す）とに基づいて車両３００の位置を標定する。
以下、車両３００の位置を自己位置２０６という。
また、航法演算部１１０は、ＧＰＳ受信機１０５により取得された測位信号情報２１１に基づいて自己位置２０６をＧＰＳ測位する。
そして、航法演算部１１０は、自己位置２０６および自己姿勢角２０７をカーナビゲーションシステム３０２に出力する。カーナビゲーションシステム３０２は自己位置２０６および自己姿勢角２０７の出力先となる出力装置の一例である。例えば、自己位置２０６および自己姿勢角２０７の出力先は、記憶装置、表示装置、プリンタ装置、通信ネットワーク（例えば、インターネット、ＬＡＮ［ローカルエリアネットワーク］）に接続するＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）（利用者端末装置、サーバ装置）であっても構わない。
【００１９】
データベース検索部１３０（地物データ取得部の一例）は、航法演算部１１０により標定された自己位置２０６と自己姿勢角２０７とに基づいて、画像センサ１０４により撮像される画像２０４に映る白線の白線座標２０３（地物データの一例）（後述）を白線データベース１０３から取得する。
データベース検索部１３０は、通信ネットワークを介して白線座標２０３を取得しても構わない。
【００２０】
白線データベース１０３には、各地の道路に標示された白線（平行直線地物の一例）の３次元の座標値が白線座標２０３として予め記憶されている。
例えば、白線座標２０３は、長方形を形成する白線の直線部分の４隅の座標値や白線のエッジ（白線の長手方向のふちの線）上の点の座標値を示す。
白線データベース１０３は、自己位置姿勢標定装置１００内に備えられていてもよいし、車両３００内に備えられていてもよいし、自己位置姿勢標定装置１００および車両３００の外部（例えば、通信ネットワークに接続するＰＣ）に備えられていてもよい。
【００２１】
消失点計算部１４０（消失点予測値算出部の一例）は、データベース検索部１３０により取得された白線座標２０３と航法演算部１１０により標定された自己姿勢角２０７とに基づいて白線の消失点の予測値（以下、消失点予測値２０８という）を算出する。
消失点については後述する。
【００２２】
消失点抽出部１２０（消失点実測値算出部の一例）は、画像センサ１０４により撮像された路面の映っている画像２０４（平行直線地物画像の一例）に基づいて路面に標示されている白線の消失点の実測値（以下、消失点実測値２０５という）を算出する。
【００２３】
残差計算部１５０（消失点残差算出部の一例）は、消失点抽出部１２０により算出された消失点実測値２０５と消失点計算部１４０により算出された消失点予測値２０８との差を消失点残差２０９として算出する。
【００２４】
カルマンフィルタ部１６０は、残差計算部１５０により算出された消失点残差２０９に基づいてカルマンフィルタ処理を行い、航法演算部１１０により標定された自己姿勢角２０７（姿勢角の測定値の一例）に含まれる誤差を姿勢誤差推定量２１０として算出する。
カルマンフィルタ処理については後述する。
【００２５】
図３は、実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図である。
図３において、自己位置姿勢標定装置１００は、プログラムを実行するＣＰＵ９１１（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサともいう）を備えている。ＣＰＵ９１１は、バス９１２を介してＲＯＭ９１３、ＲＡＭ９１４、通信ボード９１５、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。磁気ディスク装置９２０の代わりに、光ディスク装置、メモリカード読み書き装置などの記憶装置が用いられてもよい。
ＲＡＭ９１４は、揮発性メモリの一例である。ＲＯＭ９１３、磁気ディスク装置９２０の記憶媒体は、不揮発性メモリの一例である。これらは、記憶機器、記憶装置あるいは記憶部の一例である。また、入力データが記憶されている記憶機器は入力機器、入力装置あるいは入力部の一例であり、出力データが記憶される記憶機器は出力機器、出力装置あるいは出力部の一例である。
通信ボード９１５は入出力機器、入出力装置あるいは入出力部の一例である。
【００２６】
通信ボード９１５は、無線または有線により、ＬＡＮ（ローカルエリアネットワーク）、インターネット、ＷＡＮ（ワイドエリアネットワーク）、電話回線などの通信ネットワークに接続されている。
【００２７】
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２３、ファイル群９２４が記憶されている。プログラム群９２３のプログラムは、ＣＰＵ９１１、ＯＳ９２１により実行される。
【００２８】
上記プログラム群９２３には、実施の形態において「〜部」として説明する機能を実行するプログラムが記憶されている。プログラムは、ＣＰＵ９１１により読み出され実行される。
【００２９】
ファイル群９２４には、実施の形態において、「〜部」の機能を実行した際の「〜の判定結果」、「〜の計算結果」、「〜の処理結果」などの結果データ、「〜部」の機能を実行するプログラム間で受け渡しするデータ、その他の情報やデータや信号値や変数値やパラメータが、「〜ファイル」や「〜データベース」の各項目として記憶されている。自己位置２０６、自己姿勢角２０７、白線座標２０３、消失点予測値２０８、消失点実測値２０５、消失点残差２０９および姿勢誤差推定量２１０などはファイル群９２４に含まれるものの一例である。
「〜ファイル」や「〜データベース」は、ディスクやメモリなどの記録媒体に記憶される。ディスクやメモリなどの記憶媒体に記憶された情報やデータや信号値や変数値やパラメータは、読み書き回路を介してＣＰＵ９１１によりメインメモリやキャッシュメモリに読み出され、抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示などのＣＰＵの動作に用いられる。抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示のＣＰＵの動作の間、情報やデータや信号値や変数値やパラメータは、メインメモリやキャッシュメモリやバッファメモリに一時的に記憶される。
また、実施の形態において説明するフローチャートの矢印の部分は主としてデータや信号の入出力を示し、データや信号値は、ＲＡＭ９１４のメモリ、磁気ディスク装置９２０の磁気ディスク、その他光ディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ・Ｖｅｒｓａｔｉｌｅ・Ｄｉｓｃ）等の記録媒体に記録される。また、データや信号値は、バス９１２や信号線やケーブルその他の伝送媒体によりオンライン伝送される。
【００３０】
また、実施の形態において「〜部」として説明するものは、「〜回路」、「〜装置」、「〜機器」であってもよく、また、「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ＲＯＭ９１３に記憶されたファームウェアで実現されていても構わない。或いは、ソフトウェアのみ、或いは、素子・デバイス・基板・配線などのハードウェアのみ、或いは、ソフトウェアとハードウェアとの組み合わせ、さらには、ファームウェアとの組み合わせで実施されても構わない。ファームウェアとソフトウェアは、プログラムとして、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ等の記録媒体に記憶される。プログラムはＣＰＵ９１１により読み出され、ＣＰＵ９１１により実行される。すなわち、プログラムは、「〜部」としてコンピュータを機能させるものである。あるいは、「〜部」の手順や方法をコンピュータに実行させるものである。
【００３１】
図４は、実施の形態１における自己位置標定方法を示すフローチャートである。
実施の形態１における自己位置標定方法について、図４に基づいて以下に説明する。
自己位置姿勢標定装置１００の各部は、以下に説明する各処理をＣＰＵを用いて実行する。
【００３２】
＜Ｓ１１０：測位処理＞
航法演算部１１０は、ＧＰＳ受信機１０５により取得された測位信号情報２１１に基づいて自己位置２０６をＧＰＳ測位する。
測位信号情報２１１は、ＧＰＳ受信機１０５により受信された測位信号に関する情報であり、例えば、測位信号に示される航法メッセージ（測位衛星の軌道情報）、測位信号の測位衛星からの発信時刻とＧＰＳ受信機１０５での受信時刻に基づく測位衛星とＧＰＳ受信機１０５との擬似距離、ＧＰＳ受信機１０５により擬似距離に基づいてＧＰＳ測位された測位結果、測位信号として発信された搬送波の受信時の位相などの情報が含まれる。
例えば、航法演算部１１０は、搬送波の位相に基づいてＧＰＳ受信機１０５と複数の測位衛星との距離を算出し、各測位衛星から算出した距離だけ離れた地点として共通する地点をＧＰＳ受信機１０５の地点としてＧＰＳ受信機１０５の位置を算出する。そして、航法演算部１１０は、車両３００の自己位置２０６を示す点として予め定められた航法中心（例えば、車両３００の天板３０１の中心）とＧＰＳ受信機１０５の取り付け位置との差（位置オフセット）をＧＰＳ受信機１０５の位置に加算（または減算）して、車両３００の自己位置２０６を算出する。
【００３３】
＜Ｓ１２０：消失点実測値算出処理＞
また、消失点抽出部１２０は、画像センサ１０４により撮像された路面の映っている画像２０４に基づいて、路面に標示されている白線の消失点実測値２０５を算出する。
【００３４】
図５は、実施の形態１における画像２０４と消失点３１６との関係を示す図である。
図５には、白線３１１の直線部分３１９が映っている。白線３１１は直線部分３１９、円弧部分および直線部分３１９と円弧部分とを結ぶクロソイド曲線部分で構成される。
消失点抽出部１２０は、消失点実測値２０５として、白線３１１の直線部分３１９のふちを指す白線エッジ３１８の延長線上の交点（×印）の画像２０４を含む平面（画像センサ１０４の撮像面）内での２次元座標（ＵＶ座標）を算出する。図５では、左側の白線３１１の左右のエッジ（３１８ａ、３１８ｂ）と右側の白線３１１の左右のエッジ（３１８ｃ、３１８ｄ）との計４つの白線エッジ３１８の交点を消失点３１６として示している。しかし、消失点３１６の消失点実測値２０５は、左側の白線３１１の左右いずれかのエッジ（３１８ａ、３１８ｂ）と右側の白線３１１の左右いずれかのエッジ（３１８ｃ、３１８ｄ）との計２つの白線エッジ３１８の交点として算出されてもよい。また、消失点実測値２０５は、左側の白線３１１の左右のエッジ（３１８ａ、３１８ｂ）の交点として算出されてもよいし、右側の白線３１１の左右のエッジ（３１８ｃ、３１８ｄ）の交点として算出されてもよい。つまり、消失点実測値２０５は、左右２つの白線３１１の交点として算出されてもよいし、１つの白線３１１の左右の白線エッジ３１８の交点として算出されてもよい。
２つの白線３１１および２つの白線エッジ３１８は平行な直線を成す２つの地物の一例である。
また、白線認識点３１７（黒丸）は、白線のエッジ認識で得られた点である。
【００３５】
２次元の画像２０４では、３次元のものが近いものほど大きく表されると共に遠いものほど小さく表されるため、３次元において平行な直線が無限遠を表す１点に収束する。この収束点が消失点３１６である。消失点３１６は、図５に示すように画像２０４内に位置する場合もあるし、図５とは異なり画像２０４外に位置する場合もある。
消失点実測値算出処理（Ｓ１２０）の詳細については後述する。
【００３６】
図４に戻り、自己位置姿勢標定方法の説明を続ける。
【００３７】
＜Ｓ１３０：標定処理＞
航法演算部１１０は、慣性センサ１０１により計測された車両３００のヨー角およびピッチ角の角速度２０１、オドメトリ１０２により計測された速度２０２および後述するＳ１７０において算出された姿勢誤差推定量２１０に基づいて車両３００の自己位置２０６および自己姿勢角２０７を標定する。
【００３８】
Ｓ１３０において、航法演算部１１０は、慣性センサ１０１により計測された車両３００のヨー角およびピッチ角の角速度２０１を用いてデッドレコニング（または、慣性航法）を行って自己姿勢角２０７を測定する。そして、航法演算部１１０は、測定結果に含まれる誤差を姿勢誤差推定量２１０で補正して自己姿勢角２０７を標定する。具体的には、航法演算部１１０は、自己姿勢角２０７の測定値に姿勢誤差推定量２１０を加算して自己姿勢角２０７の測定値を補正し、補正後の自己姿勢角２０７を自己姿勢角２０７の標定値とする。
また、航法演算部１１０は、自己姿勢角２０７の標定値とオドメトリ１０２により計測された速度２０２とを用いてデッドレコニングを行って自己位置２０６を測定する。航法演算部１１０は、自己姿勢角２０７の標定値を用いて測定された自己位置２０６を自己位置２０６の標定値とする。
【００３９】
デッドレコニングとは、慣性センサ１０１により計測された角速度２０１やオドメトリ１０２により計測された速度２０２に基づいて位置および姿勢角を測定する方法である。
デッドレコニングでは、角速度２０１の積分により自己姿勢角２０７の変化量が算出され、前回測定時の自己姿勢角２０７に変化量が加算されることにより現在の自己姿勢角２０７が算出される。
また、デッドレコニングでは、速度２０２の積分により車両３００の移動量が算出され、前回測定時の自己位置２０６を基点にして自己姿勢角２０７の方向に移動量だけ移動した地点が自己位置２０６として算出される。
【００４０】
＜Ｓ１４０：地物データ取得処理＞
次に、データベース検索部１３０は、Ｓ１３０において標定された自己位置２０６および自己姿勢角２０７に基づいて車両３００の画像センサ１０４の撮像範囲を算出し、算出した撮像範囲内に位置する白線の白線座標２０３を白線データベース１０３から取得する。つまり、データベース検索部１３０は、画像センサ１０４により撮像される画像２０４に映る白線の白線座標２０３を白線データベース１０３から取得する。
Ｓ１４０において、データベース検索部１３０は、車両３００の走行している道路に標示されている白線（の直線部分）の白線座標２０３を白線データベース１０３から取得できればよい。
例えば、データベース検索部１３０は、Ｓ１１０またはＳ１３０で得られた自己位置２０６から所定の範囲内の座標を示す白線座標２０３を白線データベース１０３から取得する。取得した白線座標２０３のうち、Ｓ１１０またはＳ１３０で得られた自己姿勢角２０７の方向に位置する白線座標２０３が画像２０４に映る白線の白線座標２０３である。
【００４１】
＜Ｓ１５０：消失点予測値算出処理＞
次に、消失点計算部１４０は、Ｓ１４０において取得された白線座標２０３とＳ１３０において標定された自己姿勢角２０７とに基づいて白線の消失点予測値２０８を算出する。
【００４２】
消失点計算部１４０は、Ｓ１４０において取得された白線座標２０３とＳ１３０において標定された自己姿勢角２０７とに基づいて複数の白線座標２０３上を通る白線エッジ３１８の直線方程式を算出し、算出した直線方程式に基づいて白線エッジ３１８を画像２０４に投影した場合の白線の消失点の２次元座標（ＵＶ座標）を消失点予測値２０８として算出する。
消失点予測値算出処理（Ｓ１５０）の詳細については後述する。
【００４３】
図４に戻り、自己位置姿勢標定方法の説明を続ける。
【００４４】
消失点予測値算出処理（Ｓ１５０）において算出される消失点予測値２０８は、消失点実測値算出処理（Ｓ１２０）において算出される消失点実測値２０５を自己姿勢角２０７と白線座標２０３とに基づいて予測した予測値である。
画像センサ１０４により撮像された画像２０４には撮像時刻が設定されており、慣性センサ１０１により計測された角速度２０１およびオドメトリ１０２により計測された速度２０２には計測時刻が設定されている。
消失点実測値算出処理（Ｓ１２０）および標定処理（Ｓ１３０）〜消失点予測値算出処理（Ｓ１５０）では、同時刻に撮像または計測された画像２０４、角速度２０１およびオドメトリ１０２が用いられる。
このため、標定処理（Ｓ１３０）では、消失点実測値２０５の算出に用いられた画像２０４が撮像された時刻における車両３００の自己位置２０６および自己姿勢角２０７が標定される。
また、地物データ取得処理（Ｓ１４０）では、消失点実測値２０５の算出に用いられた画像２０４に映っている白線の白線座標２０３が取得される。
そして、消失点予測値算出処理（Ｓ１５０）では、消失点実測値２０５の予測値となる消失点予測値２０８が算出される。
【００４５】
＜Ｓ１６０：消失点残差算出処理＞
次に、残差計算部１５０は、Ｓ１２０において算出された消失点実測値２０５とＳ１５０において算出された消失点予測値２０８とに基づいて消失点残差２０９を算出する。
Ｓ１６０において、残差計算部１５０は、消失点実測値２０５と消失点予測値２０８との差の絶対値を消失点残差２０９として算出する。
【００４６】
＜Ｓ１７０：カルマンフィルタ処理＞
次に、カルマンフィルタ部１６０は、Ｓ１６０において算出された消失点残差２０９に基づいて姿勢誤差推定量２１０を算出する。
Ｓ１７０において算出された姿勢誤差推定量２１０は、Ｓ１３０において前回算出された自己姿勢角２０７の補正に用いられる。
カルマンフィルタ処理（Ｓ１７０）については後述する。
【００４７】
＜Ｓ１８０＞
次に、航法演算部１１０は、Ｓ１１０において自己位置２０６がＧＰＳ測位されたか否かを判定する。
ＧＰＳ測位がされなかった場合とは、ＧＰＳ受信機１０５から航法演算部１１０への測位信号情報２１１の入力が無かった場合である。
例えば、車両３００が、トンネルや高層ビルの林立する都市部や森林など、測位衛星への視線方向が遮蔽されてしまう場所を走行している場合、ＧＰＳ受信機１０５は測位衛星からの測位信号を受信できないため、ＧＰＳ受信機１０５から航法演算部１１０への測位信号情報２１１の入力は無く、ＧＰＳ測位はできない。
ＧＰＳ受信機１０５から航法演算部１１０への測位信号情報２１１の入力が有った場合、Ｓ１１０において自己位置２０６がＧＰＳ測位される。
【００４８】
＜Ｓ１８１＞
Ｓ１１０において自己位置２０６がＧＰＳ測位された場合、航法演算部１１０は、Ｓ１１０において測位された自己位置２０６と、Ｓ１３０において標定された自己姿勢角２０７とをカーナビゲーションシステム３０２に出力する。
【００４９】
＜Ｓ１８２＞
Ｓ１１０において自己位置２０６がＧＰＳ測位されなかった場合、航法演算部１１０はＳ１３０において標定された自己位置２０６と自己姿勢角２０７とをカーナビゲーションシステム３０２に出力する。
【００５０】
カーナビゲーションシステム３０２は、航法演算部１１０からの自己位置２０６と自己姿勢角２０７とに基づいて、車両３００の走行位置を道路画像にマップマッチングさせたナビゲーション画面をディスプレイ装置３０３に表示する。
【００５１】
次に、消失点実測値算出処理（Ｓ１２０）の詳細について、図５に基づいて説明する。
消失点抽出部１２０は、画像２０４を画像処理して画像２０４から複数の白線エッジ３１８（３１８ａ〜３１８ｄのうち少なくても２つ）を抽出する。例えば、消失点抽出部１２０は、直線上に連続する白色の画素群を白線３１１として抽出し、抽出した画素群から白線３１１のふち（境界線）部分の画素群を白線エッジ３１８として抽出する。
次に、消失点抽出部１２０は、画像２０４から抽出した複数の白線エッジ３１８それぞれについて、画像２０４を含む平面（画像センサ１０４の撮像面）内での２次元直線方程式を算出する。
そして、消失点抽出部１２０は、算出した複数の白線エッジ３１８の直線方程式に基づいて、複数の白線エッジ３１８の交点となる画素の撮像面内での２次元座標（ＵＶ座標）を算出する。
【００５２】
次に、消失点予測値算出処理（Ｓ１５０）の詳細について、図６および図７に基づいて説明する。
図６および図７は、実施の形態１における消失点予測値算出処理（Ｓ１５０）を説明する図である。
【００５３】
図６において、画像センサ１０４のレンズの中心位置を示すカメラ中心Ｏｂから白線３１１上の任意の点ｐｎとを結んだ直線Ｌｎは、以下の直線方程式１で表される。
以下、画像センサ１０４をカメラという。
【００５４】
【数１】
【００５５】
ここで、（ｘｖ、ｙｖ、ｚｖ）は、カメラ座標系Ｘｖ−Ｙｖ−Ｚｖにおける３次元座標を示す。図６においてカメラ座標系を一点鎖線で示す。
また、［α、β、γ］は、直線Ｌｎの方向ベクトルを示す。
また、「ｆ」は、画像センサ１０４の焦点距離を示す。
【００５６】
カメラ座標系とは、航法座標系（世界座標系、ＮＥＤ座標系）に対して、カメラの姿勢角分（車両３００の姿勢角も含む）の傾きを有する座標系である。航法座標系は、自己位置２０６、自己姿勢角２０７および白線座標２０３を表す座標系であり、例えば、緯度、経度および高さを示す。
焦点面Ｍは、カメラの視線方向（姿勢角方向、撮像方向）と直交する平面であり、カメラ中心Ｏｂからカメラの視線方向に向けて焦点距離ｆだけ離れた平面である。焦点面Ｍは、カメラの画像２０４が含まれる撮像面（投影面）を示す。画像２０４は、カメラにより撮像された３次元の地物が焦点面Ｍに投影されたものである。
【００５７】
以下、カメラの視線方向を「Ｘｖ」、カメラのヨー方向を「Ｙｖ」、カメラのピッチ方向を「Ｚｖ」とする。
画像２０４内の画素（焦点面Ｍ内の点）は横方向を「Ｕ」、縦方向を「Ｖ」とするＵＶ座標で表される。
画像２０４内でのＵ座標はカメラ座標系でのＹｖ座標に対応し、画像２０４内でのＶ座標はカメラ座標系でのＺｖ座標に対応する。
【００５８】
焦点面Ｍは、「ｘｖ＝０」（式２）で表される。
また、カメラ中心Ｏｂは、焦点面Ｍと座標軸Ｘｖとの交点となる焦点面Ｍの中心Ｏｍ（画像２０４の中心）の３次元座標を（０、０、０）とした場合、（−ｆ、０、０）で表される。
【００５９】
ここで、白線３１１上の任意の点ｐｎが焦点面Ｍに投影された点ｐｎ’は、直線Ｌｎと焦点面Ｍとの交点である。このため、点ｐｎ’の焦点面Ｍ内でのＵＶ座標は、直線Ｌｎを表す上記式１に焦点面Ｍを表す上記式２を代入して、以下の式３により表される。
【００６０】
【数２】
【００６１】
消失点計算部１４０は、データベース検索部１３０により取得された白線エッジ３１８上の２つの白線座標２０３に基づいて白線エッジ３１８を表す直線Ｌｅの方程式を生成する。
例えば、白線エッジ３１８を表す直線Ｌｅは、白線エッジ３１８上の入口側（車両３００の進行方向手前）の点ｐｉ（ｘｉ、ｙｉ、ｚｉ）と白線エッジ３１８上の出口側（車両３００の進行方向奥）の点ｐｏ（ｘｏ、ｙｏ、ｚｏ）とを用いて、以下の式４により表される。「ｔ」は直線方程式の媒介変数である。
【００６２】
【数３】
【００６３】
また、白線エッジ３１８を表す直線Ｌｅは、上記の式４を展開して以下の式５により表される。
【００６４】
【数４】
【００６５】
ここで、点ｐｉの白線座標２０３および点ｐｏの白線座標２０３は航法座標系で表されているため、上記式５は航法座標系における白線エッジ３１８を表す直線Ｌｅを示す。
そこで、上記式５をカメラ座標系に変換すると、カメラ座標系における白線エッジ３１８を表す直線Ｌｅの方向ベクトル［α、β、γ］は以下の式６で表される。
【００６６】
【数５】
【００６７】
「Ｃｎｂ」は、航法座標系から車両座標系Ｘ−Ｙ−Ｚ（図６の実線）（航法座標系に対して車両３００の自己姿勢角２０７分の傾きを有する座標系）に変換するために車両３００の自己姿勢角２０７分の回転移動を行う方向余弦行列（回転行列）である。自己姿勢角２０７には標定処理（Ｓ１３０）において標定された値が用いられる。
「Ｃｂｖ」は、車両座標系からカメラ座標系に変換するために車両３００に対するカメラの姿勢角分の回転移動を行う方向余弦行列である。車両３００に対するカメラの姿勢角はカメラの姿勢オフセットとして予め算出されているものとする。車両３００の進行方向とカメラの視線方向とが全く同じになるようにカメラが車両３００に取り付けられているのであれば、車両３００に対するカメラの姿勢角のうち、ヨー角およびピッチ角は「０」となる。
「Ｉｂ」は、車両３００の自己位置２０６を示す点として予め定められた航法中心Ｏ（例えば、車両３００の天板３０１の中心）とカメラの取り付け位置との差（カメラの位置オフセット）である。カメラ位置オフセットＩｂの減算により、座標の中心は車両座標系の中心（航法中心Ｏ）からカメラ座標系の中心（カメラ中心Ｏｂ）に移動される。カメラ位置オフセットＩｂは予め算出されているものとする。
【００６８】
また、白線３１１の消失点Ｐｄは、白線エッジ３１８を表す直線Ｌｅの無限遠の点である。
このため、カメラ中心Ｏｂから白線３１１の消失点Ｐｄへの方向ベクトル（α’、β’、γ’）は、上記式６の媒介変数ｔを「∞」にして、以下の式７で表される。
【００６９】
【数６】
【００７０】
また、カメラ座標系における任意の点は、焦点面Ｍ内でのＵＶ座標が上記式３により表される。
このため、図７において、画像２０４を含む焦点面Ｍ（撮像面）における白線３１１の消失点ＰｄのＵＶ座標は、上記式７を用いて、以下の式８で表される。
【００７１】
【数７】
【００７２】
消失点計算部１４０は、上記式８を計算して、消失点ＰｄのＵＶ座標を消失点予測値２０８として算出する。
消失点予測値算出処理（Ｓ１５０）では、一つの白線エッジ３１８に基づいて消失点予測値２０８が算出される。
【００７３】
次に、カルマンフィルタ処理（Ｓ１７０）について説明する。
カルマンフィルタ処理は、状態量のダイナミクス（時間変化）をモデル化した状態方程式と、観測量と状態量との関係を定式化した観測方程式とに基づいて状態量の誤差推定を行う処理である。
カルマンフィルタ部１６０は、消失点実測値２０５と消失点予測値２０８との差分である消失点残差２０９を観測量とし、自己姿勢角２０７を状態量として、予め定められた状態方程式および観測方程式に基づいて、自己姿勢角２０７に含まれる誤差（姿勢誤差推定量２１０）を算出する。
【００７４】
実施の形態１において、以下のような自己位置姿勢標定装置１００について説明した。
自己位置姿勢標定装置１００は、道路に通常、標示されている中央線や外側線等の白線を利用して自車の姿勢角を精度よく求める。
【００７５】
まず、自己位置姿勢標定装置１００は、白線データベースから得られる白線の方位情報と、自車に搭載された慣性センサやオドメトリ等の航法装置により得られる姿勢角とを利用して、消失点を予測する（Ｓ１５０：消失点予測値算出処理）。
また、自己位置姿勢標定装置１００は、画像センサによる路面画像を画像処理することによって消失点を実測する（Ｓ１２０：消失点実測値算出処理）。
そして、自己位置姿勢標定装置１００は、消失点の予測値と実測値とを比較し（Ｓ１６０：消失点残差算出処理）、この残差をカルマンフィルタの入力とすることで現在の姿勢角の誤差推定値を算出し（Ｓ１７０：カルマンフィルタ処理）、航法演算（Ｓ１３０：標定処理［デッドレコニング］）により得られた姿勢角を補正する。
【００７６】
例えば、白線データベースは、白線の直線区間の入口（長手方向の一端）と出口（長手方向の他端）の緯度、経度、高度を示す。白線データベースに位置情報を記憶することで、現在走行している車両の位置から撮像される範囲内に位置する白線の情報を検索することができる。但し、白線データベースから得るべき情報はあくまで白線の３次元空間における方向である。つまり、白線データベースには白線の方位角が記憶されていてもよい。
なお、ある程度の距離を持った直線区間であれば白線方向は精度よく求められるため、白線データベースに記憶される緯度、経度、高度の精度が測量クラスである必要はない。例えば、直線区間の長さが１００ｍであれば、白線の入口と出口との両端の位置に１０ｃｍ程度の誤差があったとしても、方向精度は０．０６［ｄｅｇ］以下の誤差となる。
【００７７】
実施の形態１において、自己位置姿勢標定装置１００は車両３００（標定物の一例）に搭載される内部装置として説明されているが、自己位置姿勢標定装置１００は標定物に搭載されない外部装置であっても構わない。外部装置である自己位置姿勢標定装置１００は、標定物により各時刻で取得された画像２０４、角速度２０１および速度２０２などの取得情報を入力として、各時刻における標定物の自己姿勢角２０７および自己位置２０６を標定する。例えば、取得情報はＣＤ−ＲＯＭやフラッシュメモリなどの記憶媒体に記憶されて自己位置姿勢標定装置１００に入力され、また、インターネットなどの通信ネットワークを介して利用者端末装置から自己位置姿勢標定装置１００（サーバ装置）に入力される。サーバ装置として稼働する自己位置姿勢標定装置１００は標定結果を利用者端末装置に送信する。
【００７８】
また、実施の形態１では、白線３１１の消失点３１６に基づいて標定が行われているが、白線３１１以外の地物の消失点３１６に基づいて標定が行われてもよい。例えば、道路標示３１５や歩道３１３を対象にした消失点３１６に基づいて標定が行われてもよい。消失点３１６が求められる地物は、白線エッジ３１８または道路標示３１５や歩道３１３の直線部分のエッジのように、平行な直線を成す２つの地物であればよい。平行な直線を成す２つの地物は、白線エッジ３１８と道路標示３１５の直線部分のエッジのように、異なる種類であってもよい。
【００７９】
実施の形態１では、画像処理結果（消失点）を画像処理とは異なる別の方法（Ｓ１５０：消失点予測値算出処理）で予測しているため、画像処理の誤りを容易に検出することが出来る。これにより、車両３００が白線の方位情報が得られている区間を走行する場合、標定した自己位置２０６が大きくずれることはない。
このため、実施の形態１における自己位置姿勢標定装置１００および自己位置標定方法は、将来、カーナビゲーションシステムが３次元化された場合に有効である。
さらに、実施の形態１では、車両３００のピッチ角が標定されることにより、車両３００が上り斜面を走行しているのか下り斜面を走行しているのかがわかるため、運転者にギアの切り替え（例えば、４速から２速に落とす）を促すこともできる。これにより、運転の省エネルギー化が図れる。
【図面の簡単な説明】
【００８０】
【図１】実施の形態１における自己位置姿勢標定装置１００を搭載した車両３００を示す図。
【図２】実施の形態１における自己位置姿勢標定装置１００の機能構成を示す図。
【図３】実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図。
【図４】実施の形態１における自己位置標定方法を示すフローチャート。
【図５】実施の形態１における画像２０４と消失点３１６との関係を示す図。
【図６】実施の形態１における消失点予測値算出処理（Ｓ１５０）を説明する図。
【図７】実施の形態１における消失点予測値算出処理（Ｓ１５０）を説明する図。
【符号の説明】
【００８１】
１００ 自己位置姿勢標定装置、１０１ 慣性センサ、１０２ オドメトリ、１０３ 白線データベース、１０４ 画像センサ、１０５ ＧＰＳ受信機、１１０ 航法演算部、１２０ 消失点抽出部、１３０ データベース検索部、１４０ 消失点計算部、１５０ 残差計算部、１６０ カルマンフィルタ部、２０１ 角速度、２０２ 速度、２０３ 白線座標、２０４ 画像、２０５ 消失点実測値、２０６ 自己位置、２０７ 自己姿勢角、２０８ 消失点予測値、２０９ 消失点残差、２１０ 姿勢誤差推定量、２１１ 測位信号情報、３００ 車両、３０１ 天板、３０２ カーナビゲーションシステム、３０３ ディスプレイ装置、３１１ 白線、３１２ 車道、３１３ 歩道、３１４ 道路標識、３１５ 道路標示、３１６ 消失点、３１７ 白線認識点、３１８ 白線エッジ、３１９ 直線部分、９１１ ＣＰＵ、９１３ ＲＯＭ、９１４ ＲＡＭ、９１５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群。
【技術分野】
【０００１】
本発明は、例えば、３次元白線データベースと画像センサとを用いて自己位置・姿勢を標定する標定装置、標定方法および標定プログラムに関するものである。
【背景技術】
【０００２】
従来、車両の位置を標定するために、変位センサ（慣性センサ、オドメトリ等）で得た変位量を積分して位置を算出し、算出値に含まれる誤差をＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）により取得した位置を用いて補正するという方式が一般的である。
しかし、ＧＰＳは、高層ビルの林立する都市部、樹木の生い茂った場所、トンネルなど、ＧＰＳ衛星からの測位信号を受信できない場所においては使用できない。
このため、このような場所では、精度の高い位置を標定することが困難であった。
【０００３】
また、現在、カーナビゲーションシステムに搭載されている自己位置標定装置は、オドメータと１軸（ヨー角［方位角］）のレートジャイロ（慣性センサ、慣性装置）を組合せたものである。そして、自己位置標定装置は、車両が移動する向きとして方位角（ヨー角）のみを考慮する２次元の平面モデルを仮定し、車両位置を地図画像にマップマッチングさせてナビゲーション画像を表示している。
このため、一般道路が下方を通り、高速道路が上方を通っているような高架式の道路を車両が走行している場合、カーナビゲーションシステムは車両が一般道路と高速道路とのいずれを走行しているか正確には特定することができない。
【特許文献１】特開平８−１５９７１６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
通常、３次元の位置・姿勢を標定するにはヨー角およびピッチ角（仰角）のレート（変位量）を高精度に算出するジャイロが必要となるが、このようなジャイロは非常に高価なため車両に搭載することは考えにくい。
但し、将来、カーナビゲーションシステムを３次元に対応させる際には、車両の位置・姿勢を３次元で高精度に標定することが必須の要件となる。
【０００５】
本発明は、例えば、２次元の平面モデルを拡張するなどして、高価なジャイロを用いることなく車両の３次元の位置・姿勢を標定できるようにすることを目的とする。
【課題を解決するための手段】
【０００６】
本発明の標定装置は、標定物の姿勢角を標定する標定装置であり、前記標定物から撮像された画像であり平行な直線を成す２つの地物を示す平行直線地物が映っている画像である平行直線画像に基づいて前記平行直線地物の消失点を消失点の実測値としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する消失点実測値算出部と、平行直線地物の直線の向きを示す地物データが予め記憶されている記憶機器から前記平行直線画像に映っている前記平行直線地物の地物データを取得する地物データ取得部と、前記地物データ取得部により取得された前記地物データと前記平行直線画像が撮像されたときの前記標定物の姿勢角として予め測定された姿勢角の測定値とに基づいて前記平行直線地物の消失点を消失点の予測値としてＣＰＵを用いて算出する消失点予測値算出部と、前記消失点実測値算出部により算出された前記消失点の実測値と前記消失点予測値算出部により算出された前記消失点の予測値との差を消失点の残差としてＣＰＵを用いて算出する消失点残差算出部と、前記消失点残差算出部により算出された前記消失点の残差に基づいて前記姿勢角の測定値をＣＰＵを用いて補正する標定部とを備える。
【発明の効果】
【０００７】
本発明によれば、例えば、白線（平行直線地物の一例）の消失点に基づいて姿勢角を補正することにより、高精度なジャイロを用いなくても車両（標定物の一例）の姿勢角を高精度に標定することができる。
また例えば、標定した姿勢角を用いることにより、車両の位置を高精度に標定することができる。
また例えば、標定した車両の位置を用いることにより、カーナビゲーションシステムを３次元に対応させることができる。
【発明を実施するための最良の形態】
【０００８】
実施の形態１．
図１は、実施の形態１における自己位置姿勢標定装置１００を搭載した車両３００を示す図である。
実施の形態１における車両３００について、図１に基づいて以下に説明する。
【０００９】
車両３００（標定物の一例）は、慣性センサ１０１、画像センサ１０４、およびＧＰＳ受信機１０５を有する天板３０１が車体上部に設置されている。
また、車両３００は、オドメトリ１０２、カーナビゲーションシステム３０２、ディスプレイ装置３０３および自己位置姿勢標定装置１００を備える。
【００１０】
慣性センサ１０１は、天板３０１に取り付けられ、走行中および停止中の車両３００のヨー角（方位角）およびピッチ角（仰角）の角速度（微小変位量、レート）を計測する。例えば、慣性センサ１０１は２軸または３軸ジャイロである。慣性センサ１０１により計測される角速度は高精度でなくてもよいため、慣性センサ１０１として高価なジャイロが用いられなくても構わない。
【００１１】
オドメトリ１０２は、車輪の回転数を計数して車両３００の走行速度の微小変位量を計測する。
【００１２】
画像センサ１０４は、車両３００の前方または後方に向けて天板３０１に取り付けられ、車両３００が走行する路面を撮像する。例えば、画像センサ１０４はカメラである。
【００１３】
ＧＰＳ受信機１０５は、天板３０１に１台または複数台設置され、測位衛星（ＧＰＳ衛星）からの測位信号を受信する。車両３００の位置はＧＰＳ受信機１０５により受信された測位信号に基づいてＧＰＳ測位される。車両３００が、トンネルや高層ビルの林立する都市部や森林など、測位衛星への視線方向が遮蔽されてしまう場所を走行している場合、ＧＰＳ受信機１０５が測位信号を受信できないため、車両３００の位置はＧＰＳ測位されない。
以下、測位信号を用いて測位する測位方法を「ＧＰＳ測位」という。例えば、ＧＰＳ測位では、測位信号に基づいて複数の測位衛星とＧＰＳ受信機１０５との擬似距離を算出し、複数の測位衛星それぞれから擬似距離分離れた地点として共通する地点をＧＰＳ受信機１０５の位置として算出する。
【００１４】
自己位置姿勢標定装置１００は、ＧＰＳ受信機１０５により受信された測位信号に基づいて車両３００の位置をＧＰＳ測位する。さらに、自己位置姿勢標定装置１００は、慣性センサ１０１とオドメトリ１０２とにより得られた計測値および画像センサ１０４により撮像された画像に基づいて車両３００の位置および姿勢角を標定する。
【００１５】
カーナビゲーションシステム３０２は、自己位置姿勢標定装置１００により得られた車両３００の位置および姿勢角に基づいて、現在の走行位置を地図画像にマップマッチングさせたナビゲーション画面をディスプレイ装置３０３に表示し、運転者に道案内をする。
【００１６】
図２は、実施の形態１における自己位置姿勢標定装置１００の機能構成を示す図である。
実施の形態１における自己位置姿勢標定装置１００の機能構成について、図２に基づいて以下に説明する。
【００１７】
自己位置姿勢標定装置１００は、航法演算部１１０、消失点抽出部１２０、データベース検索部１３０、消失点計算部１４０、残差計算部１５０およびカルマンフィルタ部１６０を備える。
【００１８】
航法演算部１１０（標定部の一例）は、慣性センサ１０１により計測された車両３００（標定物の一例）の姿勢角（ヨー角、ピッチ角）の角速度２０１に基づいて車両３００の姿勢角を測定し、測定した姿勢角をカルマンフィルタ部１６０（後述）により算出された姿勢誤差推定量２１０に基づいて補正して車両３００の姿勢角を標定する。
以下、車両３００の姿勢角を自己姿勢角２０７という。
さらに、航法演算部１１０は、標定した自己姿勢角２０７とオドメトリ１０２により計測された車両３００の走行速度の変位量（速度２０２と記す）とに基づいて車両３００の位置を標定する。
以下、車両３００の位置を自己位置２０６という。
また、航法演算部１１０は、ＧＰＳ受信機１０５により取得された測位信号情報２１１に基づいて自己位置２０６をＧＰＳ測位する。
そして、航法演算部１１０は、自己位置２０６および自己姿勢角２０７をカーナビゲーションシステム３０２に出力する。カーナビゲーションシステム３０２は自己位置２０６および自己姿勢角２０７の出力先となる出力装置の一例である。例えば、自己位置２０６および自己姿勢角２０７の出力先は、記憶装置、表示装置、プリンタ装置、通信ネットワーク（例えば、インターネット、ＬＡＮ［ローカルエリアネットワーク］）に接続するＰＣ（Ｐｅｒｓｏｎａｌ Ｃｏｍｐｕｔｅｒ）（利用者端末装置、サーバ装置）であっても構わない。
【００１９】
データベース検索部１３０（地物データ取得部の一例）は、航法演算部１１０により標定された自己位置２０６と自己姿勢角２０７とに基づいて、画像センサ１０４により撮像される画像２０４に映る白線の白線座標２０３（地物データの一例）（後述）を白線データベース１０３から取得する。
データベース検索部１３０は、通信ネットワークを介して白線座標２０３を取得しても構わない。
【００２０】
白線データベース１０３には、各地の道路に標示された白線（平行直線地物の一例）の３次元の座標値が白線座標２０３として予め記憶されている。
例えば、白線座標２０３は、長方形を形成する白線の直線部分の４隅の座標値や白線のエッジ（白線の長手方向のふちの線）上の点の座標値を示す。
白線データベース１０３は、自己位置姿勢標定装置１００内に備えられていてもよいし、車両３００内に備えられていてもよいし、自己位置姿勢標定装置１００および車両３００の外部（例えば、通信ネットワークに接続するＰＣ）に備えられていてもよい。
【００２１】
消失点計算部１４０（消失点予測値算出部の一例）は、データベース検索部１３０により取得された白線座標２０３と航法演算部１１０により標定された自己姿勢角２０７とに基づいて白線の消失点の予測値（以下、消失点予測値２０８という）を算出する。
消失点については後述する。
【００２２】
消失点抽出部１２０（消失点実測値算出部の一例）は、画像センサ１０４により撮像された路面の映っている画像２０４（平行直線地物画像の一例）に基づいて路面に標示されている白線の消失点の実測値（以下、消失点実測値２０５という）を算出する。
【００２３】
残差計算部１５０（消失点残差算出部の一例）は、消失点抽出部１２０により算出された消失点実測値２０５と消失点計算部１４０により算出された消失点予測値２０８との差を消失点残差２０９として算出する。
【００２４】
カルマンフィルタ部１６０は、残差計算部１５０により算出された消失点残差２０９に基づいてカルマンフィルタ処理を行い、航法演算部１１０により標定された自己姿勢角２０７（姿勢角の測定値の一例）に含まれる誤差を姿勢誤差推定量２１０として算出する。
カルマンフィルタ処理については後述する。
【００２５】
図３は、実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図である。
図３において、自己位置姿勢標定装置１００は、プログラムを実行するＣＰＵ９１１（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサともいう）を備えている。ＣＰＵ９１１は、バス９１２を介してＲＯＭ９１３、ＲＡＭ９１４、通信ボード９１５、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。磁気ディスク装置９２０の代わりに、光ディスク装置、メモリカード読み書き装置などの記憶装置が用いられてもよい。
ＲＡＭ９１４は、揮発性メモリの一例である。ＲＯＭ９１３、磁気ディスク装置９２０の記憶媒体は、不揮発性メモリの一例である。これらは、記憶機器、記憶装置あるいは記憶部の一例である。また、入力データが記憶されている記憶機器は入力機器、入力装置あるいは入力部の一例であり、出力データが記憶される記憶機器は出力機器、出力装置あるいは出力部の一例である。
通信ボード９１５は入出力機器、入出力装置あるいは入出力部の一例である。
【００２６】
通信ボード９１５は、無線または有線により、ＬＡＮ（ローカルエリアネットワーク）、インターネット、ＷＡＮ（ワイドエリアネットワーク）、電話回線などの通信ネットワークに接続されている。
【００２７】
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２３、ファイル群９２４が記憶されている。プログラム群９２３のプログラムは、ＣＰＵ９１１、ＯＳ９２１により実行される。
【００２８】
上記プログラム群９２３には、実施の形態において「〜部」として説明する機能を実行するプログラムが記憶されている。プログラムは、ＣＰＵ９１１により読み出され実行される。
【００２９】
ファイル群９２４には、実施の形態において、「〜部」の機能を実行した際の「〜の判定結果」、「〜の計算結果」、「〜の処理結果」などの結果データ、「〜部」の機能を実行するプログラム間で受け渡しするデータ、その他の情報やデータや信号値や変数値やパラメータが、「〜ファイル」や「〜データベース」の各項目として記憶されている。自己位置２０６、自己姿勢角２０７、白線座標２０３、消失点予測値２０８、消失点実測値２０５、消失点残差２０９および姿勢誤差推定量２１０などはファイル群９２４に含まれるものの一例である。
「〜ファイル」や「〜データベース」は、ディスクやメモリなどの記録媒体に記憶される。ディスクやメモリなどの記憶媒体に記憶された情報やデータや信号値や変数値やパラメータは、読み書き回路を介してＣＰＵ９１１によりメインメモリやキャッシュメモリに読み出され、抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示などのＣＰＵの動作に用いられる。抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示のＣＰＵの動作の間、情報やデータや信号値や変数値やパラメータは、メインメモリやキャッシュメモリやバッファメモリに一時的に記憶される。
また、実施の形態において説明するフローチャートの矢印の部分は主としてデータや信号の入出力を示し、データや信号値は、ＲＡＭ９１４のメモリ、磁気ディスク装置９２０の磁気ディスク、その他光ディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ・Ｖｅｒｓａｔｉｌｅ・Ｄｉｓｃ）等の記録媒体に記録される。また、データや信号値は、バス９１２や信号線やケーブルその他の伝送媒体によりオンライン伝送される。
【００３０】
また、実施の形態において「〜部」として説明するものは、「〜回路」、「〜装置」、「〜機器」であってもよく、また、「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ＲＯＭ９１３に記憶されたファームウェアで実現されていても構わない。或いは、ソフトウェアのみ、或いは、素子・デバイス・基板・配線などのハードウェアのみ、或いは、ソフトウェアとハードウェアとの組み合わせ、さらには、ファームウェアとの組み合わせで実施されても構わない。ファームウェアとソフトウェアは、プログラムとして、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ等の記録媒体に記憶される。プログラムはＣＰＵ９１１により読み出され、ＣＰＵ９１１により実行される。すなわち、プログラムは、「〜部」としてコンピュータを機能させるものである。あるいは、「〜部」の手順や方法をコンピュータに実行させるものである。
【００３１】
図４は、実施の形態１における自己位置標定方法を示すフローチャートである。
実施の形態１における自己位置標定方法について、図４に基づいて以下に説明する。
自己位置姿勢標定装置１００の各部は、以下に説明する各処理をＣＰＵを用いて実行する。
【００３２】
＜Ｓ１１０：測位処理＞
航法演算部１１０は、ＧＰＳ受信機１０５により取得された測位信号情報２１１に基づいて自己位置２０６をＧＰＳ測位する。
測位信号情報２１１は、ＧＰＳ受信機１０５により受信された測位信号に関する情報であり、例えば、測位信号に示される航法メッセージ（測位衛星の軌道情報）、測位信号の測位衛星からの発信時刻とＧＰＳ受信機１０５での受信時刻に基づく測位衛星とＧＰＳ受信機１０５との擬似距離、ＧＰＳ受信機１０５により擬似距離に基づいてＧＰＳ測位された測位結果、測位信号として発信された搬送波の受信時の位相などの情報が含まれる。
例えば、航法演算部１１０は、搬送波の位相に基づいてＧＰＳ受信機１０５と複数の測位衛星との距離を算出し、各測位衛星から算出した距離だけ離れた地点として共通する地点をＧＰＳ受信機１０５の地点としてＧＰＳ受信機１０５の位置を算出する。そして、航法演算部１１０は、車両３００の自己位置２０６を示す点として予め定められた航法中心（例えば、車両３００の天板３０１の中心）とＧＰＳ受信機１０５の取り付け位置との差（位置オフセット）をＧＰＳ受信機１０５の位置に加算（または減算）して、車両３００の自己位置２０６を算出する。
【００３３】
＜Ｓ１２０：消失点実測値算出処理＞
また、消失点抽出部１２０は、画像センサ１０４により撮像された路面の映っている画像２０４に基づいて、路面に標示されている白線の消失点実測値２０５を算出する。
【００３４】
図５は、実施の形態１における画像２０４と消失点３１６との関係を示す図である。
図５には、白線３１１の直線部分３１９が映っている。白線３１１は直線部分３１９、円弧部分および直線部分３１９と円弧部分とを結ぶクロソイド曲線部分で構成される。
消失点抽出部１２０は、消失点実測値２０５として、白線３１１の直線部分３１９のふちを指す白線エッジ３１８の延長線上の交点（×印）の画像２０４を含む平面（画像センサ１０４の撮像面）内での２次元座標（ＵＶ座標）を算出する。図５では、左側の白線３１１の左右のエッジ（３１８ａ、３１８ｂ）と右側の白線３１１の左右のエッジ（３１８ｃ、３１８ｄ）との計４つの白線エッジ３１８の交点を消失点３１６として示している。しかし、消失点３１６の消失点実測値２０５は、左側の白線３１１の左右いずれかのエッジ（３１８ａ、３１８ｂ）と右側の白線３１１の左右いずれかのエッジ（３１８ｃ、３１８ｄ）との計２つの白線エッジ３１８の交点として算出されてもよい。また、消失点実測値２０５は、左側の白線３１１の左右のエッジ（３１８ａ、３１８ｂ）の交点として算出されてもよいし、右側の白線３１１の左右のエッジ（３１８ｃ、３１８ｄ）の交点として算出されてもよい。つまり、消失点実測値２０５は、左右２つの白線３１１の交点として算出されてもよいし、１つの白線３１１の左右の白線エッジ３１８の交点として算出されてもよい。
２つの白線３１１および２つの白線エッジ３１８は平行な直線を成す２つの地物の一例である。
また、白線認識点３１７（黒丸）は、白線のエッジ認識で得られた点である。
【００３５】
２次元の画像２０４では、３次元のものが近いものほど大きく表されると共に遠いものほど小さく表されるため、３次元において平行な直線が無限遠を表す１点に収束する。この収束点が消失点３１６である。消失点３１６は、図５に示すように画像２０４内に位置する場合もあるし、図５とは異なり画像２０４外に位置する場合もある。
消失点実測値算出処理（Ｓ１２０）の詳細については後述する。
【００３６】
図４に戻り、自己位置姿勢標定方法の説明を続ける。
【００３７】
＜Ｓ１３０：標定処理＞
航法演算部１１０は、慣性センサ１０１により計測された車両３００のヨー角およびピッチ角の角速度２０１、オドメトリ１０２により計測された速度２０２および後述するＳ１７０において算出された姿勢誤差推定量２１０に基づいて車両３００の自己位置２０６および自己姿勢角２０７を標定する。
【００３８】
Ｓ１３０において、航法演算部１１０は、慣性センサ１０１により計測された車両３００のヨー角およびピッチ角の角速度２０１を用いてデッドレコニング（または、慣性航法）を行って自己姿勢角２０７を測定する。そして、航法演算部１１０は、測定結果に含まれる誤差を姿勢誤差推定量２１０で補正して自己姿勢角２０７を標定する。具体的には、航法演算部１１０は、自己姿勢角２０７の測定値に姿勢誤差推定量２１０を加算して自己姿勢角２０７の測定値を補正し、補正後の自己姿勢角２０７を自己姿勢角２０７の標定値とする。
また、航法演算部１１０は、自己姿勢角２０７の標定値とオドメトリ１０２により計測された速度２０２とを用いてデッドレコニングを行って自己位置２０６を測定する。航法演算部１１０は、自己姿勢角２０７の標定値を用いて測定された自己位置２０６を自己位置２０６の標定値とする。
【００３９】
デッドレコニングとは、慣性センサ１０１により計測された角速度２０１やオドメトリ１０２により計測された速度２０２に基づいて位置および姿勢角を測定する方法である。
デッドレコニングでは、角速度２０１の積分により自己姿勢角２０７の変化量が算出され、前回測定時の自己姿勢角２０７に変化量が加算されることにより現在の自己姿勢角２０７が算出される。
また、デッドレコニングでは、速度２０２の積分により車両３００の移動量が算出され、前回測定時の自己位置２０６を基点にして自己姿勢角２０７の方向に移動量だけ移動した地点が自己位置２０６として算出される。
【００４０】
＜Ｓ１４０：地物データ取得処理＞
次に、データベース検索部１３０は、Ｓ１３０において標定された自己位置２０６および自己姿勢角２０７に基づいて車両３００の画像センサ１０４の撮像範囲を算出し、算出した撮像範囲内に位置する白線の白線座標２０３を白線データベース１０３から取得する。つまり、データベース検索部１３０は、画像センサ１０４により撮像される画像２０４に映る白線の白線座標２０３を白線データベース１０３から取得する。
Ｓ１４０において、データベース検索部１３０は、車両３００の走行している道路に標示されている白線（の直線部分）の白線座標２０３を白線データベース１０３から取得できればよい。
例えば、データベース検索部１３０は、Ｓ１１０またはＳ１３０で得られた自己位置２０６から所定の範囲内の座標を示す白線座標２０３を白線データベース１０３から取得する。取得した白線座標２０３のうち、Ｓ１１０またはＳ１３０で得られた自己姿勢角２０７の方向に位置する白線座標２０３が画像２０４に映る白線の白線座標２０３である。
【００４１】
＜Ｓ１５０：消失点予測値算出処理＞
次に、消失点計算部１４０は、Ｓ１４０において取得された白線座標２０３とＳ１３０において標定された自己姿勢角２０７とに基づいて白線の消失点予測値２０８を算出する。
【００４２】
消失点計算部１４０は、Ｓ１４０において取得された白線座標２０３とＳ１３０において標定された自己姿勢角２０７とに基づいて複数の白線座標２０３上を通る白線エッジ３１８の直線方程式を算出し、算出した直線方程式に基づいて白線エッジ３１８を画像２０４に投影した場合の白線の消失点の２次元座標（ＵＶ座標）を消失点予測値２０８として算出する。
消失点予測値算出処理（Ｓ１５０）の詳細については後述する。
【００４３】
図４に戻り、自己位置姿勢標定方法の説明を続ける。
【００４４】
消失点予測値算出処理（Ｓ１５０）において算出される消失点予測値２０８は、消失点実測値算出処理（Ｓ１２０）において算出される消失点実測値２０５を自己姿勢角２０７と白線座標２０３とに基づいて予測した予測値である。
画像センサ１０４により撮像された画像２０４には撮像時刻が設定されており、慣性センサ１０１により計測された角速度２０１およびオドメトリ１０２により計測された速度２０２には計測時刻が設定されている。
消失点実測値算出処理（Ｓ１２０）および標定処理（Ｓ１３０）〜消失点予測値算出処理（Ｓ１５０）では、同時刻に撮像または計測された画像２０４、角速度２０１およびオドメトリ１０２が用いられる。
このため、標定処理（Ｓ１３０）では、消失点実測値２０５の算出に用いられた画像２０４が撮像された時刻における車両３００の自己位置２０６および自己姿勢角２０７が標定される。
また、地物データ取得処理（Ｓ１４０）では、消失点実測値２０５の算出に用いられた画像２０４に映っている白線の白線座標２０３が取得される。
そして、消失点予測値算出処理（Ｓ１５０）では、消失点実測値２０５の予測値となる消失点予測値２０８が算出される。
【００４５】
＜Ｓ１６０：消失点残差算出処理＞
次に、残差計算部１５０は、Ｓ１２０において算出された消失点実測値２０５とＳ１５０において算出された消失点予測値２０８とに基づいて消失点残差２０９を算出する。
Ｓ１６０において、残差計算部１５０は、消失点実測値２０５と消失点予測値２０８との差の絶対値を消失点残差２０９として算出する。
【００４６】
＜Ｓ１７０：カルマンフィルタ処理＞
次に、カルマンフィルタ部１６０は、Ｓ１６０において算出された消失点残差２０９に基づいて姿勢誤差推定量２１０を算出する。
Ｓ１７０において算出された姿勢誤差推定量２１０は、Ｓ１３０において前回算出された自己姿勢角２０７の補正に用いられる。
カルマンフィルタ処理（Ｓ１７０）については後述する。
【００４７】
＜Ｓ１８０＞
次に、航法演算部１１０は、Ｓ１１０において自己位置２０６がＧＰＳ測位されたか否かを判定する。
ＧＰＳ測位がされなかった場合とは、ＧＰＳ受信機１０５から航法演算部１１０への測位信号情報２１１の入力が無かった場合である。
例えば、車両３００が、トンネルや高層ビルの林立する都市部や森林など、測位衛星への視線方向が遮蔽されてしまう場所を走行している場合、ＧＰＳ受信機１０５は測位衛星からの測位信号を受信できないため、ＧＰＳ受信機１０５から航法演算部１１０への測位信号情報２１１の入力は無く、ＧＰＳ測位はできない。
ＧＰＳ受信機１０５から航法演算部１１０への測位信号情報２１１の入力が有った場合、Ｓ１１０において自己位置２０６がＧＰＳ測位される。
【００４８】
＜Ｓ１８１＞
Ｓ１１０において自己位置２０６がＧＰＳ測位された場合、航法演算部１１０は、Ｓ１１０において測位された自己位置２０６と、Ｓ１３０において標定された自己姿勢角２０７とをカーナビゲーションシステム３０２に出力する。
【００４９】
＜Ｓ１８２＞
Ｓ１１０において自己位置２０６がＧＰＳ測位されなかった場合、航法演算部１１０はＳ１３０において標定された自己位置２０６と自己姿勢角２０７とをカーナビゲーションシステム３０２に出力する。
【００５０】
カーナビゲーションシステム３０２は、航法演算部１１０からの自己位置２０６と自己姿勢角２０７とに基づいて、車両３００の走行位置を道路画像にマップマッチングさせたナビゲーション画面をディスプレイ装置３０３に表示する。
【００５１】
次に、消失点実測値算出処理（Ｓ１２０）の詳細について、図５に基づいて説明する。
消失点抽出部１２０は、画像２０４を画像処理して画像２０４から複数の白線エッジ３１８（３１８ａ〜３１８ｄのうち少なくても２つ）を抽出する。例えば、消失点抽出部１２０は、直線上に連続する白色の画素群を白線３１１として抽出し、抽出した画素群から白線３１１のふち（境界線）部分の画素群を白線エッジ３１８として抽出する。
次に、消失点抽出部１２０は、画像２０４から抽出した複数の白線エッジ３１８それぞれについて、画像２０４を含む平面（画像センサ１０４の撮像面）内での２次元直線方程式を算出する。
そして、消失点抽出部１２０は、算出した複数の白線エッジ３１８の直線方程式に基づいて、複数の白線エッジ３１８の交点となる画素の撮像面内での２次元座標（ＵＶ座標）を算出する。
【００５２】
次に、消失点予測値算出処理（Ｓ１５０）の詳細について、図６および図７に基づいて説明する。
図６および図７は、実施の形態１における消失点予測値算出処理（Ｓ１５０）を説明する図である。
【００５３】
図６において、画像センサ１０４のレンズの中心位置を示すカメラ中心Ｏｂから白線３１１上の任意の点ｐｎとを結んだ直線Ｌｎは、以下の直線方程式１で表される。
以下、画像センサ１０４をカメラという。
【００５４】
【数１】
【００５５】
ここで、（ｘｖ、ｙｖ、ｚｖ）は、カメラ座標系Ｘｖ−Ｙｖ−Ｚｖにおける３次元座標を示す。図６においてカメラ座標系を一点鎖線で示す。
また、［α、β、γ］は、直線Ｌｎの方向ベクトルを示す。
また、「ｆ」は、画像センサ１０４の焦点距離を示す。
【００５６】
カメラ座標系とは、航法座標系（世界座標系、ＮＥＤ座標系）に対して、カメラの姿勢角分（車両３００の姿勢角も含む）の傾きを有する座標系である。航法座標系は、自己位置２０６、自己姿勢角２０７および白線座標２０３を表す座標系であり、例えば、緯度、経度および高さを示す。
焦点面Ｍは、カメラの視線方向（姿勢角方向、撮像方向）と直交する平面であり、カメラ中心Ｏｂからカメラの視線方向に向けて焦点距離ｆだけ離れた平面である。焦点面Ｍは、カメラの画像２０４が含まれる撮像面（投影面）を示す。画像２０４は、カメラにより撮像された３次元の地物が焦点面Ｍに投影されたものである。
【００５７】
以下、カメラの視線方向を「Ｘｖ」、カメラのヨー方向を「Ｙｖ」、カメラのピッチ方向を「Ｚｖ」とする。
画像２０４内の画素（焦点面Ｍ内の点）は横方向を「Ｕ」、縦方向を「Ｖ」とするＵＶ座標で表される。
画像２０４内でのＵ座標はカメラ座標系でのＹｖ座標に対応し、画像２０４内でのＶ座標はカメラ座標系でのＺｖ座標に対応する。
【００５８】
焦点面Ｍは、「ｘｖ＝０」（式２）で表される。
また、カメラ中心Ｏｂは、焦点面Ｍと座標軸Ｘｖとの交点となる焦点面Ｍの中心Ｏｍ（画像２０４の中心）の３次元座標を（０、０、０）とした場合、（−ｆ、０、０）で表される。
【００５９】
ここで、白線３１１上の任意の点ｐｎが焦点面Ｍに投影された点ｐｎ’は、直線Ｌｎと焦点面Ｍとの交点である。このため、点ｐｎ’の焦点面Ｍ内でのＵＶ座標は、直線Ｌｎを表す上記式１に焦点面Ｍを表す上記式２を代入して、以下の式３により表される。
【００６０】
【数２】
【００６１】
消失点計算部１４０は、データベース検索部１３０により取得された白線エッジ３１８上の２つの白線座標２０３に基づいて白線エッジ３１８を表す直線Ｌｅの方程式を生成する。
例えば、白線エッジ３１８を表す直線Ｌｅは、白線エッジ３１８上の入口側（車両３００の進行方向手前）の点ｐｉ（ｘｉ、ｙｉ、ｚｉ）と白線エッジ３１８上の出口側（車両３００の進行方向奥）の点ｐｏ（ｘｏ、ｙｏ、ｚｏ）とを用いて、以下の式４により表される。「ｔ」は直線方程式の媒介変数である。
【００６２】
【数３】
【００６３】
また、白線エッジ３１８を表す直線Ｌｅは、上記の式４を展開して以下の式５により表される。
【００６４】
【数４】
【００６５】
ここで、点ｐｉの白線座標２０３および点ｐｏの白線座標２０３は航法座標系で表されているため、上記式５は航法座標系における白線エッジ３１８を表す直線Ｌｅを示す。
そこで、上記式５をカメラ座標系に変換すると、カメラ座標系における白線エッジ３１８を表す直線Ｌｅの方向ベクトル［α、β、γ］は以下の式６で表される。
【００６６】
【数５】
【００６７】
「Ｃｎｂ」は、航法座標系から車両座標系Ｘ−Ｙ−Ｚ（図６の実線）（航法座標系に対して車両３００の自己姿勢角２０７分の傾きを有する座標系）に変換するために車両３００の自己姿勢角２０７分の回転移動を行う方向余弦行列（回転行列）である。自己姿勢角２０７には標定処理（Ｓ１３０）において標定された値が用いられる。
「Ｃｂｖ」は、車両座標系からカメラ座標系に変換するために車両３００に対するカメラの姿勢角分の回転移動を行う方向余弦行列である。車両３００に対するカメラの姿勢角はカメラの姿勢オフセットとして予め算出されているものとする。車両３００の進行方向とカメラの視線方向とが全く同じになるようにカメラが車両３００に取り付けられているのであれば、車両３００に対するカメラの姿勢角のうち、ヨー角およびピッチ角は「０」となる。
「Ｉｂ」は、車両３００の自己位置２０６を示す点として予め定められた航法中心Ｏ（例えば、車両３００の天板３０１の中心）とカメラの取り付け位置との差（カメラの位置オフセット）である。カメラ位置オフセットＩｂの減算により、座標の中心は車両座標系の中心（航法中心Ｏ）からカメラ座標系の中心（カメラ中心Ｏｂ）に移動される。カメラ位置オフセットＩｂは予め算出されているものとする。
【００６８】
また、白線３１１の消失点Ｐｄは、白線エッジ３１８を表す直線Ｌｅの無限遠の点である。
このため、カメラ中心Ｏｂから白線３１１の消失点Ｐｄへの方向ベクトル（α’、β’、γ’）は、上記式６の媒介変数ｔを「∞」にして、以下の式７で表される。
【００６９】
【数６】
【００７０】
また、カメラ座標系における任意の点は、焦点面Ｍ内でのＵＶ座標が上記式３により表される。
このため、図７において、画像２０４を含む焦点面Ｍ（撮像面）における白線３１１の消失点ＰｄのＵＶ座標は、上記式７を用いて、以下の式８で表される。
【００７１】
【数７】
【００７２】
消失点計算部１４０は、上記式８を計算して、消失点ＰｄのＵＶ座標を消失点予測値２０８として算出する。
消失点予測値算出処理（Ｓ１５０）では、一つの白線エッジ３１８に基づいて消失点予測値２０８が算出される。
【００７３】
次に、カルマンフィルタ処理（Ｓ１７０）について説明する。
カルマンフィルタ処理は、状態量のダイナミクス（時間変化）をモデル化した状態方程式と、観測量と状態量との関係を定式化した観測方程式とに基づいて状態量の誤差推定を行う処理である。
カルマンフィルタ部１６０は、消失点実測値２０５と消失点予測値２０８との差分である消失点残差２０９を観測量とし、自己姿勢角２０７を状態量として、予め定められた状態方程式および観測方程式に基づいて、自己姿勢角２０７に含まれる誤差（姿勢誤差推定量２１０）を算出する。
【００７４】
実施の形態１において、以下のような自己位置姿勢標定装置１００について説明した。
自己位置姿勢標定装置１００は、道路に通常、標示されている中央線や外側線等の白線を利用して自車の姿勢角を精度よく求める。
【００７５】
まず、自己位置姿勢標定装置１００は、白線データベースから得られる白線の方位情報と、自車に搭載された慣性センサやオドメトリ等の航法装置により得られる姿勢角とを利用して、消失点を予測する（Ｓ１５０：消失点予測値算出処理）。
また、自己位置姿勢標定装置１００は、画像センサによる路面画像を画像処理することによって消失点を実測する（Ｓ１２０：消失点実測値算出処理）。
そして、自己位置姿勢標定装置１００は、消失点の予測値と実測値とを比較し（Ｓ１６０：消失点残差算出処理）、この残差をカルマンフィルタの入力とすることで現在の姿勢角の誤差推定値を算出し（Ｓ１７０：カルマンフィルタ処理）、航法演算（Ｓ１３０：標定処理［デッドレコニング］）により得られた姿勢角を補正する。
【００７６】
例えば、白線データベースは、白線の直線区間の入口（長手方向の一端）と出口（長手方向の他端）の緯度、経度、高度を示す。白線データベースに位置情報を記憶することで、現在走行している車両の位置から撮像される範囲内に位置する白線の情報を検索することができる。但し、白線データベースから得るべき情報はあくまで白線の３次元空間における方向である。つまり、白線データベースには白線の方位角が記憶されていてもよい。
なお、ある程度の距離を持った直線区間であれば白線方向は精度よく求められるため、白線データベースに記憶される緯度、経度、高度の精度が測量クラスである必要はない。例えば、直線区間の長さが１００ｍであれば、白線の入口と出口との両端の位置に１０ｃｍ程度の誤差があったとしても、方向精度は０．０６［ｄｅｇ］以下の誤差となる。
【００７７】
実施の形態１において、自己位置姿勢標定装置１００は車両３００（標定物の一例）に搭載される内部装置として説明されているが、自己位置姿勢標定装置１００は標定物に搭載されない外部装置であっても構わない。外部装置である自己位置姿勢標定装置１００は、標定物により各時刻で取得された画像２０４、角速度２０１および速度２０２などの取得情報を入力として、各時刻における標定物の自己姿勢角２０７および自己位置２０６を標定する。例えば、取得情報はＣＤ−ＲＯＭやフラッシュメモリなどの記憶媒体に記憶されて自己位置姿勢標定装置１００に入力され、また、インターネットなどの通信ネットワークを介して利用者端末装置から自己位置姿勢標定装置１００（サーバ装置）に入力される。サーバ装置として稼働する自己位置姿勢標定装置１００は標定結果を利用者端末装置に送信する。
【００７８】
また、実施の形態１では、白線３１１の消失点３１６に基づいて標定が行われているが、白線３１１以外の地物の消失点３１６に基づいて標定が行われてもよい。例えば、道路標示３１５や歩道３１３を対象にした消失点３１６に基づいて標定が行われてもよい。消失点３１６が求められる地物は、白線エッジ３１８または道路標示３１５や歩道３１３の直線部分のエッジのように、平行な直線を成す２つの地物であればよい。平行な直線を成す２つの地物は、白線エッジ３１８と道路標示３１５の直線部分のエッジのように、異なる種類であってもよい。
【００７９】
実施の形態１では、画像処理結果（消失点）を画像処理とは異なる別の方法（Ｓ１５０：消失点予測値算出処理）で予測しているため、画像処理の誤りを容易に検出することが出来る。これにより、車両３００が白線の方位情報が得られている区間を走行する場合、標定した自己位置２０６が大きくずれることはない。
このため、実施の形態１における自己位置姿勢標定装置１００および自己位置標定方法は、将来、カーナビゲーションシステムが３次元化された場合に有効である。
さらに、実施の形態１では、車両３００のピッチ角が標定されることにより、車両３００が上り斜面を走行しているのか下り斜面を走行しているのかがわかるため、運転者にギアの切り替え（例えば、４速から２速に落とす）を促すこともできる。これにより、運転の省エネルギー化が図れる。
【図面の簡単な説明】
【００８０】
【図１】実施の形態１における自己位置姿勢標定装置１００を搭載した車両３００を示す図。
【図２】実施の形態１における自己位置姿勢標定装置１００の機能構成を示す図。
【図３】実施の形態１における自己位置姿勢標定装置１００のハードウェア資源の一例を示す図。
【図４】実施の形態１における自己位置標定方法を示すフローチャート。
【図５】実施の形態１における画像２０４と消失点３１６との関係を示す図。
【図６】実施の形態１における消失点予測値算出処理（Ｓ１５０）を説明する図。
【図７】実施の形態１における消失点予測値算出処理（Ｓ１５０）を説明する図。
【符号の説明】
【００８１】
１００ 自己位置姿勢標定装置、１０１ 慣性センサ、１０２ オドメトリ、１０３ 白線データベース、１０４ 画像センサ、１０５ ＧＰＳ受信機、１１０ 航法演算部、１２０ 消失点抽出部、１３０ データベース検索部、１４０ 消失点計算部、１５０ 残差計算部、１６０ カルマンフィルタ部、２０１ 角速度、２０２ 速度、２０３ 白線座標、２０４ 画像、２０５ 消失点実測値、２０６ 自己位置、２０７ 自己姿勢角、２０８ 消失点予測値、２０９ 消失点残差、２１０ 姿勢誤差推定量、２１１ 測位信号情報、３００ 車両、３０１ 天板、３０２ カーナビゲーションシステム、３０３ ディスプレイ装置、３１１ 白線、３１２ 車道、３１３ 歩道、３１４ 道路標識、３１５ 道路標示、３１６ 消失点、３１７ 白線認識点、３１８ 白線エッジ、３１９ 直線部分、９１１ ＣＰＵ、９１３ ＲＯＭ、９１４ ＲＡＭ、９１５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群。
【特許請求の範囲】
【請求項１】
標定物の姿勢角を標定する標定装置であり、
前記標定物から撮像された画像であり平行な直線を成す２つの地物を示す平行直線地物が映っている画像である平行直線画像に基づいて前記平行直線地物の消失点を消失点の実測値としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する消失点実測値算出部と、
平行直線地物の直線の向きを示す地物データが予め記憶されている記憶機器から前記平行直線画像に映っている前記平行直線地物の地物データを取得する地物データ取得部と、
前記地物データ取得部により取得された前記地物データと前記平行直線画像が撮像されたときの前記標定物の姿勢角として予め測定された姿勢角の測定値とに基づいて前記平行直線地物の消失点を消失点の予測値としてＣＰＵを用いて算出する消失点予測値算出部と、
前記消失点実測値算出部により算出された前記消失点の実測値と前記消失点予測値算出部により算出された前記消失点の予測値との差を消失点の残差としてＣＰＵを用いて算出する消失点残差算出部と、
前記消失点残差算出部により算出された前記消失点の残差に基づいて前記姿勢角の測定値をＣＰＵを用いて補正する標定部と
を備えたことを特徴とする標定装置。
【請求項２】
前記標定装置は、さらに、
前記消失点残差算出部により算出された前記消失点の残差に基づいて前記標定物の前記姿勢角の測定値に含まれる誤差を姿勢誤差推定量としてＣＰＵを用いてカルマンフィルタ処理により算出するカルマンフィルタ部を備え、
前記標定部は、前記カルマンフィルタ部により算出された前記姿勢誤差推定量に基づいて前記姿勢角の測定値を補正する
ことを特徴とする請求項１記載の標定装置。
【請求項３】
前記地物データは、平行直線地物の座標を含み、
前記地物データ取得部は、前記地物データにより示される平行直線地物の座標と前記平行直線画像が撮像されたときの前記標定物の座標として予め測定された前記標定物の座標の測定値とに基づいて前記平行直線画像に映っている前記平行直線地物の地物データを取得する
ことを特徴とする請求項１〜請求項２いずれかに記載の標定装置。
【請求項４】
前記標定部は、補正した前記標定物の前記姿勢角に基づいて前記標定物の座標を標定する
ことを特徴とする請求項３記載の標定装置。
【請求項５】
標定物の姿勢角を標定する標定方法であり、
消失点実測値算出部は、前記標定物から撮像された画像であり平行な直線を成す２つの地物を示す平行直線地物が映っている画像である平行直線画像に基づいて前記平行直線地物の消失点を消失点の実測値としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する消失点実測値算出処理を行い、
地物データ取得部は、平行直線地物の直線の向きを示す地物データが予め記憶されている記憶機器から前記平行直線画像に映っている前記平行直線地物の地物データを取得する地物データ取得処理を行い、
消失点予測値算出部は、前記地物データ取得部により取得された前記地物データと前記平行直線画像が撮像されたときの前記標定物の姿勢角として予め測定された姿勢角の測定値とに基づいて前記平行直線地物の消失点を消失点の予測値としてＣＰＵを用いて算出する消失点予測値算出処理を行い、
消失点残差算出部は、前記消失点実測値算出部により算出された前記消失点の実測値と前記消失点予測値算出部により算出された前記消失点の予測値との差を消失点の残差としてＣＰＵを用いて算出する消失点残差算出処理を行い、
標定部は、前記消失点残差算出部により算出された前記消失点の残差に基づいて前記姿勢角の測定値をＣＰＵを用いて補正する標定処理を行う
ことを特徴とする標定方法。
【請求項６】
請求項５記載の標定方法をコンピュータに実行させることを特徴とする標定プログラム。
【請求項１】
標定物の姿勢角を標定する標定装置であり、
前記標定物から撮像された画像であり平行な直線を成す２つの地物を示す平行直線地物が映っている画像である平行直線画像に基づいて前記平行直線地物の消失点を消失点の実測値としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する消失点実測値算出部と、
平行直線地物の直線の向きを示す地物データが予め記憶されている記憶機器から前記平行直線画像に映っている前記平行直線地物の地物データを取得する地物データ取得部と、
前記地物データ取得部により取得された前記地物データと前記平行直線画像が撮像されたときの前記標定物の姿勢角として予め測定された姿勢角の測定値とに基づいて前記平行直線地物の消失点を消失点の予測値としてＣＰＵを用いて算出する消失点予測値算出部と、
前記消失点実測値算出部により算出された前記消失点の実測値と前記消失点予測値算出部により算出された前記消失点の予測値との差を消失点の残差としてＣＰＵを用いて算出する消失点残差算出部と、
前記消失点残差算出部により算出された前記消失点の残差に基づいて前記姿勢角の測定値をＣＰＵを用いて補正する標定部と
を備えたことを特徴とする標定装置。
【請求項２】
前記標定装置は、さらに、
前記消失点残差算出部により算出された前記消失点の残差に基づいて前記標定物の前記姿勢角の測定値に含まれる誤差を姿勢誤差推定量としてＣＰＵを用いてカルマンフィルタ処理により算出するカルマンフィルタ部を備え、
前記標定部は、前記カルマンフィルタ部により算出された前記姿勢誤差推定量に基づいて前記姿勢角の測定値を補正する
ことを特徴とする請求項１記載の標定装置。
【請求項３】
前記地物データは、平行直線地物の座標を含み、
前記地物データ取得部は、前記地物データにより示される平行直線地物の座標と前記平行直線画像が撮像されたときの前記標定物の座標として予め測定された前記標定物の座標の測定値とに基づいて前記平行直線画像に映っている前記平行直線地物の地物データを取得する
ことを特徴とする請求項１〜請求項２いずれかに記載の標定装置。
【請求項４】
前記標定部は、補正した前記標定物の前記姿勢角に基づいて前記標定物の座標を標定する
ことを特徴とする請求項３記載の標定装置。
【請求項５】
標定物の姿勢角を標定する標定方法であり、
消失点実測値算出部は、前記標定物から撮像された画像であり平行な直線を成す２つの地物を示す平行直線地物が映っている画像である平行直線画像に基づいて前記平行直線地物の消失点を消失点の実測値としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する消失点実測値算出処理を行い、
地物データ取得部は、平行直線地物の直線の向きを示す地物データが予め記憶されている記憶機器から前記平行直線画像に映っている前記平行直線地物の地物データを取得する地物データ取得処理を行い、
消失点予測値算出部は、前記地物データ取得部により取得された前記地物データと前記平行直線画像が撮像されたときの前記標定物の姿勢角として予め測定された姿勢角の測定値とに基づいて前記平行直線地物の消失点を消失点の予測値としてＣＰＵを用いて算出する消失点予測値算出処理を行い、
消失点残差算出部は、前記消失点実測値算出部により算出された前記消失点の実測値と前記消失点予測値算出部により算出された前記消失点の予測値との差を消失点の残差としてＣＰＵを用いて算出する消失点残差算出処理を行い、
標定部は、前記消失点残差算出部により算出された前記消失点の残差に基づいて前記姿勢角の測定値をＣＰＵを用いて補正する標定処理を行う
ことを特徴とする標定方法。
【請求項６】
請求項５記載の標定方法をコンピュータに実行させることを特徴とする標定プログラム。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【公開番号】特開２００９−２０４３８５（Ｐ２００９−２０４３８５Ａ）
【公開日】平成２１年９月１０日（２００９．９．１０）
【国際特許分類】
【出願番号】特願２００８−４５７１５（Ｐ２００８−４５７１５）
【出願日】平成２０年２月２７日（２００８．２．２７）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２１年９月１０日（２００９．９．１０）
【国際特許分類】
【出願日】平成２０年２月２７日（２００８．２．２７）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]