JP5640146B2 - 位置算出方法及び位置算出装置 - Google Patents

Description

本発明は、移動体の位置を算出する位置算出方法及び位置算出装置に関する。

車両の推定位置を決定し、推定位置の誤差及び道路地図の誤差により生じる推定位置の限界誤差を計算し、推定位置の限界誤差の範囲にあるリンクを抽出し、これらのリンクに地図マップマッチングを実施し、車両の位置を算出する。（特許文献１参照）

特許文献１の従来技術は、車両の推定位置の誤差だけでなく、リンクデータの誤差も考慮して、リンクを抽出することにより、車両の推定位置やリンクの誤差に応じて、リンクにマッチングしたり、道路から逸脱したと判定できる。

日本国特開平９−２２９６９８号公報

走行経路やＧＰＳ信号の受信状況などによる車両の推定位置及び推定方位の誤差や、地域ごとや地図メーカごとに異なるリンクの誤差に対応できないため、走行リンクの近くに別のリンクがある狭角分岐や並走路などで間違ったリンクにミスマッチしてしまう。

本発明の第１の態様によると、位置算出方法は、移動体の現在位置、移動速度及び移動方位を算出し、現在位置、移動速度及び移動方位に基づいて現在位置及び移動方位の推定誤差共分散を算出し、現在位置、移動方位、推定誤差共分散、現在位置周辺の領域に含まれる複数のリンク候補点の各リンク候補点のリンク候補点位置、各リンク候補点のリンク候補点方位、領域に含まれる複数のリンクの位置の第１誤差分散、及び複数のリンクの方位の第２誤差分散を用いた演算として、現在位置とリンク候補点位置との間の距離及び移動方位とリンク候補点方位との方位差から、リンク候補点位置までの所定距離間隔毎に得られた距離及び方位差を含む複数データを積算することによって、移動体が各リンク候補点に対応する道路を走行している確率に対応した評価量を各リンク候補点のリンク毎に計算し、複数のリンク候補点のうち確率が最も高いリンク候補点に移動体が位置しているとして、評価量に基づき現在位置を算出する。
本発明の第２の態様によると、第１の態様の位置算出方法において、第１誤差分散は、距離に基づいて算出され、第２誤差分散は、方位差に基づいて算出され、評価量は、複数データの各々に含まれる距離の二乗を第１誤差分散と現在位置の推定誤差分散との和で割った値と、複数データの各々に含まれる方位差の二乗を第２誤差分散と移動方位の推定誤差分散との和で割った値との和を、複数データについて積算して得られるのが好ましい。
本発明の第３の態様によると、第１または第２の態様の位置算出方法において、現在位置と確率が最も高いリンク候補点位置との間の距離に関する第１検定量が第１閾値よりも大きいか否かに基づいて、現在位置と該リンク候補点位置とに差があるか否かを判定し、移動方位と確率が最も高いリンク候補点方位との方位差に関する第２検定量が第２閾値よりも大きいか否かに基づいて、移動方位と該リンク候補点方位とに差があるか否かを判定し、現在位置と該リンク候補点位置とに差があるとき、または移動方位と該リンク候補点方位とに差があるとき、移動体が、確率が最も高いリンク候補点に対応する道路を逸脱したと判定するとともに、現在位置と該リンク候補点位置とに差がないとき、かつ移動方位と該リンク候補点方位とに差がないとき、移動体は、確率が最も高いリンク候補点に対応する道路を走行していると判定し、移動体が、確率が最も高いリンク候補点に対応する道路を逸脱したと判定したときは、移動体は現在位置に位置しているとして算出するとともに、移動体が、確率が最も高いリンク候補点に対応する道路を走行していると判定したときは、移動体は、該リンク候補点に位置しているとして、現在位置を算出するのが好ましい。
本発明の第４の態様によると、第３の態様の位置算出方法において、移動体が、確率が最も高いリンク候補点に対応する道路を逸脱していないと判定されたとき、推定誤差共分散が第３閾値以下であるか否かに応じて、第１誤差分散を算出するか否かが判定され、移動体が、確率が最も高いリンク候補点に対応する道路を逸脱していないと判定されたとき、推定誤差共分散が第４閾値以下であるか否かに応じて、第２誤差分散を算出するか否かが判定されるのが好ましい。
本発明の第５の態様によると、第３の態様の位置算出方法において、測位衛星の衛星位置及び現在位置からＤＯＰ値を算出し、現在位置の変化、移動速度の変化及び移動方位の変化に基づいて、現在位置が異常であるか否か及び移動方位が異常であるか否かを判定し、移動体が、確率が最も高いリンク候補点に対応する道路を逸脱していないと判定され、かつ、現在位置が異常ではないと判定されたとき、ＤＯＰ値が第５閾値以下であるか否かに応じて、第１誤差分散を算出するか否かが判定され、移動体が、確率が最も高いリンク候補点に対応する道路を逸脱していないと判定され、かつ、現在位置が異常ではないと判定されたとき、ＤＯＰ値が第５閾値以下であるか否かに応じて、第２誤差分散を算出するか否かが判定されるのが好ましい。
本発明の第６の態様によると、第３から第５のいずれかの態様の位置算出方法において、第１検定量及び第２検定量の値は、現在位置と確率が最も高いリンク候補点位置との間の距離の二乗を、現在位置及び確率が最も高いリンク候補点位置の誤差共分散の和で割った値と、移動方位と確率が最も高いリンク候補点方位との方位差の二乗を、移動方位及び確率が最も高いリンク候補点方位の誤差分散の和で割った値とを無次元化して、足し合わせた値であるのが好ましい。
本発明の第７の態様によると、第３から第６のいずれかの態様の位置算出方法において、第１検定量は、現在位置と確率が最も高いリンク候補点位置との間の距離の二乗を、現在位置及び確率が最も高いリンク候補点位置の誤差共分散の和で割った値であり、第２検定量は、移動方位と確率が最も高いリンク候補点方位との方位差の二乗を、移動方位及び確率が最も高いリンク候補点方位の誤差分散の和で割った値であるのが好ましい。
本発明の第８の態様によると、第１から第７のいずれかの態様の位置算出方法において、複数のリンクを構成する第１リンクと第２リンクとが分岐点ノードから分岐し、かつ確率が最も高いリンク候補点が第１リンクに含まれるとき、移動体が分岐点ノードに対応する交差点を通過してから、該リンク候補点のリンク候補点位置と第２リンクに対応する区間道路との間隔が所定値よりも大きくなるまでの間、複数データのデータ数を減じるのが好ましい。
本発明の第９の態様によると、第１から第７のいずれかの態様の位置算出方法において、移動体が、複数のリンクを構成する第１リンクと第２リンクとが分岐する分岐点ノードに対応する交差点を通過してから、確率が最も高いリンク候補点とは異なるリンク候補点を含むリンクに対応する道路を走行していると判定されている第１期間においては、複数データのデータ数を減じるのが好ましい。
本発明の第１０の態様によると、第９の態様の位置算出方法において、現在位置近傍の始点リンクから移動体の目的地近傍の終点リンクまでの誘導経路に基づいて移動体が経路誘導され、かつ確率が最も高いリンク候補点を含むリンクが誘導経路に含まれている場合であって、移動体が道路を逸脱していないと判定され、かつ交差点と該リンク候補点のリンク候補点位置との間の距離が所定値以下になったとき、誘導経路に関する案内表示及び案内音声を表示音声出力装置に出力し、誘導経路に基づいて移動体が経路誘導され、かつ確率が最も高いリンク候補点を含むリンクが誘導経路に含まれていない場合であって、移動体が交差点を通過して、確率が最も高いリンク候補点とは異なるリンク候補点を含むリンクに対応する道路を逸脱したと判定されたとき、現在位置近傍の始点リンクから移動体の目的地近傍の終点リンクまでの新たな誘導経路を探索するのが好ましい。
本発明の第１１の態様によると、位置算出装置は、移動体の現在位置、移動速度及び移動方位を算出する移動情報算出部と、現在位置、移動速度、及び移動方位に基づいて、現在位置及び移動方位の推定誤差共分散を算出する推定誤差共分散算出部と、現在位置、移動方位、推定誤差共分散、現在位置から現在位置を含む所定の領域に含まれる複数のリンク候補点の各リンク候補点のリンク候補点位置、各リンク候補点のリンク候補点方位、領域に含まれる複数のリンクの位置の第１誤差分散、及び複数のリンクの方位の第２誤差分散を用いた演算として、現在位置とリンク候補点位置との間の距離及び移動方位とリンク候補点方位との方位差から、リンク候補点位置までの所定距離間隔毎に得られた距離及び方位差を含む複数データを積算することによって、移動体が各リンク候補点に対応する道路を走行している確率に対応した評価量を各リンク候補点のリンク毎に計算する確率計算部と、複数のリンク候補点のうち確率が最も高いリンク候補点に移動体が位置しているとして、評価量に基づき現在位置を算出する位置算出部とを備える。
本発明の第１２の態様によると、第１１の態様の位置算出装置において、第１誤差分散を、距離に基づいて算出するとともに、第２誤差分散を、方位差に基づいて算出する誤差分散算出部をさらに備えるのが好ましい。確率計算部は、複数データの各々に含まれる距離の二乗を第１誤差分散と現在位置の推定誤差分散との和で割った値と、複数データの各々に含まれる方位差の二乗を第２誤差分散と移動方位の推定誤差分散との和で割った値との和を、複数データについて積算することによって、評価量を算出する。
本発明の第１３の態様によると、第１１または第１２の態様の位置算出装置において、現在位置と確率が最も高いリンク候補点のリンク候補点位置との間の距離の二乗を第１誤差分散と現在位置の推定誤差分散との和で割った値が第１閾値よりも大きいか否かに基づいて、現在位置と該リンク候補点位置とに差があるか否かを判定する第１判定部と、移動方位と確率が最も高いリンク候補点のリンク候補点方位との方位差を第２誤差分散と移動方位の推定誤差分散との和の平方根で割った値が第２閾値よりも大きいか否かに基づいて、移動方位と該リンク候補点方位とに差があるか否かを判定する第２判定部と、現在位置と該リンク候補点位置とに差があるとき、または移動方位と該リンク候補点方位とに差があるとき、移動体が、確率が最も高いリンク候補点に対応する道路を逸脱したと判定するとともに、現在位置と該リンク候補点位置とに差がないとき、かつ移動方位と該リンク候補点方位とに差がないとき、移動体は、確率が最も高いリンク候補点に対応する道路を走行していると判定する第３判定部とをさらに備えるのが好ましい。位置算出部は、第３判定部によって、移動体が、確率が最も高いリンク候補点に対応する道路を逸脱したと判定されたときは、移動体は現在位置に位置しているとして算出するとともに、移動体が、確率が最も高いリンク候補点に対応する道路を走行していると判定されたときは、移動体は、該リンク候補点に位置しているとして、現在位置を算出する。
本発明の第１４の態様によると、第１１から第１３のいずれかの態様の位置算出装置において、測位衛星から発信される測位信号を受信する受信部と、移動体の速度、角速度及び加速度のうちの少なくとも１つを含むセンサデータを測定する測定部とをさらに備えるのが好ましい。移動情報算出部は、現在位置、移動速度及び移動方位を、測位信号とセンサデータとに基づいて算出し、推定誤差共分散算出部は、推定誤差共分散を、測位信号及びセンサデータを用いて算出する。
本発明の第１５の態様によると、第１１から第１４のいずれかの態様の位置算出装置において、複数のリンクを構成する第１リンクと第２リンクとが分岐点ノードから分岐し、かつ確率が最も高いリンク候補点が第１リンクに含まれるとき、移動体が分岐点ノードに対応する交差点を通過してから、該リンク候補点のリンク候補点位置と第２リンクに対応する区間道路との間隔が所定値よりも大きくなるまでの間、複数データのデータ数を減じるように設定する設定部をさらに備えるのが好ましい。

本発明によれば、移動体の位置を算出する際の道路逸脱判定の誤りを低減することができる。

