自己位置標定装置、自己位置標定方法および自己位置標定プログラム
【課題】道路上の白線や、道路標識といった既存の道路インフラを利用して、自己位置標定装置の標定精度を向上させる。
【解決手段】車両に搭載した自己位置標定装置１００において、ＤＲ演算部３はデッドレコニングにより車両位置を算出する。擬似距離残差計算部２２は擬似距離と衛星軌道パラメータに基づく衛星−車両間の距離とで擬似距離残差を算出する。カメラ７は車両前方の道路を撮像し、画像処理部８は画像に映る白線の方位角を算出し、白線方位角取得部２３は白線情報データベース９から白線の方位角を取得し、白線方位角残差計算部２４は画像処理部８が算出した白線の方位角と白線方位角取得部２３が取得した白線の方位角とで方位角残差を算出する。拡張カルマンフィルタ６は擬似距離残差と方位角残差とを観測量として入力し、ＤＲ演算部３の測位結果の誤差を推定する。ＤＲ演算部３は拡張カルマンフィルタ６の推定誤差で測位結果を補正する。
【解決手段】車両に搭載した自己位置標定装置１００において、ＤＲ演算部３はデッドレコニングにより車両位置を算出する。擬似距離残差計算部２２は擬似距離と衛星軌道パラメータに基づく衛星−車両間の距離とで擬似距離残差を算出する。カメラ７は車両前方の道路を撮像し、画像処理部８は画像に映る白線の方位角を算出し、白線方位角取得部２３は白線情報データベース９から白線の方位角を取得し、白線方位角残差計算部２４は画像処理部８が算出した白線の方位角と白線方位角取得部２３が取得した白線の方位角とで方位角残差を算出する。拡張カルマンフィルタ６は擬似距離残差と方位角残差とを観測量として入力し、ＤＲ演算部３の測位結果の誤差を推定する。ＤＲ演算部３は拡張カルマンフィルタ６の推定誤差で測位結果を補正する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、例えば、ＧＩＳ（Ｇｅｏｇｒａｐｈｉｃ Ｉｎｆｏｒｍａｔｉｏｎ Ｓｙｓｔｅｍ）データベースを作成するために地理データ収集車両に搭載したり、一般車両に搭載してナビゲーションシステムに利用したりする自己位置標定装置、自己位置標定方法および自己位置標定プログラムに関するものである。
【背景技術】
【０００２】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）の受信機の価格が低下すると共にカーナビゲーションシステム（以下、カーナビ、カーナビゲーションともいう）が普及し、カーナビゲーションシステムの自己位置標定精度を向上するための技術やドライバーの運転補助のための技術などさまざまな技術が開発されてきた。例えば、自己位置標定精度を向上させる技術として、ＧＰＳの観測情報の一つである擬似距離に対して補正情報を反映して測位精度を向上させるディッファレンシャルＧＰＳ（ＤＧＰＳ）や、地図情報を使って自己位置を補正するマップマッチング技術や、白線を認識して認識した白線を基準にして行うレーンキーピングなどの技術が挙げられる。
【０００３】
国土交通省は、１９９１年からＡＳＶ（Ａｄｖａｎｃｅｄ Ｓａｆｅｔｙ Ｖｅｈｉｃｌｅ：先進安全自動車）の開発を主導し、現在は第３期の計画を進行中であり、車車間通信を使った事故回避システムの実用化を目指している。そのような高度なシステムには、現在よりも精度の高い自己位置標定装置が必要とされているが、一般の乗用車に搭載可能な装置ではＧＰＳのアベイラビリティ（Ａｖａｉｌａｂｉｌｉｔｙ，可用性）が悪い状況などにおいて十分な位置標定精度が保てないのが現状である。
【０００４】
従来のカーナビ用途などの車両用自己位置標定装置では、ＧＰＳ、慣性装置、車速パルスなどを利用するものが開発されている。この自己位置標定装置の標定精度を高めるため、道路上の通行区分帯表示用の断続線あるいは連続線（通常“白線”であるので、以下便宜上“白線”と呼ぶ）や道路標識、信号機、建物などのランドマークを利用して自己位置標定精度を高める手法が提案されてきた。
【０００５】
例えば、白線を利用する方法として、車両に搭載したビデオカメラで白線を撮影し、その画像を処理した観測値から車両と白線との間の距離を計算し、車両と白線との距離を自己位置補正に利用する方式（特許文献１）がある。
また、建物などのランドマークを利用する方式として、車両に搭載したレーザレーダ等の測距型センサによる観測値と建造物データベースに登録されている値との照合結果に基づく位置補正方式（特許文献２）などが提案されている。さらには、車両の周囲にある建物などのランドマークの並び方や位置関係から自己位置を同定する方式も考案されている（特許文献３）。
また、道路標識や信号を利用する方式として、車両に搭載したカメラによって標識を認識し、二つのカメラまたはレーダを用いて標識までの距離を検出し、データベースから得たその標識の座標と、検出した距離とを使って自己位置を補正する手法（特許文献４）がある。さらには、信号機認識装置と、信号機への方向の仰角を計算する手段を備え、信号機までの水平方向距離を計算し、信号機までの水平方向距離を自己位置補正に利用する方式（特許文献５）などが提案されている。
【０００６】
ところが、車載カメラによって認識した白線までの距離を自己位置補正に使う従来方式は、カーナビゲーションシステム等の自己位置標定装置用の地図データベースの精度が十分なものとはいえないため、補正による効果は非常に限定的である。現状、カーナビゲーションに使われているような地図データベースの精度は、車両が道路上に位置する程度の位置補正を行うレーンキーピングに対しては有効であるが、従来方式によりサブメートル級の標定精度を達成するには十分な精度ではない。地図データベースの精度は今後向上すると考えられるが、地球の地殻変動によって１年に数センチメートル程度の絶対座標変動が起きるのが普通であり、地図データベースにおいて確保できる位置座標精度には限界がある。
また、従来方式のうち特許文献２には、専用の建物データベース構築が必要となり、また、高速で高精度なレーザレーダが必要となるといった課題がある。
【０００７】
一方、このような状況において、簡便な方法で道路や建造物等のＧＩＳデータを作成する方法が開発されつつある。例えば、白線のデータベースを構築する方法（特許文献６）などは、すでに実用化段階に入っている。
【特許文献１】第２６８７６４５号公報
【特許文献２】特開２００６−１３８８３４号公報
【特許文献３】特開平８−２４７７７５号公報
【特許文献４】特開２０００−９７７１４号公報
【特許文献５】特開平７−３５５６０号公報
【特許文献６】特開２００６−４７２９１号公報
【非特許文献１】藤本、“カーナビ用地図データフォーマット ＫＩＷＩ”、デンソーテクニカルレビュー ＶＯＬ．６ Ｎｏ．１、２００１
【発明の開示】
【発明が解決しようとする課題】
【０００８】
カーナビなどに代表される車両用の自己位置標定装置は、ＧＰＳ、慣性装置、車速パルスなどを利用するものが標準的である。このような自己位置標定装置では、都市部やトンネル内などのＧＰＳ測位信号のアベイラビリティが悪い環境では標定精度が劣化するという課題があった。そのため、ＧＰＳ測位信号のアベイラビリティが悪い環境でも一定の測位精度を維持するためには、他に何らかの補助手段等が必要である。
【０００９】
そこで、本発明は、道路上の白線や、道路標識といった既存の道路インフラを利用して、自己位置標定装置の標定精度を向上させることを目的とする。
【課題を解決するための手段】
【００１０】
本発明の自己位置標定装置は、自己位置を測位する測位部と、前記測位部が測位した際の特定の方向を撮像するカメラと、前記カメラが撮像した画像に基づいて前記画像に映っている特定の地物の長さ方向が基準方向に対して成す前記特定の地物の方位角をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角部と、地物の長さ方向が基準方向に対して成す当該地物の方位角を記憶する方位角データベースと、前記測位部が測位した自己位置に基づいて前記方位角データベースから前記特定の地物に対応する地物の方位角を取得するデータベース方位角部と、前記画像方位角部が算出した地物の方位角と前記データベース方位角部が取得した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出部と、前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定するカルマンフィルタとを備え、前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とすることを特徴とする。
【発明の効果】
【００１１】
本発明によれば、例えば、画像方位角部が画像に映った白線の方位角を算出し、カルマンフィルタが方位角データベースに記憶されている白線の方位角と画像から算出した白線の方位角との方位角残差を観測量として誤差推定を行うことにより、自己位置標定精度を向上させることができる。
【００１２】
従来方式は、白線の方位角データを自己位置補正に利用していなかったり（特許文献１）、道路標識が存在する位置の（車両から見た）方位角データを自己位置補正に利用していなかったり（特許文献４）、自己位置標定装置としては改良の余地があった。
そこで、本発明は、方位角が地球の地殻変動による変化が少なく、方位角が絶対位置座標よりも比較的精度良く検出可能であるという点に着目し、白線の方位角や道路標識の方位角を自己位置補正に利用することにより自己位置標定精度を向上させる。
【発明を実施するための最良の形態】
【００１３】
実施の形態１．
図１は、実施の形態１における自己位置標定装置１００の構成図である。
図２は、実施の形態１における自己位置標定装置１００の自己位置標定方法を示すフローチャートである。
実施の形態１における自己位置標定装置１００の構成の概要および自己位置標定装置１００の実行する自己位置標定方法の概要について、図１および図２に基づいて以下に説明する。
自己位置標定装置１００の各部は図２に基づいて説明する各処理をＣＰＵを用いて実行する。
【００１４】
図１において、自己位置標定装置１００は車両（移動体の一例）に搭載され、任意の測位方式によって測位した測位部１１０の測位結果に含まれる誤差を誤差推定部１４０を用いて推定し、推定した誤差により測位部１１０の測位結果を補正して車両の位置や姿勢角を高精度に標定する。
【００１５】
＜Ｓ１１０：測位処理＞
図２において、測位部１１０は車両の方位角データと速度データを用いたデッドレコニング計算や、３軸の加速度計および３軸のジャイロを利用するストラップダウン演算によって、車両の位置および姿勢角を算出する。デッドレコニング計算に使われる方位角データはジャイロから、速度データは車速パルスから取得することが多い。
【００１６】
＜Ｓ１２０：ＧＰＳ観測残差算出処理＞
次に、ＧＰＳ観測残差算出部１２０は、擬似距離や位相差やドップラー量などのＧＰＳ観測量について、ＧＰＳ観測部１２１の観測値とＧＰＳ観測量演算部１２２の演算値との差分をＧＰＳ観測残差として算出する。
このとき、ＧＰＳ観測部１２１はＧＰＳ受信機を用いて測位信号を受信してＧＰＳ観測量を取得し、ＧＰＳ観測量演算部１２２はＧＰＳ観測部１２１のＧＰＳ観測量に相当する量を演算により算出する。そして、ＧＰＳ観測残差算出部１２０はＧＰＳ観測部１２１の観測値とＧＰＳ観測量演算部１２２の演算値との差分をＧＰＳ観測残差として誤差推定部１４０の入力にする。ＧＰＳ観測部１２１によるＧＰＳ観測量には、例えば、ＧＰＳ衛星が測位信号を発信してからＧＰＳ受信機が測位信号を受信するまでの時間に光速を乗じることにより算出されるＧＰＳ衛星とＧＰＳ受信機との間の擬似距離がある。また例えば、特定のＧＰＳ衛星からの測位信号（搬送波）を複数のＧＰＳ受信機で受信した際のＧＰＳ受信機間での受信測位信号の位相差（一重位相差）やＧＰＳ受信機間の一重位相差を複数のＧＰＳ衛星について得た際の一重位相差同士の差を示す二重位相差もＧＰＳ観測量の一例である。また例えば、受信した測位信号の周波数と測位信号の基準周波数との差に基づいて算出されるＧＰＳ衛星とＧＰＳ受信機との相対速度を示すドップラー量もＧＰＳ観測量の一例である。ＧＰＳ観測量演算部１２２は、例えば、測位部１１０が測位した車両の位置と測位信号に含まれる衛星軌道パラメータが示すＧＰＳ衛星の位置とに基づいて擬似距離に相当する距離を算出する。また例えば、ＧＰＳ観測量演算部１２２は各ＧＰＳ受信機から各ＧＰＳ衛星への方向を示すＬＯＳ（Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ）ベクトルとあるＧＰＳ受信機から別のＧＰＳ受信機への方向を示す基線ベクトルとに基づいて位相差や二重位相差を算出する。また例えば、ＧＰＳ観測量演算部１２２はＧＰＳ衛星の位置・速度と車両の位置・速度とに基づいてドップラー量を算出する。そして、ＧＰＳ観測残差算出部１２０はＧＰＳ観測部１２１のＧＰＳ観測値とＧＰＳ観測量演算部１２２のＧＰＳ観測値に相当する演算値との差分をＧＰＳ観測残差として算出する。
【００１７】
＜Ｓ１３０：方位角残差算出処理＞
次に、方位角残差算出部１３０は、車両が走行している道路における白線の直線部分の方位角や車両から見た標識の方位角について、方位角観測部１３１の観測値と方位角演算部１３２の演算値との差分を方位角残差として算出する。
このとき、方位角観測部１３１はカメラを用いて車両の進行方向を撮像して白線の直線部分の方位角や車両から見た標識の方位角の観測値を取得し、方位角演算部１３２はデータベースに登録されているデータに基づいて白線の方位角や標識の方位角の演算値を取得する。そして、方位角残差算出部１３０は方位角観測部１３１の観測値と方位角演算部１３２の演算値との差分を方位角残差として誤差推定部１４０の入力にする。
【００１８】
＜Ｓ１４０：誤差推定処理・補正処理＞
そして、誤差推定部１４０は、ＧＰＳ観測残差と方位角残差とに基づいて、測位部１１０の測位結果に含まれる誤差を推定する。
測位部１１０は誤差推定部１４０が推定した誤差に基づいて位置および姿勢角を補正する。
このとき、誤差推定部１４０はカルマンフィルタを用いて位置および姿勢角の誤差推定を行う。
このカルマンフィルタは、状態量のダイナミクスをモデル化した状態方程式と、観測量と状態量との関係を定式化した観測方程式とに基づいて状態量の誤差推定を行う。自己位置を標定するような場合、カルマンフィルタは、ＧＰＳ受信機による特定の観測量の観測値と特定の観測値に相当する演算値（理論値）との差分（観測残差という）を観測量として、自己位置の座標や姿勢角（方位角、仰角、回転角）などの状態量について誤差推定を行う。カルマンフィルタは自己位置の座標や姿勢角の誤差を状態量として算出する。
【００１９】
しかし、自己位置標定装置１００を搭載した車両がトンネル内やビルの間を移動する際にＧＰＳ衛星から測位信号を受信できないというような場合、誤差推定部１４０（カルマンフィルタ）が誤差推定を行うことができず自己位置標定装置１００の標定精度は悪化する。
そこで、実施の形態１における誤差推定部１４０（カルマンフィルタ）は、測位信号が受信できない場合でも画像に基づいて観測可能な白線や標識に関する方位角残差を観測量として、測位部１１０の測位結果について誤差推定を行う。
【００２０】
ここで、ＧＰＳ観測量以外の観測量として車両と白線との距離や車両と標識との距離を用いる方法も考えられるが、距離を観測量として用いる方法は、距離の観測値を得るためにステレオカメラやレーザレーダが必要になったり、地殻変動によるデータベースの精度の低下によりデータベースに登録された白線や標識の位置に基づいて算出する距離の演算値の精度が低くなったりするという課題がある。
【００２１】
一方、実施の形態１のようにＧＰＳ観測量以外の観測量として方位角を用いた場合、単眼カメラが一台あればそのカメラで撮像した画像から方位角の観測値を得ることができ、方位角は地殻変動による変化が少ないためデータベースに記憶された白線や標識の位置に基づく方位角の演算値の精度が比較的高いという利点がある。
【００２２】
また、誤差推定部１４０（カルマンフィルタ）は測位部１１０の測位結果の誤差以外に、測位部１１０が用いるジャイロについてのレート誤差やＧＰＳ観測残差算出部１２０が用いるＧＰＳ受信機についてのクロック誤差や方位角残差算出部１３０が用いるカメラについてのアライメント誤差などについても誤差推定を行い、推定した誤差を補正用に出力する。
【００２３】
図３は、実施の形態１における自己位置標定装置１００のハードウェア資源の一例を示す図である。
図３において、自己位置標定装置１００は、プログラムを実行するＣＰＵ９１１（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサともいう）を備えている。ＣＰＵ９１１は、バス９１２を介してＲＯＭ９１３、ＲＡＭ９１４、通信ボード９１５、表示装置９０１、ＧＰＳ受信機１、ジャイロ４、オドメータ５、カメラ７、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。磁気ディスク装置９２０の代わりに、光ディスク装置、メモリカード読み書き装置などの記憶装置でもよい。
ＲＡＭ９１４は、揮発性メモリの一例である。ＲＯＭ９１３、磁気ディスク装置９２０の記憶媒体は、不揮発性メモリの一例である。これらは、記憶機器、記憶装置あるいは記憶部の一例である。また、入力データが記憶されている記憶機器は入力機器、入力装置あるいは入力部の一例であり、出力データが記憶される記憶機器は出力機器、出力装置あるいは出力部の一例である。
通信ボード９１５は入出力機器、入出力装置あるいは入出力部の一例である。
【００２４】
通信ボード９１５は、無線または有線により、ＬＡＮ（ローカルエリアネットワーク）、インターネット、ＷＡＮ（ワイドエリアネットワーク）、電話回線などの通信網に接続されている。
【００２５】
ジャイロ４は自己位置標定装置１００の搭載された車両の方位角、仰角、回転角の角加速度（以下、レートとする）を計測するセンサである。また、ジャイロ４は後述するように方位角のレートのみ計測する１軸ジャイロであってもよい。
オドメータ５は自己位置標定装置１００の搭載された車両の車速に応じてパルス（以下、車速または車速パルスとする）を出力する車速センサである。
カメラ７は自己位置標定装置１００の搭載された車両の前方に向けて設置され、車両の進行方向に位置する白線や道路標識を撮像する。カメラ７は単眼カメラで構わない。但し、ステレオカメラであっても構わない。また、カメラ７以外に、白線や標識などの地物との距離を計測するためにレーザレーダを備えても構わない。
ＧＰＳ受信機１はＧＰＳ衛星から測位信号を受信し、ＧＰＳ衛星が測位信号を発信してから測位信号を受信するまでの時間に基づいて擬似距離を算出したり、擬似距離や受信した測位信号の位相に基づいて自己位置を算出したりする。ＧＰＳ受信機１は車両には１台備える。ＧＰＳ観測量として二重位相差を観測する場合、位置座標が既知である車両外の地点にもう一台のＧＰＳ受信機を備えておき、そのＧＰＳ受信機の観測データを車両に送信して利用する場合もある。
【００２６】
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２３、ファイル群９２４が記憶されている。プログラム群９２３のプログラムは、ＣＰＵ９１１、ＯＳ９２１により実行される。
【００２７】
上記プログラム群９２３には、実施の形態において「〜部」として説明する機能を実行するプログラムが記憶されている。プログラムは、ＣＰＵ９１１により読み出され実行される。
【００２８】
ファイル群９２４には、実施の形態において、「〜部」の機能を実行した際の「〜の判定結果」、「〜の計算結果」、「〜の処理結果」などの結果データ、「〜部」の機能を実行するプログラム間で受け渡しするデータ、その他の情報やデータや信号値や変数値やパラメータが、「〜ファイル」や「〜データベース」の各項目として記憶されている。測位部１１０が測位した自己位置・姿勢角、カメラ７が撮像した画像、方位角残差算出部１３０が方位角の取得に用いる白線情報や標識情報、ＧＰＳ観測残差算出部１２０が観測したＧＰＳ観測量やＧＰＳ観測残差算出部１２０が算出したＧＰＳ観測残差、方位角残差算出部１３０が画像から得た白線・標識の方位角や方位角残差算出部１３０が算出した方位角残差などはファイル群９２４に含まれるものの一例である。
「〜ファイル」や「〜データベース」は、ディスクやメモリなどの記録媒体に記憶される。ディスクやメモリなどの記憶媒体に記憶された情報やデータや信号値や変数値やパラメータは、読み書き回路を介してＣＰＵ９１１によりメインメモリやキャッシュメモリに読み出され、抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示などのＣＰＵの動作に用いられる。抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示のＣＰＵの動作の間、情報やデータや信号値や変数値やパラメータは、メインメモリやキャッシュメモリやバッファメモリに一時的に記憶される。
また、実施の形態において説明するフローチャートの矢印の部分は主としてデータや信号の入出力を示し、データや信号値は、ＲＡＭ９１４のメモリ、ＦＤＤ９０４のフレキシブルディスク、ＣＤＤ９０５のコンパクトディスク、磁気ディスク装置９２０の磁気ディスク、その他光ディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ・Ｖｅｒｓａｔｉｌｅ・Ｄｉｓｃ）等の記録媒体に記録される。また、データや信号値は、バス９１２や信号線やケーブルその他の伝送媒体によりオンライン伝送される。
【００２９】
また、実施の形態において「〜部」として説明するものは、「〜回路」、「〜装置」、「〜機器」であってもよく、また、「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ＲＯＭ９１３に記憶されたファームウェアで実現されていても構わない。或いは、ソフトウェアのみ、或いは、素子・デバイス・基板・配線などのハードウェアのみ、或いは、ソフトウェアとハードウェアとの組み合わせ、さらには、ファームウェアとの組み合わせで実施されても構わない。ファームウェアとソフトウェアは、プログラムとして、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ等の記録媒体に記憶される。プログラムはＣＰＵ９１１により読み出され、ＣＰＵ９１１により実行される。すなわち、自己位置標定プログラムは、「〜部」としてコンピュータを機能させるものである。あるいは、「〜部」の手順や方法をコンピュータに実行させるものである。
【００３０】
図４は、実施の形態１における自己位置標定装置１００の構成図である。
図１に基づいて説明した自己位置標定装置１００の具体例として、測位部１１０がジャイロ４やオドメータ５を用いたデッドレコニング（Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ、推測航法）により測位を行い、ＧＰＳ観測残差算出部１２０がＧＰＳ観測量として擬似距離を用いて擬似距離残差を算出し、方位角残差算出部１３０が白線の方位角残差を算出し、誤差推定部１４０が擬似距離残差と白線方位角残差とを観測量として入力し、測位部１１０の測位結果に含まれる航法誤差と方位角残差算出部１３０が白線の撮像に用いたカメラ７のアライメント誤差とを状態量として計算して補正量として出力する構成について、図４に基づいて以下に説明する。
【００３１】
図４において、自己位置標定装置１００はＤＲ演算部３、ジャイロ４およびオドメータ５を測位部１１０として備える。
ジャイロ４は、自己位置標定装置１００が搭載された車両の方位角のレートを計測する１軸ジャイロである。
オドメータ５は、自己位置標定装置１００が搭載された車両の車速を出力する。
ＤＲ演算部３は、ジャイロ４が出力した方位角レートとオドメータ５が出力した車速とに基づいて車両の移動量を推測し、推測した移動量を前回の測位結果に反映して車両の位置や方位角を算出するデッドレコニングをＣＰＵを用いて実行する。
【００３２】
また、自己位置標定装置１００はＧＰＳ受信機１、衛星軌道演算部２、衛星距離計算部２１および擬似距離残差計算部２２をＧＰＳ観測残差算出部１２０として備え、ＧＰＳ受信機１および衛星軌道演算部２はＧＰＳ観測部１２１を構成し、衛星距離計算部２１はＧＰＳ観測量演算部１２２を構成する。
ＧＰＳ受信機１は、ＧＰＳ衛星が発信した測位信号を受信し、ＧＰＳ衛星が測位信号を発信してから測位信号を受信するまでの時間に基づいて車両とＧＰＳ衛星との擬似距離を算出する。また、ＧＰＳ受信機１は受信した測位信号に含まれるＧＰＳ衛星の軌道を示す衛星軌道パラメータを出力する。
衛星軌道演算部２は、ＧＰＳ受信機１が出力した衛星軌道パラメータに基づいてＧＰＳ衛星の位置をＣＰＵを用いて算出する。
衛星距離計算部２１は、ＤＲ演算部３が算出した車両の位置と衛星軌道演算部２が算出したＧＰＳ衛星の位置とに基づいて車両とＧＰＳ衛星との距離を衛星距離としてＣＰＵを用いて算出する。
擬似距離残差計算部２２は、ＧＰＳ受信機１が算出した擬似距離と衛星距離計算部２１が算出した衛星距離との差分を擬似距離残差としてＣＰＵを用いて算出する。
【００３３】
また、自己位置標定装置１００はカメラ７、画像処理部８、白線情報データベース９、白線方位角取得部２３および白線方位角残差計算部２４を方位角残差算出部１３０として備え、カメラ７および画像処理部８は方位角観測部１３１を構成し、白線情報データベース９および白線方位角取得部２３は方位角演算部１３２を構成する。
カメラ７は車両に対して前方に向けて設置された単眼カメラであり、車両が走行している道路の進行方向を撮像する。
画像処理部８（画像方位角部の一例）は、カメラ７が撮像した画像に映っている白線が成す角度とＤＲ演算部３が算出した車両の方位角とに基づいて画像に映っている白線の方位角を算出する。
白線情報データベース９（方位角データベース、位置データベースの一例）には各白線について位置（座標、レーンなど）や形状（直線、カーブなど）や方位角などの情報が予め記憶されている。例えば、方位角の情報は白線が位置するレーンの通行方向に応じて白線の上流側を基点として白線の下流側（進行先）が向いている方位を示す。図１０に、白線と白線（の直線部分）の方位角とレーンの通行方向との関係を示す。白線情報データベース９は、例えば、特許文献６を利用することにより構築できる。特許文献６によって作成されるデータベースは、平面線形については直線／円曲線／クロソイドの３種類、縦断曲線については直線／２次曲線の２種類によって構成される。
白線方位角取得部２３（データベース方位角部）は、ＤＲ演算部３が算出した車両の位置と方位角とに基づいて白線情報データベース９から車両の進行方向に位置する白線の方位角を取得する。
白線方位角残差計算部２４は、画像処理部８が出力した画像に基づく白線の方位角と白線方位角取得部２３が出力した白線情報データベース９に基づく白線の方位角との差分を白線方位角残差としてＣＰＵを用いて算出する。
【００３４】
また、自己位置標定装置１００は拡張カルマンフィルタ６を誤差推定部１４０として備える。
拡張カルマンフィルタ６は、擬似距離残差計算部２２が算出した擬似距離残差と白線方位角残差計算部２４が算出した白線方位角残差とを観測量として入力し、ＤＲ演算部３のデッドレコニングによる測位結果に含まれる航法誤差とカメラ７のアライメント誤差とを状態量として計算して補正量として出力する。
【００３５】
図５は、実施の形態１における自己位置標定装置１００の自己位置標定方法１を示すフローチャートである。
図４に示した自己位置標定装置１００の自己位置標定方法１について、図５に基づいて以下に説明する。
図５において、Ｓ２１０は図２の測位処理（Ｓ１１０）に対応し、Ｓ２２１〜Ｓ２２４は図２のＧＰＳ残差算出処理（Ｓ１２０）に対応し、Ｓ２３１〜Ｓ２３４は図２の方位角残差算出処理（Ｓ１３０）に対応し、Ｓ２４０は図２の誤差推定処理（Ｓ１４０）に対応する。
自己位置標定装置１００の各部は以下に説明する各処理をＣＰＵを用いて実行する。
【００３６】
＜Ｓ２１０：測位処理の一例＞
まず、ＤＲ演算部３はジャイロ４から車両の方位角のレートを、オドメータ５から車両の速度を随時入力する。そして、ＤＲ演算部３は方位角のレートと車両の速度とを積分して前回測位時からの移動量を算出し、前回測位した車両の位置および方位角に前回測位時からの移動量を加算して現在の車両の位置および方位角を算出するデッドレコニング処理（または、慣性航法処理）を行う。例えば、ＤＲ演算部３は東西南北を基準方位として車両の方位角を算出する。
また、ＤＲ演算部３は算出した車両の位置および方位角を拡張カルマンフィルタ６が推定した航法誤差に基づいて補正し、補正した値を車両の位置および方位角として出力する。
【００３７】
＜Ｓ２２１＞
また、ＧＰＳ受信機１は、ＤＲ演算部３が測位を行った際にＧＰＳ衛星から受信した測位信号に基づいて、ＤＲ演算部３が測位を行った際のＧＰＳ受信機１（車両）からＧＰＳ衛星までの擬似距離と、測位信号が示す衛星軌道パラメータとを出力する。
＜Ｓ２２２＞
次に、衛星軌道演算部２はＧＰＳ受信機１の出力した衛星軌道パラメータに基づいてＧＰＳ衛星の位置を算出する。
＜Ｓ２２３＞
次に、衛星距離計算部２１はＤＲ演算部３が測位した車両の位置と衛星軌道演算部２が算出したＧＰＳ衛星の位置とに基づいて車両からＧＰＳ衛星までの距離を衛星距離として算出する。
＜Ｓ２２４＞
次に、擬似距離残差計算部２２はＧＰＳ受信機１が算出した擬似距離と衛星距離計算部２１が算出した衛星距離との差分を擬似距離残差として算出する。
【００３８】
＜Ｓ２３１：撮像処理の一例＞
また、ＤＲ演算部３が測位を行った際、カメラ７は車両の走行している道路の進行方向を撮像する。
＜Ｓ２３２：画像方位角算出処理の一例＞
次に、画像処理部８はカメラ７が撮像した画像から画像に映っている道路上の白線を検出する。例えば、画像処理部８は画像を二値化してエッジ部分などの特徴点を抽出し、各特徴点が成す形状や特徴点の位置（路面上か否か）に基づいて画像上の白線を検出する。また例えば、画像処理部８は画像の色情報に基づいて白線を検出する。
そして、画像処理部８は画像上の白線の長さ方向（直線部分）が成す角度（車両に対する白線の相対角度）を算出し、ＤＲ演算部３が算出した車両の方位角に画像上の白線の角度を加算して白線の方位角を算出する。もしくは、白線の絶対方位角を直接計算する方法もある。
画像処理部８は、カメラ７の特性に応じて２次元の撮像面に画像として投影された白線について、画像上での白線の角度をカメラ７の特性に応じて実世界の３次元上の角度に変換することにより、実際の白線の相対角、方位角を算出する。
例えば、白線の画像から得られる白線上の複数の点の目視線角の情報と、カメラの道路面からの高さ情報と、道路の形状の情報と、ＤＲ演算部３が算出した車両位置および方位角から計算されるカメラの位置座標と向きの情報を用いて、白線上の複数の点の位置座標を計算することで白線の方位角を計算できる。
ここで、カメラ７は撮像方向を車両の進行方向と同じにするように取り付けられているものとする。但し、カメラ７が車両に設置された際に車両に対してカメラ７の設置角に多少のずれが生じることが考えられるため、画像処理部８は拡張カルマンフィルタ６が推定したカメラアライメント誤差を加算して白線の方位角を算出することも可能である。その場合は、カメラアライメント誤差を推定する場合には車両の左右両方の白線を検出して角度を算出する必要がある。
＜Ｓ２３３：データベース方位角取得処理の一例＞
次に、白線方位角取得部２３は、ＤＲ演算部３が測位した車両の位置および方位角に基づいて、車両の進行方向に位置する白線の方位角を白線情報データベース９から取得する。つまり、白線方位角取得部２３はカメラ７が撮像した画像に映っている白線について白線情報データベース９から方位角を取得する。例えば、白線情報データベース９は東西南北を基準方位として白線の方位角を示している。
＜Ｓ２３４：方位角残差算出処理の一例＞
次に、白線方位角残差計算部２４は画像処理部８が画像に基づいて算出した白線の方位角と白線方位角取得部２３が白線情報データベース９から取得した白線の方位角との差分を白線方位角残差として算出する。
【００３９】
＜Ｓ２４０：誤差推定処理の一例＞
そして、拡張カルマンフィルタ６は擬似距離残差計算部２２が算出した擬似距離残差と白線方位角残差計算部２４が算出した白線方位角残差とを観測量として入力し、ＤＲ演算部３の測位結果に含まれる航法誤差とＤＲ計算に用いられるジャイロ４の誤差とＤＲ計算に用いられるオドメータ５の誤差とＧＰＳ受信機１のＧＰＳクロック誤差とカメラ７のアライメント誤差とを状態量として計算して補正量として出力する。
【００４０】
ＤＲ演算部３は拡張カルマンフィルタ６が出力した航法誤差に基づいて測位した車両の位置および方位角を補正し、補正した車両の位置および方位角を出力する。
また、画像処理部８は拡張カルマンフィルタ６が出力したカメラアライメント誤差に基づいてカメラ７の車両に対する設置角を算出し、次回、算出したカメラ７の設置角に基づいて画像に映る白線の方位角を算出する。
【００４１】
カルマンフィルタは１９６０年にＫａｌｍａｎが提唱したアルゴリズムで、計算により求める物理量（状態量と呼ぶ）のダイナミクス（時間発展、時間変化）を線形モデルで与え、状態量の誤差の確率分布関数をガウス分布であるとした場合に、推定誤差を最小とする解を与えるものである。ここで推定誤差を最小にするとは、状態量に選んだ変数（例えば、航法誤差、カメラアライメント誤差、自己位置・姿勢角）のダイナミクスも考慮して残差の二乗和が最小になるように解を推定することを意味する。
一般的なカルマンフィルタの基本方程式は、離散時間系（ｄｉｓｃｒｅｔｅ ｔｉｍｅ ｓｙｓｔｅｍ）の場合、以下の状態方程式（式１）と観測方程式（式２）とで表される。
【００４２】
【数１】
【００４３】
特に、拡張カルマンフィルタ６（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ，ＥＫＦ）は、系の方程式が非線形の場合に、現在の推定量のまわりで線形化した方程式を作り、それをもとに上記基本方程式を構成したカルマンフィルタである。
すなわち、ｘとｚが、以下の式３、式４のように、それぞれｘｋ−１，ｕｋ，ｗｋの関数ｆおよびｘｋ，ｖｋの関数ｈとして表される場合、Ｊａｃｏｂｉａｎを使って式５、式６のように線形化する。
【００４４】
【数２】
【００４５】
そして、状態方程式（式７）と観測方程式（式８）とは、式５、式６に示す線形化によって得られたＦとＨとを用いて次のように表される。
【００４６】
【数３】
【００４７】
また、カルマンフィルタの演算は時間伝播（Ｔｉｍｅ Ｕｐｄａｔｅ）と観測更新（Ｍｅａｓｕｒｅｍｅｎｔ Ｕｐｄａｔｅ）とからなる。
【００４８】
まず、状態量ｘの誤差共分散行列Ｐの時間伝播は、システムのダイナミクスを表す行列Ｆを用いて以下の式９および式１０で表される。
式９については、例えば、第二項あるいは第三項までを使って近似計算する。
【００４９】
【数４】
【００５０】
また、観測更新は以下の式１１〜式１３で計算される。
【００５１】
【数５】
【００５２】
以上の各式において、実施の形態１では、ＤＲ演算部３の測位した自己位置や方位角に含まれる航法誤差と航法計算に利用するセンサ（ジャイロ４、オドメータ５、ＧＰＳクロックなど）の誤差とカメラ７のアライメント誤差とが状態量ｘに対応し、擬似距離残差と白線方位角残差とが観測残差Δｚに対応する。
拡張カルマンフィルタ６は、以上の各式および理論に基づいて、航法誤差およびカメラアライメント誤差を推定する。
【００５３】
実施の形態１では、以下のような自己位置標定装置１００について説明した。
実施の形態１における自己位置標定装置１００は、ハードウェアとして、既存のカーナビレベルのハードウェア構成（ＧＰＳ受信機１＋１軸ジャイロ４＋車速パルス［オドメータ５］）以外に、単眼カメラ７を備える。
また、実施の形態１における自己位置標定装置１００は、カメラ７の画像から白線の方位角を得る画像処理部８と、道路線形データベース（白線情報データベース９）と、そのデータから白線の直線部分の方位角を計算する方位角算出部（白線方位角取得部２３）と、自己位置・姿勢角を算出するためのＤＲ演算部３と拡張カルマンフィルタ６とを備える。
道路線形データベースは一から構築する必要はなく、すでに実用化段階に入っている特許文献６により構築されたデータベースに対して、白線の直線部分を示すデータに新たに方位角を加えるだけでよいところが特徴である。また、自己位置標定装置１００が使用するカメラ７も単眼でよく低価格のものを利用できる点が特徴である。
ＤＲ演算部３は、慣性装置（１軸ジャイロ４（方位角方向））と、車速パルスデータを利用してデッドレコニング計算を行う。そして、拡張カルマンフィルタ６は、ＧＰＳ観測データと上記白線方位角データとを観測量として自己位置・姿勢角の誤差推定を行ってＤＲ演算部３の自己位置標定精度を向上させる。
つまり、実施の形態１で説明した白線の方位角を自己位置補正に利用する方式では、ハードウェアの構成として単眼のカメラ７を１台追加するだけでよい利点がある。つまり、実施の形態１における自己位置標定装置１００は自己位置から白線までの距離を観測するためのステレオカメラやレーザレーダなどが不要である。また、白線の直線部分の角度（白線の方位角）が単眼カメラ７で撮像した画像から比較的正確に検出でき、一度計測され道路線形データベースに記録された方位角は地球の地殻変動による劣化がほとんどないことも白線の方位角を自己位置補正に利用する利点の一つである。言い換えると、道路の座標のデータベースを正確に作るのは難しく、現在使われているカーナビゲーション用のデジタル地図データの座標精度を考慮すると、座標または座標に基づいて算出される距離を自己位置補正に利用する方式ではデシメートル級の座標精度を達成するのは困難である。
このような事情を考慮すると、白線の方位角データを利用する実施の形態１における方式は有望な自己位置標定手法であると言える。
【００５４】
また、実施の形態１の自己位置標定手法は、白線の直線部のような車両移動方向に対して画像エッジ位置の変化率が小さい特徴量をランドマークとしているため、低価格単眼カメラ＋低価格画像処理器で十分に精度が出せる点が特徴である。
また、実施の形態１では、車載カメラ７の取り付け角誤差（アライメント誤差）も拡張カルマンフィルタ６で推定し、その誤差を補正できる。このため、アライメントの厳しいステレオカメラが用いられる場合でも自己位置標定精度を高めることができる。
【００５５】
実施の形態１に示した自己位置標定方法により、既存のカーナビレベルのハードウェア構成に単眼カメラ７を加え、道路線形データベースとＧＰＳ／ＤＲ／画像複合アルゴリズムを適用するだけで、都市部やトンネル内等における自己位置標定精度を向上させることができる。
【００５６】
実施の形態２．
実施の形態２では、前記実施の形態１において図１に基づいて説明した自己位置標定装置１００の具体例として、方位角残差算出部１３０が、白線の方位角の代わりに、車両から見た道路標識の位置する方向（以下、標識の方位角という）についての残差（以下、標識方位角残差という）を算出する形態について説明する。
また、実施の形態２において、方位角残差算出部１３０は車両から道路標識までの距離（以下、標識距離という）についての残差（以下、標識距離残差という）を算出する。そして、誤差推定部１４０は擬似距離残差と標識方位角残差と標識距離残差とを観測量として入力して航法誤差とカメラアライメント誤差とを算出する。
以下、前記実施の形態１と異なる事項について説明し、説明を省略した事項については前記実施の形態１と同様とする。
【００５７】
図６は、実施の形態２における自己位置標定装置１００の構成図である。
図６において、実施の形態２における自己位置標定装置１００は、前記実施の形態１における自己位置標定装置１００に対して、白線情報データベース９、白線方位角取得部２３および白線方位角残差計算部２４の代わりに標識情報データベース１１、標識方位角・距離算出部３１および標識方位角・距離残差計算部３２を備える点を特徴とする。
また、実施の形態２における自己位置標定装置１００は、画像処理部８が、白線の方位角の代わりに、カメラ７の撮像した画像に映っている道路標識について、車両に対する道路標識の位置する方向を示す標識の方位角と、車両から道路標識までの距離を示す標識距離とを算出することを特徴とする。
【００５８】
標識情報データベース１１（位置データベースの一例）には各道路標識の位置情報が予め記憶されている。例えば、道路標識を含む各種ランドマークの位置情報が登録されカーナビゲーションシステムに一般的に用いられている道路情報データベースを標識情報データベース１１として利用することができる。
【００５９】
標識方位角・距離算出部３１（データベース距離部の一例）は、ＤＲ演算部３が算出した車両の位置および方位角に基づいて、標識情報データベース１１から車両の進行方向に位置する道路標識の位置情報を取得し、車両の進行方向に位置する道路標識について方位角と距離とをＣＰＵを用いて算出する。
【００６０】
標識方位角・距離残差計算部３２（距離残差算出部の一例）は、画像処理部８（画像距離部の一例）が算出した画像に基づく標識の方位角と標識方位角・距離算出部３１が算出した標識情報データベース１１に基づく標識の方位角との差分を標識方位角残差としてＣＰＵを用いて算出する。また、標識方位角・距離残差計算部３２は画像処理部８が算出した画像に基づく標識距離と標識方位角・距離算出部３１が算出した標識情報データベース１１に基づく標識距離との差分を標識距離残差としてＣＰＵを用いて算出する。
【００６１】
図７は、実施の形態２における自己位置標定装置１００の自己位置標定方法２を示すフローチャートである。
図７において、Ｓ３１０は図２の測位処理（Ｓ１１０）に対応し、Ｓ３２１〜Ｓ３２４は図２のＧＰＳ残差算出処理（Ｓ１２０）に対応し、Ｓ３３１〜Ｓ３３４ｂは図２の方位角残差算出処理（Ｓ１３０）に対応し、Ｓ３４０は図２の誤差推定処理（Ｓ１４０）に対応する。
自己位置標定装置１００の各部は以下に説明する各処理をＣＰＵを用いて実行する。
【００６２】
＜Ｓ３１０〜Ｓ３２４＞
測位処理（Ｓ１１０）およびＧＰＳ残差算出処理（Ｓ１２０）に関するＳ３１０〜Ｓ３２４は、前記実施の形態１における自己位置標定方法１（図５参照）のＳ２１０〜Ｓ２２４と同じである。
【００６３】
＜Ｓ３３１：撮像処理の一例＞
ＤＲ演算部３が測位を行った際、カメラ７は車両が走行している道路の進行方向を撮像する。
【００６４】
＜Ｓ３３２ａ：画像方位角算出処理の一例＞
次に、画像処理部８はカメラ７が撮像した画像から画像に映っている道路標識を検出する。例えば、画像処理部８は、画像を二値化して抽出した特徴点が成す形状に基づいて特定形状のマークを表示する道路標識を検出する。また例えば、画像処理部８は画像に対して文字認識処理を行い、認識した文字情報に基づいて特定文字列を表示する道路標識を検出する。
そして、画像処理部８は画像上の道路標識の位置（道路標識の画素位置）に基づいて車両から見た道路標識の位置する方向を示す相対角を算出し、ＤＲ演算部３が算出した車両の方位角に画像上の道路標識の相対角を加算して車両を基点として道路標識が位置する方向を示す標識の方位角を算出する。例えば、画像処理部８は画像の中心から道路標識が位置する画素位置までの水平方向の画素数に１画素が成す角度を乗じて画像に映っている道路標識の車両に対する実際の相対角を算出する。各画素に相当する角度はカメラのパラメータ（例えば、焦点距離）に依存し、カメラのパラメータに基づいて算出される。画像処理部８は、カメラ７の特性に応じて２次元の撮像面に画像として投影された標識について、画像上での標識の位置をカメラ７の特性に応じて実世界の３次元上の位置に変換することにより、実際の標識の方位角および距離を算出する。
＜Ｓ３３２ｂ：画像距離算出処理の一例＞
また、画像処理部８は画像上の道路標識の位置に基づいて車両から道路標識までの標識距離を算出する。
例えば、画像処理部８はカメラ７が連続する異なる時刻に撮像した２枚の画像を用いてモーションステレオ視処理を行い車両から道路標識までの標識距離を算出する。
【００６５】
＜Ｓ３３３ａ＞
次に、標識方位角・距離算出部３１は、ＤＲ演算部３が測位した車両の位置および方位角に基づいて、車両の進行方向に位置する道路標識の位置情報を標識情報データベース１１から取得する。つまり、標識方位角・距離算出部３１はカメラ７が撮像した画像に映っている道路標識について標識情報データベース１１から位置情報を取得する。
＜Ｓ３３３ｂ＞
次に、標識方位角・距離算出部３１は、標識情報データベース１１から取得した道路標識の位置とＤＲ演算部３が算出した車両の位置とに基づいて車両から見て道路標識が位置する相対角を算出し、ＤＲ演算部３が算出した車両の方位角に車両から見た道路標識の相対角を加算して車両を基点とした道路標識の方位角を算出する。
＜Ｓ３３３ｃ＞
また、標識方位角・距離算出部３１は、標識情報データベース１１から取得した道路標識の位置とＤＲ演算部３が算出した車両の位置とに基づいて、車両から道路標識までの標識距離を算出する。
【００６６】
＜Ｓ３３４ａ＞
次に、標識方位角・距離残差計算部３２は画像処理部８が画像に基づいて算出した標識の方位角と標識方位角・距離算出部３１が標識情報データベース１１に基づいて算出した標識の方位角との差分を標識方位角残差として算出する。
＜Ｓ３３４ｂ＞
また、標識方位角・距離残差計算部３２は画像処理部８が画像に基づいて算出した標識距離と標識方位角・距離算出部３１が標識情報データベース１１に基づいて算出した標識距離との差分を標識距離残差として算出する。
【００６７】
＜Ｓ３４０＞
そして、拡張カルマンフィルタ６は擬似距離残差計算部２２が算出した擬似距離残差と標識方位角・距離残差計算部３２が算出した標識方位角残差および標識距離残差とを観測量として入力し、ＤＲ演算部３の測位結果に含まれる航法誤差とカメラ７のアライメント誤差とを状態量として計算して補正量として出力する。
【００６８】
実施の形態２では、以下のような自己位置標定装置１００について説明した。
実施の形態２における自己位置標定装置１００は、ハードウェアとして、既存のカーナビレベルのハードウェア構成（ＧＰＳ受信機１＋１軸ジャイロ４＋車速パルス［オドメータ５］）以外に、単眼カメラ７を備える。
また、実施の形態２における自己位置標定装置１００は、カメラ７の画像から「車両−標識間の距離」および「車両から見た標識の相対方位角」を得る画像処理部８と、標識座標のデータベース（標識情報データベース１１）と、自己位置・姿勢角を算出するためのＤＲ演算部３と拡張カルマンフィルタ６とを備える。
ＤＲ演算部３は、慣性装置（１軸ジャイロ４（方位角方向））と、車速パルスデータを利用してデッドレコニング計算を行う。そして、拡張カルマンフィルタ６は、ＧＰＳ観測データと上記標識データとを観測量として自己位置・姿勢角の誤差推定を行ってＤＲ演算部３の自己位置標定精度を向上させる。
【００６９】
実施の形態３．
実施の形態３では、前記実施の形態１において図１に基づいて説明した自己位置標定装置１００の具体例として、方位角残差算出部１３０が前記実施の形態１で説明した白線方位角残差と前記実施の形態２で説明した標識方位角残差および標識距離残差とを算出する形態について説明する。
また、実施の形態３において、測位部１１０はデッドレコニングにより算出した自己位置を道路データベース４９のデータを利用したマップマッチングにより補正して自己位置を算出する。
また、実施の形態３において、誤差推定部１４０は擬似距離残差と白線方位角残差と標識方位角残差と標識距離残差とを観測量として入力して航法誤差とカメラアライメント誤差とを算出する。
以下、前記実施の形態１および前記実施の形態２と異なる事項について説明し、説明を省略した事項については前記実施の形態１または前記実施の形態２と同様とする。
【００７０】
図８は、実施の形態３における自己位置標定装置１００の構成図である。
図８において、実施の形態３における自己位置標定装置１００は、前記実施の形態１における自己位置標定装置１００に対して、白線情報データベース９、白線方位角取得部２３および白線方位角残差計算部２４の代わりに道路データベース４９、方位角・距離算出部４１および方位角・距離残差算出部４２を備える点を特徴とする。
また、実施の形態３における自己位置標定装置１００は、ＤＲ演算部３が車両の位置する周辺の道路情報を道路データベース４９から取得してマップマッチングにより車両の位置を補正することを特徴とする。
【００７１】
道路データベース４９には前記実施の形態１で説明した白線情報データベース９の白線情報や前記実施の形態２で説明した道路標識情報や道路のレーン情報（例えば、位置、幅、上り下り）などの各種道路情報が予め記憶されている。
【００７２】
方位角・距離算出部４１は、ＤＲ演算部３が算出した車両の位置および方位角に基づいて、道路データベース４９から車両の進行方向に位置する白線の方位角と車両の進行方向に位置する道路標識の位置情報とを取得し、車両の進行方向に位置する道路標識の方位角と車両から道路標識までの標識距離とをＣＰＵを用いて算出する。
【００７３】
方位角・距離残差算出部４２は、画像処理部８が算出した画像に基づく白線の方位角と方位角・距離算出部４１が取得した道路データベース４９に基づく白線の方位角との差分を白線方位角残差としてＣＰＵを用いて算出する。また、方位角・距離残差算出部４２は、画像処理部８が算出した画像に基づく標識の方位角と方位角・距離算出部４１が算出した道路データベース４９に基づく標識の方位角との差分を標識方位角残差としてＣＰＵを用いて算出する。また、方位角・距離残差算出部４２は画像処理部８が算出した画像に基づく標識距離と方位角・距離算出部４１が算出した道路データベース４９に基づく標識距離との差分を標識距離残差としてＣＰＵを用いて算出する。
【００７４】
図９は、実施の形態３における自己位置標定装置１００の自己位置標定方法３を示すフローチャートである。
図９において、Ｓ４１０ａ〜Ｓ４１０ｃは図２の測位処理（Ｓ１１０）に対応し、Ｓ４２１〜Ｓ４２４は図２のＧＰＳ残差算出処理（Ｓ１２０）に対応し、Ｓ４３１〜Ｓ４３４ｃは図２の方位角残差算出処理（Ｓ１３０）に対応し、Ｓ４４０は図２の誤差推定処理（Ｓ１４０）に対応する。
自己位置標定装置１００の各部は以下に説明する各処理をＣＰＵを用いて実行する。
【００７５】
＜Ｓ４１０ａ＞
まず、ＤＲ演算部３はデッドレコニングにより車両の位置および方位角を算出する。
＜Ｓ４１０ｂ＞
次に、ＤＲ演算部３は算出した車両の位置に基づいて周辺の道路情報を道路データベース４９から取得する。
＜Ｓ４１０ｃ：測位処理の一例＞
次に、ＤＲ演算部３は、道路データベース４９から取得した道路情報が示すレーン情報に基づいて、車両の進行方向（方位角）と通行方向が対応するレーン上に車両が位置するように車両の位置を補正するマップマッチング（または、レーンキーピング）を行う。また、ＤＲ演算部３はマップマッチングを行った車両の位置および方位角を拡張カルマンフィルタ６が算出した航法誤差に基づいて補正して車両の位置および方位角を算出する。ＤＲ演算部３はマップマッチング処理としてカーナビゲーションシステムで一般的に行われている処理を用いる。ただし、マップマッチング処理は、走行中の道路が１レーンしかないなど走行レーンが明らかな場合や、拡張カルマンフィルタの状態量誤差標準偏差が適切に設定した閾値を超えた場合などの自己位置標定精度が悪化していると推定される場合のみ行うものとする。
【００７６】
＜Ｓ４２１〜Ｓ４２４＞
ＧＰＳ残差算出処理（Ｓ１２０）に関するＳ４２１〜Ｓ４２４は、前記実施の形態１における自己位置標定方法１（図５参照）のＳ２２１〜Ｓ２２４と同じである。
【００７７】
＜Ｓ４３１〜Ｓ４３４ｃ＞
白線の方位角残差算出処理に関するＳ４３１、Ｓ４３２ａ、Ｓ４３３ａおよびＳ４３４ａは、前記実施の形態１における自己位置標定方法１（図５参照）のＳ２３１〜Ｓ２３４と同じである。
また、道路標識の方位角残差算出処理および標識距離残差算出処理に関するＳ４３２ｂ、Ｓ４３３ｂおよびＳ４３４ｂは、前記実施の形態２における自己位置標定方法２（図７参照）のＳ３３２ａ〜Ｓ３３４ｂと同じである。
【００７８】
そして、拡張カルマンフィルタ６は擬似距離残差計算部２２が算出した擬似距離残差と方位角・距離残差算出部４２が算出した白線方位角残差、標識方位角残差および標識距離残差とを観測量として入力し、ＤＲ演算部３のデッドレコニングによる測位結果に含まれる航法誤差とカメラ７のアライメント誤差とを状態量として計算して補正量として出力する。
【００７９】
上記では、白線について方位角のみを対象としたが、標識と同様に車両から白線までの距離の残差を拡張カルマンフィルタ６の観測量としてもよいし、画像に基づいて算出した白線の長さとデータベースから取得した白線の長さとの残差を拡張カルマンフィルタ６の観測量としてもよい。
【００８０】
実施の形態３では、以下のような自己位置標定装置１００について説明した。
実施の形態３における自己位置標定装置１００は、前記実施の形態１に記載した自己位置標定装置１００に、前記実施の形態２に記載した「車両−標識間の距離」と「車両からみた標識の相対方位角」を得る画像処理部８の機能と、標識座標のデータベースとを加え、拡張カルマンフィルタ６は白線と標識との両方を観測量として自己位置・姿勢角の誤差推定を行ってＤＲ演算部３の自己位置標定精度を向上させる。
【００８１】
実施の形態４．
上記実施の形態１では、各白線について位置や形状や方位角などの情報が予め記憶されている白線情報データベース９を利用する形態について説明した。但し、各白線の情報を用意することがコスト的に難しい場合も考えられる。
そこで、実施の形態４では、上記実施の形態１で示した白線情報データベース９の代わりに道路の方位角を示す道路情報（道路線形情報）を記憶するデータベースを用い、道路の方位角を白線の方位角として用いる形態について説明する。
【００８２】
（１）ＫＩＷＩデータベースを用いる方法
道路情報を記憶するデータベースとして、カーナビゲーションシステムに使用されているデータベースを使用することができる。カーナビゲーションシステムのデータベースにはＫＩＷＩフォーマットが使用されている。以下、道路情報をＫＩＷＩフォーマット（日本工業規格 ＪＩＳ−Ｄ０８１０）で示すデータベースをＫＩＷＩデータベースとする。非特許文献１にＫＩＷＩフォーマットに関する文献を示す。
ＫＩＷＩデータベースのデータ構造には、交差点に関する情報を示すノード情報、道路が接続する交差点を示すリンク情報、新たに情報を追加することが可能な拡張部などが含まれる。ノード情報には各交差点をノードとしてノード名と交差点の座標（例えば、交差点中央の２次元座標）とが対応付けて記憶されている。リンク情報には“ノードＡ（交差点Ａ）とノードＢ（交差点Ｂ）が接続されている（Ａ、Ｂ）”というような道路が接続している交差点を示す情報が記憶されている。そして、実施の形態４では、ＫＩＷＩデータベースの拡張部に、リンク情報（接続情報）と方位角とを対応付けて、リンク情報が表す道路について方位角を示す方位角情報を記憶する。例えば、ノード情報とリンク情報とが図１１のような関係を示す場合、拡張部には「接続情報」として（Ａ、Ｂ）、「方位角」としてθ１が記憶され、また、「接続情報」として（Ｂ、Ｃ）、「方位角」としてθ２が記憶される。
【００８３】
そして、上記実施の形態１における白線方位角取得部２３は、図１２に示すようにして、ＫＩＷＩデータベースから道路情報を取得し、道路の方位角を白線の方位角として算出する。
【００８４】
＜Ｓ５１０＞
白線方位角取得部２３は、ＤＲ演算部３から自己位置Ｎ、自己方位角φＮを取得する。
＜Ｓ５２０＞
次に、白線方位角取得部２３はＫＩＷＩデータベースにおいてノード情報が示す各交差点の座標と自己位置Ｎとを比較して車両が位置する道路を示すリンク情報を検索する。
＜Ｓ５３０＞
次に、白線方位角取得部２３は検索したリンク情報に対応する道路の方位角θｎを拡張部から抽出する。
＜Ｓ５４０＞
次に、白線方位角取得部２３は自己方位角φＮが示す進行方向における道路の方位角θｎを取得する。ただし、白線を見る向きによって方位角の表現は１８０度異なるため、自己方位角φＮに応じて、θｎに１８０度だけ加算することがある。
＜Ｓ５５０＞
そして、白線方位角取得部２３は取得した道路の方位角θｎを白線方位角として出力する。
【００８５】
（２）道路線形データベースを用いる方法
道路の幾何構造は横断面と平面線形とで表すことができるが、実施の形態４で用いる道路線形データベースは道路の平面線形について直線や円曲線や緩和曲線（クロソイド）などの線形要素で表した道路情報を記憶する。道路の直線は例えば始点の座標と終点の座標とで表される。
そして、実施の形態４における白線方位角取得部２３は、図１３に示すようにして、道路線形データベースに記憶された道路の直線部分のデータに基づいて、道路の方位角を白線方位角として算出する。
【００８６】
＜Ｓ６１０＞
白線方位角取得部２３は、ＤＲ演算部３から自己位置Ｎを取得する。
＜Ｓ６２０＞
次に、白線方位角取得部２３は道路線形データベースにおいて自己位置Ｎが位置する道路の直線部分のデータを検索する。
＜Ｓ６３０＞
次に、白線方位角取得部２３は検索した道路の直線部分のデータが示す始点と終点の座標に基づいて道路の方位角を算出する。但し、道路の方位角を予め道路線形データベースに記憶しておいても構わない。
＜Ｓ６４０＞
そして、白線方位角取得部２３は算出した道路の方位角を白線方位角として出力する。
【００８７】
各実施の形態において、ＧＰＳ受信機１がＭＳＡＳ（ＭＴＳＡＴ Ｓａｔｅｌｌｉｔｅ−ｂａｓｅｄ Ａｕｇｍｅｎｔａｔｉｏｎ Ｓｙｓｔｅｍ）からのＧＰＳ補正情報やＦＭ多重放送を受信することなどで得られるＧＰＳ補正情報を用いて擬似距離の補正をする方式も考えられる。また、将来的には準天頂衛星からの放送データの中にＧＰＳ補正情報が含まれる予定であり、ＧＰＳ受信機１がこれを利用してＧＰＳの擬似距離補正を行う方法も考えられる。
また、ＧＰＳ受信機１が擬似距離を搬送波位相観測データを使ってスムージングする方法（キャリアスムージング）や、ＤＲ演算部３がＧＰＳ基準局の搬送波位相データとＧＰＳ受信機１で観測するＧＰＳ搬送波位相データとを用いてＣＰＤＧＰＳ（Ｃａｒｒｉｅｒ Ｐｈａｓｅ Ｄｉｆｆｅｒｅｎｔｉａｌ ＧＰＳ）計算を行い測位する方法なども考えられる。
さらに、拡張カルマンフィルタ６の構成方法には、前記実施の形態１で説明したＧＰＳの観測生データ（擬似距離、位相差など）をＧＰＳ観測量として利用するタイトカプリング（Ｔｉｇｈｔｌｙ Ｃｏｕｐｌｅｄ、密結合）方式の他に、ＧＰＳ観測値から得た測位解をＧＰＳ観測量として利用するルーズカプリング（Ｌｏｏｓｅｌｙ Ｃｏｕｐｌｅｄ、粗結合）方式も考えられる。
【図面の簡単な説明】
【００８８】
【図１】実施の形態１における自己位置標定装置１００の構成図。
【図２】実施の形態１における自己位置標定装置１００の自己位置標定方法を示すフローチャート。
【図３】実施の形態１における自己位置標定装置１００のハードウェア資源の一例を示す図。
【図４】実施の形態１における自己位置標定装置１００の構成図。
【図５】実施の形態１における自己位置標定装置１００の自己位置標定方法ＧＰＳ受信機１を示すフローチャート。
【図６】実施の形態２における自己位置標定装置１００の構成図。
【図７】実施の形態２における自己位置標定装置１００の自己位置標定方法２を示すフローチャート。
【図８】実施の形態３における自己位置標定装置１００の構成図。
【図９】実施の形態３における自己位置標定装置１００の自己位置標定方法を示すフローチャート。
【図１０】実施の形態１における白線と白線（の直線部分）の方位角とレーンの通行方向との関係図。
【図１１】実施の形態４におけるＫＩＷＩデータの関係図。
【図１２】実施の形態４における方位角取得処理のフローチャート。
【図１３】実施の形態４における方位角取得処理のフローチャート。
【符号の説明】
【００８９】
１ ＧＰＳ受信機、２ 衛星軌道演算部、３ ＤＲ演算部、４ ジャイロ、５ オドメータ、６ 拡張カルマンフィルタ、７ カメラ、８ 画像処理部、９ 白線情報データベース、１１ 標識情報データベース、２１ 衛星距離計算部、２２ 擬似距離残差計算部、２３ 白線方位角取得部、２４ 白線方位角残差計算部、３１ 標識方位角・距離算出部、３２ 標識方位角・距離残差計算部、４１ 方位角・距離算出部、４２ 方位角・距離残差算出部、４９ 道路データベース、１００ 自己位置標定装置、１１０ 測位部、１２０ ＧＰＳ観測残差算出部、１２１ ＧＰＳ観測部、１２２ ＧＰＳ観測量演算部、１３０ 方位角残差算出部、１３１ 方位角観測部、１３２ 方位角演算部、１４０ 誤差推定部、９０１ 表示装置、９１１ ＣＰＵ、９１２ バス、９１３ ＲＯＭ、９１４ ＲＡＭ、９１５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群。
【技術分野】
【０００１】
本発明は、例えば、ＧＩＳ（Ｇｅｏｇｒａｐｈｉｃ Ｉｎｆｏｒｍａｔｉｏｎ Ｓｙｓｔｅｍ）データベースを作成するために地理データ収集車両に搭載したり、一般車両に搭載してナビゲーションシステムに利用したりする自己位置標定装置、自己位置標定方法および自己位置標定プログラムに関するものである。
【背景技術】
【０００２】
ＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）の受信機の価格が低下すると共にカーナビゲーションシステム（以下、カーナビ、カーナビゲーションともいう）が普及し、カーナビゲーションシステムの自己位置標定精度を向上するための技術やドライバーの運転補助のための技術などさまざまな技術が開発されてきた。例えば、自己位置標定精度を向上させる技術として、ＧＰＳの観測情報の一つである擬似距離に対して補正情報を反映して測位精度を向上させるディッファレンシャルＧＰＳ（ＤＧＰＳ）や、地図情報を使って自己位置を補正するマップマッチング技術や、白線を認識して認識した白線を基準にして行うレーンキーピングなどの技術が挙げられる。
【０００３】
国土交通省は、１９９１年からＡＳＶ（Ａｄｖａｎｃｅｄ Ｓａｆｅｔｙ Ｖｅｈｉｃｌｅ：先進安全自動車）の開発を主導し、現在は第３期の計画を進行中であり、車車間通信を使った事故回避システムの実用化を目指している。そのような高度なシステムには、現在よりも精度の高い自己位置標定装置が必要とされているが、一般の乗用車に搭載可能な装置ではＧＰＳのアベイラビリティ（Ａｖａｉｌａｂｉｌｉｔｙ，可用性）が悪い状況などにおいて十分な位置標定精度が保てないのが現状である。
【０００４】
従来のカーナビ用途などの車両用自己位置標定装置では、ＧＰＳ、慣性装置、車速パルスなどを利用するものが開発されている。この自己位置標定装置の標定精度を高めるため、道路上の通行区分帯表示用の断続線あるいは連続線（通常“白線”であるので、以下便宜上“白線”と呼ぶ）や道路標識、信号機、建物などのランドマークを利用して自己位置標定精度を高める手法が提案されてきた。
【０００５】
例えば、白線を利用する方法として、車両に搭載したビデオカメラで白線を撮影し、その画像を処理した観測値から車両と白線との間の距離を計算し、車両と白線との距離を自己位置補正に利用する方式（特許文献１）がある。
また、建物などのランドマークを利用する方式として、車両に搭載したレーザレーダ等の測距型センサによる観測値と建造物データベースに登録されている値との照合結果に基づく位置補正方式（特許文献２）などが提案されている。さらには、車両の周囲にある建物などのランドマークの並び方や位置関係から自己位置を同定する方式も考案されている（特許文献３）。
また、道路標識や信号を利用する方式として、車両に搭載したカメラによって標識を認識し、二つのカメラまたはレーダを用いて標識までの距離を検出し、データベースから得たその標識の座標と、検出した距離とを使って自己位置を補正する手法（特許文献４）がある。さらには、信号機認識装置と、信号機への方向の仰角を計算する手段を備え、信号機までの水平方向距離を計算し、信号機までの水平方向距離を自己位置補正に利用する方式（特許文献５）などが提案されている。
【０００６】
ところが、車載カメラによって認識した白線までの距離を自己位置補正に使う従来方式は、カーナビゲーションシステム等の自己位置標定装置用の地図データベースの精度が十分なものとはいえないため、補正による効果は非常に限定的である。現状、カーナビゲーションに使われているような地図データベースの精度は、車両が道路上に位置する程度の位置補正を行うレーンキーピングに対しては有効であるが、従来方式によりサブメートル級の標定精度を達成するには十分な精度ではない。地図データベースの精度は今後向上すると考えられるが、地球の地殻変動によって１年に数センチメートル程度の絶対座標変動が起きるのが普通であり、地図データベースにおいて確保できる位置座標精度には限界がある。
また、従来方式のうち特許文献２には、専用の建物データベース構築が必要となり、また、高速で高精度なレーザレーダが必要となるといった課題がある。
【０００７】
一方、このような状況において、簡便な方法で道路や建造物等のＧＩＳデータを作成する方法が開発されつつある。例えば、白線のデータベースを構築する方法（特許文献６）などは、すでに実用化段階に入っている。
【特許文献１】第２６８７６４５号公報
【特許文献２】特開２００６−１３８８３４号公報
【特許文献３】特開平８−２４７７７５号公報
【特許文献４】特開２０００−９７７１４号公報
【特許文献５】特開平７−３５５６０号公報
【特許文献６】特開２００６−４７２９１号公報
【非特許文献１】藤本、“カーナビ用地図データフォーマット ＫＩＷＩ”、デンソーテクニカルレビュー ＶＯＬ．６ Ｎｏ．１、２００１
【発明の開示】
【発明が解決しようとする課題】
【０００８】
カーナビなどに代表される車両用の自己位置標定装置は、ＧＰＳ、慣性装置、車速パルスなどを利用するものが標準的である。このような自己位置標定装置では、都市部やトンネル内などのＧＰＳ測位信号のアベイラビリティが悪い環境では標定精度が劣化するという課題があった。そのため、ＧＰＳ測位信号のアベイラビリティが悪い環境でも一定の測位精度を維持するためには、他に何らかの補助手段等が必要である。
【０００９】
そこで、本発明は、道路上の白線や、道路標識といった既存の道路インフラを利用して、自己位置標定装置の標定精度を向上させることを目的とする。
【課題を解決するための手段】
【００１０】
本発明の自己位置標定装置は、自己位置を測位する測位部と、前記測位部が測位した際の特定の方向を撮像するカメラと、前記カメラが撮像した画像に基づいて前記画像に映っている特定の地物の長さ方向が基準方向に対して成す前記特定の地物の方位角をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角部と、地物の長さ方向が基準方向に対して成す当該地物の方位角を記憶する方位角データベースと、前記測位部が測位した自己位置に基づいて前記方位角データベースから前記特定の地物に対応する地物の方位角を取得するデータベース方位角部と、前記画像方位角部が算出した地物の方位角と前記データベース方位角部が取得した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出部と、前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定するカルマンフィルタとを備え、前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とすることを特徴とする。
【発明の効果】
【００１１】
本発明によれば、例えば、画像方位角部が画像に映った白線の方位角を算出し、カルマンフィルタが方位角データベースに記憶されている白線の方位角と画像から算出した白線の方位角との方位角残差を観測量として誤差推定を行うことにより、自己位置標定精度を向上させることができる。
【００１２】
従来方式は、白線の方位角データを自己位置補正に利用していなかったり（特許文献１）、道路標識が存在する位置の（車両から見た）方位角データを自己位置補正に利用していなかったり（特許文献４）、自己位置標定装置としては改良の余地があった。
そこで、本発明は、方位角が地球の地殻変動による変化が少なく、方位角が絶対位置座標よりも比較的精度良く検出可能であるという点に着目し、白線の方位角や道路標識の方位角を自己位置補正に利用することにより自己位置標定精度を向上させる。
【発明を実施するための最良の形態】
【００１３】
実施の形態１．
図１は、実施の形態１における自己位置標定装置１００の構成図である。
図２は、実施の形態１における自己位置標定装置１００の自己位置標定方法を示すフローチャートである。
実施の形態１における自己位置標定装置１００の構成の概要および自己位置標定装置１００の実行する自己位置標定方法の概要について、図１および図２に基づいて以下に説明する。
自己位置標定装置１００の各部は図２に基づいて説明する各処理をＣＰＵを用いて実行する。
【００１４】
図１において、自己位置標定装置１００は車両（移動体の一例）に搭載され、任意の測位方式によって測位した測位部１１０の測位結果に含まれる誤差を誤差推定部１４０を用いて推定し、推定した誤差により測位部１１０の測位結果を補正して車両の位置や姿勢角を高精度に標定する。
【００１５】
＜Ｓ１１０：測位処理＞
図２において、測位部１１０は車両の方位角データと速度データを用いたデッドレコニング計算や、３軸の加速度計および３軸のジャイロを利用するストラップダウン演算によって、車両の位置および姿勢角を算出する。デッドレコニング計算に使われる方位角データはジャイロから、速度データは車速パルスから取得することが多い。
【００１６】
＜Ｓ１２０：ＧＰＳ観測残差算出処理＞
次に、ＧＰＳ観測残差算出部１２０は、擬似距離や位相差やドップラー量などのＧＰＳ観測量について、ＧＰＳ観測部１２１の観測値とＧＰＳ観測量演算部１２２の演算値との差分をＧＰＳ観測残差として算出する。
このとき、ＧＰＳ観測部１２１はＧＰＳ受信機を用いて測位信号を受信してＧＰＳ観測量を取得し、ＧＰＳ観測量演算部１２２はＧＰＳ観測部１２１のＧＰＳ観測量に相当する量を演算により算出する。そして、ＧＰＳ観測残差算出部１２０はＧＰＳ観測部１２１の観測値とＧＰＳ観測量演算部１２２の演算値との差分をＧＰＳ観測残差として誤差推定部１４０の入力にする。ＧＰＳ観測部１２１によるＧＰＳ観測量には、例えば、ＧＰＳ衛星が測位信号を発信してからＧＰＳ受信機が測位信号を受信するまでの時間に光速を乗じることにより算出されるＧＰＳ衛星とＧＰＳ受信機との間の擬似距離がある。また例えば、特定のＧＰＳ衛星からの測位信号（搬送波）を複数のＧＰＳ受信機で受信した際のＧＰＳ受信機間での受信測位信号の位相差（一重位相差）やＧＰＳ受信機間の一重位相差を複数のＧＰＳ衛星について得た際の一重位相差同士の差を示す二重位相差もＧＰＳ観測量の一例である。また例えば、受信した測位信号の周波数と測位信号の基準周波数との差に基づいて算出されるＧＰＳ衛星とＧＰＳ受信機との相対速度を示すドップラー量もＧＰＳ観測量の一例である。ＧＰＳ観測量演算部１２２は、例えば、測位部１１０が測位した車両の位置と測位信号に含まれる衛星軌道パラメータが示すＧＰＳ衛星の位置とに基づいて擬似距離に相当する距離を算出する。また例えば、ＧＰＳ観測量演算部１２２は各ＧＰＳ受信機から各ＧＰＳ衛星への方向を示すＬＯＳ（Ｌｉｎｅ Ｏｆ Ｓｉｇｈｔ）ベクトルとあるＧＰＳ受信機から別のＧＰＳ受信機への方向を示す基線ベクトルとに基づいて位相差や二重位相差を算出する。また例えば、ＧＰＳ観測量演算部１２２はＧＰＳ衛星の位置・速度と車両の位置・速度とに基づいてドップラー量を算出する。そして、ＧＰＳ観測残差算出部１２０はＧＰＳ観測部１２１のＧＰＳ観測値とＧＰＳ観測量演算部１２２のＧＰＳ観測値に相当する演算値との差分をＧＰＳ観測残差として算出する。
【００１７】
＜Ｓ１３０：方位角残差算出処理＞
次に、方位角残差算出部１３０は、車両が走行している道路における白線の直線部分の方位角や車両から見た標識の方位角について、方位角観測部１３１の観測値と方位角演算部１３２の演算値との差分を方位角残差として算出する。
このとき、方位角観測部１３１はカメラを用いて車両の進行方向を撮像して白線の直線部分の方位角や車両から見た標識の方位角の観測値を取得し、方位角演算部１３２はデータベースに登録されているデータに基づいて白線の方位角や標識の方位角の演算値を取得する。そして、方位角残差算出部１３０は方位角観測部１３１の観測値と方位角演算部１３２の演算値との差分を方位角残差として誤差推定部１４０の入力にする。
【００１８】
＜Ｓ１４０：誤差推定処理・補正処理＞
そして、誤差推定部１４０は、ＧＰＳ観測残差と方位角残差とに基づいて、測位部１１０の測位結果に含まれる誤差を推定する。
測位部１１０は誤差推定部１４０が推定した誤差に基づいて位置および姿勢角を補正する。
このとき、誤差推定部１４０はカルマンフィルタを用いて位置および姿勢角の誤差推定を行う。
このカルマンフィルタは、状態量のダイナミクスをモデル化した状態方程式と、観測量と状態量との関係を定式化した観測方程式とに基づいて状態量の誤差推定を行う。自己位置を標定するような場合、カルマンフィルタは、ＧＰＳ受信機による特定の観測量の観測値と特定の観測値に相当する演算値（理論値）との差分（観測残差という）を観測量として、自己位置の座標や姿勢角（方位角、仰角、回転角）などの状態量について誤差推定を行う。カルマンフィルタは自己位置の座標や姿勢角の誤差を状態量として算出する。
【００１９】
しかし、自己位置標定装置１００を搭載した車両がトンネル内やビルの間を移動する際にＧＰＳ衛星から測位信号を受信できないというような場合、誤差推定部１４０（カルマンフィルタ）が誤差推定を行うことができず自己位置標定装置１００の標定精度は悪化する。
そこで、実施の形態１における誤差推定部１４０（カルマンフィルタ）は、測位信号が受信できない場合でも画像に基づいて観測可能な白線や標識に関する方位角残差を観測量として、測位部１１０の測位結果について誤差推定を行う。
【００２０】
ここで、ＧＰＳ観測量以外の観測量として車両と白線との距離や車両と標識との距離を用いる方法も考えられるが、距離を観測量として用いる方法は、距離の観測値を得るためにステレオカメラやレーザレーダが必要になったり、地殻変動によるデータベースの精度の低下によりデータベースに登録された白線や標識の位置に基づいて算出する距離の演算値の精度が低くなったりするという課題がある。
【００２１】
一方、実施の形態１のようにＧＰＳ観測量以外の観測量として方位角を用いた場合、単眼カメラが一台あればそのカメラで撮像した画像から方位角の観測値を得ることができ、方位角は地殻変動による変化が少ないためデータベースに記憶された白線や標識の位置に基づく方位角の演算値の精度が比較的高いという利点がある。
【００２２】
また、誤差推定部１４０（カルマンフィルタ）は測位部１１０の測位結果の誤差以外に、測位部１１０が用いるジャイロについてのレート誤差やＧＰＳ観測残差算出部１２０が用いるＧＰＳ受信機についてのクロック誤差や方位角残差算出部１３０が用いるカメラについてのアライメント誤差などについても誤差推定を行い、推定した誤差を補正用に出力する。
【００２３】
図３は、実施の形態１における自己位置標定装置１００のハードウェア資源の一例を示す図である。
図３において、自己位置標定装置１００は、プログラムを実行するＣＰＵ９１１（Ｃｅｎｔｒａｌ・Ｐｒｏｃｅｓｓｉｎｇ・Ｕｎｉｔ、中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、プロセッサともいう）を備えている。ＣＰＵ９１１は、バス９１２を介してＲＯＭ９１３、ＲＡＭ９１４、通信ボード９１５、表示装置９０１、ＧＰＳ受信機１、ジャイロ４、オドメータ５、カメラ７、磁気ディスク装置９２０と接続され、これらのハードウェアデバイスを制御する。磁気ディスク装置９２０の代わりに、光ディスク装置、メモリカード読み書き装置などの記憶装置でもよい。
ＲＡＭ９１４は、揮発性メモリの一例である。ＲＯＭ９１３、磁気ディスク装置９２０の記憶媒体は、不揮発性メモリの一例である。これらは、記憶機器、記憶装置あるいは記憶部の一例である。また、入力データが記憶されている記憶機器は入力機器、入力装置あるいは入力部の一例であり、出力データが記憶される記憶機器は出力機器、出力装置あるいは出力部の一例である。
通信ボード９１５は入出力機器、入出力装置あるいは入出力部の一例である。
【００２４】
通信ボード９１５は、無線または有線により、ＬＡＮ（ローカルエリアネットワーク）、インターネット、ＷＡＮ（ワイドエリアネットワーク）、電話回線などの通信網に接続されている。
【００２５】
ジャイロ４は自己位置標定装置１００の搭載された車両の方位角、仰角、回転角の角加速度（以下、レートとする）を計測するセンサである。また、ジャイロ４は後述するように方位角のレートのみ計測する１軸ジャイロであってもよい。
オドメータ５は自己位置標定装置１００の搭載された車両の車速に応じてパルス（以下、車速または車速パルスとする）を出力する車速センサである。
カメラ７は自己位置標定装置１００の搭載された車両の前方に向けて設置され、車両の進行方向に位置する白線や道路標識を撮像する。カメラ７は単眼カメラで構わない。但し、ステレオカメラであっても構わない。また、カメラ７以外に、白線や標識などの地物との距離を計測するためにレーザレーダを備えても構わない。
ＧＰＳ受信機１はＧＰＳ衛星から測位信号を受信し、ＧＰＳ衛星が測位信号を発信してから測位信号を受信するまでの時間に基づいて擬似距離を算出したり、擬似距離や受信した測位信号の位相に基づいて自己位置を算出したりする。ＧＰＳ受信機１は車両には１台備える。ＧＰＳ観測量として二重位相差を観測する場合、位置座標が既知である車両外の地点にもう一台のＧＰＳ受信機を備えておき、そのＧＰＳ受信機の観測データを車両に送信して利用する場合もある。
【００２６】
磁気ディスク装置９２０には、ＯＳ９２１（オペレーティングシステム）、プログラム群９２３、ファイル群９２４が記憶されている。プログラム群９２３のプログラムは、ＣＰＵ９１１、ＯＳ９２１により実行される。
【００２７】
上記プログラム群９２３には、実施の形態において「〜部」として説明する機能を実行するプログラムが記憶されている。プログラムは、ＣＰＵ９１１により読み出され実行される。
【００２８】
ファイル群９２４には、実施の形態において、「〜部」の機能を実行した際の「〜の判定結果」、「〜の計算結果」、「〜の処理結果」などの結果データ、「〜部」の機能を実行するプログラム間で受け渡しするデータ、その他の情報やデータや信号値や変数値やパラメータが、「〜ファイル」や「〜データベース」の各項目として記憶されている。測位部１１０が測位した自己位置・姿勢角、カメラ７が撮像した画像、方位角残差算出部１３０が方位角の取得に用いる白線情報や標識情報、ＧＰＳ観測残差算出部１２０が観測したＧＰＳ観測量やＧＰＳ観測残差算出部１２０が算出したＧＰＳ観測残差、方位角残差算出部１３０が画像から得た白線・標識の方位角や方位角残差算出部１３０が算出した方位角残差などはファイル群９２４に含まれるものの一例である。
「〜ファイル」や「〜データベース」は、ディスクやメモリなどの記録媒体に記憶される。ディスクやメモリなどの記憶媒体に記憶された情報やデータや信号値や変数値やパラメータは、読み書き回路を介してＣＰＵ９１１によりメインメモリやキャッシュメモリに読み出され、抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示などのＣＰＵの動作に用いられる。抽出・検索・参照・比較・演算・計算・処理・出力・印刷・表示のＣＰＵの動作の間、情報やデータや信号値や変数値やパラメータは、メインメモリやキャッシュメモリやバッファメモリに一時的に記憶される。
また、実施の形態において説明するフローチャートの矢印の部分は主としてデータや信号の入出力を示し、データや信号値は、ＲＡＭ９１４のメモリ、ＦＤＤ９０４のフレキシブルディスク、ＣＤＤ９０５のコンパクトディスク、磁気ディスク装置９２０の磁気ディスク、その他光ディスク、ミニディスク、ＤＶＤ（Ｄｉｇｉｔａｌ・Ｖｅｒｓａｔｉｌｅ・Ｄｉｓｃ）等の記録媒体に記録される。また、データや信号値は、バス９１２や信号線やケーブルその他の伝送媒体によりオンライン伝送される。
【００２９】
また、実施の形態において「〜部」として説明するものは、「〜回路」、「〜装置」、「〜機器」であってもよく、また、「〜ステップ」、「〜手順」、「〜処理」であってもよい。すなわち、「〜部」として説明するものは、ＲＯＭ９１３に記憶されたファームウェアで実現されていても構わない。或いは、ソフトウェアのみ、或いは、素子・デバイス・基板・配線などのハードウェアのみ、或いは、ソフトウェアとハードウェアとの組み合わせ、さらには、ファームウェアとの組み合わせで実施されても構わない。ファームウェアとソフトウェアは、プログラムとして、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ等の記録媒体に記憶される。プログラムはＣＰＵ９１１により読み出され、ＣＰＵ９１１により実行される。すなわち、自己位置標定プログラムは、「〜部」としてコンピュータを機能させるものである。あるいは、「〜部」の手順や方法をコンピュータに実行させるものである。
【００３０】
図４は、実施の形態１における自己位置標定装置１００の構成図である。
図１に基づいて説明した自己位置標定装置１００の具体例として、測位部１１０がジャイロ４やオドメータ５を用いたデッドレコニング（Ｄｅａｄ Ｒｅｃｋｏｎｉｎｇ、推測航法）により測位を行い、ＧＰＳ観測残差算出部１２０がＧＰＳ観測量として擬似距離を用いて擬似距離残差を算出し、方位角残差算出部１３０が白線の方位角残差を算出し、誤差推定部１４０が擬似距離残差と白線方位角残差とを観測量として入力し、測位部１１０の測位結果に含まれる航法誤差と方位角残差算出部１３０が白線の撮像に用いたカメラ７のアライメント誤差とを状態量として計算して補正量として出力する構成について、図４に基づいて以下に説明する。
【００３１】
図４において、自己位置標定装置１００はＤＲ演算部３、ジャイロ４およびオドメータ５を測位部１１０として備える。
ジャイロ４は、自己位置標定装置１００が搭載された車両の方位角のレートを計測する１軸ジャイロである。
オドメータ５は、自己位置標定装置１００が搭載された車両の車速を出力する。
ＤＲ演算部３は、ジャイロ４が出力した方位角レートとオドメータ５が出力した車速とに基づいて車両の移動量を推測し、推測した移動量を前回の測位結果に反映して車両の位置や方位角を算出するデッドレコニングをＣＰＵを用いて実行する。
【００３２】
また、自己位置標定装置１００はＧＰＳ受信機１、衛星軌道演算部２、衛星距離計算部２１および擬似距離残差計算部２２をＧＰＳ観測残差算出部１２０として備え、ＧＰＳ受信機１および衛星軌道演算部２はＧＰＳ観測部１２１を構成し、衛星距離計算部２１はＧＰＳ観測量演算部１２２を構成する。
ＧＰＳ受信機１は、ＧＰＳ衛星が発信した測位信号を受信し、ＧＰＳ衛星が測位信号を発信してから測位信号を受信するまでの時間に基づいて車両とＧＰＳ衛星との擬似距離を算出する。また、ＧＰＳ受信機１は受信した測位信号に含まれるＧＰＳ衛星の軌道を示す衛星軌道パラメータを出力する。
衛星軌道演算部２は、ＧＰＳ受信機１が出力した衛星軌道パラメータに基づいてＧＰＳ衛星の位置をＣＰＵを用いて算出する。
衛星距離計算部２１は、ＤＲ演算部３が算出した車両の位置と衛星軌道演算部２が算出したＧＰＳ衛星の位置とに基づいて車両とＧＰＳ衛星との距離を衛星距離としてＣＰＵを用いて算出する。
擬似距離残差計算部２２は、ＧＰＳ受信機１が算出した擬似距離と衛星距離計算部２１が算出した衛星距離との差分を擬似距離残差としてＣＰＵを用いて算出する。
【００３３】
また、自己位置標定装置１００はカメラ７、画像処理部８、白線情報データベース９、白線方位角取得部２３および白線方位角残差計算部２４を方位角残差算出部１３０として備え、カメラ７および画像処理部８は方位角観測部１３１を構成し、白線情報データベース９および白線方位角取得部２３は方位角演算部１３２を構成する。
カメラ７は車両に対して前方に向けて設置された単眼カメラであり、車両が走行している道路の進行方向を撮像する。
画像処理部８（画像方位角部の一例）は、カメラ７が撮像した画像に映っている白線が成す角度とＤＲ演算部３が算出した車両の方位角とに基づいて画像に映っている白線の方位角を算出する。
白線情報データベース９（方位角データベース、位置データベースの一例）には各白線について位置（座標、レーンなど）や形状（直線、カーブなど）や方位角などの情報が予め記憶されている。例えば、方位角の情報は白線が位置するレーンの通行方向に応じて白線の上流側を基点として白線の下流側（進行先）が向いている方位を示す。図１０に、白線と白線（の直線部分）の方位角とレーンの通行方向との関係を示す。白線情報データベース９は、例えば、特許文献６を利用することにより構築できる。特許文献６によって作成されるデータベースは、平面線形については直線／円曲線／クロソイドの３種類、縦断曲線については直線／２次曲線の２種類によって構成される。
白線方位角取得部２３（データベース方位角部）は、ＤＲ演算部３が算出した車両の位置と方位角とに基づいて白線情報データベース９から車両の進行方向に位置する白線の方位角を取得する。
白線方位角残差計算部２４は、画像処理部８が出力した画像に基づく白線の方位角と白線方位角取得部２３が出力した白線情報データベース９に基づく白線の方位角との差分を白線方位角残差としてＣＰＵを用いて算出する。
【００３４】
また、自己位置標定装置１００は拡張カルマンフィルタ６を誤差推定部１４０として備える。
拡張カルマンフィルタ６は、擬似距離残差計算部２２が算出した擬似距離残差と白線方位角残差計算部２４が算出した白線方位角残差とを観測量として入力し、ＤＲ演算部３のデッドレコニングによる測位結果に含まれる航法誤差とカメラ７のアライメント誤差とを状態量として計算して補正量として出力する。
【００３５】
図５は、実施の形態１における自己位置標定装置１００の自己位置標定方法１を示すフローチャートである。
図４に示した自己位置標定装置１００の自己位置標定方法１について、図５に基づいて以下に説明する。
図５において、Ｓ２１０は図２の測位処理（Ｓ１１０）に対応し、Ｓ２２１〜Ｓ２２４は図２のＧＰＳ残差算出処理（Ｓ１２０）に対応し、Ｓ２３１〜Ｓ２３４は図２の方位角残差算出処理（Ｓ１３０）に対応し、Ｓ２４０は図２の誤差推定処理（Ｓ１４０）に対応する。
自己位置標定装置１００の各部は以下に説明する各処理をＣＰＵを用いて実行する。
【００３６】
＜Ｓ２１０：測位処理の一例＞
まず、ＤＲ演算部３はジャイロ４から車両の方位角のレートを、オドメータ５から車両の速度を随時入力する。そして、ＤＲ演算部３は方位角のレートと車両の速度とを積分して前回測位時からの移動量を算出し、前回測位した車両の位置および方位角に前回測位時からの移動量を加算して現在の車両の位置および方位角を算出するデッドレコニング処理（または、慣性航法処理）を行う。例えば、ＤＲ演算部３は東西南北を基準方位として車両の方位角を算出する。
また、ＤＲ演算部３は算出した車両の位置および方位角を拡張カルマンフィルタ６が推定した航法誤差に基づいて補正し、補正した値を車両の位置および方位角として出力する。
【００３７】
＜Ｓ２２１＞
また、ＧＰＳ受信機１は、ＤＲ演算部３が測位を行った際にＧＰＳ衛星から受信した測位信号に基づいて、ＤＲ演算部３が測位を行った際のＧＰＳ受信機１（車両）からＧＰＳ衛星までの擬似距離と、測位信号が示す衛星軌道パラメータとを出力する。
＜Ｓ２２２＞
次に、衛星軌道演算部２はＧＰＳ受信機１の出力した衛星軌道パラメータに基づいてＧＰＳ衛星の位置を算出する。
＜Ｓ２２３＞
次に、衛星距離計算部２１はＤＲ演算部３が測位した車両の位置と衛星軌道演算部２が算出したＧＰＳ衛星の位置とに基づいて車両からＧＰＳ衛星までの距離を衛星距離として算出する。
＜Ｓ２２４＞
次に、擬似距離残差計算部２２はＧＰＳ受信機１が算出した擬似距離と衛星距離計算部２１が算出した衛星距離との差分を擬似距離残差として算出する。
【００３８】
＜Ｓ２３１：撮像処理の一例＞
また、ＤＲ演算部３が測位を行った際、カメラ７は車両の走行している道路の進行方向を撮像する。
＜Ｓ２３２：画像方位角算出処理の一例＞
次に、画像処理部８はカメラ７が撮像した画像から画像に映っている道路上の白線を検出する。例えば、画像処理部８は画像を二値化してエッジ部分などの特徴点を抽出し、各特徴点が成す形状や特徴点の位置（路面上か否か）に基づいて画像上の白線を検出する。また例えば、画像処理部８は画像の色情報に基づいて白線を検出する。
そして、画像処理部８は画像上の白線の長さ方向（直線部分）が成す角度（車両に対する白線の相対角度）を算出し、ＤＲ演算部３が算出した車両の方位角に画像上の白線の角度を加算して白線の方位角を算出する。もしくは、白線の絶対方位角を直接計算する方法もある。
画像処理部８は、カメラ７の特性に応じて２次元の撮像面に画像として投影された白線について、画像上での白線の角度をカメラ７の特性に応じて実世界の３次元上の角度に変換することにより、実際の白線の相対角、方位角を算出する。
例えば、白線の画像から得られる白線上の複数の点の目視線角の情報と、カメラの道路面からの高さ情報と、道路の形状の情報と、ＤＲ演算部３が算出した車両位置および方位角から計算されるカメラの位置座標と向きの情報を用いて、白線上の複数の点の位置座標を計算することで白線の方位角を計算できる。
ここで、カメラ７は撮像方向を車両の進行方向と同じにするように取り付けられているものとする。但し、カメラ７が車両に設置された際に車両に対してカメラ７の設置角に多少のずれが生じることが考えられるため、画像処理部８は拡張カルマンフィルタ６が推定したカメラアライメント誤差を加算して白線の方位角を算出することも可能である。その場合は、カメラアライメント誤差を推定する場合には車両の左右両方の白線を検出して角度を算出する必要がある。
＜Ｓ２３３：データベース方位角取得処理の一例＞
次に、白線方位角取得部２３は、ＤＲ演算部３が測位した車両の位置および方位角に基づいて、車両の進行方向に位置する白線の方位角を白線情報データベース９から取得する。つまり、白線方位角取得部２３はカメラ７が撮像した画像に映っている白線について白線情報データベース９から方位角を取得する。例えば、白線情報データベース９は東西南北を基準方位として白線の方位角を示している。
＜Ｓ２３４：方位角残差算出処理の一例＞
次に、白線方位角残差計算部２４は画像処理部８が画像に基づいて算出した白線の方位角と白線方位角取得部２３が白線情報データベース９から取得した白線の方位角との差分を白線方位角残差として算出する。
【００３９】
＜Ｓ２４０：誤差推定処理の一例＞
そして、拡張カルマンフィルタ６は擬似距離残差計算部２２が算出した擬似距離残差と白線方位角残差計算部２４が算出した白線方位角残差とを観測量として入力し、ＤＲ演算部３の測位結果に含まれる航法誤差とＤＲ計算に用いられるジャイロ４の誤差とＤＲ計算に用いられるオドメータ５の誤差とＧＰＳ受信機１のＧＰＳクロック誤差とカメラ７のアライメント誤差とを状態量として計算して補正量として出力する。
【００４０】
ＤＲ演算部３は拡張カルマンフィルタ６が出力した航法誤差に基づいて測位した車両の位置および方位角を補正し、補正した車両の位置および方位角を出力する。
また、画像処理部８は拡張カルマンフィルタ６が出力したカメラアライメント誤差に基づいてカメラ７の車両に対する設置角を算出し、次回、算出したカメラ７の設置角に基づいて画像に映る白線の方位角を算出する。
【００４１】
カルマンフィルタは１９６０年にＫａｌｍａｎが提唱したアルゴリズムで、計算により求める物理量（状態量と呼ぶ）のダイナミクス（時間発展、時間変化）を線形モデルで与え、状態量の誤差の確率分布関数をガウス分布であるとした場合に、推定誤差を最小とする解を与えるものである。ここで推定誤差を最小にするとは、状態量に選んだ変数（例えば、航法誤差、カメラアライメント誤差、自己位置・姿勢角）のダイナミクスも考慮して残差の二乗和が最小になるように解を推定することを意味する。
一般的なカルマンフィルタの基本方程式は、離散時間系（ｄｉｓｃｒｅｔｅ ｔｉｍｅ ｓｙｓｔｅｍ）の場合、以下の状態方程式（式１）と観測方程式（式２）とで表される。
【００４２】
【数１】
【００４３】
特に、拡張カルマンフィルタ６（Ｅｘｔｅｎｄｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ，ＥＫＦ）は、系の方程式が非線形の場合に、現在の推定量のまわりで線形化した方程式を作り、それをもとに上記基本方程式を構成したカルマンフィルタである。
すなわち、ｘとｚが、以下の式３、式４のように、それぞれｘｋ−１，ｕｋ，ｗｋの関数ｆおよびｘｋ，ｖｋの関数ｈとして表される場合、Ｊａｃｏｂｉａｎを使って式５、式６のように線形化する。
【００４４】
【数２】
【００４５】
そして、状態方程式（式７）と観測方程式（式８）とは、式５、式６に示す線形化によって得られたＦとＨとを用いて次のように表される。
【００４６】
【数３】
【００４７】
また、カルマンフィルタの演算は時間伝播（Ｔｉｍｅ Ｕｐｄａｔｅ）と観測更新（Ｍｅａｓｕｒｅｍｅｎｔ Ｕｐｄａｔｅ）とからなる。
【００４８】
まず、状態量ｘの誤差共分散行列Ｐの時間伝播は、システムのダイナミクスを表す行列Ｆを用いて以下の式９および式１０で表される。
式９については、例えば、第二項あるいは第三項までを使って近似計算する。
【００４９】
【数４】
【００５０】
また、観測更新は以下の式１１〜式１３で計算される。
【００５１】
【数５】
【００５２】
以上の各式において、実施の形態１では、ＤＲ演算部３の測位した自己位置や方位角に含まれる航法誤差と航法計算に利用するセンサ（ジャイロ４、オドメータ５、ＧＰＳクロックなど）の誤差とカメラ７のアライメント誤差とが状態量ｘに対応し、擬似距離残差と白線方位角残差とが観測残差Δｚに対応する。
拡張カルマンフィルタ６は、以上の各式および理論に基づいて、航法誤差およびカメラアライメント誤差を推定する。
【００５３】
実施の形態１では、以下のような自己位置標定装置１００について説明した。
実施の形態１における自己位置標定装置１００は、ハードウェアとして、既存のカーナビレベルのハードウェア構成（ＧＰＳ受信機１＋１軸ジャイロ４＋車速パルス［オドメータ５］）以外に、単眼カメラ７を備える。
また、実施の形態１における自己位置標定装置１００は、カメラ７の画像から白線の方位角を得る画像処理部８と、道路線形データベース（白線情報データベース９）と、そのデータから白線の直線部分の方位角を計算する方位角算出部（白線方位角取得部２３）と、自己位置・姿勢角を算出するためのＤＲ演算部３と拡張カルマンフィルタ６とを備える。
道路線形データベースは一から構築する必要はなく、すでに実用化段階に入っている特許文献６により構築されたデータベースに対して、白線の直線部分を示すデータに新たに方位角を加えるだけでよいところが特徴である。また、自己位置標定装置１００が使用するカメラ７も単眼でよく低価格のものを利用できる点が特徴である。
ＤＲ演算部３は、慣性装置（１軸ジャイロ４（方位角方向））と、車速パルスデータを利用してデッドレコニング計算を行う。そして、拡張カルマンフィルタ６は、ＧＰＳ観測データと上記白線方位角データとを観測量として自己位置・姿勢角の誤差推定を行ってＤＲ演算部３の自己位置標定精度を向上させる。
つまり、実施の形態１で説明した白線の方位角を自己位置補正に利用する方式では、ハードウェアの構成として単眼のカメラ７を１台追加するだけでよい利点がある。つまり、実施の形態１における自己位置標定装置１００は自己位置から白線までの距離を観測するためのステレオカメラやレーザレーダなどが不要である。また、白線の直線部分の角度（白線の方位角）が単眼カメラ７で撮像した画像から比較的正確に検出でき、一度計測され道路線形データベースに記録された方位角は地球の地殻変動による劣化がほとんどないことも白線の方位角を自己位置補正に利用する利点の一つである。言い換えると、道路の座標のデータベースを正確に作るのは難しく、現在使われているカーナビゲーション用のデジタル地図データの座標精度を考慮すると、座標または座標に基づいて算出される距離を自己位置補正に利用する方式ではデシメートル級の座標精度を達成するのは困難である。
このような事情を考慮すると、白線の方位角データを利用する実施の形態１における方式は有望な自己位置標定手法であると言える。
【００５４】
また、実施の形態１の自己位置標定手法は、白線の直線部のような車両移動方向に対して画像エッジ位置の変化率が小さい特徴量をランドマークとしているため、低価格単眼カメラ＋低価格画像処理器で十分に精度が出せる点が特徴である。
また、実施の形態１では、車載カメラ７の取り付け角誤差（アライメント誤差）も拡張カルマンフィルタ６で推定し、その誤差を補正できる。このため、アライメントの厳しいステレオカメラが用いられる場合でも自己位置標定精度を高めることができる。
【００５５】
実施の形態１に示した自己位置標定方法により、既存のカーナビレベルのハードウェア構成に単眼カメラ７を加え、道路線形データベースとＧＰＳ／ＤＲ／画像複合アルゴリズムを適用するだけで、都市部やトンネル内等における自己位置標定精度を向上させることができる。
【００５６】
実施の形態２．
実施の形態２では、前記実施の形態１において図１に基づいて説明した自己位置標定装置１００の具体例として、方位角残差算出部１３０が、白線の方位角の代わりに、車両から見た道路標識の位置する方向（以下、標識の方位角という）についての残差（以下、標識方位角残差という）を算出する形態について説明する。
また、実施の形態２において、方位角残差算出部１３０は車両から道路標識までの距離（以下、標識距離という）についての残差（以下、標識距離残差という）を算出する。そして、誤差推定部１４０は擬似距離残差と標識方位角残差と標識距離残差とを観測量として入力して航法誤差とカメラアライメント誤差とを算出する。
以下、前記実施の形態１と異なる事項について説明し、説明を省略した事項については前記実施の形態１と同様とする。
【００５７】
図６は、実施の形態２における自己位置標定装置１００の構成図である。
図６において、実施の形態２における自己位置標定装置１００は、前記実施の形態１における自己位置標定装置１００に対して、白線情報データベース９、白線方位角取得部２３および白線方位角残差計算部２４の代わりに標識情報データベース１１、標識方位角・距離算出部３１および標識方位角・距離残差計算部３２を備える点を特徴とする。
また、実施の形態２における自己位置標定装置１００は、画像処理部８が、白線の方位角の代わりに、カメラ７の撮像した画像に映っている道路標識について、車両に対する道路標識の位置する方向を示す標識の方位角と、車両から道路標識までの距離を示す標識距離とを算出することを特徴とする。
【００５８】
標識情報データベース１１（位置データベースの一例）には各道路標識の位置情報が予め記憶されている。例えば、道路標識を含む各種ランドマークの位置情報が登録されカーナビゲーションシステムに一般的に用いられている道路情報データベースを標識情報データベース１１として利用することができる。
【００５９】
標識方位角・距離算出部３１（データベース距離部の一例）は、ＤＲ演算部３が算出した車両の位置および方位角に基づいて、標識情報データベース１１から車両の進行方向に位置する道路標識の位置情報を取得し、車両の進行方向に位置する道路標識について方位角と距離とをＣＰＵを用いて算出する。
【００６０】
標識方位角・距離残差計算部３２（距離残差算出部の一例）は、画像処理部８（画像距離部の一例）が算出した画像に基づく標識の方位角と標識方位角・距離算出部３１が算出した標識情報データベース１１に基づく標識の方位角との差分を標識方位角残差としてＣＰＵを用いて算出する。また、標識方位角・距離残差計算部３２は画像処理部８が算出した画像に基づく標識距離と標識方位角・距離算出部３１が算出した標識情報データベース１１に基づく標識距離との差分を標識距離残差としてＣＰＵを用いて算出する。
【００６１】
図７は、実施の形態２における自己位置標定装置１００の自己位置標定方法２を示すフローチャートである。
図７において、Ｓ３１０は図２の測位処理（Ｓ１１０）に対応し、Ｓ３２１〜Ｓ３２４は図２のＧＰＳ残差算出処理（Ｓ１２０）に対応し、Ｓ３３１〜Ｓ３３４ｂは図２の方位角残差算出処理（Ｓ１３０）に対応し、Ｓ３４０は図２の誤差推定処理（Ｓ１４０）に対応する。
自己位置標定装置１００の各部は以下に説明する各処理をＣＰＵを用いて実行する。
【００６２】
＜Ｓ３１０〜Ｓ３２４＞
測位処理（Ｓ１１０）およびＧＰＳ残差算出処理（Ｓ１２０）に関するＳ３１０〜Ｓ３２４は、前記実施の形態１における自己位置標定方法１（図５参照）のＳ２１０〜Ｓ２２４と同じである。
【００６３】
＜Ｓ３３１：撮像処理の一例＞
ＤＲ演算部３が測位を行った際、カメラ７は車両が走行している道路の進行方向を撮像する。
【００６４】
＜Ｓ３３２ａ：画像方位角算出処理の一例＞
次に、画像処理部８はカメラ７が撮像した画像から画像に映っている道路標識を検出する。例えば、画像処理部８は、画像を二値化して抽出した特徴点が成す形状に基づいて特定形状のマークを表示する道路標識を検出する。また例えば、画像処理部８は画像に対して文字認識処理を行い、認識した文字情報に基づいて特定文字列を表示する道路標識を検出する。
そして、画像処理部８は画像上の道路標識の位置（道路標識の画素位置）に基づいて車両から見た道路標識の位置する方向を示す相対角を算出し、ＤＲ演算部３が算出した車両の方位角に画像上の道路標識の相対角を加算して車両を基点として道路標識が位置する方向を示す標識の方位角を算出する。例えば、画像処理部８は画像の中心から道路標識が位置する画素位置までの水平方向の画素数に１画素が成す角度を乗じて画像に映っている道路標識の車両に対する実際の相対角を算出する。各画素に相当する角度はカメラのパラメータ（例えば、焦点距離）に依存し、カメラのパラメータに基づいて算出される。画像処理部８は、カメラ７の特性に応じて２次元の撮像面に画像として投影された標識について、画像上での標識の位置をカメラ７の特性に応じて実世界の３次元上の位置に変換することにより、実際の標識の方位角および距離を算出する。
＜Ｓ３３２ｂ：画像距離算出処理の一例＞
また、画像処理部８は画像上の道路標識の位置に基づいて車両から道路標識までの標識距離を算出する。
例えば、画像処理部８はカメラ７が連続する異なる時刻に撮像した２枚の画像を用いてモーションステレオ視処理を行い車両から道路標識までの標識距離を算出する。
【００６５】
＜Ｓ３３３ａ＞
次に、標識方位角・距離算出部３１は、ＤＲ演算部３が測位した車両の位置および方位角に基づいて、車両の進行方向に位置する道路標識の位置情報を標識情報データベース１１から取得する。つまり、標識方位角・距離算出部３１はカメラ７が撮像した画像に映っている道路標識について標識情報データベース１１から位置情報を取得する。
＜Ｓ３３３ｂ＞
次に、標識方位角・距離算出部３１は、標識情報データベース１１から取得した道路標識の位置とＤＲ演算部３が算出した車両の位置とに基づいて車両から見て道路標識が位置する相対角を算出し、ＤＲ演算部３が算出した車両の方位角に車両から見た道路標識の相対角を加算して車両を基点とした道路標識の方位角を算出する。
＜Ｓ３３３ｃ＞
また、標識方位角・距離算出部３１は、標識情報データベース１１から取得した道路標識の位置とＤＲ演算部３が算出した車両の位置とに基づいて、車両から道路標識までの標識距離を算出する。
【００６６】
＜Ｓ３３４ａ＞
次に、標識方位角・距離残差計算部３２は画像処理部８が画像に基づいて算出した標識の方位角と標識方位角・距離算出部３１が標識情報データベース１１に基づいて算出した標識の方位角との差分を標識方位角残差として算出する。
＜Ｓ３３４ｂ＞
また、標識方位角・距離残差計算部３２は画像処理部８が画像に基づいて算出した標識距離と標識方位角・距離算出部３１が標識情報データベース１１に基づいて算出した標識距離との差分を標識距離残差として算出する。
【００６７】
＜Ｓ３４０＞
そして、拡張カルマンフィルタ６は擬似距離残差計算部２２が算出した擬似距離残差と標識方位角・距離残差計算部３２が算出した標識方位角残差および標識距離残差とを観測量として入力し、ＤＲ演算部３の測位結果に含まれる航法誤差とカメラ７のアライメント誤差とを状態量として計算して補正量として出力する。
【００６８】
実施の形態２では、以下のような自己位置標定装置１００について説明した。
実施の形態２における自己位置標定装置１００は、ハードウェアとして、既存のカーナビレベルのハードウェア構成（ＧＰＳ受信機１＋１軸ジャイロ４＋車速パルス［オドメータ５］）以外に、単眼カメラ７を備える。
また、実施の形態２における自己位置標定装置１００は、カメラ７の画像から「車両−標識間の距離」および「車両から見た標識の相対方位角」を得る画像処理部８と、標識座標のデータベース（標識情報データベース１１）と、自己位置・姿勢角を算出するためのＤＲ演算部３と拡張カルマンフィルタ６とを備える。
ＤＲ演算部３は、慣性装置（１軸ジャイロ４（方位角方向））と、車速パルスデータを利用してデッドレコニング計算を行う。そして、拡張カルマンフィルタ６は、ＧＰＳ観測データと上記標識データとを観測量として自己位置・姿勢角の誤差推定を行ってＤＲ演算部３の自己位置標定精度を向上させる。
【００６９】
実施の形態３．
実施の形態３では、前記実施の形態１において図１に基づいて説明した自己位置標定装置１００の具体例として、方位角残差算出部１３０が前記実施の形態１で説明した白線方位角残差と前記実施の形態２で説明した標識方位角残差および標識距離残差とを算出する形態について説明する。
また、実施の形態３において、測位部１１０はデッドレコニングにより算出した自己位置を道路データベース４９のデータを利用したマップマッチングにより補正して自己位置を算出する。
また、実施の形態３において、誤差推定部１４０は擬似距離残差と白線方位角残差と標識方位角残差と標識距離残差とを観測量として入力して航法誤差とカメラアライメント誤差とを算出する。
以下、前記実施の形態１および前記実施の形態２と異なる事項について説明し、説明を省略した事項については前記実施の形態１または前記実施の形態２と同様とする。
【００７０】
図８は、実施の形態３における自己位置標定装置１００の構成図である。
図８において、実施の形態３における自己位置標定装置１００は、前記実施の形態１における自己位置標定装置１００に対して、白線情報データベース９、白線方位角取得部２３および白線方位角残差計算部２４の代わりに道路データベース４９、方位角・距離算出部４１および方位角・距離残差算出部４２を備える点を特徴とする。
また、実施の形態３における自己位置標定装置１００は、ＤＲ演算部３が車両の位置する周辺の道路情報を道路データベース４９から取得してマップマッチングにより車両の位置を補正することを特徴とする。
【００７１】
道路データベース４９には前記実施の形態１で説明した白線情報データベース９の白線情報や前記実施の形態２で説明した道路標識情報や道路のレーン情報（例えば、位置、幅、上り下り）などの各種道路情報が予め記憶されている。
【００７２】
方位角・距離算出部４１は、ＤＲ演算部３が算出した車両の位置および方位角に基づいて、道路データベース４９から車両の進行方向に位置する白線の方位角と車両の進行方向に位置する道路標識の位置情報とを取得し、車両の進行方向に位置する道路標識の方位角と車両から道路標識までの標識距離とをＣＰＵを用いて算出する。
【００７３】
方位角・距離残差算出部４２は、画像処理部８が算出した画像に基づく白線の方位角と方位角・距離算出部４１が取得した道路データベース４９に基づく白線の方位角との差分を白線方位角残差としてＣＰＵを用いて算出する。また、方位角・距離残差算出部４２は、画像処理部８が算出した画像に基づく標識の方位角と方位角・距離算出部４１が算出した道路データベース４９に基づく標識の方位角との差分を標識方位角残差としてＣＰＵを用いて算出する。また、方位角・距離残差算出部４２は画像処理部８が算出した画像に基づく標識距離と方位角・距離算出部４１が算出した道路データベース４９に基づく標識距離との差分を標識距離残差としてＣＰＵを用いて算出する。
【００７４】
図９は、実施の形態３における自己位置標定装置１００の自己位置標定方法３を示すフローチャートである。
図９において、Ｓ４１０ａ〜Ｓ４１０ｃは図２の測位処理（Ｓ１１０）に対応し、Ｓ４２１〜Ｓ４２４は図２のＧＰＳ残差算出処理（Ｓ１２０）に対応し、Ｓ４３１〜Ｓ４３４ｃは図２の方位角残差算出処理（Ｓ１３０）に対応し、Ｓ４４０は図２の誤差推定処理（Ｓ１４０）に対応する。
自己位置標定装置１００の各部は以下に説明する各処理をＣＰＵを用いて実行する。
【００７５】
＜Ｓ４１０ａ＞
まず、ＤＲ演算部３はデッドレコニングにより車両の位置および方位角を算出する。
＜Ｓ４１０ｂ＞
次に、ＤＲ演算部３は算出した車両の位置に基づいて周辺の道路情報を道路データベース４９から取得する。
＜Ｓ４１０ｃ：測位処理の一例＞
次に、ＤＲ演算部３は、道路データベース４９から取得した道路情報が示すレーン情報に基づいて、車両の進行方向（方位角）と通行方向が対応するレーン上に車両が位置するように車両の位置を補正するマップマッチング（または、レーンキーピング）を行う。また、ＤＲ演算部３はマップマッチングを行った車両の位置および方位角を拡張カルマンフィルタ６が算出した航法誤差に基づいて補正して車両の位置および方位角を算出する。ＤＲ演算部３はマップマッチング処理としてカーナビゲーションシステムで一般的に行われている処理を用いる。ただし、マップマッチング処理は、走行中の道路が１レーンしかないなど走行レーンが明らかな場合や、拡張カルマンフィルタの状態量誤差標準偏差が適切に設定した閾値を超えた場合などの自己位置標定精度が悪化していると推定される場合のみ行うものとする。
【００７６】
＜Ｓ４２１〜Ｓ４２４＞
ＧＰＳ残差算出処理（Ｓ１２０）に関するＳ４２１〜Ｓ４２４は、前記実施の形態１における自己位置標定方法１（図５参照）のＳ２２１〜Ｓ２２４と同じである。
【００７７】
＜Ｓ４３１〜Ｓ４３４ｃ＞
白線の方位角残差算出処理に関するＳ４３１、Ｓ４３２ａ、Ｓ４３３ａおよびＳ４３４ａは、前記実施の形態１における自己位置標定方法１（図５参照）のＳ２３１〜Ｓ２３４と同じである。
また、道路標識の方位角残差算出処理および標識距離残差算出処理に関するＳ４３２ｂ、Ｓ４３３ｂおよびＳ４３４ｂは、前記実施の形態２における自己位置標定方法２（図７参照）のＳ３３２ａ〜Ｓ３３４ｂと同じである。
【００７８】
そして、拡張カルマンフィルタ６は擬似距離残差計算部２２が算出した擬似距離残差と方位角・距離残差算出部４２が算出した白線方位角残差、標識方位角残差および標識距離残差とを観測量として入力し、ＤＲ演算部３のデッドレコニングによる測位結果に含まれる航法誤差とカメラ７のアライメント誤差とを状態量として計算して補正量として出力する。
【００７９】
上記では、白線について方位角のみを対象としたが、標識と同様に車両から白線までの距離の残差を拡張カルマンフィルタ６の観測量としてもよいし、画像に基づいて算出した白線の長さとデータベースから取得した白線の長さとの残差を拡張カルマンフィルタ６の観測量としてもよい。
【００８０】
実施の形態３では、以下のような自己位置標定装置１００について説明した。
実施の形態３における自己位置標定装置１００は、前記実施の形態１に記載した自己位置標定装置１００に、前記実施の形態２に記載した「車両−標識間の距離」と「車両からみた標識の相対方位角」を得る画像処理部８の機能と、標識座標のデータベースとを加え、拡張カルマンフィルタ６は白線と標識との両方を観測量として自己位置・姿勢角の誤差推定を行ってＤＲ演算部３の自己位置標定精度を向上させる。
【００８１】
実施の形態４．
上記実施の形態１では、各白線について位置や形状や方位角などの情報が予め記憶されている白線情報データベース９を利用する形態について説明した。但し、各白線の情報を用意することがコスト的に難しい場合も考えられる。
そこで、実施の形態４では、上記実施の形態１で示した白線情報データベース９の代わりに道路の方位角を示す道路情報（道路線形情報）を記憶するデータベースを用い、道路の方位角を白線の方位角として用いる形態について説明する。
【００８２】
（１）ＫＩＷＩデータベースを用いる方法
道路情報を記憶するデータベースとして、カーナビゲーションシステムに使用されているデータベースを使用することができる。カーナビゲーションシステムのデータベースにはＫＩＷＩフォーマットが使用されている。以下、道路情報をＫＩＷＩフォーマット（日本工業規格 ＪＩＳ−Ｄ０８１０）で示すデータベースをＫＩＷＩデータベースとする。非特許文献１にＫＩＷＩフォーマットに関する文献を示す。
ＫＩＷＩデータベースのデータ構造には、交差点に関する情報を示すノード情報、道路が接続する交差点を示すリンク情報、新たに情報を追加することが可能な拡張部などが含まれる。ノード情報には各交差点をノードとしてノード名と交差点の座標（例えば、交差点中央の２次元座標）とが対応付けて記憶されている。リンク情報には“ノードＡ（交差点Ａ）とノードＢ（交差点Ｂ）が接続されている（Ａ、Ｂ）”というような道路が接続している交差点を示す情報が記憶されている。そして、実施の形態４では、ＫＩＷＩデータベースの拡張部に、リンク情報（接続情報）と方位角とを対応付けて、リンク情報が表す道路について方位角を示す方位角情報を記憶する。例えば、ノード情報とリンク情報とが図１１のような関係を示す場合、拡張部には「接続情報」として（Ａ、Ｂ）、「方位角」としてθ１が記憶され、また、「接続情報」として（Ｂ、Ｃ）、「方位角」としてθ２が記憶される。
【００８３】
そして、上記実施の形態１における白線方位角取得部２３は、図１２に示すようにして、ＫＩＷＩデータベースから道路情報を取得し、道路の方位角を白線の方位角として算出する。
【００８４】
＜Ｓ５１０＞
白線方位角取得部２３は、ＤＲ演算部３から自己位置Ｎ、自己方位角φＮを取得する。
＜Ｓ５２０＞
次に、白線方位角取得部２３はＫＩＷＩデータベースにおいてノード情報が示す各交差点の座標と自己位置Ｎとを比較して車両が位置する道路を示すリンク情報を検索する。
＜Ｓ５３０＞
次に、白線方位角取得部２３は検索したリンク情報に対応する道路の方位角θｎを拡張部から抽出する。
＜Ｓ５４０＞
次に、白線方位角取得部２３は自己方位角φＮが示す進行方向における道路の方位角θｎを取得する。ただし、白線を見る向きによって方位角の表現は１８０度異なるため、自己方位角φＮに応じて、θｎに１８０度だけ加算することがある。
＜Ｓ５５０＞
そして、白線方位角取得部２３は取得した道路の方位角θｎを白線方位角として出力する。
【００８５】
（２）道路線形データベースを用いる方法
道路の幾何構造は横断面と平面線形とで表すことができるが、実施の形態４で用いる道路線形データベースは道路の平面線形について直線や円曲線や緩和曲線（クロソイド）などの線形要素で表した道路情報を記憶する。道路の直線は例えば始点の座標と終点の座標とで表される。
そして、実施の形態４における白線方位角取得部２３は、図１３に示すようにして、道路線形データベースに記憶された道路の直線部分のデータに基づいて、道路の方位角を白線方位角として算出する。
【００８６】
＜Ｓ６１０＞
白線方位角取得部２３は、ＤＲ演算部３から自己位置Ｎを取得する。
＜Ｓ６２０＞
次に、白線方位角取得部２３は道路線形データベースにおいて自己位置Ｎが位置する道路の直線部分のデータを検索する。
＜Ｓ６３０＞
次に、白線方位角取得部２３は検索した道路の直線部分のデータが示す始点と終点の座標に基づいて道路の方位角を算出する。但し、道路の方位角を予め道路線形データベースに記憶しておいても構わない。
＜Ｓ６４０＞
そして、白線方位角取得部２３は算出した道路の方位角を白線方位角として出力する。
【００８７】
各実施の形態において、ＧＰＳ受信機１がＭＳＡＳ（ＭＴＳＡＴ Ｓａｔｅｌｌｉｔｅ−ｂａｓｅｄ Ａｕｇｍｅｎｔａｔｉｏｎ Ｓｙｓｔｅｍ）からのＧＰＳ補正情報やＦＭ多重放送を受信することなどで得られるＧＰＳ補正情報を用いて擬似距離の補正をする方式も考えられる。また、将来的には準天頂衛星からの放送データの中にＧＰＳ補正情報が含まれる予定であり、ＧＰＳ受信機１がこれを利用してＧＰＳの擬似距離補正を行う方法も考えられる。
また、ＧＰＳ受信機１が擬似距離を搬送波位相観測データを使ってスムージングする方法（キャリアスムージング）や、ＤＲ演算部３がＧＰＳ基準局の搬送波位相データとＧＰＳ受信機１で観測するＧＰＳ搬送波位相データとを用いてＣＰＤＧＰＳ（Ｃａｒｒｉｅｒ Ｐｈａｓｅ Ｄｉｆｆｅｒｅｎｔｉａｌ ＧＰＳ）計算を行い測位する方法なども考えられる。
さらに、拡張カルマンフィルタ６の構成方法には、前記実施の形態１で説明したＧＰＳの観測生データ（擬似距離、位相差など）をＧＰＳ観測量として利用するタイトカプリング（Ｔｉｇｈｔｌｙ Ｃｏｕｐｌｅｄ、密結合）方式の他に、ＧＰＳ観測値から得た測位解をＧＰＳ観測量として利用するルーズカプリング（Ｌｏｏｓｅｌｙ Ｃｏｕｐｌｅｄ、粗結合）方式も考えられる。
【図面の簡単な説明】
【００８８】
【図１】実施の形態１における自己位置標定装置１００の構成図。
【図２】実施の形態１における自己位置標定装置１００の自己位置標定方法を示すフローチャート。
【図３】実施の形態１における自己位置標定装置１００のハードウェア資源の一例を示す図。
【図４】実施の形態１における自己位置標定装置１００の構成図。
【図５】実施の形態１における自己位置標定装置１００の自己位置標定方法ＧＰＳ受信機１を示すフローチャート。
【図６】実施の形態２における自己位置標定装置１００の構成図。
【図７】実施の形態２における自己位置標定装置１００の自己位置標定方法２を示すフローチャート。
【図８】実施の形態３における自己位置標定装置１００の構成図。
【図９】実施の形態３における自己位置標定装置１００の自己位置標定方法を示すフローチャート。
【図１０】実施の形態１における白線と白線（の直線部分）の方位角とレーンの通行方向との関係図。
【図１１】実施の形態４におけるＫＩＷＩデータの関係図。
【図１２】実施の形態４における方位角取得処理のフローチャート。
【図１３】実施の形態４における方位角取得処理のフローチャート。
【符号の説明】
【００８９】
１ ＧＰＳ受信機、２ 衛星軌道演算部、３ ＤＲ演算部、４ ジャイロ、５ オドメータ、６ 拡張カルマンフィルタ、７ カメラ、８ 画像処理部、９ 白線情報データベース、１１ 標識情報データベース、２１ 衛星距離計算部、２２ 擬似距離残差計算部、２３ 白線方位角取得部、２４ 白線方位角残差計算部、３１ 標識方位角・距離算出部、３２ 標識方位角・距離残差計算部、４１ 方位角・距離算出部、４２ 方位角・距離残差算出部、４９ 道路データベース、１００ 自己位置標定装置、１１０ 測位部、１２０ ＧＰＳ観測残差算出部、１２１ ＧＰＳ観測部、１２２ ＧＰＳ観測量演算部、１３０ 方位角残差算出部、１３１ 方位角観測部、１３２ 方位角演算部、１４０ 誤差推定部、９０１ 表示装置、９１１ ＣＰＵ、９１２ バス、９１３ ＲＯＭ、９１４ ＲＡＭ、９１５ 通信ボード、９２０ 磁気ディスク装置、９２１ ＯＳ、９２３ プログラム群、９２４ ファイル群。
【特許請求の範囲】
【請求項１】
自己位置を測位する測位部と、
前記測位部が測位した際の特定の方向を撮像するカメラと、
前記カメラが撮像した画像に基づいて前記画像に映っている特定の地物の長さ方向が基準方向に対して成す前記特定の地物の方位角をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角部と、
地物の長さ方向が基準方向に対して成す当該地物の方位角を記憶する方位角データベースと、
前記測位部が測位した自己位置に基づいて前記方位角データベースから前記特定の地物に対応する地物の方位角を取得するデータベース方位角部と、
前記画像方位角部が算出した地物の方位角と前記データベース方位角部が取得した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出部と、
前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定するカルマンフィルタと
を備え、
前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とする
ことを特徴とする自己位置標定装置。
【請求項２】
自己位置を測位する測位部と、
前記測位部が測位した際の特定の方向を撮像するカメラと、
前記カメラが撮像した画像に基づいて前記画像に映っている特定の地物の方位角を自己位置を基点としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角部と、
地物の位置を記憶する位置データベースと、
前記測位部が測位した自己位置に基づいて前記位置データベースから前記特定の地物に対応する地物の位置を取得し、取得した地物の位置に基づいて地物の方位角を前記測位部が測位した自己位置を基点としてＣＰＵを用いて算出するデータベース方位角部と、
前記画像方位角部が算出した地物の方位角と前記データベース方位角部が算出した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出部と、
前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定するカルマンフィルタと
を備え、
前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とする
ことを特徴とする自己位置標定装置。
【請求項３】
前記カルマンフィルタは、
自己位置の誤差を含む状態量について状態量のダイナミクスをモデル化した状態方程式と、地物方位角残差を含む観測量と自己位置の誤差を含む状態量とについて観測量と状態量との関係を定式化した観測方程式とに基づいて状態量の誤差推定を行う
ことを特徴とする請求項１〜請求項２いずれかに記載の自己位置標定装置。
【請求項４】
前記自己位置標定装置は、さらに、
前記カメラが撮像した画像に基づいて前記特定の地物までの距離をＣＰＵを用いて算出する画像距離部と、
地物の位置を記憶する位置データベースと、
前記測位部が測位した自己位置に基づいて前記位置データベースから前記特定の地物に対応する地物の位置を取得し、前記測位部が測位した自己位置と前記位置データベースから取得した地物の位置とに基づいて地物までの距離を算出するデータベース距離部と、
前記画像距離部が算出した地物までの距離と前記データベース距離部が算出した地物までの距離との差分を地物距離残差としてＣＰＵを用いて算出する距離残差算出部と
を備え、
前記カルマンフィルタは前記距離残差算出部が算出した地物距離残差を観測量に含めて自己位置の誤差を含む状態量の誤差推定を行う
ことを特徴とする請求項３記載の自己位置標定装置。
【請求項５】
測位部が自己位置を測位する測位処理を行い、
カメラが前記測位部が測位した際の特定の方向を撮像する撮像処理を行い、
画像方位角部が前記カメラの撮像した画像に基づいて前記画像に映っている特定の地物の長さ方向が基準方向に対して成す前記特定の地物の方位角をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角算出処理を行い、
データベース方位角部が前記測位部が測位した自己位置に基づいて地物の長さ方向が基準方向に対して成す当該地物の方位角を記憶する方位角データベースから前記特定の地物に対応する地物の方位角を取得するデータベース方位角取得処理を行い、
方位角残差算出部が前記画像方位角部が算出した地物の方位角と前記データベース方位角部が取得した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出処理を行い、
カルマンフィルタが前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定する誤差推定処理を行い、
前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とする測位結果補正処理を行う
ことを特徴とする自己位置標定方法。
【請求項６】
測位部が自己位置を測位する測位処理を行い、
カメラが前記測位部が測位した際の特定の方向を撮像する撮像処理を行い、
画像方位角部が前記カメラが撮像した画像に基づいて前記画像に映っている特定の地物の方位角を自己位置を基点としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角算出処理を行い、
データベース方位角部が前記測位部が測位した自己位置に基づいて地物の位置を記憶する位置データベースから前記特定の地物に対応する地物の位置を取得し、取得した地物の位置に基づいて地物の方位角を前記測位部が測位した自己位置を基点としてＣＰＵを用いて算出するデータベース方位角算出処理を行い、
方位角残差算出部が前記画像方位角部が算出した地物の方位角と前記データベース方位角部が算出した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出処理を行い、
カルマンフィルタが前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定する誤差推定処理を行い、
前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とする測位結果補正処理を行う
ことを特徴とする自己位置標定方法。
【請求項７】
請求項５〜請求項６いずれかに記載の自己位置標定方法をコンピュータに実行させる自己位置標定プログラム。
【請求項８】
前記画像方位角部は、前記画像に映っている白線の方位角を算出し、
前記方位角データベースは、道路の直線部分が基準方向に対して成す当該道路の方位角を示す道路情報をＫＩＷＩフォーマットで記憶し、
前記データベース方位角部は、前記方位角データベースに記憶された道路の直線部分の方位角を道路に標示された白線の方位角として取得し、
前記方位角残差算出部は、白線の方位角残差を前記地物方位角残差として算出し、
前記カルマンフィルタは、白線の方位角残差に基づいて自己位置の誤差を推定する
ことを特徴とする請求項１記載の自己位置標定装置。
【請求項９】
前記画像方位角部は、前記画像に映っている白線の方位角を算出し、
前記方位角データベースは、道路の平面形状を円弧と直線とクロソイドとの少なくともいずれかにより示す道路情報を記憶し、
前記データベース方位角部は、前記方位角データベースから取得した道路情報が示す道路の平面形状に基づいて道路の直線部分の方位角を道路に標示された白線の方位角として算出し、
前記方位角残差算出部は、白線の方位角残差を前記地物方位角残差として算出し、
前記カルマンフィルタは、白線の方位角残差に基づいて自己位置の誤差を推定する
ことを特徴とする請求項１記載の自己位置標定装置。
【請求項１】
自己位置を測位する測位部と、
前記測位部が測位した際の特定の方向を撮像するカメラと、
前記カメラが撮像した画像に基づいて前記画像に映っている特定の地物の長さ方向が基準方向に対して成す前記特定の地物の方位角をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角部と、
地物の長さ方向が基準方向に対して成す当該地物の方位角を記憶する方位角データベースと、
前記測位部が測位した自己位置に基づいて前記方位角データベースから前記特定の地物に対応する地物の方位角を取得するデータベース方位角部と、
前記画像方位角部が算出した地物の方位角と前記データベース方位角部が取得した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出部と、
前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定するカルマンフィルタと
を備え、
前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とする
ことを特徴とする自己位置標定装置。
【請求項２】
自己位置を測位する測位部と、
前記測位部が測位した際の特定の方向を撮像するカメラと、
前記カメラが撮像した画像に基づいて前記画像に映っている特定の地物の方位角を自己位置を基点としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角部と、
地物の位置を記憶する位置データベースと、
前記測位部が測位した自己位置に基づいて前記位置データベースから前記特定の地物に対応する地物の位置を取得し、取得した地物の位置に基づいて地物の方位角を前記測位部が測位した自己位置を基点としてＣＰＵを用いて算出するデータベース方位角部と、
前記画像方位角部が算出した地物の方位角と前記データベース方位角部が算出した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出部と、
前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定するカルマンフィルタと
を備え、
前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とする
ことを特徴とする自己位置標定装置。
【請求項３】
前記カルマンフィルタは、
自己位置の誤差を含む状態量について状態量のダイナミクスをモデル化した状態方程式と、地物方位角残差を含む観測量と自己位置の誤差を含む状態量とについて観測量と状態量との関係を定式化した観測方程式とに基づいて状態量の誤差推定を行う
ことを特徴とする請求項１〜請求項２いずれかに記載の自己位置標定装置。
【請求項４】
前記自己位置標定装置は、さらに、
前記カメラが撮像した画像に基づいて前記特定の地物までの距離をＣＰＵを用いて算出する画像距離部と、
地物の位置を記憶する位置データベースと、
前記測位部が測位した自己位置に基づいて前記位置データベースから前記特定の地物に対応する地物の位置を取得し、前記測位部が測位した自己位置と前記位置データベースから取得した地物の位置とに基づいて地物までの距離を算出するデータベース距離部と、
前記画像距離部が算出した地物までの距離と前記データベース距離部が算出した地物までの距離との差分を地物距離残差としてＣＰＵを用いて算出する距離残差算出部と
を備え、
前記カルマンフィルタは前記距離残差算出部が算出した地物距離残差を観測量に含めて自己位置の誤差を含む状態量の誤差推定を行う
ことを特徴とする請求項３記載の自己位置標定装置。
【請求項５】
測位部が自己位置を測位する測位処理を行い、
カメラが前記測位部が測位した際の特定の方向を撮像する撮像処理を行い、
画像方位角部が前記カメラの撮像した画像に基づいて前記画像に映っている特定の地物の長さ方向が基準方向に対して成す前記特定の地物の方位角をＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角算出処理を行い、
データベース方位角部が前記測位部が測位した自己位置に基づいて地物の長さ方向が基準方向に対して成す当該地物の方位角を記憶する方位角データベースから前記特定の地物に対応する地物の方位角を取得するデータベース方位角取得処理を行い、
方位角残差算出部が前記画像方位角部が算出した地物の方位角と前記データベース方位角部が取得した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出処理を行い、
カルマンフィルタが前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定する誤差推定処理を行い、
前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とする測位結果補正処理を行う
ことを特徴とする自己位置標定方法。
【請求項６】
測位部が自己位置を測位する測位処理を行い、
カメラが前記測位部が測位した際の特定の方向を撮像する撮像処理を行い、
画像方位角部が前記カメラが撮像した画像に基づいて前記画像に映っている特定の地物の方位角を自己位置を基点としてＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｃｅｓｓｉｎｇ Ｕｎｉｔ）を用いて算出する画像方位角算出処理を行い、
データベース方位角部が前記測位部が測位した自己位置に基づいて地物の位置を記憶する位置データベースから前記特定の地物に対応する地物の位置を取得し、取得した地物の位置に基づいて地物の方位角を前記測位部が測位した自己位置を基点としてＣＰＵを用いて算出するデータベース方位角算出処理を行い、
方位角残差算出部が前記画像方位角部が算出した地物の方位角と前記データベース方位角部が算出した地物の方位角との差分を地物方位角残差としてＣＰＵを用いて算出する方位角残差算出処理を行い、
カルマンフィルタが前記方位角残差算出部が算出した地物方位角残差に基づいて前記測位部が測位した自己位置の誤差をＣＰＵを用いて推定する誤差推定処理を行い、
前記測位部は測位した自己位置を前記カルマンフィルタが推定した誤差に基づいて補正し、補正した自己位置を測位結果とする測位結果補正処理を行う
ことを特徴とする自己位置標定方法。
【請求項７】
請求項５〜請求項６いずれかに記載の自己位置標定方法をコンピュータに実行させる自己位置標定プログラム。
【請求項８】
前記画像方位角部は、前記画像に映っている白線の方位角を算出し、
前記方位角データベースは、道路の直線部分が基準方向に対して成す当該道路の方位角を示す道路情報をＫＩＷＩフォーマットで記憶し、
前記データベース方位角部は、前記方位角データベースに記憶された道路の直線部分の方位角を道路に標示された白線の方位角として取得し、
前記方位角残差算出部は、白線の方位角残差を前記地物方位角残差として算出し、
前記カルマンフィルタは、白線の方位角残差に基づいて自己位置の誤差を推定する
ことを特徴とする請求項１記載の自己位置標定装置。
【請求項９】
前記画像方位角部は、前記画像に映っている白線の方位角を算出し、
前記方位角データベースは、道路の平面形状を円弧と直線とクロソイドとの少なくともいずれかにより示す道路情報を記憶し、
前記データベース方位角部は、前記方位角データベースから取得した道路情報が示す道路の平面形状に基づいて道路の直線部分の方位角を道路に標示された白線の方位角として算出し、
前記方位角残差算出部は、白線の方位角残差を前記地物方位角残差として算出し、
前記カルマンフィルタは、白線の方位角残差に基づいて自己位置の誤差を推定する
ことを特徴とする請求項１記載の自己位置標定装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【公開番号】特開２００８−２４９６３９（Ｐ２００８−２４９６３９Ａ）
【公開日】平成２０年１０月１６日（２００８．１０．１６）
【国際特許分類】
【出願番号】特願２００７−９４１３４（Ｐ２００７−９４１３４）
【出願日】平成１９年３月３０日（２００７．３．３０）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
【公開日】平成２０年１０月１６日（２００８．１０．１６）
【国際特許分類】
【出願日】平成１９年３月３０日（２００７．３．３０）
【出願人】（０００００６０１３）三菱電機株式会社 (33,312)
【Ｆターム（参考）】
[ Back to top ]