一実施の形態の位置算出装置の構成を示す図である。 位置算出装置の動作を示す図である。 位置算出装置のマップマッチング処理の動作を示す図である。 地図情報を読み込むときの４つのメッシュ領域における位置算出装置の位置を示す図である。 リンク間の距離を示す図である。 位置算出装置及びリンク候補点の位置を示す図である。 位置算出装置の道路逸脱判定処理の動作を示す図である。 位置算出装置の地図精度算出処理の動作を示す図である。 分岐後にミスマッチングが続く状況を示す図である。 分岐後にミスマッチングから正しいマッチングになる状況を示す図である。 位置算出装置の道路逸脱判定処理の動作を示す図である。 分岐後のマップマッチングの状況を示す図である。 分岐後のマップマッチングの状況を示す図である。 位置算出装置の道路逸脱判定処理の動作を示す図である。 位置算出装置の距離系列データ数の設定処理の動作を示す図である。 分岐直後の第一リンク候補点の表示を説明する図である。 位置算出装置の表示及び音声の出力処理の動作を示す図である。 分岐直後の第一リンク候補点の表示を説明する図である。 位置算出装置の表示及び音声の出力処理の動作を示す図である。 位置算出装置の経路探索の実施判定処理の動作を示す図である。

図１に本発明の一実施の形態における位置算出装置１００の構成を示す。位置算出装置１００は、受信装置１０１、速度センサ装置１０２、角速度センサ装置１０３、加速度センサ装置１０４、経路探索指示装置１０５、地図情報記憶装置１０６、表示音声出力装置１０７、地図精度記憶装置１０８、および演算装置１１０を含んでいる。

受信装置１０１は、アンテナを含んでおり、ダウンコンバート、アナログ・デジタル・コンバート、直交検波、Ｃ／Ａ（coarse/acquisition）コード生成、相関検出、復号の処理機能を有している。ＧＰＳ衛星、ＧＬＯＮＡＳＳ衛星や擬似衛星などの測位衛星は宇宙あるいは地上から測位のための信号を発信する装置である。測位衛星から送られてきた信号（測位信号）をアンテナで受信し、測位衛星の軌道情報、発信状態の情報や電離層遅延計算パラメータなどを含む航法メッセージを検出し、受信時刻、擬似距離（衛星からの距離）、ドップラ周波数及び信号強度などの観測データを測定する。受信時刻及び軌道情報をもとに測位衛星の位置を算出し、測位衛星の位置及び擬似距離をもとに受信装置１０１の位置（受信位置）を算出する。また、軌道情報をもとに求めた受信時刻付近の測位衛星の位置から測位衛星の速度を算出し、測位衛星の位置及び速度、ならびにドップラ周波数をもとに受信装置１０１の速度及び方位（速度ベクトル、受信速度及び受信方位）を算出する。

車速センサなどの速度センサ装置１０２は、位置算出装置１００が搭載されている自動車などの移動体では車軸などに設置され、車軸の回転に応じたパルス数を計測し、パルス数を出力する。ジャイロなどの角速度センサ装置１０３は、角速度に応じた信号を出力する。加速度計などの加速度センサ装置１０４は、自動車などの移動体の進行方向に対して垂直方向に設置され、加速度に応じた信号を出力する。

リモートコントローラ、タッチパネルやマイクなどの経路探索指示装置１０５は、入力された目的地を出力する。

ＣＰＵ（central processing unit，中央演算処理装置）及びメモリなどから構成される演算装置１１０は、センサ出力補正装置１１１、受信異常判定装置１１２、測位演算装置１１３、マップマッチング装置１１４、経路探索装置１１５、経路誘導装置１１６、および地図精度算出装置１１７を含んでいる。

センサ出力補正装置１１１は、速度センサ装置１０２からの出力に速度センサ装置１０２のスケールファクタを掛け合わせ、速度（センサ速度）を算出する。角速度センサ装置１０３からの出力から角速度センサ装置１０３のバイアスを引き、角速度センサ装置１０３のスケールファクタを掛け合わせることにより、角速度（センサ角速度）を算出する。また、加速度センサ装置１０４からの出力から加速度センサ装置１０４のバイアスを引き、加速度（センサ加速度）を算出する。

受信異常判定装置１１２は、受信装置１０１からの受信位置、受信速度及び受信方位、センサ出力補正装置１１１からのセンサ速度、センサ角速度、センサ加速度を用いて、受信位置、受信速度及び受信方位の異常を判定する。

測位演算装置１１３は、受信異常判定装置１１２からの受信位置、受信速度及び受信方位、それらの受信異常判定結果、センサ出力補正装置１１１からのセンサ速度、センサ角速度、センサ加速度を用いて、位置算出装置１００の位置、方位及び速度、位置及び方位の誤差の共分散を算出する。

マップマッチング装置１１４は、地図情報記憶装置１０６に格納されているリンク情報を読み込む。測位演算装置１１３からの位置算出装置１００の位置及び方位、それらの共分散をもとに、位置算出装置１００の位置からリンクに垂線を下ろした点（リンク候補点）の評価量を計算し、道路を走行している確率が最も高い評価量を持つリンク候補点を第一リンク候補点とする。ノードの位置、リンク方位、それらの分散、幅員情報、位置算出装置１００の位置及び方位、それらの共分散をもとに、位置算出装置１００が道路を逸脱しているかどうかを判定する。

経路探索装置１１５は、マップマッチング装置１１４からの第一リンク候補点のリンク番号、目的地に最も近いリンク番号をもとに、それらのリンクをつなぐ複数の経路を探索し、距離が最も短い経路を選定する。

経路誘導装置１１６は、マップマッチング装置１１４から第一リンク候補点のリンク番号及び位置を取得し、経路探索装置１１５から誘導経路のリンク番号を取得し、地図情報記憶装置１０６から案内表示などの情報を取得する。右左折する分岐と第一リンク候補点の位置までの距離が所定の値以下になったときに、誘導開始信号をオンにして、誘導方向を表す誘導方向信号、案内表示、誘導開始信号などの誘導情報を表示音声出力装置１０７に送る。また、第一リンク候補点のリンク番号が、誘導経路のリンク番号と異なった場合、経路探索開始信号をオンにして、経路探索開始信号を経路探索装置１１５に送る。

地図精度算出装置１１７は、道路を逸脱していないと判定されていて、かつ推定位置の誤差の分散が閾値以下である場合、第一リンク候補点から推定位置までのベクトルを算出し、北方向のベクトル成分により正負を決定し、ベクトルの大きさ及び正負を用いて、現在位置におけるリンク位置の誤差を算出する。また、道路を逸脱していないと判定されていて、かつ推定方位の誤差の分散が閾値以下である場合、推定方位と第一リンク候補点の方位との差を現在位置におけるリンク方位の誤差としてカウントする。現在位置を含む所定の領域におけるリンク位置及び方位の誤差の積算値及び二乗和にそれぞれ現在位置におけるリンク位置及び方位の誤差及び二乗値を加算し、現在位置を含む所定の領域におけるリンク位置及び方位の誤差のカウント数に１を加算する。リンク位置及び方位の誤差の積算値、二乗和及びカウント数を用いて、現在位置を含む所定の領域におけるリンク位置及び方位の誤差分散を算出する。

ハードディスクやメモリなどの地図情報記憶装置１０６は、ノード番号、ノード座標、リンク番号、リンクの幅員や属性などのリンク情報、および案内表示などの情報を所定のメッシュ領域ごとに格納している。ノードは道路上の点であり、リンクはノードを結んだ直線である。

モニタ及びスピーカなどの表示音声出力装置１０７は、マップマッチング装置１１４から位置算出装置１００の位置及び方位、第一リンク候補点のリンク番号、位置及び方位、道路逸脱判定結果などのマップマッチング結果を取得し、経路誘導装置１１６から誘導開始信号及び誘導情報を取得し、地図情報記憶装置１０６から位置算出装置の位置周辺のリンク情報を読み込み、リンク及び第一リンク候補点の位置及び方位を描画する。また、誘導開始信号がオンである場合、案内表示を出力し、誘導方向を音声で出力する。

ハードディスクやメモリなどの地図精度記憶装置１０８は、所定の領域ごとのリンク位置の誤差及び方位の誤差に関する分散、カウント数、積算値及び二乗和の積算値を格納している。

図１に示す位置算出装置１００の動作手順を、図２を用いて説明する。

ステップＳ２０１において、受信装置１０１は、測位衛星から送られてきた信号（測位信号）をアンテナで受信し、測位衛星の軌道情報、発信状態の情報や電離層遅延計算パラメータなどを含む航法メッセージを検出し、受信時刻、擬似距離、ドップラ周波数及び信号強度などの観測データを測定する。受信時刻及び軌道情報をもとに測位衛星の位置を算出し、測位衛星の位置及び擬似距離をもとに受信位置を算出する。また、軌道情報をもとに受信時刻付近の測位衛星の位置から測位衛星の速度を算出し、測位衛星の位置及び速度、ならびにドップラ周波数をもとに受信速度及び受信方位を算出し、受信異常判定装置１１２に受信位置、受信速度及び受信方位を送る。受信異常判定装置１１２はこれらの情報を受け取る。

ステップＳ２０２において、速度センサ装置１０２は、移動体の車軸の回転に伴なうパルス数を計測し、センサ出力補正装置１１１に送る。センサ出力補正装置１１１は速度センサ装置１０２の出力を受け取る。角速度センサ装置１０３は、移動体の角速度に対応した信号をセンサ出力補正装置１１１に送る。センサ出力補正装置１１１は角速度センサ装置１０３の出力を受け取る。加速度センサ装置１０４は、移動体の加速度に対応した信号をセンサ出力補正装置１１１に送る。センサ出力補正装置１１１は加速度センサ装置１０４の出力を受け取る。

ステップＳ２０３において、センサ出力補正装置１１１は、速度センサ装置１０２からの出力に速度センサ装置１０２のスケールファクタを掛け合わせ、センサ速度を算出する。角速度センサ装置１０３からの出力から角速度センサ装置１０３のバイアスを引き、角速度センサ装置１０３のスケールファクタを掛け合わせることにより、センサ角速度を算出する。また、加速度センサ装置１０４からの出力から加速度センサ装置１０４のバイアスを引き、センサ加速度を算出する。算出した値を受信異常判定装置１１２及び測位演算装置１１３に送る。

ステップＳ２０４において、受信異常判定装置１１２は、受信装置１０１から受信位置、受信速度及び受信方位を、センサ出力補正装置１１１からセンサ速度、センサ角速度及びセンサ加速度を受け取る。センサ速度及びセンサ角速度から所定時間の位置変化を算出し、この位置変化と所定時間の受信位置の変化との差が閾値以上になった場合、受信位置は異常であると判定し、そうでない場合、受信位置は正常であると判定する。受信速度とセンサ速度との差を算出し、その差が閾値以上である場合、受信速度は異常であると判定し、そうでない場合、受信速度は正常であると判定する。センサ角速度から所定時間の方位変化を算出し、その方位変化と所定時間の受信方位の変化との差が閾値以上である場合、受信方位は異常であると判定し、そうでない場合、正常であると判定する。受信異常判定結果、受信位置、受信速度及び受信方位を測位演算装置１１３に送る。

ステップＳ２０５において、測位演算装置１１３は、受信異常判定装置１１２から受信異常判定結果、受信位置、受信速度及び受信方位を、センサ出力補正装置１１１からセンサ速度、センサ角速度、センサ加速度を受け取って、測位処理を実行する。測位処理の動作手順では、位置算出装置の位置、進行方向の速度及び加速度、方位、方位の角速度及びピッチ角を状態量とし、センサ速度、センサ角速度、センサ加速度、受信位置、受信速度及び受信方位を観測量とすると、式（１）で表される状態方程式（連続型）、および式（２）で表される観測方程式（連続型）が成り立つ。式（１）では、加速度及び角速度を一次マルコフ過程とおいた。ここで、ｘ（ｔ）及びｙ（ｔ）は位置算出装置１００の経度方向の位置及び緯度方向の位置（現在位置）、ｖ（ｔ）及びａ（ｔ）は進行方向（移動方位）の速度（移動速度）及び加速度、θ（ｔ）及びω（ｔ）は方位（移動方位）及び方位の角速度、φ（ｔ）はピッチ角、θｐは方位（移動方位）の予測値、φｐはピッチ角の予測値、αａ及びαωは加速度の時定数の逆数及び角速度の時定数の逆数、σａは加速度の標準偏差、σω及びσφは方位（移動方位）の角速度の標準偏差及びピッチ角の標準偏差、ｗ（ｔ）は平均０、標準偏差１の白色雑音、η（ｔ）は状態量ベクトル、ｖｓ（ｔ）はセンサ速度、ｇｓ（ｔ）はセンサ加速度、ωｓ（ｔ）はセンサ角速度、（ｘｒ（ｔ），ｙｒ（ｔ），ｚｒ（ｔ））は受信位置、ｖｒ（ｔ）は受信速度、θｒ（ｔ）は受信方位、ε（ｔ）は観測雑音ベクトル、ｙ（ｔ）は観測量ベクトルｙ（ｔ）、Ｆ、ＧおよびＨは行列、である。方位（移動方位）の予測値θｐ及びピッチ角の予測値φｐは、式（５）で算出される予測量ベクトルから得られる。

測位演算装置１１３は、受信位置、受信速度及び受信方位が正常（有効）である場合、式（３）〜（７）を用いて、位置算出装置１００の推定位置、推定速度、推定加速度、推定方位、推定角速度及び推定ピッチ角を算出する。式（３）〜（７）によって、カルマンフィルタによる計算ができる。ここで、Ｋ（ｋ）はゲイン行列、Ｒは観測雑音行列、η（ｋ｜ｋ）は推定量ベクトル、η（ｋ｜ｋ＋１）は予測量ベクトル、Ｐ（ｋ｜ｋ）及びＰ（ｋ｜ｋ＋１）は推定量ベクトルη（ｋ｜ｋ）の推定誤差共分散行列及び予測量ベクトルη（ｋ｜ｋ＋１）の推定誤差共分散行列、Φ（Δｔ，αａ，αω）は状態遷移行列、Δｔはサンプリング間隔、Ｑ（ｋ）はシステム雑音行列である。

ここで、受信位置が異常であると判定された場合、観測量ベクトルｙ、行列Ｈ及び観測雑音行列Ｒから、受信位置に関連する要素を削除する。受信速度が異常であると判定された場合、観測量ベクトルｙ、行列Ｈ及び観測雑音行列Ｒから、受信速度に関連する要素を削除する。受信方位が異常であると判定された場合、観測量ベクトルｙ、行列Ｈ及び観測雑音行列Ｒから、受信方位に関連する要素を削除する。また、受信装置１０１から受信位置、受信速度及び受信方位の出力がない場合、観測量ベクトルｙ、行列Ｈ及び観測雑音行列Ｒから、受信位置、受信速度及び受信方位に関連する要素を削除する。

測位演算装置１１３は、位置算出装置１００の位置（現在位置）、方位（移動方位）及び速度（移動速度）、位置及び方位の推定誤差共分散をマップマッチング装置１１４及び地図精度算出装置１１７に送る。

ステップＳ２０６において、マップマッチング装置１１４は、地図情報記憶装置１０６からノード番号、ノード位置、リンク番号、リンクの幅員などの情報を読み込むとともに、地図精度記憶装置１０８から位置算出装置１００の位置を含む領域におけるリンクの位置誤差の分散及び方位誤差の分散を読み込む。所定の数のリンク候補点を進め、その所定距離の前方のリンクにリンク候補点を作成する。全てのリンク候補点について、リンク候補点のリンク方位及び位置算出装置１００の位置を含む所定の領域のリンクの位置誤差の分散をもとにリンク候補点の位置誤差の共分散を計算する。リンク候補点の方位の分散を位置算出装置１００の位置を含む所定の領域のリンクの方位誤差の分散とする。リンク候補点の位置誤差の共分散及び位置算出装置１００の位置を含む所定の領域のリンクの方位誤差の分散、位置算出装置１００の位置の共分散及び方位の共分散、位置算出装置１００及びリンク候補点の位置及び方位をもとに、リンク候補点の評価量を計算する。これらのリンク候補点のうち、道路を走行している確率が最も高い評価量を持つリンク候補点を第一リンク候補点と決定する。次に、リンク候補点の位置及び方位、位置誤差の共分散、方位誤差の分散、幅員情報、位置算出装置１００の位置及び方位、ならびに位置算出装置１００の位置及び方位の各々の共分散をもとに、位置算出装置１００が道路を逸脱しているかどうかを判定する。マップマッチング装置１１４は、第一リンク候補点の位置、方位及びリンク番号、ならびに道路逸脱判定結果を経路探索装置１１５及び経路誘導装置１１６に送るとともに、表示音声出力装置１０７に位置算出装置１００の位置及び方位、ならびに第一リンク候補点のリンク番号、位置及び方位及び道路逸脱判定結果を送る。ステップＳ２０６における処理の詳細については、図３および７を用いて後述する。

ステップＳ２０７において、地図精度算出装置１１７は、測位演算装置１１３から推定位置及び推定方位、それらの誤差の分散を、マップマッチング装置１１４から道路逸脱の判定結果及び第一リンク候補点の位置及び方位を受け取るとともに、推定位置を含む所定の領域におけるリンク位置の誤差及び方位の誤差に関してそれぞれカウント数、積算値及び二乗和の積算値を読み込む。道路を逸脱していないと判定していて、かつ推定位置の誤差の分散が閾値以下である場合、第一リンク候補点から推定位置までのベクトルを算出し、北方向のベクトル成分により正負を決定し、ベクトルの大きさ及び正負を用いて、現在位置におけるリンク位置の誤差を算出する。ここで、ベクトルの方向が東方向から反時計回りに西方向までである（真東を含む）場合、正として、西方向から反時計回りに東方向までである（真西を含む）場合、負とする。現在位置を含む所定の領域におけるリンク位置誤差の積算値及び二乗和の積算値にリンク位置の誤差及びその二乗値を加算し、現在位置を含む所定の領域におけるリンク位置誤差のカウント数に１を加算する。リンク位置誤差の積算値、二乗和の積算値及びカウント数を用いて、現在位置を含む所定の領域におけるリンク位置誤差の分散を算出する。また、道路を逸脱していないと判定していて、かつ推定方位の誤差の分散が閾値以下である場合、推定方位と第一リンク候補点の方位との差を現在位置におけるリンク方位の誤差とする。現在位置を含む所定の領域におけるリンク方位の誤差の積算値及び二乗和の積算値にそれぞれ現在位置におけるリンク方位の誤差及びその二乗値を加算し、現在位置を含む所定の領域におけるリンク方位の誤差のカウント数に１を加算する。リンク方位の誤差の積算値、二乗和の積算値及びカウント数を用いて、現在位置を含む所定の領域におけるリンク方位の誤差分散を算出する。地図精度算出装置１１７は、推定位置を含む所定の領域におけるリンク位置の誤差及び方位の誤差に関して、分散、カウント数、積算値及び二乗和の積算値を地図精度記憶装置１０８に格納する。

ステップＳ２０８において、経路探索指示装置１０５に目的地の入力があった場合、経路探索指示装置１０５は、入力された目的地を経路探索装置１１５に送り、ステップＳ２１１に進む。目的地の入力がない場合、ステップＳ２０９に進む。

ステップＳ２０９において、誘導経路がある場合、ステップＳ２１０に進む。そうでない場合、ステップＳ２１３に進む。

ステップＳ２１０において、第一リンク候補点があるリンクが誘導経路のリンクに含まれている場合、ステップＳ２１２に進む。含まれていない場合、ステップＳ２１１に進む。

ステップＳ２１１において、経路探索装置１１５は、地図情報記憶装置１０６から、目的地付近のリンク情報を読み込み、目的地からリンクまでの距離を計算し、最も近いリンク番号を目的地のリンクとする。マップマッチング装置１１４から第一リンク候補点のリンク番号、位置及び方位を受け取り、第一リンク候補点のリンクからその方位に向かって、目的地のリンクまでつなぐ複数の経路を探索し、最も短い距離の経路を誘導経路とする。経路探索装置１１５は、誘導経路上のリンク番号を経路誘導装置１１６に送る。

ステップＳ２１２において、経路誘導装置１１６は、経路探索装置１１５から誘導経路上のリンク番号を受け取り、マップマッチング装置１１４から第一リンク候補点の位置、方位及びリンク番号、ならびに道路逸脱判定結果を受け取り、地図情報記憶装置１０６から案内表示などの情報を読み込む。道路逸脱判定結果が道路走行であり、すなわち道路を逸脱していないと判定していて、かつ右左折する分岐と第一リンク候補点の位置との間の距離が所定の値以下になったときに、誘導開始信号をオンにして、誘導方向を表す誘導方向信号、案内表示、誘導開始信号などの誘導情報を表示音声出力装置１０７に送る。

ステップＳ２１３において、表示音声出力装置１０７は、マップマッチング装置１１４から位置算出装置１００の位置及び方位、第一リンク候補点のリンク番号、位置及び方位、ならびに道路逸脱判定結果を取得し、経路誘導装置１１６から誘導開始信号及び誘導情報を取得し、地図情報記憶装置１０６から位置算出装置１００の位置周辺のリンク情報を読み込む。道路逸脱判定結果が道路走行である場合、すなわち道路を逸脱していないと判定した場合、第一リンク候補点の位置及び方位で表したカーマーク、ならびに第一リンク候補点の位置周辺のリンクを描画する。道路逸脱判定結果が道路逸脱である場合、位置算出装置１００の位置及び方位で表したカーマーク、ならびに位置算出装置１００の位置周辺のリンクを描画する。また、誘導開始信号がオンである場合、案内表示を出力し、誘導方向を音声で出力する。

図１に示す位置算出装置１００の動作手順（図２）のステップＳ２０６を、図３を用いて説明する。

ステップＳ４０１において、マップマッチング装置１１４が、地図情報記憶装置１０６から過去にリンク情報を読み込んだ場合、ステップＳ４０２に進む。そうでない場合、ステップＳ４０３に進む。

ステップＳ４０２において、位置算出装置１００の位置６０１と４つのメッシュ領域６０２の端との距離が所定距離よりも短い場合、すなわち、図４（ａ）に示す４つのメッシュ領域６０２の端付近の領域６０３（斜線部）に位置算出装置１００の位置６０１がある場合、ステップＳ４０３に進む。そうでない場合、ステップＳ４０４に進む。

ステップＳ４０３において、マップマッチング装置１１４は、位置算出装置の位置を含むメッシュ領域と、位置算出装置１００の位置６０１から近い３つのメッシュ領域のリンク情報を読み込む。図４（ｂ）に示すように、それらの３つのメッシュ領域と位置算出装置１００の位置６０１を含むメッシュ領域とで構成される４つのメッシュ領域６１２においては、位置算出装置１００の位置６０１は４つのメッシュ領域６１２の端付近の領域６１３（斜線部）に含まれていない。ステップＳ４０４に進む。

ステップＳ４０４において、リンク候補点がある場合、ステップＳ４０６に進む。リンク候補点がない場合、ステップＳ４０５に進む。

ステップＳ４０５において、マップマッチング装置１１４は、位置算出装置１００の位置を含むメッシュ領域の全てのリンクと位置算出装置１００の位置との距離を算出し、距離が短い順位に所定数のリンクを選定し、それらのリンクに対してリンク候補点を生成する。リンク候補点のリンク方位と位置算出装置の方位との差が９０度以下になるリンク方位をリンク候補点の方位と設定する。

ステップＳ４０６において、マップマッチング装置１１４は、測位演算装置１１３から位置算出装置の位置、方位及び速度、ならびに位置算出装置の位置及び方位の誤差共分散を受け取る。各リンク候補点の位置を前方にリンクに沿って位置算出装置１００の速度に処理周期をかけることにより求められる走行距離分移動させる。

ステップＳ４０７において、マップマッチング装置１１４は、位置算出装置１００の速度に処理周期をかけ、前回の走行距離に足し合わせる。走行距離が所定距離以上である場合、マップマッチングを行うため、走行距離をゼロに設定して、ステップＳ４０８に進む。そうでない場合、ステップＳ４１４に進む。

ステップＳ４０８において、マップマッチング装置１１４は、各リンク候補点の所定距離の前方にあるリンク全てにリンク候補点を新たに追加する。

ステップＳ４０９において、マップマッチング装置１１４は、前回の第一リンク候補点（前方移動済み）が所定角度以下である分岐からリンク間の距離が所定距離以下の地点までにある場合、評価量の計算で用いる所定距離間隔の位置算出装置１００の位置及び方位の距離系列データの数を、通常の第一の所定数よりも小さい第二の所定数に設定する。所定距離間隔ごとの位置算出装置１００の位置及び方位に基づくデータを距離系列データと呼ぶことにする。

前回の第一リンク候補点が、所定角度以下である分岐から先であって、リンク間の距離が所定距離より大きくなる地点よりも先にある場合、リンク間の距離が所定距離より大きくなってからの距離系列データの数を設定する。但し、距離系列データの数が通常の第一の所定数よりも大きい場合、通常の第一の所定数を設定する。前回の第一リンク候補点が所定角度よりも大きい分岐を通過した場合、距離系列データに通常の第一の所定数を設定する。図５は、リンク７０１に対応する区間道路を含む２車線の道路７０６から、リンク７０２に対応する区間道路を含む１車線の道路７０７が分岐している状況を示している。リンク７０１とリンク７０２間の距離７０３は、図５に示すように、リンク７０１が表す道路及びリンク７０２が表す道路の幅員を考慮して、お互いの道路に近い車線の中央７０４および７０５間の距離である。なお、中央７０４及び７０５のいずれか一方が第一リンク候補点の近傍に位置するため、距離７０３は、リンク７０１及び７０２のうちの第一リンク候補点を含まないリンクと第一リンク候補点との距離に置き換えても良い。

ステップＳ４１０において、マップマッチング装置１１４は、地図精度記憶装置１０８から、位置算出装置１００の位置を含む所定の領域のリンク位置及び方位の分散を読み込む。式（８）〜（１１）を用いて、各リンク候補点の評価量Ｔを計算する。ここで、（ｘｅ，ｙｅ）は位置算出装置１００の位置、θｅは位置算出装置１００の方位、（ｘｌ，ｙｌ）は位置算出装置１００の位置からリンクに垂線を下ろしたリンク候補点の位置、θｌはリンク候補点の方位、Σｅは位置算出装置１００の位置及び方位の誤差共分散行列、Σｌはリンク候補点の位置及び方位の誤差共分散行列、σｐｌ及びσθｌは位置算出装置１００の位置を含む周辺領域のリンクの位置誤差の標準偏差及び方位誤差の標準偏差、ｍは距離系列データの番号、ｎは距離系列データの数である。但し、リンク８０３の幅員、及び位置算出装置１００が搭載されている移動体の走行方向を考慮して、図６に示すように、リンク候補点８０２の位置（ｘｌ，ｙｌ）を、位置算出装置１００の位置に対応する位置８０１に最も近い車線の中央に配置する。上述したように、複数個の距離系列データのそれぞれに含まれる位置算出装置１００の位置（ｘｅ，ｙｅ）の履歴は、ステップＳ４０７における所定距離毎に等距離間隔に位置する。式（８）において、評価量Ｔは距離系列データの番号ｉ＝ｍ−ｎ＋１からｉ＝ｍまでのｎ個の距離系列データ、すなわちリンク候補点の位置（ｘｌ，ｙｌ）から手前ｎ個（移動方向の逆方向にｎ個）の距離系列データが積算されて求められる。距離系列データの番号ｉにおける距離系列データには、位置算出装置１００の位置（ｘｅ，ｙｅ）からリンク候補点の位置（ｘｌ，ｙｌ）までの距離を表すベクトル（Δｘ，Δｙ）、位置算出装置１００の方位θｅ及びリンク候補点の方位θｌの方位差Δθ、位置算出装置及びリンク候補点の位置及び方位の誤差分散、式（８）のサンメーションΣ内の要素の値などが含まれる。サンメーションΣ内の要素の値は、１個の距離系列データに対応し、位置算出装置１００の位置（ｘｅ，ｙｅ）とリンク候補点の位置（ｘｌ，ｙｌ）との間の距離を無次元化した値と、位置算出装置１００の移動方位θｅとリンク候補点の方位θｌとの方位差Δθを無次元化した値との和である。位置算出装置１００の位置（ｘｅ，ｙｅ）とリンク候補点の位置（ｘｌ，ｙｌ）との間の距離を無次元化した値は、その距離を表すベクトル（Δｘ，Δｙ）の長さ（距離）の二乗を、位置算出装置１００の位置（ｘｅ，ｙｅ）を含む周辺領域のリンクの位置誤差の標準偏差σｐｌの二乗（誤差分散）と位置算出装置１００の位置（ｘｅ，ｙｅ）の推定誤差分散との和で割ることによって得られる。位置算出装置１００の移動方位θｅとリンク候補点の方位θｌとの方位差Δθを無次元化した値は、その方位差Δθの二乗を、上記周辺領域のリンクの方位誤差の標準偏差σθｌの二乗（誤差分散）と位置算出装置１００の方位θｅの推定誤差分散との和で割ることによって得られる。位置算出装置１００及びリンク候補点８０２の位置及び方位の誤差共分散が正規分布に従うと仮定すると、評価量Ｔは自由度２×ｎのχ二乗分布に従う。自由度２×ｎは、位置（距離）及び方位からなる２種類のパラメータと、距離系列データの数ｎとに基づいている。このため、その自由度２のχ二乗分布の確率密度関数に評価量Ｔを代入すると、位置算出装置が道路（リンク）上を走行しているときに、位置算出装置１００の位置及び方位とリンク候補点の位置及び方位とから算出される差の値が生じる確率、すなわち、位置及び方位の差を考慮した存在確率に変換できる。言い換えると、位置算出装置１００が道路（リンク）上を走行している確率に変換できる。

ステップＳ４１１において、マップマッチング装置１１４は、リンク候補点の中から評価量Ｔが小さい（道路を走行している確率が高い）順に所定数のリンク候補点を選定し、それ以外のリンク候補点の情報を除去する。マップマッチングの演算量を低減するためである。

ステップＳ４１２において、マップマッチング装置１１４は、リンク候補点の中から評価量Ｔが最も小さい（道路を走行している確率が最も高い）リンク候補点を第一リンク候補点とする。

ステップＳ４１３において、マップマッチング装置１１４は、位置算出装置１００の位置及び方位、リンク候補点の位置及び方位、それらの共分散、ならびに受信装置１０１が受信している対向の衛星数をもとに道路を逸脱したかどうかを判定する。ステップＳ４１３における処理の詳細については、図７を用いて後述する。

ステップＳ４１４において、マップマッチング装置１１４は、経路探索装置１１５及び経路誘導装置１１６に位置算出装置１００の位置及び方位、第一リンク候補点の位置、方位及びリンク番号、ならびに道路逸脱判定結果を送る。

図１に示す位置算出装置１００の動作手順（図３）のステップＳ４１３を、図７を用いて説明する。

ステップＳ９０１において、下記に示すように、自車の推定位置とリンクとの距離に差があるかどうかの仮説を立てる。
・帰無仮説Ｈｐ０：位置算出装置１００の推定位置とリンクとの距離に差がない。
・対立仮説Ｈｐ１：位置算出装置１００の推定位置とリンクとの距離に差がある。

式（１２）〜（１５）を用いて、位置算出装置１００の推定位置とリンクとの距離の二乗を、位置算出装置１００の推定位置の誤差共分散Σｐｅとリンク位置の誤差共分散Σｐｌとの和で割ることによって無次元化し、検定量Ｔｐを算出する。式（１２）において、位置算出装置１００の推定位置とリンクとの距離（位置の差）は位置データΔｘ及びΔｙで表されている。

位置算出装置１００の推定位置の誤差及びリンク位置の誤差は正規分布に従うと仮定すると、検定量Ｔｐは自由度１のχ二乗分布に従う。自由度１は、位置（距離）のみの１種類のパラメータに基づいている。そこで、有意水準αを定め、この仮説に対する検定を自由度１のχ二乗分布に基づいて行うと、以下のように仮説を採択できる。検定量Ｔｐが自由度１及び有意水準αのχ二乗値以下である場合、帰無仮説Ｈｐ０を棄却できず、自車の推定位置とリンクとの距離に差がないと判定する。検定量Ｔｐが自由度１及び有意水準αのχ二乗値よりも大きい場合、帰無仮説Ｈｐ０を棄却し、自車の推定位置とリンクとの距離に差があると判定する。

ステップＳ９０２においては、下記に示すように、位置算出装置１００の推定方位とリンク方位とに差があるかどうかの仮説を立てる。
・帰無仮説Ｈθ０：位置算出装置１００の推定方位とリンク方位とに差がない。
・対立仮説Ｈθ１：位置算出装置１００の推定方位とリンク方位とに差がある。

式（１６）〜（１８）を用いて、位置算出装置１００の推定方位とリンク方位との方位差を、位置算出装置１００の推定方位の誤差共分散（σθｅ）＾２とリンク位置の誤差共分散（σθｌ）＾２との和の平方根で割ることによって無次元化し、検定量Ｔθを算出する。式（１６）において、位置算出装置１００の推定方位とリンク候補点の方位との差はΔθで表されている。

位置算出装置１００の推定方位の誤差及びリンク方位の誤差は正規分布に従うと仮定すると、検定量Ｔθは正規分布に従う。そこで、有意水準αを定め、この仮説に対する両側検定を正規分布に基づいて行うと、以下のように仮説を採択できる。検定量Ｔθが有意水準αの正規分布の値以下である場合、帰無仮説Ｈθ０を棄却できず、位置算出装置１００の推定方位とリンク方位とに差がないと判定する。検定量Ｔθが有意水準αの正規分布の値よりも大きい場合、帰無仮説Ｈθ０を棄却し、位置算出装置１００の推定方位とリンク方位とに差があると判定する。

ステップＳ９０３において、下記の条件（ａ−１）〜（ａ−３）の全てを満たした場合、移動体が地下駐車場に入庫したと判定する。
（ａ−１）位置算出装置１００と第一リンク候補点との距離が所定距離以上である。
（ａ−２）受信装置１０１が受信している対向の衛星数が０である。
（ａ−３）第一リンク候補点のリンク属性がトンネルでない。

ステップＳ９０３において、下記の条件（ｂ−１）及び（ｂ−２）のいずれかを満たした場合、地下駐車場を出庫したと判定する。
（ｂ−１）受信装置１０１が受信している対向の衛星数が１以上である。
（ｂ−２）第一リンク候補点のリンク属性がトンネルである。

ステップＳ９０３において、上記以外の場合、前回の判定結果を今回の判定結果とする。

ステップＳ９０４において、下記の条件（ｃ−１）〜（ｃ−３）のいずれかひとつを満たした場合、道路逸脱と判定する。そうでない場合、道路走行と判定する。
（ｃ−１）位置算出装置１００の推定位置とリンクとの距離に差がある。
（ｃ−２）位置算出装置１００の推定方位とリンク方位とに差がある。
（ｃ−３）位置算出装置１００が地下駐車場にいる。

ステップＳ９０５において、ステップＳ９０４で道路逸脱と判定した場合、ステップＳ９０８に進む。そうでない場合、ステップＳ４１４に進む。

ステップＳ９０６において、下記の条件（ｄ−１）および（ｄ−２）のいずれかを満たす場合、入庫中と判定する。そうでない場合、出庫したと判定する。
（ｄ−１）位置算出装置１００が地下駐車場にいると判定した。
（ｄ−２）位置算出装置１００と第一リンク候補点との距離が所定距離以上である。

ステップＳ９０７において、入庫中と判定した場合、ステップＳ９０８に進む。そうでない場合、ステップＳ４１４に進む。

ステップＳ９０８において、リンク候補点の現在の位置及び方位を残し、それ以外の過去の距離系列データを消去して、距離系列データの数を１とする。

図１に示す位置算出装置１００の動作手順（図３）のステップＳ２０７を、図８を用いて説明する。

ステップＳ１００１において、地図精度算出装置１１７は、測位演算装置１１３から推定位置の誤差の分散を受け取るとともに、マップマッチング装置１１４から道路逸脱の判定結果を受け取る。道路逸脱の判定結果が道路走行、すなわち道路逸脱していないという判定結果であり、かつ、推定位置の誤差の分散が閾値以下である場合、ステップＳ１００２に進み、そうでない場合、ステップＳ１００４に進む。

ステップＳ１００２において、地図精度算出装置１１７は、測位演算装置１１３から推定位置を受け取るとともに、マップマッチング装置１１４から第一リンク候補点の位置を受け取る。推定位置と第一リンク候補点との距離を計算し、現在位置におけるリンク位置の誤差とする。ここで、第一リンク候補点から推定位置までのベクトルの北方向成分が正である場合、すなわちベクトルの方向が東方向から反時計回りに西方向までである（真東を含む）場合、リンク位置の誤差を正とする。また、第一リンク候補点から推定位置までのベクトルの北方向成分が負である場合、すなわち西方向から反時計回りに東方向までである（真西を含む）場合、リンク位置の誤差を負とする。

ステップＳ１００３において、地図精度算出装置１１７は、地図精度記憶装置１０８から現在位置を含む所定の領域におけるリンク位置の誤差に関するカウント数、積算値及び二乗和の積算値を読み込む。現在位置を含む所定の領域におけるリンク位置の誤差の積算値に現在位置におけるリンク位置の誤差を加算し、現在位置を含む所定の領域におけるリンク位置の誤差の二乗和の積算値に現在位置におけるリンク位置の誤差の二乗値を加算し、現在位置を含む所定の領域におけるリンク位置の誤差のカウント数に１を加算する。加算後のリンク位置の誤差の積算値を、加算後のリンク位置の誤差のカウント数で割ることにより、リンク位置の誤差の平均値を算出する。加算後のリンク位置の誤差の二乗和の積算値を、加算後のリンク位置の誤差のカウント数で割ることにより、リンク位置の誤差の二乗の平均値を算出する。リンク位置の誤差の二乗の平均値からリンク位置の誤差の平均値の二乗を引くことにより、現在位置を含む所定の領域におけるリンク位置の誤差の分散を算出する。地図精度記憶装置１０８に現在位置を含む所定の領域におけるリンク位置の誤差に関する分散、カウント数、積算値及び二乗和の積算値を格納する。

ステップＳ１００４において、地図精度算出装置１１７は、測位演算装置１１３から推定方位の誤差の分散を受け取る。道路逸脱の判定結果が道路走行、すなわち道路逸脱していないという判定結果であり、かつ、推定方位の誤差の分散が閾値以下である場合、ステップＳ１００４に進み、そうでない場合、ステップＳ２０８に進む。

ステップＳ１００５において、地図精度算出装置１１７は、測位演算装置１１３から推定方位を受け取るとともに、マップマッチング装置１１４から第一リンク候補点の方位を受け取る。推定方位と第一リンク候補点の方位との差を計算し、現在位置におけるリンク方位の誤差とする。

ステップＳ１００６において、地図精度算出装置１１７は、地図精度記憶装置１０８から現在位置を含む所定の領域におけるリンク方位の誤差に関するカウント数、積算値及び二乗和の積算値を読み込む。現在位置を含む所定の領域におけるリンク方位の誤差の積算値に現在位置におけるリンク方位の誤差を加算し、現在位置を含む所定の領域におけるリンク方位の誤差の二乗和の積算値に現在位置におけるリンク方位の誤差の二乗値を加算し、現在位置を含む所定の領域におけるリンク方位の誤差のカウント数に１を加算する。加算後のリンク方位の誤差の積算値を、加算後のリンク方位の誤差のカウント数で割ることにより、リンク方位の誤差の平均値を算出する。加算後のリンク方位の誤差の二乗和の積算値を、加算後のリンク方位の誤差のカウント数で割ることにより、リンク方位の誤差の二乗の平均値を算出する。リンク方位の誤差の二乗の平均値からリンク方位の誤差の平均値の二乗を引くことにより、現在位置を含む所定の領域におけるリンク方位の誤差の分散を算出する。地図精度記憶装置１０８に現在位置を含む所定の領域におけるリンク方位の誤差に関する分散、カウント数、積算値及び二乗和の積算値を格納する。

以上で説明した本実施の形態の位置算出装置１００は、以下のような作用効果を奏する。
（１）地図のリンクデータの誤差は地域ごと及び地図メーカごとに異なる。このため、地図誤差を考慮しないマップマッチングや地図誤差が一定であるとしたマップマッチングでは、道路を逸脱したか否かの判定に誤りを生じることがある。例えば、実際のリンクの誤差分散が想定よりも大きい場合、道路を走行していても、想定した範囲にリンクが含まれず、誤って道路を逸脱したと判定してしまうことが頻繁に発生する。また、実際のリンクの誤差分散が想定よりも小さい場合、道路を逸脱しても、想定した範囲にリンクが含まれ、誤ってリンクにマッチングしてしまうことが頻繁に発生する。本実施の形態の位置算出装置１００の動作手順（図２、図３、図７、図８）によれば、推定位置及び推定方位の誤差が閾値以下であるとき、すなわち精度が良いときに、推定位置及び推定方位、ならびにリンク位置及びリンク方位をもとに現在位置を含む所定の領域におけるリンク位置及び方位の誤差の分散を算出する。これにより、地図のリンク誤差が地域ごとや地図メーカごとに異なっている場合でも、正確なリンク位置及び方位の誤差分散を用いて、マップマッチングを行うことができ、道路逸脱判定の誤りを低減することができる。

（２）位置算出装置１００の位置とリンク候補点の位置との距離、および位置算出装置１００の方位とリンク候補点の方位との差をもとに、位置算出装置１００の位置・方位とリンク候補点の位置・方位との距離系列データの一致度を評価量として計算して、第一リンク候補点を選定する。この選定手法は、過去の経路をもとに計算するため、合流手前で間違ったリンクにマッチングすることを防ぐことができる。しかし、従来技術によると、図９に示す狭角分岐のケースでは、間違ったリンクにマッチングする場合がある。図９において、リンク１１００はリンク１１０４および１１０５に分岐する。移動体は、リンク１１００およびリンク１１０４を走行する。リンク１１００からの分岐直後におけるリンク１１０４の位置及び方位の誤差、ならびに位置算出装置１００の位置（移動体の検出位置）１１０１の位置誤差の影響で、分岐直後のリンク１１０４を走行する移動体の検出位置と、移動体が走行していない分岐直後のリンク１１０５との距離及び方位の差が、移動体の検出位置とリンク１１０４との距離及び方位の差よりも小さくなる場合がある。このため、移動体が走行していないリンク１１０５上の誤ったリンク候補点が第一リンク候補点１１０３ａと決定されてしまい、距離系列データが長いと、分岐直後における移動体の検出位置１１０１とリンク１１０４との距離及び方位の差の影響で、しばらく走行しても正しいリンク１１０４のリンク候補点１１０２が選定されない。

本実施の形態の位置算出装置１００の動作手順（図２，図３，図７，図８）によれば、ステップＳ４０９でマップマッチング装置１１４が、所定角度以下の分岐からリンク１１０４とリンク１１０５との間の距離が所定距離以下を保つ間は、評価量の計算に用いる距離系列データの数を減らす。距離系列データの数Ｃは、図９及び図１０に示すように、移動体の検出位置１１０１の数で表される。図９及び図１０において、移動体が分岐前のリンク１１００を走行中の距離系列データの数Ｃは同数であり、例えば５０個であるが、分岐点通過後の距離系列データの数Ｃは、図９と異なって図１０においては例えば５０個から３個に減らしている。このようにして、分岐直後の距離系列データをなるべく評価量の計算に用いないことにより、図１０に示すように、分岐直後におけるリンク１１０４の位置及び方位の誤差、移動体の検出位置１１０１の位置誤差の影響を早く取り除くことができ、早く正しいリンク候補点１１０３ｂを決定できる。

すなわち、推定位置とリンクとの距離、推定方位とリンク方位との差、推定位置、リンク位置、ならびに推定方位及びリンク方位の誤差共分散の距離系列データをもとに評価量を算出するときに、分岐後に距離系列データの数を少なし、分岐直後の距離系列データをなるべく評価量の計算に用いない。これにより、推定位置、リンク位置、推定方位及びリンク方位の誤差による分岐直後のミスマッチする評価量の影響を除去できる。したがって、分岐後に間違ったリンク候補点を選定しても、早く正しいリンク候補点に復帰できる。

（３）図１に示す位置算出装置１００の動作手順（図２，図３，図７，図８）によれば、ステップＳ４１０で位置算出装置１００の位置とリンク候補点の位置との距離、位置算出装置１００の方位とリンク候補点の方位との差をそれらの共分散行列で割る。これにより、無次元化し、位置及び方位の要素を足し合わせた評価量を算出して、第一リンク候補の決定に用いているため、統計学的に正しい距離と方位との足し合わせができ、位置及び方位の誤差や存在確率を考慮した精度の高い決定を行うことができる。また、位置算出装置１００及びリンク候補点の位置及び方位の誤差共分散が正規分布に従うと仮定すると、評価量は自由度２×ｎのχ二乗分布に従う。このため、この自由度２のχ二乗分布の確率密度関数に評価量を代入すると、位置算出装置が道路（リンク）上を走行しているときに、位置算出装置の位置及び方位とリンク候補点の位置及び方位から算出される差の値が生じる確率、すなわち、位置及び方位の差を考慮した存在確率に変換できる。言い換えると、位置算出装置が道路（リンク）上を走行している確率に変換できる。

（４）距離及び方位という単位が異なる変数を経験的な係数でかけて、足し合わせた評価量を用いる手法では、正しいリンク候補の決定精度を上げるためには、数多くの分岐を走行して、そこで収集したデータを用いて、係数の値を導き出す必要があり、開発に長い時間を費やしてしまう。図１に示す位置算出装置１００の動作手順（図２、図３、図７、図８）によれば、ステップＳ２０７でリンクの位置及び方位の誤差分散を算出し、ステップＳ４１０でこれらの誤差分散を用いて、評価量を計算し、ステップＳ４１２で第一リンク候補点を決定できる。そのため、数多くの分岐を走行して、データを収集する必要がなくなり、短い時間で開発を終えることができる。

（５）図１に示す位置算出装置１００の動作手順（図２，図３，図７，図８）によれば、ステップＳ９０１及びステップＳ９０２で仮説検定を用いて、位置算出装置１００の位置及び方位とリンクの位置及び方位とに差があるかどうかを判定する。こうして判定することにより、位置算出装置１００の位置及び方位の誤差、リンクの位置及び方位の誤差や位置算出装置１００の存在確率を考慮するため、統計学的に正しく、精度の高い道路逸脱判定できる。

（６）距離及び方位という単位が異なる変数を経験的な係数でかけて、足し合わせた値をもとに道路逸脱を判定する手法では、道路逸脱の決定精度を上げるためには、数多くの分岐を走行して、そこで収集したデータを用いて、係数の値を導き出す必要があり、開発に長い時間を費やしてしまう。図１に示す位置算出装置１００の動作手順（図２、図３、図７、図８）によれば、ステップＳ２０７でリンクの位置及び方位の誤差分散を算出し、ステップＳ９０１及びＳ９０２でそれらの誤差分散を用いて、評価量を算出して、推定量と第一リンク候補点の位置及び方位とに差があるかどうかを判定する。こうして判定することにより、数多くの分岐を走行して、データを収集する必要がなくなり、短い時間で開発を終えることができる。

―変形例―
（１）図１に示す位置算出装置１００の動作手順（図２，図３，図７，図８）において、ステップＳ９０１及びステップＳ９０２の代わりに、図１１に示すようにステップＳ９１１を設けて、下記の仮説を立てることとしても良い。なお、図１１において、ステップＳ９１１の後に続くステップＳ９０３以降のステップにおける処理は図７と同様であるため、説明を省略する。
・帰無仮説Ｈｐθ０：位置算出装置１００の推定位置とリンクとの距離、位置算出装置１００の推定方位とリンク方位とに差がない。
・対立仮説Ｈｐθ１：位置算出装置１００の推定位置とリンクとの距離、位置算出装置１００の推定方位とリンク方位とに差がある。

式（１９）を用いて、位置算出装置１００の推定位置とリンクとの距離の二乗、および位置算出装置１００の推定方位とリンク方位との方位差の二乗をそれぞれその距離を表す位置データの誤差共分散及びその方位差を表す方位差データの分散で割ることによって無次元化して足し合わせて、検定量Ｔｐθを算出する。式（１９）において、位置算出装置１００の推定位置とリンクとの距離は位置データの差Δｘ及びΔｙで表され、位置算出装置１００の推定方位とリンク方位との方位差は方位差データΔθで表されている。式（８）はｎ個の距離系列データについての積算式であるが、式（１９）は１個の距離系列データについての式である。

位置算出装置１００及びリンク候補点の位置及び方位の誤差共分散が正規分布に従うと仮定すると、検定量Ｔｐθは自由度２のχ二乗分布に従う。自由度２は、位置（距離）及び方位の２種類のパラメータに基づいている。そこで、有意水準αを定め、この仮説に対する検定をχ二乗分布に基づいて行うと、以下のように仮説を採択できる。検定量Ｔｐθが有意水準αのχ二乗値以下である場合、帰無仮説を棄却できず、位置算出装置１００の推定位置とリンクとの距離、位置算出装置１００の推定方位とリンク方位とに差がないと判定する。検定量Ｔｐθが有意水準αのχ二乗値よりも大きい場合、帰無仮説を棄却し、位置算出装置１００の推定位置とリンクとの距離、位置算出装置１００の推定方位とリンク方位とに差があると判定する。このことにより、位置算出装置１００及びリンク候補点の位置及び方位の誤差や存在確率を考慮しており、統計学的に正しく、高精度な道路逸脱判定ができる。

（２）図１に示す位置算出装置１００の動作手順（図２、図３、図７、図８）において、ステップＳ２０１で受信装置１０１は複数の測位衛星の位置及び受信位置をもとにＨＤＯＰ（Ｈｏｒｉｚｏｎｔａｌ Ｄｉｌｕｔｉｏｎ Ｐｏｓｉｔｉｏｎ）も算出し、受信異常判定装置１１２に送る。ステップＳ２０７で地図精度算出装置１１７は、受信異常判定装置１１２から受信位置及び受信方位、それらの異常判定結果、ならびにＨＤＯＰを受け取るとともに、マップマッチング１１４装置から道路逸脱の判定結果及び第一リンク候補点の位置及び方位を受け取る。また、地図精度記憶装置１０８から受信位置を含む所定の領域におけるリンク位置の誤差及び方位の誤差に関してそれぞれカウント数、積算値及び二乗和の積算値を読み込む。道路を逸脱していないと判定して、かつ、受信位置に異常がないと判定して、かつ、ＨＤＯＰが閾値以下である場合、第一リンク候補点から受信位置までのベクトルを算出し、北方向のベクトル成分から正負を決定し、ベクトルの大きさ及び正負から、現在位置におけるリンク位置の誤差を算出する。現在位置を含む所定の領域におけるリンク位置の誤差の積算値及び二乗和の積算値にそれぞれ現在位置におけるリンク位置の誤差及びその二乗値を加算し、現在位置を含む所定の領域におけるリンク位置の誤差のカウント数に１を加算する。リンク位置の誤差の積算値、二乗和の積算値及びカウント数から、現在位置を含む所定の領域におけるリンク位置の誤差の分散を算出する。

また、道路を逸脱していないと判定して、かつ、受信方位に異常がないと判定して、かつ、ＨＤＯＰが閾値以下である場合、受信方位と第一リンク候補点の方位との差を現在位置におけるリンク方位の誤差とする。現在位置を含む所定の領域におけるリンク方位の誤差の積算値及び二乗和の積算値にそれぞれ現在位置におけるリンク方位の誤差及びその二乗値を加算し、現在位置を含む所定の領域におけるリンク方位の誤差のカウント数に１を加算する。リンク方位の誤差の積算値、二乗和の積算値及びカウント数から、現在位置を含む所定の領域におけるリンク方位の誤差の分散を算出する。地図精度算出装置１１７は、現在位置を含む所定の領域におけるリンク位置の誤差及び方位の誤差に関する分散、カウント数、積算値及び二乗和の積算値を地図精度記憶装置１０８に格納する。このことにより、受信位置及び受信方位の精度が良いときに、受信位置及び受信方位をもとにリンク位置及びリンク方位の誤差分散を算出するため、正確なリンク位置及び方位の誤差分散を用いて、マップマッチングを行うことができ、道路逸脱を正しく判定することができる。

（３）図１に示す位置算出装置１００の動作手順（図２，図３，図７，図８）において、ステップＳ４０９の代わりに以下の処理を行っても良い。図１２は、ステップＳ４０９の代わりに行う以下の処理で前回の第一リンク候補点（前方移動済み）１３０１が所定角度以下の分岐を通過した後の様子を示す図である。分岐点でリンク１３００がリンク１３０３及び１３０５に分岐し、リンク１３０３はノード１３０７でリンク１３０４に接続している。一方、リンク１３０５はノード１３０８でリンク１３０６に接続している。第一リンク候補点（前方移動済み）１３０１あるいはリンク候補点１３０２が、図１２に示すように、所定角度以下の分岐後１つ目のリンク１３０３あるいは１３０５上にある場合、距離系列データの数を通常の第一の所定数よりも小さい第二の所定数を設定する。第一リンク候補点１３０１あるいはリンク候補点１３０２が、所定角度以下の分岐後２つ目のリンク１３０４あるいは１３０６以降のリンク上にある場合、距離系列データの数を通常の第一の所定数を上限として、第二の所定数を下限として、２つ目以降のリンクからの距離系列データの数を設定する。つまり、第一リンク候補点１３０１あるいはリンク候補点１３０２が所定角度以下の分岐後２つ目以降のリンク上にあるとき、２つ目以降のリンクからの距離系列データの数に応じて、評価量の計算で用いる距離系列データの数を設定する。２つ目以降のリンクからの距離系列データの数が第一の所定数よりも多い場合、評価量の計算に用いる距離系列データの数に第一の所定数を設定し、２つ目以降のリンクからの距離系列データの数が第二の所定数よりも少ない場合、評価量の計算に用いる距離系列データの数第二の所定数を設定する。それら以外の場合、２つ目以降のリンクからの距離系列データの数を評価量の計算に用いる距離系列データの数に設定する。このことにより、分岐直後におけるリンクの位置及び方位の誤差、ならびに位置算出装置１００の位置誤差の影響を早く取り除くことができ、早く正しいリンク候補点を決定できる。

（４）図１３は、ステップＳ４０９及びステップＳ４１３の代わりの以下の処理で前回の第一リンク候補点（前方移動済み）１４０１が、所定角度以下の分岐を通過した後の様子を示す図である。図１に示す位置算出装置１００の動作手順（図２，図３，図７，図８）において、ステップＳ４１３の代わりに以下の処理を行う。すなわち、第一リンク候補点について道路逸脱判定を行うとともに、所定角度以下の分岐後に第一リンク候補点と異なる分岐したリンク上のリンク候補点がある場合、そのリンク候補点についても道路逸脱判定を行う。また、ステップＳ４０９の代わりに以下の処理を行う。すなわち、前回の第一リンク候補点（前方移動済み）１４０１が所定角度以下の分岐１４００を通過後、分岐１４００で分岐した一方の分岐リンク１４１１上のリンク候補点（以降、分岐リンク候補点と呼ぶ）１４０２について道路走行であると判定された場合、評価量の計算に用いる距離系列データの数を通常の第一の所定数よりも小さい第二の所定数を設定する。第一リンク候補点１４０１が所定角度以下の分岐１４００を通過後、分岐１４００で分岐した他方の分岐リンク１４１２上の分岐リンク候補点１４０３について道路逸脱であると判定された場合、距離系列データの数を通常の第一の所定数を上限として、かつ第二の所定数を下限として、道路逸脱と判定された箇所からの距離系列データの数を評価量の計算に用いる距離系列データの数に設定しても良い。このことにより、分岐直後におけるリンクの位置及び方位の誤差、ならびに位置算出装置１００の位置誤差の影響を早く取り除くことができ、早く正しいリンク候補点を決定できる。

具体的には、ステップＳ４１３及びステップＳ４０９の代わりに図１４及び図１５に示す処理を行う。図１に示す位置算出装置１００の動作手順（図３）のステップＳ４１３の代わりのステップＳ１６０１〜Ｓ１６０３、Ｓ１９０１〜Ｓ１９０４及びＳ４１３を、図１４を用いて説明する。

ステップＳ１６０１において、第一リンク候補点１４０１から、ステップＳ４０７で述べた所定距離後方に所定角度以下の分岐がある場合、第一リンク候補点１４０１が所定角度以下の分岐を通過した直後であると判定して、ステップＳ１６０３に進む。そうでない場合、所定角度以下の分岐を通過した直後でないと判定して、ステップＳ１６０２に進む。

ステップＳ１６０２において、距離系列データの数が第一の所定数未満である場合、第一リンク候補点１４０１と異なる分岐したリンク上の分岐リンク候補点があると判定して、ステップＳ１６０３に進む。そうでない場合、ステップＳ４１３に進む。

ステップＳ１６０３において、第一リンク候補点１４０１のリンクと異なる分岐したリンクを特定し、その上にある分岐リンク候補点を探索する。

ステップＳ１９０１において、図７のステップＳ９０１と同様にして、位置算出装置１００の推定位置と分岐リンクとの距離とに差があるかどうか（自車両の推定位置と分岐リンク候補点の位置とに差があるかどうか）を判定する。

ステップＳ１９０２において、図７のステップＳ９０２と同様にして、位置算出装置１００の推定方位と分岐リンク候補点のリンク方位とに差があるかどうかを判定する。

ステップＳ１９０３において、図７のステップＳ９０３と同様にして、但し、第一リンク候補点の代わりに分岐リンク候補点を用いて、位置算出装置１００を搭載した自車両が地下駐車場に入庫したか否かを判定する。

ステップＳ１９０４において、図７のステップＳ９０４と同様にして、道路逸脱の状態にあるか否かを判定する。但し、第一リンク候補点の代わりに分岐リンク候補点を用いる。

ステップ４１３において、図７に示すステップＳ９０１〜Ｓ９０８の処理を行うことによって、第一リンク候補点１４０１に関して道路逸脱判定を行う。

図１に示す位置算出装置１００の動作手順（図３）のステップＳ４０９の代わりのステップＳ１７０１〜Ｓ１７０５を、図１５を用いて説明する。

ステップＳ１７０１において、ステップＳ４０６により前方に進んだ第一リンク候補点１４０１（前回の処理で決定されたもの）から、ステップＳ４０７で述べた所定距離後方に所定角度以下の分岐がある場合、第一リンク候補点１４０１が所定角度以下の分岐を通過した直後であると判定して、ステップＳ１７０５に進む。そうでない場合、所定角度以下の分岐を通過した直後でないと判定して、ステップＳ１７０２に進む。

ステップＳ１７０２において、前回処理の距離系列データの数が第一の所定数未満である場合、ステップＳ１７０３に進む。そうでない場合、ステップＳ４１０に進む。

前回処理のステップＳ１９０１〜Ｓ１９０４を通じて、分岐リンク候補点について道路逸脱であると判定されたか否かが、ステップＳ１７０３において判定される。分岐リンク候補点１４０３のように、道路逸脱であると判定された場合、ステップＳ１７０４に進む。分岐リンク候補点１４０２にように、道路走行であると判定された場合、ステップＳ１７０５に進む。

ステップＳ１７０４において、前回の距離系列データの数に１を加算する。

ステップＳ１７０５において、距離系列データの数を第二の所定数に設定する。

（５）例えば変形例（４）によれば、図１に示す位置算出装置１００の動作手順（図２，図３，図７，図８）のうち、ステップＳ４１３において、第一リンク候補点が所定角度以下の分岐を通過した後、第一リンク候補点について道路逸脱判定を行うとともに、分岐リンク候補点についても道路逸脱判定を行うことができる。ステップＳ２１３においては、第一リンク候補点が所定角度以下の分岐を通過した後、ステップＳ４１３で行われた分岐リンク候補点の逸脱判定結果が道路走行である場合、それ以外の場合と異ならせるように第一リンク候補点の表示の色を変えることとしても良い。あるいは、第一リンク候補点の表示を点滅させることとしても良い。このときは、分岐した２つのリンクのいずれのリンクについても、移動体が走行している可能性が考えられるリンクであることを意味している。図１６において、分岐直後のハッチングされた第一リンク候補点１５０３ａ及び１５０３ｂの表示色を変えたり、点滅させたりする。こうした表示を行うことにより、運転手は位置算出装置１００が分岐判定を検討中であると考えるため、もし移動体が分岐リンク候補点に位置しているにも関わらず間違って第一リンク候補点を表示しても、その間違いに対する運転手の不安を低減できる。

具体的には、図１に示す位置算出装置１００の動作手順（図３）のステップＳ４１３及びステップ４０９の代わりに図１４及び図１５に示す処理を行うとともに、図１に示す位置算出装置１００の動作手順（図２）のステップＳ２１３の代わりに図１７に示す処理を行う。ステップＳ２１３の代わりに行うステップＳ１８０１〜Ｓ１８１０について、図１７を用いて説明する。

ステップＳ１８０１において、表示音声出力装置１０７は、マップマッチング装置１１４から位置算出装置１００の位置及び方位、第一リンク候補点のリンク番号、位置及び方位、ならびに道路逸脱判定結果を取得し、経路誘導装置１１６から誘導開始信号及び誘導情報を取得し、地図情報記憶装置１０６から位置算出装置１００の位置周辺のリンク情報を読み込む。

ステップＳ１８０２において、位置算出装置１００の位置周辺のリンク情報を画面に描画する。

ステップＳ１８０３において、図１４に示す道路逸脱判定のステップＳ４１３で第一リンク候補点が道路逸脱と判定された場合、ステップＳ１８０４に進む。道路走行であると判定された場合、ステップＳ１８０５に進む。

ステップＳ１８０４において、位置算出装置１００の位置及び方位をもとに所定の色でカーマークを描画し、ステップＳ２０１に進む。

ステップＳ１８０５において、分岐リンク候補点道路逸脱判定を実施したか否かを判定する。図１４に示す道路逸脱判定のステップＳ１９０１〜Ｓ１９０４において、分岐リンク候補点について道路逸脱判定を実施した場合、ステップＳ１８０６に進む。実施しなかった場合、ステップＳ１８０８に進む。

ステップＳ１８０６において、分岐リンク候補点道路逸脱判定で道路走行と判定したか否かを判定する。図１４に示す道路逸脱判定のステップＳ１９０１〜Ｓ１９０４において分岐リンク候補点が道路走行であると判定された場合、ステップＳ１８０７に進む。図１６におけるリンク候補点１５０２ａ及び１５０２ｂに対応している。道路逸脱であると判定された場合、ステップＳ１８０８に進む。図１６ではリンク候補点１５０４に対応している。

ステップＳ１８０７において、第一リンク候補点の位置及び方位を、所定の色以外の色を付したカーマークにより描画する。あるいは、カーマークを点滅させても良い。図１６では第一リンク候補点１５０３ａ及び１５０３ｂに対応している。

ステップＳ１８０８において、第一リンク候補点の位置及び方位を、所定の色を付したカーマークにより描画する。図１６では第一リンク候補点１５０１に対応している。

ステップＳ１８０９において、誘導開始信号がオンである場合はステップＳ１８１０に進み、オフである場合はステップＳ２０１に進む。

ステップＳ１８１０において、案内表示を出力し、誘導方向を音声で出力する。

（６）例えば変形例（４）によれば、図１に示す位置算出装置１００の動作手順（図２，図３，図７，図８）のうち、ステップＳ４１３において、第一リンク候補点が所定角度以下の分岐を通過した後、第一リンク候補点について道路逸脱判定を行うとともに、第一リンク候補点とは異なる分岐したリンク上の分岐リンク候補点についても道路逸脱判定を行うことができる。ステップＳ２１３においては、第一リンク候補点が所定角度以下の分岐を通過した後、ステップＳ４１３で行われた分岐リンク候補点の逸脱判定結果が道路走行である場合、第一リンク候補点の位置及び方位でなく、分岐した二つのリンクの間に位置算出装置１００の位置を表示しても良い。位置算出装置１００の位置をカーマークによって表示する際、カーマークの方位を二つのリンク方位の平均値として表示する。これは、分岐した２つのリンクのいずれのリンクについても、移動体が走行している可能性が考えられるリンクであることを意味している。図１８において、分岐直後の斜線でハッチングされたカーマーク１５０５を、分岐した二つのリンクの間に表示したり、カーマーク１５０５の方位を二つのリンク方位の平均値として表示する。こうした表示を行うことにより、もし移動体が第一リンク候補点とは異なる分岐リンク候補点に位置しているにも関わらず間違って第一リンク候補点の位置及び方位にカーマークを表示してしまうことを防ぐことができ、運転手は第一リンク候補点の間違いに対する不安がなくなる。

具体的には、図１に示す位置算出装置１００の動作手順（図３）のステップＳ４１３及びステップ４０９の代わりに図１４及び図１５に示す処理を行うとともに、図１に示す位置算出装置１００の動作手順（図２）のステップＳ２１３の代わりに図１９に示す処理を行う。図１９の動作手順では、図１７の動作手順のステップＳ１８０１〜Ｓ１８１０のうち、ステップＳ１８０７の代わりにステップＳ１８１１を実施する。ステップＳ１８０１〜Ｓ１８０６およびＳ１８０８〜Ｓ１８１０については図１７を用いて上述したため、ここでは、ステップＳ１８０７の代わりに新たに追加したステップＳ１８１１のみを下記に説明する。

ステップＳ１８１１において、第一リンク候補点と異なる分岐したリンク上の分岐リンク候補点と第一リンク候補点との間にカーマークを描画する。また、カーマークの方位は二つのリンクの方位の間の値とする。図１８ではカーマーク１５０５に対応している。このときの第一リンク候補点１５０６ａ及び１５０６ｂは表示されない。図１８において、図１６と同一の符号を付した図形については、図１６の説明で上述したため、ここでは説明を省略する。

（７）図１に示す位置算出装置１００の動作手順（図２，図３，図７，図８）において、ステップＳ２１０で第一リンク候補点があるリンクが誘導経路のリンクに含まれている場合に限られなくても良い。例えば、分岐通過後、第一リンク候補点とは異なる分岐したリンク上の分岐リンク候補点が道路走行であると判定された場合であっても、ステップＳ２１２に進む処理に変更することとして良い。これにより、分岐後のリンク位置及び方位の誤差で誤ったリンク候補点を第一リンク候補点としたときに、間違ったリンク候補点をもとにした経路の再探索を防ぐことができる。また、存在確率を考慮して、第一リンク候補点のみが道路走行と判定される状態で経路探索を行うため、誤った経路の再探索を行わない確率が高くなる。

具体的には、図１に示す位置算出装置１００の動作手順（図３）のステップＳ４１３及びステップＳ４０９の代わりに図１４及び図１５に示す処理を行うとともに、図１に示す位置算出装置１００の動作手順（図２）において、ステップＳ２１０とステップＳ２１１との間にＳ２００１及びＳ２００２を挿入した処理を行う。ステップＳ２０８〜Ｓ２１０およびＳ２１１〜Ｓ２１３については図２を用いて上述したため、図２０を用いて、ステップＳ２１０とステップＳ２１１との間に新たに挿入したステップＳ２００１及びＳ２００２のみを下記に説明する。

ステップＳ２００１において、分岐リンク候補点道路逸脱判定を実施したか否かを判定する。図１４に示す道路逸脱判定のステップＳ１９０１〜Ｓ１９０４において、分岐リンク候補点について道路逸脱判定を実施した場合、ステップＳ２００２に進む。実施しなかった場合、ステップＳ２１１に進む。

ステップＳ２００２において、分岐リンク候補点道路逸脱判定で道路走行と判定したか否かを判定する。図１４に示す道路逸脱判定のステップＳ１９０１〜Ｓ１９０４において分岐リンク候補点が道路走行であると判定された場合、ステップＳ２１２に進む。道路逸脱であると判定された場合、ステップＳ２１１に進む。

（８）図１に示す位置算出装置１００において、加速度センサ装置１０４を取り外しても良い。図２に示す動作手順において、ステップＳ２０２で、位置算出装置１００の加速度に対応した信号をセンサ出力補正装置１１１に送る処理を行わないこととなる。ステップＳ２０３で、加速度センサ装置１０４からの出力から加速度センサ装置１０４のバイアスを引き、センサ加速度を算出する処理を行わないこととなる。ステップＳ２０５で、式（１）において状態量ベクトルη（ｔ）、行列Ｆ、Ｇ、状態遷移行列Φ（Δｔ、αａ、αω）及びＱ（ｋ）からピッチφ（ｔ）に関連する要素を削除し、観測量ベクトルｙ、行列Ｈ及び観測雑音行列Ｒから、センサ加速度ｇｓ（ｔ）に関連する要素を削除する。こうすることにより、センサの数を削減でき、装置コストを低減できる。

（９）図１に示す位置算出装置１００において、速度センサ装置１０２、角速度センサ装置１０３、加速度センサ装置１０４及びセンサ出力補正装置１１１を取り外しても良い。図２に示す動作手順において、ステップＳ２０２及びステップＳ２０３を削除し、ステップＳ２０４で受信異常判定装置１１２は、受信装置１０１から受信位置、受信速度及び受信方位を受け取る。受信速度及び受信方位から所定時間の位置変化を算出し、この位置変化と所定時間の受信位置の変化との差が閾値以上になった場合、受信位置は異常であると判定し、そうでない場合、受信位置は正常であると判定する。受信速度及び受信方位は常に正常であるとする。ステップＳ２０５の測位処理で推定位置及び推定方位を最新化する必要があるため、常に何らかの情報を入力する必要がある。ここでは、受信速度及び受信方位を常に入力する。受信位置は、正常であると判定されたときに入力される。ステップＳ２０５で、式（１）において状態量ベクトルη（ｔ）、行列Ｆ、Ｇ、状態遷移行列Φ（Δｔ，αａ，αω）及びＱ（ｋ）からピッチφ（ｔ）に関連する要素を削除するとともに、観測量ベクトルｙ、行列Ｈ及び観測雑音行列Ｒから、センサ速度ｖｓ（ｔ）、センサ角速度ωｓ（ｔ）、センサ加速度ｇｓ（ｔ）に関連する要素を削除する。こうすることにより、センサ及び計算処理量を削減でき、装置コストを低減できる。

（１０）位置算出装置１００をカーナビゲーションシステムに搭載することにより、地域ごと、地図メーカごとに地図のリンク誤差が異なっていても、道路を逸脱したかを正しく判定できるカーナビゲーションシステムを実現できる。また、そのカーナビゲーションシステムは、狭角分岐後に誤ったリンクに自車をマッチしても、早く正しいリンクに復帰することができる。

（１１）図１に示す位置算出装置１００において、所定の領域ごとに、リンク位置の誤差及び方位の誤差に関する分散をそれぞれ予め算出し、地図精度記憶装置１０８に格納しておくこととしてもよい。人工衛星または航空機からの航空写真上に道路リンクを重ねて描画し、その写真とリンクの位置の差を基に、リンク位置の誤差分散及びリンク方位の誤差分散を予め算出することができる。あるいは、実際に車両を走行させて、すなわち移動体を移動させてリンク位置の誤差及び方位の誤差を計算し、その計算結果を用いてそれぞれの誤差分散を予め算出することとしてもよい。これにより、動作手順（図２、図３、図７）において、動作の度毎にステップＳ２０７を実行する必要が無くなる。この場合、地図精度記憶装置１０８に、所定の領域毎にカウント数、積算値、及び二乗和の積算値を格納する必要が無い。

また、リンク位置の誤差及び方位の誤差に関する分散を地図精度記憶装置１０８に予め格納した上で、さらに図８におけるステップＳ２０７の処理も実行させることとしてもよい。これにより、実際のマップマッチング結果を基に、リンク位置の誤差分散及びリンク方位の誤差分散を算出し、地図精度記憶装置１０８に格納した値を更新することが出来る。

予め算出された所定の領域ごとのリンク位置の誤差及び方位の誤差に関する分散をＳ４１０及びＳ４１３で用いることとしてもよい。これにより、ステップＳ４１０におけるリンク候補点の評価量の算出や、ステップＳ４１３における道路逸脱判定を、それぞれ行うようにすることができる。これにより、リンク候補点の評価量や道路逸脱の検定量が地域ごと及びメーカごとに異なる地図のリンクデータの誤差の影響を受けず、マッチングや道路逸脱判定の誤りを低減することができる。

（１２）上述した実施の形態および変形例は、それぞれ組み合わせてもよい。

上記では、種々の実施の形態および変形例を説明したが、本発明はこれらの内容に限定されるものではない。本発明の技術的思想の範囲内で考えられるその他の態様も本発明の範囲内に含まれる。

次の優先権基礎出願の開示内容は引用文としてここに組み込まれる。
日本国特許出願２０１１年第８７２３７号（２０１１年４月１１日出願）

Claims (15)

1. 移動体の現在位置、移動速度及び移動方位を算出し、
   前記現在位置、前記移動速度及び前記移動方位に基づいて前記現在位置及び前記移動方位の推定誤差共分散を算出し、
   前記現在位置、前記移動方位、前記推定誤差共分散、前記現在位置周辺の領域に含まれる複数のリンク候補点の各リンク候補点のリンク候補点位置、前記各リンク候補点のリンク候補点方位、前記領域に含まれる複数のリンクの位置の第１誤差分散、及び前記複数のリンクの方位の第２誤差分散を用いた演算として、前記現在位置と前記リンク候補点位置との間の距離及び前記移動方位と前記リンク候補点方位との方位差から、前記リンク候補点位置までの所定距離間隔毎に得られた前記距離及び前記方位差を含む複数データを積算することによって、前記移動体が前記各リンク候補点に対応する道路を走行している確率に対応した評価量を前記各リンク候補点のリンク毎に計算し、
   前記複数のリンク候補点のうち前記確率が最も高いリンク候補点に前記移動体が位置しているとして、前記評価量に基づき前記現在位置を算出する位置算出方法。
2. 請求項１に記載の位置算出方法において、
   前記第１誤差分散は、前記距離に基づいて算出され、
   前記第２誤差分散は、前記方位差に基づいて算出され、
   前記評価量は、前記複数データの各々に含まれる前記距離の二乗を前記第１誤差分散と前記現在位置の推定誤差分散との和で割った値と、前記複数データの各々に含まれる前記方位差の二乗を前記第２誤差分散と前記移動方位の推定誤差分散との和で割った値との和を、前記複数データについて積算して得られる位置算出方法。
3. 請求項１または２に記載の位置算出方法において、
   前記現在位置と前記確率が最も高い前記リンク候補点位置との間の距離に関する第１検定量が第１閾値よりも大きいか否かに基づいて、前記現在位置と該リンク候補点位置とに差があるか否かを判定し、
   前記移動方位と前記確率が最も高い前記リンク候補点方位との方位差に関する第２検定量が第２閾値よりも大きいか否かに基づいて、前記移動方位と該リンク候補点方位とに差があるか否かを判定し、
   前記現在位置と該リンク候補点位置とに差があるとき、または前記移動方位と該リンク候補点方位とに差があるとき、前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を逸脱したと判定するとともに、前記現在位置と該リンク候補点位置とに差がないとき、かつ前記移動方位と該リンク候補点方位とに差がないとき、前記移動体は、前記確率が最も高い前記リンク候補点に対応する前記道路を走行していると判定し、
   前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を逸脱したと判定したときは、前記移動体は前記現在位置に位置しているとして算出するとともに、前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を走行していると判定したときは、前記移動体は、該リンク候補点に位置しているとして、前記現在位置を算出する位置算出方法。
4. 請求項３に記載の位置算出方法において、
   前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を逸脱していないと判定されたとき、前記推定誤差共分散が第３閾値以下であるか否かに応じて、前記第１誤差分散を算出するか否かが判定され、
   前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を逸脱していないと判定されたとき、前記推定誤差共分散が第４閾値以下であるか否かに応じて、前記第２誤差分散を算出するか否かが判定される位置算出方法。
5. 請求項３に記載の位置算出方法において、
   前記測位衛星の衛星位置及び前記現在位置からＤＯＰ値を算出し、
   前記現在位置の変化、前記移動速度の変化及び前記移動方位の変化に基づいて、前記現在位置が異常であるか否か及び前記移動方位が異常であるか否かを判定し、
   前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を逸脱していないと判定され、かつ、前記現在位置が異常ではないと判定されたとき、前記ＤＯＰ値が第５閾値以下であるか否かに応じて、前記第１誤差分散を算出するか否かが判定され、
   前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を逸脱していないと判定され、かつ、前記現在位置が異常ではないと判定されたとき、前記ＤＯＰ値が前記第５閾値以下であるか否かに応じて、前記第２誤差分散を算出するか否かが判定される位置算出方法。
6. 請求項３〜５のいずれか１項に記載の位置算出方法において、
   前記第１検定量及び前記第２検定量の値は、前記現在位置と前記確率が最も高い前記リンク候補点位置との間の前記距離の二乗を、前記現在位置及び前記確率が最も高い前記リンク候補点位置の誤差共分散の和で割った値と、前記移動方位と前記確率が最も高い前記リンク候補点方位との前記方位差の二乗を、前記移動方位及び前記確率が最も高い前記リンク候補点方位の誤差分散の和で割った値とを無次元化して、足し合わせた値である位置算出方法。
7. 請求項３〜６のいずれか１項に記載の位置算出方法において、
   前記第１検定量は、前記現在位置と前記確率が最も高い前記リンク候補点位置との間の前記距離の二乗を、前記現在位置及び前記確率が最も高い前記リンク候補点位置の誤差共分散の和で割った値であり、
   前記第２検定量は、前記移動方位と前記確率が最も高い前記リンク候補点方位との前記方位差の二乗を、前記移動方位及び前記確率が最も高い前記リンク候補点方位の誤差分散の和で割った値である位置算出方法。
8. 請求項１〜７のいずれか１項に記載の位置算出方法において、
   前記複数のリンクを構成する第１リンクと第２リンクとが分岐点ノードから分岐し、かつ前記確率が最も高い前記リンク候補点が前記第１リンクに含まれるとき、前記移動体が前記分岐点ノードに対応する交差点を通過してから、該リンク候補点の前記リンク候補点位置と前記第２リンクに対応する区間道路との間隔が所定値よりも大きくなるまでの間、前記複数データのデータ数を減じる位置算出方法。
9. 請求項１〜７のいずれか１項に記載の位置算出方法において、
   前記移動体が、前記複数のリンクを構成する第１リンクと第２リンクとが分岐する分岐点ノードに対応する交差点を通過してから、前記確率が最も高い前記リンク候補点とは異なるリンク候補点を含むリンクに対応する道路を走行していると判定されている第１期間においては、前記複数データのデータ数を減じる位置算出方法。
10. 請求項９に記載の位置算出方法において、
    前記現在位置近傍の始点リンクから前記移動体の目的地近傍の終点リンクまでの誘導経路に基づいて前記移動体が経路誘導され、かつ前記確率が最も高い前記リンク候補点を含むリンクが前記誘導経路に含まれている場合であって、前記移動体が前記道路を逸脱していないと判定され、かつ前記交差点と該リンク候補点の前記リンク候補点位置との間の距離が所定値以下になったとき、前記誘導経路に関する案内表示及び案内音声を表示音声出力装置に出力し、
    前記誘導経路に基づいて前記移動体が経路誘導され、かつ前記確率が最も高い前記リンク候補点を含むリンクが前記誘導経路に含まれていない場合であって、前記移動体が前記交差点を通過して、前記確率が最も高い前記リンク候補点とは異なるリンク候補点を含むリンクに対応する道路を逸脱したと判定されたとき、前記現在位置近傍の始点リンクから前記移動体の目的地近傍の終点リンクまでの新たな誘導経路を探索する位置算出方法。
11. 移動体の現在位置、移動速度及び移動方位を算出する移動情報算出部と、
    前記現在位置、前記移動速度、及び前記移動方位に基づいて、前記現在位置及び前記移動方位の推定誤差共分散を算出する推定誤差共分散算出部と、
    前記現在位置、前記移動方位、前記推定誤差共分散、前記現在位置から前記現在位置を含む所定の領域に含まれる複数のリンク候補点の各リンク候補点のリンク候補点位置、前記各リンク候補点のリンク候補点方位、前記領域に含まれる複数のリンクの位置の第１誤差分散、及び前記複数のリンクの方位の第２誤差分散を用いた演算として、前記現在位置と前記リンク候補点位置との間の距離及び前記移動方位と前記リンク候補点方位との方位差から、前記リンク候補点位置までの所定距離間隔毎に得られた前記距離及び前記方位差を含む複数データを積算することによって、前記移動体が前記各リンク候補点に対応する道路を走行している確率に対応した評価量を前記各リンク候補点のリンク毎に計算する確率計算部と、
    前記複数のリンク候補点のうち前記確率が最も高い前記リンク候補点に前記移動体が位置しているとして、前記評価量に基づき前記現在位置を算出する位置算出部とを備える位置算出装置。
12. 請求項１１に記載の位置算出装置において、
    前記第１誤差分散を、前記距離に基づいて算出するとともに、前記第２誤差分散を、前記方位差に基づいて算出する誤差分散算出部をさらに備え、
    前記確率計算部は、前記複数データの各々に含まれる前記距離の二乗を前記第１誤差分散と前記現在位置の推定誤差分散との和で割った値と、前記複数データの各々に含まれる前記方位差の二乗を前記第２誤差分散と前記移動方位の推定誤差分散との和で割った値との和を、前記複数データについて積算することによって、前記評価量を算出する位置算出装置。
13. 請求項１１または１２に記載の位置算出装置において、
    前記現在位置と前記確率が最も高い前記リンク候補点の前記リンク候補点位置との間の距離の二乗を前記第１誤差分散と前記現在位置の推定誤差分散との和で割った値が第１閾値よりも大きいか否かに基づいて、前記現在位置と該リンク候補点位置とに差があるか否かを判定する第１判定部と、
    前記移動方位と前記確率が最も高い前記リンク候補点の前記リンク候補点方位との方位差を前記第２誤差分散と前記移動方位の推定誤差分散との和の平方根で割った値が第２閾値よりも大きいか否かに基づいて、前記移動方位と該リンク候補点方位とに差があるか否かを判定する第２判定部と、
    前記現在位置と該リンク候補点位置とに差があるとき、または前記移動方位と該リンク候補点方位とに差があるとき、前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を逸脱したと判定するとともに、前記現在位置と該リンク候補点位置とに差がないとき、かつ前記移動方位と該リンク候補点方位とに差がないとき、前記移動体は、前記確率が最も高い前記リンク候補点に対応する前記道路を走行していると判定する第３判定部とをさらに備え、
    前記位置算出部は、前記第３判定部によって、前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を逸脱したと判定されたときは、前記移動体は前記現在位置に位置しているとして算出するとともに、前記移動体が、前記確率が最も高い前記リンク候補点に対応する前記道路を走行していると判定されたときは、前記移動体は、該リンク候補点に位置しているとして、前記現在位置を算出する位置算出装置。
14. 請求項１１〜１３のいずれか１項に記載の位置算出装置において、
    測位衛星から発信される測位信号を受信する受信部と、
    前記移動体の速度、角速度及び加速度のうちの少なくとも１つを含むセンサデータを測定する測定部とをさらに備え、
    前記移動情報算出部は、前記現在位置、前記移動速度及び前記移動方位を、前記測位信号と前記センサデータとに基づいて算出し、
    前記推定誤差共分散算出部は、前記推定誤差共分散を、前記測位信号及び前記センサデータを用いて算出する位置算出装置。
15. 請求項１１〜１４のいずれか１項に記載の位置算出装置において、
    前記複数のリンクを構成する第１リンクと第２リンクとが分岐点ノードから分岐し、かつ前記確率が最も高い前記リンク候補点が前記第１リンクに含まれるとき、前記移動体が前記分岐点ノードに対応する交差点を通過してから、該リンク候補点の前記リンク候補点位置と前記第２リンクに対応する区間道路との間隔が所定値よりも大きくなるまでの間、前記複数データのデータ数を減じるように設定する設定部をさらに備える位置算出装置。

Images