自律測位装置、および自律測位プログラム
【課題】高精度かつ低負荷の自律測位を実現すること。
【解決手段】（Ａ）の状態から三角形△ａｂｃを平行移動し、点ｇを点Ｇに一致させる。（Ｂ）で重心ｇを中心として三角形△ａｂｃを回転角φ分回転させる。回転角φは０°≦φ≦３６０°であり、所定量Δφずつ加算されていく。この状態が（Ｃ）である。回転前に、頂点Ｐａ，ＰＡ間距離ａＡ、頂点Ｐｂ，ＰＢ間距離ｂＢ、頂点Ｐｃ，ＰＣ間距離ｃＣの和を算出する。（Ｄ）は回転後の状態を示している。回転角φの回転後においても、頂点Ｐａ，ＰＡ間距離ａＡ、頂点Ｐｂ，ＰＢ間距離ｂＢ、頂点Ｐｃ，ＰＣ間距離ｃＣの和を算出して、回転前の距離ａＡ〜ｃＣの和と比較する。回転後のほうが小さいときの回転角φを保存する。そして、最終的に得られた回転角φと地点Ｐｃに到達する移動軌跡Ｖｂｃの最終ベクトルｖendにおける移動回転角とを加算することで初期方位が算出される。
【解決手段】（Ａ）の状態から三角形△ａｂｃを平行移動し、点ｇを点Ｇに一致させる。（Ｂ）で重心ｇを中心として三角形△ａｂｃを回転角φ分回転させる。回転角φは０°≦φ≦３６０°であり、所定量Δφずつ加算されていく。この状態が（Ｃ）である。回転前に、頂点Ｐａ，ＰＡ間距離ａＡ、頂点Ｐｂ，ＰＢ間距離ｂＢ、頂点Ｐｃ，ＰＣ間距離ｃＣの和を算出する。（Ｄ）は回転後の状態を示している。回転角φの回転後においても、頂点Ｐａ，ＰＡ間距離ａＡ、頂点Ｐｂ，ＰＢ間距離ｂＢ、頂点Ｐｃ，ＰＣ間距離ｃＣの和を算出して、回転前の距離ａＡ〜ｃＣの和と比較する。回転後のほうが小さいときの回転角φを保存する。そして、最終的に得られた回転角φと地点Ｐｃに到達する移動軌跡Ｖｂｃの最終ベクトルｖendにおける移動回転角とを加算することで初期方位が算出される。
【発明の詳細な説明】
【技術分野】
【０００１】
この発明は、自律測位をおこなう自律測位プログラムおよび自律測位装置に関する。
【背景技術】
【０００２】
現在の自律測位技術では、移動距離と移動相対角度により自身の移動軌跡を算出し、自身の位置を検出している。自律測位は主にカーナビゲーションの車両測位としてＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）と併用して利用されており、自車の車速パルスとジャイロセンサを用いて実現している。
【０００３】
車速パルスとジャイロセンサは環境が変化することで出力する値が変化する。たとえば、タイヤの磨耗や急停車による熱膨張により車速パルスに応じた移動距離が変化する。また、車内の温度変化によりジャイロセンサの特定が変化する。このため、自律測位については補正が必要となる。
【０００４】
また、ＧＰＳ測位と自律航法測位とを複合した自律測位技術が開示されている（たとえば、下記特許文献１を参照。）。この特許文献１では、ＧＰＳによる測位結果を正解としカルマンフィルタを用いて車速パルスとジャイロセンサ出力を補正している。
【０００５】
さらに、従来のカーナビゲーションでは、マップマッチングにより自律測位結果を補正することで、走行道路が判別できる程度の測位精度を実現している。
【０００６】
【特許文献１】特開２００８−１７５７２１号公報（段落［００３２］〜［００３５］）
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかしながら、上述した特許文献１のような自律測位技術では、カルマンフィルタを用いているため、計算量が膨大になり、自律測位を実行するコンピュータの負荷が増大するという問題があった。また、走行車線を判別するような高精度な測位を実現するためには、マップマッチングでは補正しきれないという問題があった。
【０００８】
この発明は、上述した従来技術による問題点を解消するため、高精度かつ低負荷の自律測位を実現することができる自律測位プログラムおよび自律測位装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決し、目的を達成するため、第１の自律測位プログラムおよび自律測位装置は、速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するにあたって、前記移動体が通過する任意の複数の地点の位置情報を取得し、前記複数の地点間における前記出力電圧の変動が所定電圧以下であるか否かを判断し、その判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出し、算出された補正値を前記移動距離に加算することにより、前記複数の地点のうち最後に通過した地点からの移動距離を算出し、前記複数の地点のうち最後に通過した地点からの移動距離を出力することを要件とする。
【００１０】
第２の自律測位プログラムおよび自律測位装置は、速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するにあたって、前記移動体が通過する複数の地点の位置情報を取得し、前記複数の地点間における前記出力電圧の変動が所定電圧以下であるか否かを判断し、その判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出し、算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出し、前記複数の地点のうち最後に通過した地点からの回転角を出力することを要件とする。
【００１１】
第３の自律測位プログラムおよび自律測位装置は、速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するにあたって、前記移動体が第１〜第３の地点を通過する際に位置情報を取得し、前記第１〜第３の地点間における前記出力電圧の変動が所定電圧以下であるか否かを判断し、その判断結果に基づいて、前記第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理によって算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理とを、前記判断手段によって判断された判断結果に基づいて実行し、前記回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とに基づいて、前記移動体が前記第３の地点に到達したときの初期方位を算出し、算出された初期方位と前記第３の地点からの移動距離および回転角とに基づいて、前記移動体の絶対位置および絶対方位を算出し、算出された前記移動体の絶対位置および絶対方位を出力することを要件とする。
【発明の効果】
【００１２】
自律測位プログラムおよび自律測位装置によれば、高精度かつ低負荷の自律測位を実現することができるという効果を奏する。
【発明を実施するための最良の形態】
【００１３】
以下に添付図面を参照して、自律測位プログラムおよび自律測位装置の好適な実施の形態を詳細に説明する。移動体が自律測位をおこなう場合には、位置算出に必要な計算処理を移動体に備えたコンピュータ（車載器等）上で実行することが考えられる。移動体が安全に移動を続けるためには、移動体の駆動系に必要なエネルギーを位置算出処理のためにコンピュータが消費してしまわないよう、位置算出処理にかかるエネルギー量を少なくすることが望ましい。たとえば、電気自動車は駆動系も車載器も両方電気を利用するため、コンピュータに負荷がかからないようにするのが好ましい。このため、本実施の形態では、計算量を抑えて、かつ自律測位の精度向上を図る。
【００１４】
（移動軌跡）
図１は、車両の移動軌跡を示す説明図である。（Ａ）において、車両１０１は、道路１００に設置された光ビーコンＰからダウンリンクされる位置情報（光ビーコンの位置座標）を受信しながら道路１００を走行する。
【００１５】
（Ｂ）は、（Ａ）の任意の領域である楕円部分を拡大した図である。ここでは、光ビーコンＰを車両１０１が通過した順にＰＡ，ＰＢ，ＰＣとする。ＬＡＢは光ビーコンＰＡ，ＰＢ間距離であり、ＬＢＣは光ビーコンＰＢ，ＰＣ間距離である。また、白丸で示した地点Ｐａ〜Ｐｃは、光ビーコンＰＡ〜ＰＣを通過したときの車両１０１の位置である。なお、車両１０１の実際の現在位置はＰＣであり、自律測位された車両１０１の計算上の現在位置はＰｃである。
【００１６】
また、Ｖａｂは地点Ｐａ，Ｐｂ間の車両１０１の移動軌跡であり、Ｖｂｃは地点Ｐｂ，Ｐｃ間の車両１０１の移動軌跡である。移動軌跡Ｖａｂ，Ｖｂｃは、ベクトルｖを連結したベクトル列である。ベクトルｖは車両１０１の進行方向を示しており、車両１０１の車速パルス１パルス分の移動距離ｒおよび移動回転角θという成分を含む。移動回転角θはジャイロセンサにより検出可能な成分である。
【００１７】
（Ｃ）は、（Ｂ）の任意の領域である楕円部分を拡大した図である。（Ｃ）では、車速パルスが連続するベクトルｖn-1とベクトルｖnを示している。ベクトルｖn-1はｎ−１番目の車速パルスにおけるベクトルを示しており、ベクトルｖnはｎ番目の車速パルスにおけるベクトルを示している。移動距離ｒは車速パルス１パルスあたりの車両１０１の移動距離である。移動回転角θnは、ベクトルｖn-1が指し示す進行方向を基準とした移動回転角θである。本実施の形態では、これらの情報を有効活用することで、計算量を抑えて、かつ自律測位の精度向上を図る。
【００１８】
（ハードウェア構成）
図２は、例えば図１の車両１０１などの移動体に搭載されている自律測位装置のハードウェア構成を示すブロック図である。自律測位装置２００は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２０１と記憶装置２０２とディスプレイ２０３と受信装置２０４と車速パルス発生器（車速センサ）２０５とジャイロセンサ２０６と温度センサ２０７とがバス２０８を介して接続されている。
【００１９】
ＣＰＵ２０１は、自律測位装置２００の全体の制御を司る中央処理装置である。記憶装置２０２は、自律測位プログラムや地図データを格納したり、ＣＰＵ２０１のワークエリアとして使用される。
【００２０】
記憶装置２０２は、たとえば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、フラッシュメモリ、磁気ディスクドライブおよび磁気ディスク、光ディスクドライブおよび光ディスクなどにより構成される。
【００２１】
ディスプレイ２０３は、その表示画面に地図データや車両１０１の移動軌跡といった情報を表示する表示装置である。受信装置２０４は、特定地点の位置情報を受信するインターフェースである。たとえば、光ビーコンからダウンリンクされてくる位置情報を受信する。また、所定のタイミングでＧＰＳからの位置情報を受信するＧＰＳレシーバでもよい。
【００２２】
車速パルス発生器２０５は、車両１０１の車輪の回転を検出して、１回転ごとに車速パルスを発生させる。タイヤの径は既知であるため、１車速パルスあたりの車両１０１の移動距離が計算される。ジャイロセンサ２０６は、車両１０１の進行方向に対する回転角の角速度を検出する。
【００２３】
具体的には、たとえば、回転角の変化に応じた電圧を出力する。また、ジャイロセンサ２０６はゼロ点電圧と呼ばれる基準電圧を有しており、出力電圧とゼロ点電圧の差分により角速度が計算される。温度センサ２０７は温度を検出するセンサであり、車内に搭載されているジャイロセンサ２０６の温度を検出する。
【００２４】
なお、自律測位装置２００に示す要素の一部は、通信を介して接続可能であれば、移動体上に搭載されていなくても構わない。例えば、受信装置２０４、車速パルス発生器２０５、ジャイロセンサ２０６、温度センサ２０７からの出力信号を通信を介して受信可能であれば、ＣＰＵ２０１は移動体上に搭載されていなくても構わない。
【００２５】
（機能的構成）
図３は、自律測位装置２００の機能的構成を示すブロック図である。自律測位装置２００は、取得部３０１と判断部３０２と補正処理部３０３と出力部３０４とを含む構成である。また、補正処理部３０３は、補正値算出部３３１と相対位置算出部３３２と初期方位算出部３３３と絶対位置算出部３３４とを含む構成である。これら各機能は、具体的には、たとえば、図２に示した記憶装置２０２に記憶された自律測位プログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００２６】
取得部３０１は、各種センサからの情報を取得する機能を有する。具体的には、温度センサ２０７からは温度に応じた電圧値、車速パルス発生器２０５からは車速パルス、ジャイロセンサ２０６からは角速度に応じた出力電圧を取得する。これら情報は常時取得する。また、受信装置２０４は、特定地点通過時における特定地点の位置情報（位置座標）を取得する。たとえば、位置情報をダウンリンクする光ビーコンの直下を通過する際にその位置情報を受信する。これにより、光ビーコン通過時の自車位置を特定することができる。
【００２７】
また、判断部３０２は、補正処理部３０３の実行可否を判断する機能を有する。具体的には、ジャイロセンサ２０６からの出力電圧の変動と温度センサ２０７から検出された温度の変動により、補正処理部３０３の実行可否を判断する。
【００２８】
出力電圧の変動が所定のしきい値α以下の場合、車両１０１が車線進路変更や右左折など進路変更をしていないため、この出力電圧から算出した移動軌跡上の回転角度は小さい。この移動軌跡を用いて車両の回転角に影響を与える感度の補正をする場合、補正元の移動軌跡上の回転角度が小さいため、補正に適さない。したがって、補正処理部３０３による補正を実行しない。このしきい値αは、右左折のような大きな角度変化を検出したいのか、進路変更（車線変更）などの小さな角度変化をも検出したいのかにより設定すべき値が異なる。しきい値αを小さく設定すれば進路変更の検出が可能となり、それよりも大きく設定すれば右左折を検出することができる。
【００２９】
また、検出温度の変動が所定のしきい値β以下の場合、ジャイロセンサ２０６が正常に動作しており、ジャイロセンサ２０６は正確な電圧を出力していると考えられる。そして、ジャイロセンサ２０６が出力した電圧から算出した移動軌跡は、正確であると判断できる。したがって、ジャイロセンサ２０６が出力した電圧から算出した移動軌跡を用いて、補正処理部３０３による補正を実行する。なお、補正処理部３０３による補正は、出力電圧の変動が所定のしきい値αよりも大きい場合でかつ検出温度の変動が所定のしきい値β以下の場合に実行するのが最も好ましい。
【００３０】
このように、ジャイロセンサ２０６の出力変動や温度変化に応じて自律測位の補正の適否を自動判断することができるため、不要な計算量を抑制することができ、コンピュータの負荷軽減を図ることができる。
【００３１】
補正処理部３０３は、補正値算出部３３１と相対位置算出部３３２と初期方位算出部３３３と絶対位置算出部３３４とを含む構成である。補正値算出部３３１は、感度補正値Δｖｓ、移動距離補正値Δｒ、ゼロ点電圧補正値ΔＥｂといった補正値を算出する機能を有する。
【００３２】
具体的には、点Ｐａ〜Ｐｃまでの間で、車速パルス発生器２０５からの車速パルスの検出回数ｎを計数するとともに、検出時刻ｔとそのときの状態（Ｈ／Ｌ）を記録する。そして、下記式（１）に示すように、１車速パルスあたりの車両１０１の回転角度を算出する。
【００３３】
【数１】
【００３４】
ただし、θnは、ｎ番目の車速パルスについての車両１０１の移動回転角度、ｔnはｎ番目の車速パルスの検出時刻、ｔn-1はｎ−１番目の車速パルスの検出時刻、Ｅ（ｔ）は時刻ｔにおけるジャイロセンサ２０６からの出力電圧、Ｅｂはゼロ点電圧、ｖｓjは感度補正の試行回数ｊ番目のジャイロセンサ２０６の感度である。
【００３５】
また、補正値算出部３３１は、点Ｐａ〜Ｐｃまでの間で、自車移動履歴を算出する機能を有する。具体的には、下記式（２）に示すように、車速パルスごとの車両１０１の位置座標を算出する。
【００３６】
（ｘn，ｙn）＝（ｘn-1＋ｒi・ｃｏｓθn，ｙn-1＋ｒi・ｓｉｎθn）
・・・・・・（２）
【００３７】
ただし、ｘnはｎ番目の車速パルス検出時の車両１０１のＸ座標値、ｘn-1はｎ−１番目の車速パルス検出時の車両１０１のＸ座標値、ｙnはｎ番目の車速パルス検出時の車両１０１のＹ座標値、ｙn-1はｎ−１番目の車速パルス検出時の車両１０１のＹ座標値、である。ｒiは移動距離補正の試行回数ｉ番目のときの車両移動距離である。
【００３８】
また、補正値算出部３３１は、上記式（２）で算出された自車移動履歴の正誤判定値を算出する機能を有する。具体的には、下記式（３）に示すように、移動距離補正の試行回数ｉごとに正誤判定の評価値を算出する。
【００３９】
【数２】
【００４０】
ただし、Ｌｓｔijは試行回数ｉ番目の正誤判定値、ｘaは点Ｐａで位置情報を受け取っ
たときの車両１０１のＸ座標値、ｘbは点Ｐｂで位置情報を受け取ったときの車両１０１のＸ座標値、ｘcは点Ｐｃで位置情報を受け取ったときの車両１０１のＸ座標値、ｙaは点Ｐａで位置情報を受け取ったときの車両１０１のＹ座標値、ｙbは点Ｐｂで位置情報を受け取ったときの車両１０１のＹ座標値、ｙcは点Ｐｃで位置情報を受け取ったときの車両１０１のＹ座標値である。
【００４１】
また、ＸAは点ＰＡで位置情報を受け取ったときの車両１０１のＸ座標値、ＸBは点ＰＢで位置情報を受け取ったときの車両１０１のＸ座標値、ＸCは点ＰＣで位置情報を受け取ったときの車両１０１のＸ座標値、ＹAは点ＰＡで位置情報を受け取ったときの車両１０１のＹ座標値、ＹBは点ＰＢで位置情報を受け取ったときの車両１０１のＹ座標値、ＹCは点ＰＣで位置情報を受け取ったときの車両１０１のＹ座標値である。
【００４２】
Ｌｓｔijは、車両１０１軌跡Ｖａｂにおける点Ｐａ〜Ｐｂ間距離と点ＰＡ，ＰＢの２点間距離ＬＡＢとの差分、および車両１０１軌跡Ｖｂｃにおける点Ｐｂ〜Ｐｃ間距離と点ＰＢ，ＰＣの２点間距離ＬＢＣとの差分の総和である。このＬｓｔijを可能な限り小さくしたときの移動距離ｒiと感度ｖｓjとを求め、１車速パルスあたりの最適な移動距離ｒおよび感度ｖｓとする。そして、前回求めた最適な移動距離ｒ＿ｏｌｄおよび感度ｖｓ＿ｏｌｄとの差分をもとめ、移動距離補正値Δｒおよび感度補正値Δｖｓとする。
【００４３】
Δｒ＝ｒ−ｒ＿ｏｌｄ・・・・・・・・（４）
Δｖｓ＝ｖｓ−ｖｓ＿ｏｌｄ・・・・・（５）
【００４４】
また、補正値算出部３３１は、移動距離補正値Δｒおよび感度補正値Δｖｓのほか、ゼロ点電圧補正値ΔＥｂも算出する機能を有する。具体的には、たとえば、車両１０１が停車してから発車するまでの間のジャイロセンサ２０６からの出力電圧の平均値をゼロ点電圧Ｅｂとする。そして、前回算出したゼロ点電圧Ｅｂ＿ｏｌｄとの差分をとることで、ゼロ点電圧補正値ΔＥｂがもとめられる。
【００４５】
ΔＥｂ＝Ｅｂ−Ｅｂ＿ｏｌｄ・・・・・・・・（６）
【００４６】
初期方位算出部３３３は、車両１０１の初期方位を算出する機能を有する。初期方位とは、車両１０１が地点Ｐｃを位置（到達または通過）したときの北を基準とした車両１０１の絶対方位である。初期方位は、以下のように求める。
【００４７】
図４は、初期方位の導出方法を示す説明図である。図４の（Ａ）において、三角形△ＡＢＣは、図１に示した地点ＰＡ〜ＰＣを頂点とする図形であり、三角形△ａｂｃは、図１に示した地点Ｐａ〜Ｐｃを頂点とする図形である。点Ｇは三角形△ＡＢＣの重心であり、点ｇは三角形△ａｂｃの重心である。（Ａ）の状態から三角形△ａｂｃを平行移動し、点ｇを点Ｇに一致させる。
【００４８】
（Ｂ）は、重心Ｇ，ｇが一致した状態を示す。つぎに、重心ｇを中心として三角形△ａｂｃを回転角φ分回転させる。回転角φは（Ｂ）のように平行移動により重心ｇ，Ｇが一致した状態を基準とする。回転角φは０°≦φ≦３６０°であり、所定量Δφずつ加算されていく。この状態が（Ｃ）である。この回転前に、頂点Ｐａ，ＰＡ間距離ａＡ、頂点Ｐｂ，ＰＢ間距離ｂＢ、頂点Ｐｃ，ＰＣ間距離ｃＣの和を算出する。
【００４９】
（Ｄ）は回転後の状態を示している。回転角φの回転後においても、頂点Ｐａ，ＰＡ間距離ａＡ、頂点Ｐｂ，ＰＢ間距離ｂＢ、頂点Ｐｃ，ＰＣ間距離ｃＣの和を算出して、回転前の距離ａＡ〜ｃＣの和と比較する。回転後の和の方が小さければ、回転後の三角形△ａｂｃおよび三角形△ＡＢＣの重複度が回転前よりも高いこととなる。
【００５０】
したがって、回転後のほうが小さいときの回転角φを保存する。そして、最終的に得られた回転角φと地点Ｐｃに到達する移動軌跡Ｖｂｃの最終ベクトルｖendにおける移動回転角とを加算することで初期方位が算出される。
【００５１】
相対位置算出部３３２は、車両１０１の相対位置を算出する機能を有する。具体的には、車速パルスを１パルス検出するごとに移動距離ｒおよび移動回転角θを移動履歴として保存する。保存される移動距離ｒおよび移動回転角θは、補正値Δｒ，Δｖｓ，ΔＥｂにより補正された値である。これにより、図１に示した車両１０１の移動軌跡Ｖａｂ，Ｖｂｃを得ることができる。
【００５２】
絶対位置算出部３３４は、相対位置算出部３３２から得られた移動距離ｒおよび移動回転角θを初期方位を用いて絶対方位に変換する機能を有する。これにより、地点Ｐｃでの車両１０１の詳細な緯度Ｌａｔおよび経度Ｌｏｎと車両方位Θが得られる。したがって、車両１０１は地点Ｐｃからの自律測位の精度が向上することとなる。
【００５３】
出力部３０４は、補正処理部３０３で得られた車両１０１の緯度および経度、車両方位を出力する機能を有する。具体的には、たとえば、緯度および経度や車両方位を示すアイコンを表示画面に表示する。また、緯度および経度により表示画面に表示された地図上に現在位置を表示する。
【００５４】
（自律測位処理）
図５は、本実施の形態にかかる自律測位装置２００の自律測位処理手順を示すフローチャートである。図５において、連続不更新カウンタｈをｈ＝０にセットする（ステップＳ５０１）。連続不更新カウンタｈとは、移動距離補正値Δｒおよび感度補正値Δｖｓを更新しなかった回数を示す。回数が多いほど補正がなされていないことを示す。そして、出力電圧変動値Ｅｖを算出する（ステップＳ５０２）。
【００５５】
Ｅｖ＝Σ｜Ｅ（ｔ）−Ｅｂ｜・・・（７）
【００５６】
出力電圧変動値Ｅｖは、ジャイロセンサ２０６からの出力電圧Ｅ（ｔ）とゼロ点電圧Ｅｂとの差分を、車両１０１が連続通過した３地点ＰＡ〜ＰＣにおいて時間積分した値である。そして、Ｅｖ≧αであるか否かを判断する（ステップＳ５０３）。αは出力電圧変動値Ｅｖのしきい値である。Ｅｖ≧αである場合（ステップＳ５０３：Ｙｅｓ）、ジャイロ温度変動値Ｃｖを算出する（ステップＳ５０４）。
【００５７】
Ｃｖ＝Σ｜Ｃ（ｔ）−Ｃ’（ｔ）｜・・・（８）
【００５８】
Ｃ（ｔ）は温度センサから得られる検出温度、Ｃ’（ｔ）は温度センサから得られた地点ＰＡ通過時の検出温度、ジャイロ温度変動値Ｃｖは、Ｃ（ｔ）とＣ’（ｔ）との差分を、車両１０１が連続通過した３地点ＰＡ〜ＰＣにおいて時間積分した値である。
【００５９】
そして、Ｃｖ≦βであるか否かを判断する（ステップＳ５０５）。βはジャイロ温度変動値Ｃｖのしきい値である。Ｃｖ≦βである場合（ステップＳ５０５：Ｙｅｓ）、補正処理部３０３による補正処理を実行する（ステップＳ５０６）。このあと、連続不更新カウンタｈをｈ＝０にリセットして（ステップＳ５０７）、ステップＳ５１０に移行する。
【００６０】
一方、ステップＳ５０３において、Ｅｖ≧αでない場合（ステップＳ５０３：Ｎｏ）、または、ステップＳ５０５において、Ｃｖ≦βでない場合（ステップＳ５０５：Ｎｏ）、今回の移動距離補正値Δｒおよび感度補正値ΔｖｓをそれぞれΔｒ＝０、Δｖｓ＝０とする（ステップＳ５０８）。すなわち、両補正値を算出しないため、移動距離ｒおよび感度ｖｓは補正されないこととなる。したがって、連続不更新カウンタｈをインクリメントし（ステップＳ５０９）、ステップＳ５１０に移行する。
【００６１】
そして、出力部３０４による出力処理を実行する（ステップＳ５１０）。なお、出力処理においては、連続不更新カウンタｈの大きさにより、出力結果（絶対位置および絶対方位）を視覚的（強調表示など）に評価することとしてもよい。これにより、ユーザは直感的に補正がおこなわれた出力結果かそうでない出力結果かがわかることとなる。
【００６２】
そして、エンジンがＯＦＦとなったか否かを判断する（ステップＳ５１１）。エンジンＯＦＦでない場合（ステップＳ５１１：Ｎｏ）、ステップＳ５０２に戻る。一方、エンジンＯＦＦの場合（ステップＳ５１１：Ｙｅｓ）、一連の処理を終了する。
【００６３】
（補正処理手順）
図６は、図５に示した補正処理（ステップＳ５０６）の詳細な処理手順を示すフローチャートである。まず、補正値算出部３３１による補正値算出処理（ステップＳ６０１）と相対位置算出部３３２による相対位置算出処理（ステップＳ６０２）とを並列実行する。そして、補正値算出処理（ステップＳ６０１）および相対位置算出処理（ステップＳ６０２）の算出結果を用いて、初期方位算出部３３３による初期方位算出処理を実行し（ステップＳ６０３）、絶対位置算出部３３４による絶対位置算出処理を実行する（ステップＳ６０４）。そして、図５のステップＳ５０２に戻る。
【００６４】
（補正値算出処理手順）
図７は、補正値算出処理（ステップＳ６０１）の詳細な処理手順を示すフローチャートである。まず、移動距離補正値・感度補正値算出処理（ステップＳ７０１）とゼロ点電圧補正値算出処理（ステップＳ７０２）とを並列実行する。そして、ステップＳ６０３に移行する。
【００６５】
（移動距離補正値・感度補正値算出処理手順）
図８は、移動距離補正値・感度補正値算出処理（ステップＳ７０１）の詳細な処理手順を示すフローチャートである。この補正値算出処理では、２つのバッファを用いる。１つは履歴Ｖ１＿ｏｒでもう１つは履歴Ｖ２＿ｏｒである。
【００６６】
履歴Ｖ１＿ｏｒは、現在通過した地点とその直前に通過した地点との間の移動軌跡の情報（車速パルスのＨ（Ｈｉｇｈ）／Ｌ（Ｌｏｗ）およびジャイロセンサ２０６からの出力電圧値Ｅ（ｔ））を１車速パルスごとに記録するバッファである。たとえば、現在位置が地点ＰＢである場合、移動軌跡Ｖａｂの情報を記録し、現在位置が地点ＰＣである場合、移動軌跡Ｖｂｃの情報を記録する。
【００６７】
履歴Ｖ２＿ｏｒは、さらにその前の区間の移動軌跡の情報を記録する。たとえば、現在位置が地点ＰＣである場合、履歴Ｖ１＿ｏｒを空けるために、移動軌跡Ｖａｂの情報を履歴Ｖ１＿ｏｒからコピーする。
【００６８】
まず、位置情報が取得されていない場合（ステップＳ８０１：Ｎｏ）、車速パルスのＨ／Ｌおよびジャイロセンサ２０６からの出力電圧値Ｅ（ｔ）を履歴Ｖ１＿ｏｒに記録する（ステップＳ８０２）。そして、移動距離補正値ΔｒをΔｒ＝０、感度補正値ΔｖｓをΔｖｓ＝０とする（ステップＳ８０３）。このあと、ステップＳ６０３に移行する。
【００６９】
一方、ステップＳ８０１において、位置情報が取得された場合（ステップＳ８０１：Ｙｅｓ）、すなわち、車両１０１が光ビーコンの直下を通過した場合、履歴Ｖ１＿ｏｒおよび履歴Ｖ２＿ｏｒに電圧値Ｅ（ｔ）が記録されているか否かを判断する（ステップＳ８０４）。
【００７０】
両履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒに電圧値Ｅ（ｔ）が記録されていない場合（ステップＳ８０４：Ｎｏ）、すなわち、履歴Ｖ２＿ｏｒに電圧値Ｅ（ｔ）が記録されていない場合、ステップＳ８０６に移行する。一方、両履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒに電圧値Ｅ（ｔ）が記録されている場合（ステップＳ８０４：Ｙｅｓ）、図１に示した地点ＰＣの直下を通過したことになるため、補正値演算処理を実行する（ステップＳ８０５）。そして、履歴Ｖ１＿ｏｒの記録内容を履歴Ｖ２＿ｏｒに上書きし、履歴Ｖ１＿ｏｒを空にする（ステップＳ８０６）。このあと、ステップＳ６０３に移行する。
【００７１】
（補正値演算処理手順）
図９および図１０は、補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャートである。まず、図９において、車速パルス１パルスあたりの移動距離ｒをｒ＝ｒ＿ｍｉｎとし、車速パルス１パルスあたりの感度ｖｓをｖｓ＝ｖｓ＿ｍｉｎとし、上記式（３）で示した評価値Ｌｓｔijを下限値Ｌｓｔij＿ｍｉｎをＬｓｔ＿ｍｉｎ＝∞とする（ステップＳ９０１）。ｒ＝ｒ＿ｍｉｎは移動距離ｒの下限値である。ｖｓ＿ｍｉｎは感度ｖｓの下限値である。
【００７２】
そして、入力用履歴Ｖ＿ｏｒとして履歴Ｖ１＿ｏｒを設定する（ステップＳ９０２）。このあと、車両移動距離算出処理（ステップＳ９０３）および車両移動方位算出処理（ステップＳ９０４）を実行する。
【００７３】
簡単に説明すると、車両移動距離算出処理（ステップＳ９０３）では、車速パルス１パルスあたりのデフォルトの車両移動距離ｒをそのまま用いたり、車両移動補正値Δｒが算出されている場合にはその車両移動補正値Δｒを加算した値をあらたな車両移動距離ｒとする。
【００７４】
また、車両移動方位算出処理（ステップＳ９０４）では、上記式（１）を用いて、車速パルス１パルスあたりの移動回転角θnを算出する。車両移動距離算出処理（ステップＳ９０３）および車両移動方位算出処理（ステップＳ９０４）の詳細なフローチャートについては後述する。
【００７５】
車速パルス１パルスあたりの移動距離ｒおよび移動回転角θが求まると、移動距離ｒおよび移動回転角θをベクトル変換して、ベクトルｖを得る（ステップＳ９０５）。そして、上記式（２）を用いて、このベクトルｖを位置座標（ｘn，ｙn）に変換し（ステップＳ９０６）、バッファに蓄積する（ステップＳ９０７）。
【００７６】
このあと、入力用履歴Ｖ＿ｏｒがすべて処理されたか否かを判断する（ステップＳ９０８）。すなわち、記録されているすべての電圧値Ｅ（ｔ）について車両移動方位算出処理（ステップＳ９０４）が実行されたか否かを判断する。処理済みでない場合（ステップＳ９０８：Ｎｏ）、ステップＳ９０３に戻り、つぎの車速パルスについて移動距離ｒおよび移動回転角θを求める。
【００７７】
一方、処理済みの場合（ステップＳ９０８：Ｙｅｓ）、連続通過したビーコンから得られた２つの位置情報からビーコン間距離を算出する（ステップＳ９０９）。たとえば、入力用履歴Ｖ＿ｏｒに移動軌跡Ｖｂｃの情報が記録されている場合には、すでに取得済みの地点ＰＢ，ＰＣの位置情報（緯度経度情報）を読み出し、この２点間距離Ｌｂを算出する。
【００７８】
つぎに、ステップＳ９０７においてバッファ内に蓄積された位置座標群から対象となる移動軌跡の始点終点間距離Ｌｖを算出する（ステップＳ９１０）。たとえば、入力用履歴Ｖ＿ｏｒに移動軌跡Ｖｂｃの情報が記録されている場合には、移動軌跡Ｖｂｃの始点となる地点Ｐｂの位置座標と終点となる地点Ｐｃの位置座標を読み出し、移動軌跡Ｖｂｃの始点終点間距離Ｌｖを算出する。そして、図１０のステップＳ１００１に移行する。
【００７９】
また、図１０では距離差バッファＬ♯（♯＝１，２）を用いる。距離差バッファＬ♯とは、入力用履歴Ｖ＿ｏｒについての始点終点間距離Ｌｖから２点間距離Ｌｂを引いた値を記憶するバッファである。本フローチャートでは、いずれの距離差であっても一旦、距離差バッファＬ２に記録し、履歴Ｖ２＿ｏｒでないと判明した時点で距離差バッファＬ１にコピーすることとする。すなわち、式（３）の右辺の第１項が距離差バッファＬ２に記録された値となり、第２項が距離差バッファＬ１に記録された値となる。なお、距離差バッファＬ♯に記録された値もＬ♯とする。
【００８０】
まず、図１０において、距離差バッファＬ２に、始点終点間距離Ｌｖから２点間距離Ｌｂを引いた値（Ｌｖ−Ｌｂ）を記録し（ステップＳ１００１）、現在の入力用履歴Ｖ＿ｏｒが履歴Ｖ２＿ｏｒであるか否か（Ｖ＿ｏｒ＝Ｖ２＿ｏｒ）を判断する（ステップＳ１００２）。
【００８１】
Ｖ＿ｏｒ＝Ｖ２＿ｏｒでない場合（ステップＳ１００２：Ｎｏ）、入力用履歴Ｖ＿ｏｒを履歴Ｖ２＿ｏｒとし（ステップＳ１００３）、位置座標が蓄積されたバッファをクリアする（ステップＳ１００４）。そして、距離差バッファＬ２に記録された値を距離差バッファＬ１にコピーする（ステップＳ１００５）。このあと、図９の車両移動距離算出処理（ステップＳ９０３）に戻る。
【００８２】
一方、Ｖ＿ｏｒ＝Ｖ２＿ｏｒである場合、評価値Ｌｓｔij（＝Ｌ１＋Ｌ２）を算出する（ステップＳ１００６）。そして、評価値ＬｓｔijがＬｓｔij＜Ｌｓｔ＿ｍｉｎであるか否かを判断する（ステップＳ１００７）。
【００８３】
Ｌｓｔij＜Ｌｓｔ＿ｍｉｎでない場合（ステップＳ１００７：Ｎｏ）、ステップＳ１０１０に移行する。一方、Ｌｓｔij＜Ｌｓｔ＿ｍｉｎである場合（ステップＳ１００７：Ｙｅｓ）、Ｌｓｔ＿ｍｉｎをＬｓｔijに更新する（ステップＳ１００８）。そして、このときの移動距離ｒおよび感度ｖｓを保存する（ステップＳ１００９）。このあと、ｖｓ＞ｖｓ＿ｍａｘであるか否かを判断する（ステップＳ１０１０）。ｖｓ＿ｍａｘは感度ｖｓの上限値である。
【００８４】
ｖｓ＞ｖｓ＿ｍａｘでない場合（ステップＳ１０１０：Ｎｏ）、感度ｖｓを所定量加算して、試行回数ｊをインクリメントする（ステップＳ１０１１）。そして、ステップＳ９０２にもどる。
【００８５】
一方、ｖｓ＞ｖｓ＿ｍａｘである場合（ステップＳ１０１０：Ｙｅｓ）、ｒ＜ｒ＿ｍａｘであるか否かを判断する（ステップＳ１０１２）。ｒ＿ｍａｘは移動距離ｒの上限値である。ｒ＞ｒ＿ｍａｘでない場合（ステップＳ１０１２：Ｎｏ）、移動距離ｒを所定量加算して、試行回数ｉをインクリメントする（ステップＳ１０１３）。そして、ステップＳ９０２にもどる。
【００８６】
一方、ｒ＞ｒ＿ｍａｘである場合（ステップＳ１０１２：Ｙｅｓ）、前回検出された車速パルスにおける移動距離ｒ＿ｏｌｄと今回の移動距離ｒとを用いて、移動距離補正値Δｒ（＝ｒ−ｒ＿ｏｌｄ）を算出する。同様に、前回検出された車速パルスにおける感度ｖｓ＿ｏｌｄと今回の感度ｖｓとを用いて、感度補正値Δｖｓ（＝ｖｓ−ｖｓ＿ｏｌｄ）を算出する（ステップＳ１０１４）。そして、今回の移動距離ｒおよび感度ｖｓをつぎの車速パルス検出時に用いるため、それぞれ移動距離ｒ＿ｏｌｄ、感度ｖｓ＿ｏｌｄとして保存する（ステップＳ１０１５）。そして、ステップＳ８０６に移行する。
【００８７】
なお、履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒごとにバッファに蓄積された位置座標は、初期方位算出処理（ステップＳ６０３）で利用するため、履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒに退避させておく。
【００８８】
（車両移動距離算出処理手順）
図１１は、車両移動距離算出処理（ステップＳ９０３）の詳細な処理手順を示すフローチャートである。まず、車速パルス１パルスあたりの車両移動距離ｒを読み込み（ステップＳ１１０１）、車両移動距離ｒに値が設定済みであるか否かを判断する（ステップＳ１１０２）。
【００８９】
車両移動距離ｒに値が設定済みでない場合（ステップＳ１１０２：Ｎｏ）、初期設定値（たとえば、３９．２５［ｃｍ］）をｒとする（ステップＳ１１０３）。そして、ステップＳ９０４に移行する。一方、値が設定済みである場合（ステップＳ１１０２：Ｙｅｓ）、補正結果を反映する（ステップＳ１１０４）。すなわち、車両移動距離ｒに車両移動距離Δｒを加算してあらたな車両移動距離ｒとする。そして、ステップＳ９０４に移行する。
【００９０】
（車両移動方位算出処理手順）
図１２および図１３は、車両移動方位算出処理（ステップＳ９０４）の詳細な処理手順を示すフローチャートである。まず、感度ｖｓを読み込み（ステップＳ１２０１）、感度ｖｓに値が設定済みか否かを判断する（ステップＳ１２０２）。値が設定済みでない場合（ステップＳ１２０２：Ｎｏ）、初期設定値（たとえば、２５［ｍＶ］）をｖｓとする（ステップＳ１２０３）。
【００９１】
そして、ステップＳ１２０５に移行する。一方、値が設定済みである場合（ステップＳ１２０２：Ｙｅｓ）、補正結果を反映する（ステップＳ１２０４）。すなわち、感度ｖｓに感度補正値Δｖｓを加算してあらたな感度ｖｓとする。そして、ステップＳ１２０５に移行する。
【００９２】
また、ゼロ点電圧Ｅｂを読み込み（ステップＳ１２０５）、ゼロ点電圧Ｅｂに値が設定済みか否かを判断する（ステップＳ１２０６）。値が設定済みでない場合（ステップＳ１２０６：Ｎｏ）、初期設定値（たとえば、２．５［Ｖ］）をＥｂとする（ステップＳ１２０７）。そして、ステップＳ１２０９に移行する。一方、値が設定済みである場合（ステップＳ１２０６：Ｙｅｓ）、補正結果を反映する（ステップＳ１２０８）。すなわち、ゼロ点電圧Ｅｂにゼロ点電圧補正値ΔＥｂを加算してあらたなゼロ点電圧Ｅｂとする。そして、ステップＳ１２０９に移行する。
【００９３】
また、車速パルスのＨ／Ｌを入力するとともに（ステップＳ１２０９）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１２１０）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１２１１）。このあと、図１３のステップＳ１３０１に移行する。
【００９４】
図１３において、ステップＳ１３０１では、車速パルスがＨであるか否かを判断する（ステップＳ１３０１）。Ｈである場合（ステップＳ１３０１：Ｙｅｓ）、そのあとに検出される車速パルスのＨ／Ｌを入力するとともに（ステップＳ１３０２）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１３０３）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１３０４）。
【００９５】
そして、ステップＳ１３０５において、車速パルスがＬであるか否かを判断する（ステップＳ１３０５）。Ｌでない場合（ステップＳ１３０５：Ｎｏ）、ステップＳ１３０２に戻る。一方、Ｌである場合（ステップＳ１３０５：Ｙｅｓ）、そのあとに検出される車速パルスのＨ／Ｌを入力するとともに（ステップＳ１３０６）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１３０７）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１３０８）。
【００９６】
そして、ステップＳ１３０９において、車速パルスがＨであるか否かを判断する（ステップＳ１３０９）。Ｈでない場合（ステップＳ１３０９：Ｎｏ）、ステップＳ１３０６に戻る。一方、Ｈである場合（ステップＳ１３０９：Ｙｅｓ）、ステップＳ１３１８に移行する。
【００９７】
また、ステップＳ１３０１において、Ｌである場合（ステップＳ１３０１：Ｎｏ）、そのあとに検出される車速パルスのＨ／Ｌを入力するとともに（ステップＳ１３１０）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１３１１）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１３１２）。
【００９８】
そして、ステップＳ１３１３において、車速パルスがＨであるか否かを判断する（ステップＳ１３１３）。Ｈでない場合（ステップＳ１３１３：Ｎｏ）、ステップＳ１３１０に戻る。一方、Ｈである場合（ステップＳ１３１３：Ｙｅｓ）、そのあとに検出される車速パルスのＨ／Ｌを入力するとともに（ステップＳ１３１４）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１３１５）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１３１６）。
【００９９】
そして、ステップＳ１３１７において、車速パルスがＬであるか否かを判断する（ステップＳ１３１７）。Ｌでない場合（ステップＳ１３１７：Ｎｏ）、ステップＳ１３１４に戻る。一方、Ｌである場合（ステップＳ１３１７：Ｙｅｓ）、ステップＳ１３１８に移行する。これにより、車速パルス１パルスあたりのジャイロセンサ２０６からの出力電圧Ｅ（ｔ）を蓄積することができる。
【０１００】
そして、ステップＳ１３１８において、バッファに蓄積された出力電圧Ｅ（ｔ）を読み出し、上記式（１）を用いて移動回転角θを算出する（ステップＳ１３１８）。そして、出力電圧が蓄積されたバッファをクリアして（ステップＳ１３１９）、ステップＳ９０５に移行する。
【０１０１】
（ゼロ点電圧補正値算出処理手順）
図１４は、ゼロ点電圧補正の動作概要を示す波形図である。図１４において、横軸は時刻であり縦軸は電圧である。まず、車速パルスの変化が一定時間なかった場合に車両１０１が停車していると判断し、停車中においてブレーキがＯＦＦになったことを検出した場合に発車と判断する。ゼロ点電圧の値は、この停車から発車までの間におけるジャイロセンサ２０６からの出力電圧の平均値を算出し、この平均値をゼロ点電圧Ｅｂとして用いる。
【０１０２】
図１５は、ゼロ点電圧補正値算出処理（ステップＳ７０２）の詳細な処理手順を示すフローチャートである。まず、車速パルス不動判定用評価値ＰｂをＮＵＬＬとする（ステップＳ１５０１）。そして、タイマを起動する（ステップＳ１５０２）。このあと、車速パルスのＨ／Ｌを入力し（ステップＳ１５０３）、Ｈ／Ｌが評価値Ｐｂと同一であるか否かを判断する（ステップＳ１５０４）。
【０１０３】
同一でない場合（ステップＳ１５０４：Ｎｏ）、このときの車速パルスのＨ／Ｌを評価値Ｐｂにコピーして（ステップＳ１５０５）、タイマを再起動し（ステップＳ１５０６）、ステップＳ１５０３に戻る。一方、ステップＳ１５０４において、同一である場合（ステップＳ１５０４：Ｙｅｓ）、タイマ計測結果Ｔが一定値以上であるか否かを判断する（ステップＳ１５０７）。
【０１０４】
タイマ計測結果Ｔが一定値以上でない場合（ステップＳ１５０７：Ｎｏ）、ステップＳ１５０３に戻る。一方、タイマ計測結果Ｔが一定値以上である場合（ステップＳ１５０７：Ｙｅｓ）、ジャイロセンサ２０６からの出力電圧Ｅ（ｔ）を入力して（ステップＳ１５０８）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１５０９）。そして、ブレーキ信号を入力し（ステップＳ１５１０）、ブレーキ信号がＯＦＦであるか否かを判断する（ステップＳ１５１１）。
【０１０５】
ＯＦＦでない場合（ステップＳ１５１１：Ｎｏ）、ステップＳ１５０８に戻る。一方、ＯＦＦである場合（ステップＳ１５１１：Ｙｅｓ）、バッファに蓄積された出力電圧Ｅ（ｔ）とタイマ計測結果Ｔから出力電圧Ｅ（ｔ）の平均値Ｅａｖを算出する（ステップＳ１５１２）。そして、算出された平均値Ｅａｖをゼロ点電圧Ｅｂとする（ステップＳ１５１３）。
【０１０６】
このあと、ゼロ点電圧Ｅｂの補正値ΔＥｂ（＝Ｅｂ−Ｅｂ＿ｏｌｄ）を算出する（ステップＳ１５１４）。そして、次回に用いるため、今回算出されたゼロ点電圧Ｅｂをゼロ点電圧Ｅｂ＿ｏｌｄとして保存する（ステップＳ１５１５）。そして、ステップＳ６０３に移行する。
【０１０７】
（相対位置算出処理手順）
図１６は、相対位置算出処理（ステップＳ６０２）の詳細な処理手順を示すフローチャートである。まず、車両移動距離算出処理を実行する（ステップＳ１６０１）。この処理は、図１１のフローチャートで示した処理と同一処理であるため、説明を省略する。つぎに、車両移動方位算出処理を実行する（ステップＳ１６０２）。この処理は、図１２および図１３のフローチャートで示した処理と同一処理であるため、説明を省略する。
【０１０８】
そして、これらから得られた車速パルス１パルスあたりの車両移動距離ｒおよび移動回転角θをバッファに蓄積する（ステップＳ１６０３）。つぎに、位置情報が取得されたか否かを判断する（ステップＳ１６０４）。位置情報が取得されていない場合（ステップＳ１６０４：Ｎｏ）、ステップＳ６０４に移行する。一方、位置情報が取得された場合（ステップＳ１６０４：Ｙｅｓ）、自車移動履歴Ｗ１に車両移動距離ｒおよび移動回転角θが記録されているか否かを判断する（ステップＳ１６０５）。
【０１０９】
自車移動履歴Ｗ１とは、現在通過した地点とその直前に通過した地点との間において蓄積された車両移動距離ｒおよび移動回転角θを移動履歴情報として記録するバッファである。たとえば、現在位置が地点ＰＢである場合、移動軌跡Ｖａｂの移動履歴情報（車両移動距離ｒおよび移動回転角θ）を記録し、現在位置が地点ＰＣである場合、移動軌跡Ｖｂｃの移動履歴情報（車両移動距離ｒおよび移動回転角θ）を記録する。
【０１１０】
また、自車移動履歴Ｗ２は、さらにその前の区間の移動軌跡の移動履歴情報（車両移動距離ｒおよび移動回転角θ）を記録する。たとえば、現在位置が地点ＰＣである場合、自車移動履歴Ｗ１を空けるために、移動履歴Ｖａｂの移動履歴情報（車両移動距離ｒおよび移動回転角θ）を自車移動履歴Ｗ１から自車移動履歴Ｗ２にコピーする。これにより、自車移動履歴Ｗ１に、移動履歴Ｖｂｃの移動履歴情報を記録することができる。
【０１１１】
そして、ステップＳ１６０５において、自車移動履歴Ｗ１に移動履歴情報が記録されていない場合（ステップＳ１６０５：Ｎｏ）、ステップＳ１６０７に移行する。一方、記録されている場合（ステップＳ１６０５：Ｙｅｓ）、自車移動履歴Ｗ１に記録されている移動履歴情報を自車移動履歴Ｗ２に上書きする（ステップＳ１６０６）。そして、ステップＳ１６０３でバッファに蓄積された車両移動距離ｒおよび移動回転角θを移動履歴情報として自車移動履歴Ｗ１に記録する（ステップＳ１６０７）。そして、バッファをクリアして（ステップＳ１６０８）、初期方位算出処理（ステップＳ６０３）に移行する。
【０１１２】
（初期方位算出処理手順）
図１７および図１８は、初期方位算出処理（ステップＳ６０３）の詳細な処理手順を示すフローチャートである。まず、履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒに位置座標が記録されているか否かを判断する（ステップＳ１７０１）。記録されていない場合（ステップＳ１７０１：Ｎｏ）、ステップＳ６０４に移行する。
【０１１３】
一方、記録されている場合（ステップＳ１７０１：Ｙｅｓ）、移動軌跡Ｖａｂの始点座標として履歴Ｖ２＿ｏｒの始点座標を抽出し、移動軌跡Ｖｂｃの始点座標として履歴Ｖ１＿ｏｒの始点座標を抽出し、移動軌跡Ｖｂｃの終点座標として履歴Ｖ１＿ｏｒの終点座標を抽出する（ステップＳ１７０２）。移動軌跡Ｖａｂの始点座標が頂点Ｐａの座標となり、移動軌跡Ｖｂｃの始点座標が頂点Ｐｂの座標となり、移動軌跡Ｖｂｃの終点座標が頂点Ｐｃの座標となる。そして、これらを頂点とする三角形△ａｂｃの重心ｇを算出する（ステップＳ１７０３）。
【０１１４】
また、ビーコンＰＡ〜ＰＣからダウンリンクされたそれぞれのビーコンＰＡ〜ＰＣの位置座標を抽出し（ステップＳ１７０４）、それぞれを頂点とする三角形△ＡＢＣの重心Ｇを算出する（ステップＳ１７０５）。このあと、重心ｇが重心Ｇと一致するように、三角形ａｂｃを平行移動させる座標変換をおこなう（ステップＳ１７０６）。
【０１１５】
つぎに、回転量φの初期値としてφ＝０、回転評価値Ｌｓｔ’の下限値Ｌｓｔ’＿ｍｉｎをＬｓｔ’＿ｍｉｎ＝∞とする（ステップＳ１７０７）。そして、重心ｇを中心として三角形△ａｂｃを回転量φだけ回転させる座標変換をおこなう（ステップＳ１７０８）。この回転後の頂点Ｐａ，ＰＡ間距離、頂点Ｐｂ，ＰＢ間距離、頂点Ｐｃ，ＰＣ間距離を算出し、その和を算出し、回転評価値Ｌｓｔ’とする（ステップＳ１７０９）。そして、図１８のステップＳ１８０１に移行する。
【０１１６】
図１８において、回転評価値Ｌｓｔ’が下限値Ｌｓｔ’＿ｍｉｎに対して、Ｌｓｔ’＜Ｌｓｔ’＿ｍｉｎであるか否かを判断する（ステップＳ１８０１）。Ｌｓｔ’＜Ｌｓｔ’＿ｍｉｎでない場合（ステップＳ１８０１：Ｎｏ）、ステップＳ１８０４に移行する。一方、Ｌｓｔ’＜Ｌｓｔ’＿ｍｉｎである場合（ステップＳ１８０１：Ｙｅｓ）、Ｌｓｔ’＿ｍｉｎをＬｓｔ’に更新することで、下限値Ｌｓｔ’＿ｍｉｎを引き下げる（ステップＳ１８０２）。そして、このときの回転量φをバッファに上書き保存し（ステップＳ１８０３）、ステップＳ１８０４に移行する。
【０１１７】
ステップＳ１８０４において、回転量φがφ＞３６０°となったか否かを判断する（ステップＳ１８０４）。すなわち、三角形△ａｂｃが１回転したか否かを判断する。φ＞３６０°でない場合（ステップＳ１８０４：Ｎｏ）、回転量φを所定量インクリメントして（ステップＳ１８０５）、ステップＳ１７０８に戻る。
【０１１８】
一方、φ＞３６０°となった場合（ステップＳ１８０４：Ｙｅｓ）、移動軌跡Ｖｂｃの最終ベクトルｖend（図４の（Ｄ）を参照）の成分（ｒ，θend）から、θendを相対進行方位として抽出する（ステップＳ１８０６）。そして、最終的に保存された回転量φと相対進行方位θendとを加算することで初期方位Ｄを算出する（ステップＳ１８０７）。そして、絶対位置算出処理（ステップＳ６０４）に移行する。
【０１１９】
（絶対位置算出処理）
図１９は、絶対位置算出処理（ステップＳ６０４）の詳細な処理手順を示すフローチャートである。まず、車両方位Θ（＝Ｄ＋θ）を算出する（ステップＳ１９０１）。Ｄは初期方位であり、θは相対位置算出処理で得られた移動回転角θである。つぎに、初期方位Ｄを考慮した車両移動ベクトルを算出する（ステップＳ１９０２）。
【０１２０】
すなわち、このベクトルは、移動距離をｒ、移動回転角をΘとし、移動軌跡Ｖｂｃの最終ベクトルｖendのつぎに出現するベクトルである。これにより、地点Ｐｃからの絶対的な進行方位が特定される。このベクトルを式（２）と同様の手法により位置座標に変換し（ステップＳ１９０３）、その位置座標を緯度ＬＡＴおよび経度ＬＯＮに変換することで（ステップＳ１９０４）、車両１０１の絶対位置（ＬＡＴ，ＬＯＮ）および絶対方位Θを得ることができる。
【０１２１】
このように、上述した実施の形態では、高精度かつ低負荷の自律測位を実現することができる。また、上述した実施の形態では、移動距離ｒ、感度ｖｓおよびゼロ点電圧Ｅｂの３つの補正値を算出することとしているが、少なくともいずれか１つの補正値を算出することとしてもよい。
【０１２２】
例えば、図１における移動軌跡Ｖａｂと、地点ＰＡ、ＰＢの２地点の位置座標のみを用いて感度ｖｓの補正値を本実施の形態により算出し、移動距離ｒの補正値を他の方法（たとえば、特開２００６−３４３２１９など）を用いて算出することでも実現可能である。具体的には、図８を図２０、図９を図２１、図１０を図２２に置き換え、初期方位算出部３３３を他の方法（地磁気センサなど）を用いることでも実現可能である。また、いずれの補正値も算出せず、初期方位算出処理を実行してもよい。なお、以下に説明する図２０〜図２２において、図８〜図１０に示した処理と同一処理については同一ステップ番号を付し、その説明を省略する。
【０１２３】
図２０は、感度ｖｓのみを算出する場合の移動距離補正値・感度補正値算出処理（ステップＳ７０１）の詳細な処理手順を示すフローチャートである。まず、位置情報が取得されていない場合（ステップＳ８０１：Ｎｏ）、車速パルスのＨ／Ｌおよびジャイロセンサ２０６からの出力電圧値Ｅ（ｔ）を履歴Ｖ１＿ｏｒに記録する（ステップＳ８０２）。そして、移動距離補正値ΔｒをΔｒ＝０、感度補正値ΔｖｓをΔｖｓ＝０とする（ステップＳ８０３）。このあと、ステップＳ６０３に移行する。
【０１２４】
一方、ステップＳ８０１において、位置情報が取得された場合（ステップＳ８０１：Ｙｅｓ）、すなわち、車両１０１が光ビーコンの直下を通過した場合、図１に示した地点ＰＣの直下を通過したことになるため、補正値演算処理を実行する（ステップＳ８０５）。すなわち、図８と違い、ステップＳ８０４の処理は実行しない。そして、履歴Ｖ１＿ｏｒの記録内容を履歴Ｖ２＿ｏｒに上書きし、履歴Ｖ１＿ｏｒを空にする（ステップＳ８０６）。このあと、ステップＳ６０３に移行する。
【０１２５】
図２１および図２２は、感度ｖｓのみを算出する場合の補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャートである。まず、図２１において、車速パルス１パルスあたりの感度ｖｓをｖｓ＝ｖｓ＿ｍｉｎとし、上記式（３）で示した評価値Ｌｓｔijを下限値Ｌｓｔij＿ｍｉｎをＬｓｔ＿ｍｉｎ＝∞とする（ステップＳ２１０１）。ｖｓ＿ｍｉｎは感度ｖｓの下限値である。なお、車速パルス１パルスあたりの移動距離ｒの設定は他の方法により得られているため不要である。これ以降、ステップＳ９０２〜ステップＳ９１０を実行して、図２２のステップＳ２２０１に移行する。
【０１２６】
まず、図１０において、距離差バッファＬ２に、始点終点間距離Ｌｖから２点間距離Ｌｂを引いた値（Ｌｖ−Ｌｂ）を評価値Ｌｓｔijとして記録し（ステップＳ２２０１）、評価値ＬｓｔijがＬｓｔij＜Ｌｓｔ＿ｍｉｎであるか否かを判断する（ステップＳ１００７）。
【０１２７】
Ｌｓｔij＜Ｌｓｔ＿ｍｉｎでない場合（ステップＳ１００７：Ｎｏ）、ステップＳ１０１０に移行する。一方、Ｌｓｔij＜Ｌｓｔ＿ｍｉｎである場合（ステップＳ１００７：Ｙｅｓ）、Ｌｓｔ＿ｍｉｎをＬｓｔijに更新する（ステップＳ１００８）。そして、このときの感度ｖｓを保存する（ステップＳ２２０２）。このあと、ｖｓ＞ｖｓ＿ｍａｘであるか否かを判断する（ステップＳ１０１０）。ｖｓ＿ｍａｘは感度ｖｓの上限値である。
【０１２８】
ｖｓ＞ｖｓ＿ｍａｘでない場合（ステップＳ１０１０：Ｎｏ）、感度ｖｓを所定量加算して、試行回数ｊをインクリメントする（ステップＳ１０１１）。そして、図２１のステップＳ９０２にもどる。
【０１２９】
一方、ｖｓ＞ｖｓ＿ｍａｘである場合（ステップＳ１０１０：Ｙｅｓ）、補正済みの移動距離ｒを入力し（ステップＳ２２０３）、前回入力された移動距離ｒ＿ｏｌｄと今回入力された移動距離ｒとを用いて、移動距離補正値Δｒ（＝ｒ−ｒ＿ｏｌｄ）を算出する。同様に、前回検出された車速パルスにおける感度ｖｓ＿ｏｌｄと今回の感度ｖｓとを用いて、感度補正値Δｖｓ（＝ｖｓ−ｖｓ＿ｏｌｄ）を算出する（ステップＳ１０１４）。そして、今回の移動距離ｒおよび感度ｖｓをつぎの車速パルス検出時に用いるため、それぞれ移動距離ｒ＿ｏｌｄ、感度ｖｓ＿ｏｌｄとして保存する（ステップＳ１０１５）。そして、ステップＳ８０６に移行する。
【０１３０】
このように、感度ｖｓの補正値Δｖｓのみを算出することにより、ジャイロセンサ２０６の感度が高くなり、移動回転角θの検出精度が向上することとなる。
【０１３１】
なお、本実施の形態で説明した自律測位方法は、予め用意されたプログラムをパーソナル・コンピュータやワークステーション等のコンピュータで実行することにより実現することができる。このプログラムは、ハードディスク、フレキシブルディスク、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤ等のコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。またこのプログラムは、インターネット等のネットワークを介して配布することが可能としてもよい。
【０１３２】
上述した実施の形態に関し、さらに以下の付記を開示する。
【０１３３】
（付記１）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が通過する複数の地点の位置情報を取得する取得手段、
前記複数の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出する補正値算出手段、
前記補正値算出手段によって算出された補正値を前記移動距離に加算することにより、前記複数の地点のうち最後に通過した地点からの移動距離を算出する補正処理手段、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの移動距離を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【０１３４】
（付記２）前記補正値算出手段は、
前記判断手段により前記出力電圧の変動が所定電圧未満であると判断された場合、前記移動距離の補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの移動距離を出力することを特徴とする付記１に記載の自律測位プログラム。
【０１３５】
（付記３）前記判断手段は、
前記ジャイロセンサの温度を検出する温度センサからの前記複数の地点間における検出温度の変動が所定温度以下であるか否かを判断し、
前記補正値算出手段は、
前記判断手段によって判断された判断結果に基づいて、前記移動距離の補正値を算出することを特徴とする付記１または２に記載の自律測位プログラム。
【０１３６】
（付記４）前記補正値算出手段は、
前記判断手段により前記検出温度の変動が所定温度より高いと判断された場合、前記移動距離の補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの移動距離を出力することを特徴とする付記３に記載の自律測位プログラム。
【０１３７】
（付記５）前記補正値算出手段は、
前記判断結果に基づいて、前記複数の地点の位置情報と、前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出し、
前記補正処理手段は、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出し、
前記出力手段は、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする付記１に記載の自律測位プログラム。
【０１３８】
（付記６）速度センサからの車速パルスごとに検出される移動体の移動距離とジャイロセンサからの出力電圧に応じた前記車速パルスごとに算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が連続通過する複数の地点の位置情報を取得する取得手段、
前記複数の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出する補正値算出手段、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出する補正処理手段、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【０１３９】
（付記７）前記補正値算出手段は、
前記判断結果に基づいて、前記移動体の挙動により前記出力電圧の比較対象となるゼロ点電圧を算出するとともに、前回算出されたゼロ点電圧との差をとることにより、前記ゼロ点電圧の補正値を算出し、
前記補正処理手段は、
前記ゼロ点電圧の補正値により補正された補正後のゼロ点電圧と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出し、
前記出力手段は、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする付記５または６に記載の自律測位プログラム。
【０１４０】
（付記８）前記補正値算出手段は、
前記判断手段により前記出力電圧の変動が所定電圧未満であると判断された場合、前記補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする付記５〜７のいずれか一つに記載の自律測位プログラム。
【０１４１】
（付記９）前記判断手段は、
前記ジャイロセンサの温度を検出する温度センサからの前記複数の地点間における検出温度の変動が所定温度以下であるか否かを判断し、
前記補正値算出手段は、
前記判断手段によって判断された判断結果に基づいて、前記補正値を算出することを特徴とする付記５〜８のいずれか一つに記載の自律測位プログラム。
【０１４２】
（付記１０）前記補正値算出手段は、
前記判断手段により前記検出温度の変動が所定温度より高いと判断された場合、前記補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする付記９に記載の自律測位プログラム。
【０１４３】
（付記１１）前記コンピュータを、
前記複数の地点は任意の第１〜第３の地点であって、該第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理により算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理と、を実行し、当該回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とにより、前記移動体が前記第３の地点に到達したときの初期方位を算出する初期方位算出手段、
前記初期方位算出手段によって算出された初期方位と前記第３の地点からの移動距離および回転角とにより、前記移動体の絶対位置および絶対方位を算出する絶対位置・絶対方位算出手段、として機能させ、
前記出力手段は、
前記絶対位置・絶対方位算出手段によって算出された前記移動体の絶対位置および絶対方位を出力することを特徴とする付記１に記載の自律測位プログラム。
【０１４４】
（付記１２）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が第１〜第３の地点を通過する際に位置情報を取得する取得手段、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
判断された判断結果に基づいて、前記第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理によって算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理とを、前記判断手段によって判断された判断結果に基づいて実行し、前記回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とに基づいて、前記移動体が前記第３の地点に到達したときの初期方位を算出する初期方位算出手段、
前記初期方位算出手段によって算出された初期方位と前記第３の地点からの移動距離および回転角とに基づいて、前記移動体の絶対位置および絶対方位を算出する絶対位置・絶対方位算出手段、
前記絶対位置・絶対方位算出手段によって算出された前記移動体の絶対位置および絶対方位を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【０１４５】
（付記１３）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行する自律測位装置であって、
前記移動体が第１〜第３の地点を通過する際に位置情報を取得する取得手段と、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段と、
前記判断手段によって判断された判断結果に基づいて、前記第１〜第３の地点の位置情報と、前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出する補正値算出手段と、
前記補正値算出手段によって算出された補正値を前記移動距離に加算することにより、前記第３の地点からの移動距離を算出する補正処理手段と、
前記補正処理手段によって得られた前記第３の地点からの移動距離を出力する出力手段と、
を備えることを特徴とする自律測位装置。
【０１４６】
（付記１４）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行する自律測位装置であって、
前記移動体が通過する複数の地点の位置情報を取得する取得手段と、
前記複数の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段と、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出する補正値算出手段と、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出する補正処理手段と、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力する出力手段と、
を備えることを特徴とする自律測位装置。
【０１４７】
（付記１５）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行する自律測位装置であって、
前記移動体が通過する第１〜第３の地点の位置情報を取得する取得手段と、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段と、
判断された判断結果に基づいて、前記第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理によって算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理とを、前記判断手段によって判断された判断結果に基づいて実行し、前記回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とに基づいて、前記移動体が前記第３の地点に到達したときの初期方位を算出する初期方位算出手段と、
前記初期方位算出手段によって算出された初期方位と前記第３の地点からの移動距離および回転角とに基づいて、前記移動体の絶対位置および絶対方位を算出する絶対位置・絶対方位算出手段と、
前記絶対位置・絶対方位算出手段によって算出された前記移動体の絶対位置および絶対方位を出力する出力手段と、
を備えることを特徴とする自律測位装置。
【図面の簡単な説明】
【０１４８】
【図１】車両の移動軌跡を示す説明図である。
【図２】自律測位装置のハードウェア構成を示すブロック図である。
【図３】自律測位装置の機能的構成を示すブロック図である。
【図４】初期方位の導出方法を示す説明図である。
【図５】本実施の形態にかかる自律測位装置の自律測位処理手順を示すフローチャートである。
【図６】図５に示した補正処理（ステップＳ５０６）の詳細な処理手順を示すフローチャートである。
【図７】補正値算出処理（ステップＳ６０１）の詳細な処理手順を示すフローチャートである。
【図８】移動距離補正値・感度補正値算出処理（ステップＳ７０１）の詳細な処理手順を示すフローチャートである。
【図９】補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャート（前半）である。
【図１０】補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャート（後半）である。
【図１１】車両移動距離算出処理（ステップＳ９０３）の詳細な処理手順を示すフローチャートである。
【図１２】車両移動方位算出処理（ステップＳ９０４）の詳細な処理手順を示すフローチャート（前半）である。
【図１３】車両移動方位算出処理（ステップＳ９０４）の詳細な処理手順を示すフローチャート（後半）である。
【図１４】ゼロ点電圧補正の動作概要を示す波形図である。
【図１５】ゼロ点電圧補正値算出処理（ステップＳ７０２）の詳細な処理手順を示すフローチャートである。
【図１６】相対位置算出処理（ステップＳ６０２）の詳細な処理手順を示すフローチャートである。
【図１７】初期方位算出処理（ステップＳ６０３）の詳細な処理手順を示すフローチャート（前半）である。
【図１８】初期方位算出処理（ステップＳ６０３）の詳細な処理手順を示すフローチャート（後半）である。
【図１９】絶対位置算出処理（ステップＳ６０４）の詳細な処理手順を示すフローチャートである。
【図２０】感度ｖｓのみを算出する場合の移動距離補正値・感度補正値算出処理（ステップＳ７０１）の詳細な処理手順を示すフローチャートである。
【図２１】感度ｖｓのみを算出する場合の補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャート（前半）である。
【図２２】感度ｖｓのみを算出する場合の補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャート（後半）である。
【符号の説明】
【０１４９】
１０１ 車両
２００ 自律測位装置
３０１ 取得部
３０２ 判断部
３０３ 補正処理部
３０４ 出力部
３３１ 補正値算出部
３３２ 相対位置算出部
３３３ 初期方位算出部
３３４ 絶対位置算出部
【技術分野】
【０００１】
この発明は、自律測位をおこなう自律測位プログラムおよび自律測位装置に関する。
【背景技術】
【０００２】
現在の自律測位技術では、移動距離と移動相対角度により自身の移動軌跡を算出し、自身の位置を検出している。自律測位は主にカーナビゲーションの車両測位としてＧＰＳ（Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ）と併用して利用されており、自車の車速パルスとジャイロセンサを用いて実現している。
【０００３】
車速パルスとジャイロセンサは環境が変化することで出力する値が変化する。たとえば、タイヤの磨耗や急停車による熱膨張により車速パルスに応じた移動距離が変化する。また、車内の温度変化によりジャイロセンサの特定が変化する。このため、自律測位については補正が必要となる。
【０００４】
また、ＧＰＳ測位と自律航法測位とを複合した自律測位技術が開示されている（たとえば、下記特許文献１を参照。）。この特許文献１では、ＧＰＳによる測位結果を正解としカルマンフィルタを用いて車速パルスとジャイロセンサ出力を補正している。
【０００５】
さらに、従来のカーナビゲーションでは、マップマッチングにより自律測位結果を補正することで、走行道路が判別できる程度の測位精度を実現している。
【０００６】
【特許文献１】特開２００８−１７５７２１号公報（段落［００３２］〜［００３５］）
【発明の開示】
【発明が解決しようとする課題】
【０００７】
しかしながら、上述した特許文献１のような自律測位技術では、カルマンフィルタを用いているため、計算量が膨大になり、自律測位を実行するコンピュータの負荷が増大するという問題があった。また、走行車線を判別するような高精度な測位を実現するためには、マップマッチングでは補正しきれないという問題があった。
【０００８】
この発明は、上述した従来技術による問題点を解消するため、高精度かつ低負荷の自律測位を実現することができる自律測位プログラムおよび自律測位装置を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上述した課題を解決し、目的を達成するため、第１の自律測位プログラムおよび自律測位装置は、速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するにあたって、前記移動体が通過する任意の複数の地点の位置情報を取得し、前記複数の地点間における前記出力電圧の変動が所定電圧以下であるか否かを判断し、その判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出し、算出された補正値を前記移動距離に加算することにより、前記複数の地点のうち最後に通過した地点からの移動距離を算出し、前記複数の地点のうち最後に通過した地点からの移動距離を出力することを要件とする。
【００１０】
第２の自律測位プログラムおよび自律測位装置は、速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するにあたって、前記移動体が通過する複数の地点の位置情報を取得し、前記複数の地点間における前記出力電圧の変動が所定電圧以下であるか否かを判断し、その判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出し、算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出し、前記複数の地点のうち最後に通過した地点からの回転角を出力することを要件とする。
【００１１】
第３の自律測位プログラムおよび自律測位装置は、速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するにあたって、前記移動体が第１〜第３の地点を通過する際に位置情報を取得し、前記第１〜第３の地点間における前記出力電圧の変動が所定電圧以下であるか否かを判断し、その判断結果に基づいて、前記第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理によって算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理とを、前記判断手段によって判断された判断結果に基づいて実行し、前記回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とに基づいて、前記移動体が前記第３の地点に到達したときの初期方位を算出し、算出された初期方位と前記第３の地点からの移動距離および回転角とに基づいて、前記移動体の絶対位置および絶対方位を算出し、算出された前記移動体の絶対位置および絶対方位を出力することを要件とする。
【発明の効果】
【００１２】
自律測位プログラムおよび自律測位装置によれば、高精度かつ低負荷の自律測位を実現することができるという効果を奏する。
【発明を実施するための最良の形態】
【００１３】
以下に添付図面を参照して、自律測位プログラムおよび自律測位装置の好適な実施の形態を詳細に説明する。移動体が自律測位をおこなう場合には、位置算出に必要な計算処理を移動体に備えたコンピュータ（車載器等）上で実行することが考えられる。移動体が安全に移動を続けるためには、移動体の駆動系に必要なエネルギーを位置算出処理のためにコンピュータが消費してしまわないよう、位置算出処理にかかるエネルギー量を少なくすることが望ましい。たとえば、電気自動車は駆動系も車載器も両方電気を利用するため、コンピュータに負荷がかからないようにするのが好ましい。このため、本実施の形態では、計算量を抑えて、かつ自律測位の精度向上を図る。
【００１４】
（移動軌跡）
図１は、車両の移動軌跡を示す説明図である。（Ａ）において、車両１０１は、道路１００に設置された光ビーコンＰからダウンリンクされる位置情報（光ビーコンの位置座標）を受信しながら道路１００を走行する。
【００１５】
（Ｂ）は、（Ａ）の任意の領域である楕円部分を拡大した図である。ここでは、光ビーコンＰを車両１０１が通過した順にＰＡ，ＰＢ，ＰＣとする。ＬＡＢは光ビーコンＰＡ，ＰＢ間距離であり、ＬＢＣは光ビーコンＰＢ，ＰＣ間距離である。また、白丸で示した地点Ｐａ〜Ｐｃは、光ビーコンＰＡ〜ＰＣを通過したときの車両１０１の位置である。なお、車両１０１の実際の現在位置はＰＣであり、自律測位された車両１０１の計算上の現在位置はＰｃである。
【００１６】
また、Ｖａｂは地点Ｐａ，Ｐｂ間の車両１０１の移動軌跡であり、Ｖｂｃは地点Ｐｂ，Ｐｃ間の車両１０１の移動軌跡である。移動軌跡Ｖａｂ，Ｖｂｃは、ベクトルｖを連結したベクトル列である。ベクトルｖは車両１０１の進行方向を示しており、車両１０１の車速パルス１パルス分の移動距離ｒおよび移動回転角θという成分を含む。移動回転角θはジャイロセンサにより検出可能な成分である。
【００１７】
（Ｃ）は、（Ｂ）の任意の領域である楕円部分を拡大した図である。（Ｃ）では、車速パルスが連続するベクトルｖn-1とベクトルｖnを示している。ベクトルｖn-1はｎ−１番目の車速パルスにおけるベクトルを示しており、ベクトルｖnはｎ番目の車速パルスにおけるベクトルを示している。移動距離ｒは車速パルス１パルスあたりの車両１０１の移動距離である。移動回転角θnは、ベクトルｖn-1が指し示す進行方向を基準とした移動回転角θである。本実施の形態では、これらの情報を有効活用することで、計算量を抑えて、かつ自律測位の精度向上を図る。
【００１８】
（ハードウェア構成）
図２は、例えば図１の車両１０１などの移動体に搭載されている自律測位装置のハードウェア構成を示すブロック図である。自律測位装置２００は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）２０１と記憶装置２０２とディスプレイ２０３と受信装置２０４と車速パルス発生器（車速センサ）２０５とジャイロセンサ２０６と温度センサ２０７とがバス２０８を介して接続されている。
【００１９】
ＣＰＵ２０１は、自律測位装置２００の全体の制御を司る中央処理装置である。記憶装置２０２は、自律測位プログラムや地図データを格納したり、ＣＰＵ２０１のワークエリアとして使用される。
【００２０】
記憶装置２０２は、たとえば、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）、フラッシュメモリ、磁気ディスクドライブおよび磁気ディスク、光ディスクドライブおよび光ディスクなどにより構成される。
【００２１】
ディスプレイ２０３は、その表示画面に地図データや車両１０１の移動軌跡といった情報を表示する表示装置である。受信装置２０４は、特定地点の位置情報を受信するインターフェースである。たとえば、光ビーコンからダウンリンクされてくる位置情報を受信する。また、所定のタイミングでＧＰＳからの位置情報を受信するＧＰＳレシーバでもよい。
【００２２】
車速パルス発生器２０５は、車両１０１の車輪の回転を検出して、１回転ごとに車速パルスを発生させる。タイヤの径は既知であるため、１車速パルスあたりの車両１０１の移動距離が計算される。ジャイロセンサ２０６は、車両１０１の進行方向に対する回転角の角速度を検出する。
【００２３】
具体的には、たとえば、回転角の変化に応じた電圧を出力する。また、ジャイロセンサ２０６はゼロ点電圧と呼ばれる基準電圧を有しており、出力電圧とゼロ点電圧の差分により角速度が計算される。温度センサ２０７は温度を検出するセンサであり、車内に搭載されているジャイロセンサ２０６の温度を検出する。
【００２４】
なお、自律測位装置２００に示す要素の一部は、通信を介して接続可能であれば、移動体上に搭載されていなくても構わない。例えば、受信装置２０４、車速パルス発生器２０５、ジャイロセンサ２０６、温度センサ２０７からの出力信号を通信を介して受信可能であれば、ＣＰＵ２０１は移動体上に搭載されていなくても構わない。
【００２５】
（機能的構成）
図３は、自律測位装置２００の機能的構成を示すブロック図である。自律測位装置２００は、取得部３０１と判断部３０２と補正処理部３０３と出力部３０４とを含む構成である。また、補正処理部３０３は、補正値算出部３３１と相対位置算出部３３２と初期方位算出部３３３と絶対位置算出部３３４とを含む構成である。これら各機能は、具体的には、たとえば、図２に示した記憶装置２０２に記憶された自律測位プログラムをＣＰＵ２０１に実行させることにより、その機能を実現する。
【００２６】
取得部３０１は、各種センサからの情報を取得する機能を有する。具体的には、温度センサ２０７からは温度に応じた電圧値、車速パルス発生器２０５からは車速パルス、ジャイロセンサ２０６からは角速度に応じた出力電圧を取得する。これら情報は常時取得する。また、受信装置２０４は、特定地点通過時における特定地点の位置情報（位置座標）を取得する。たとえば、位置情報をダウンリンクする光ビーコンの直下を通過する際にその位置情報を受信する。これにより、光ビーコン通過時の自車位置を特定することができる。
【００２７】
また、判断部３０２は、補正処理部３０３の実行可否を判断する機能を有する。具体的には、ジャイロセンサ２０６からの出力電圧の変動と温度センサ２０７から検出された温度の変動により、補正処理部３０３の実行可否を判断する。
【００２８】
出力電圧の変動が所定のしきい値α以下の場合、車両１０１が車線進路変更や右左折など進路変更をしていないため、この出力電圧から算出した移動軌跡上の回転角度は小さい。この移動軌跡を用いて車両の回転角に影響を与える感度の補正をする場合、補正元の移動軌跡上の回転角度が小さいため、補正に適さない。したがって、補正処理部３０３による補正を実行しない。このしきい値αは、右左折のような大きな角度変化を検出したいのか、進路変更（車線変更）などの小さな角度変化をも検出したいのかにより設定すべき値が異なる。しきい値αを小さく設定すれば進路変更の検出が可能となり、それよりも大きく設定すれば右左折を検出することができる。
【００２９】
また、検出温度の変動が所定のしきい値β以下の場合、ジャイロセンサ２０６が正常に動作しており、ジャイロセンサ２０６は正確な電圧を出力していると考えられる。そして、ジャイロセンサ２０６が出力した電圧から算出した移動軌跡は、正確であると判断できる。したがって、ジャイロセンサ２０６が出力した電圧から算出した移動軌跡を用いて、補正処理部３０３による補正を実行する。なお、補正処理部３０３による補正は、出力電圧の変動が所定のしきい値αよりも大きい場合でかつ検出温度の変動が所定のしきい値β以下の場合に実行するのが最も好ましい。
【００３０】
このように、ジャイロセンサ２０６の出力変動や温度変化に応じて自律測位の補正の適否を自動判断することができるため、不要な計算量を抑制することができ、コンピュータの負荷軽減を図ることができる。
【００３１】
補正処理部３０３は、補正値算出部３３１と相対位置算出部３３２と初期方位算出部３３３と絶対位置算出部３３４とを含む構成である。補正値算出部３３１は、感度補正値Δｖｓ、移動距離補正値Δｒ、ゼロ点電圧補正値ΔＥｂといった補正値を算出する機能を有する。
【００３２】
具体的には、点Ｐａ〜Ｐｃまでの間で、車速パルス発生器２０５からの車速パルスの検出回数ｎを計数するとともに、検出時刻ｔとそのときの状態（Ｈ／Ｌ）を記録する。そして、下記式（１）に示すように、１車速パルスあたりの車両１０１の回転角度を算出する。
【００３３】
【数１】
【００３４】
ただし、θnは、ｎ番目の車速パルスについての車両１０１の移動回転角度、ｔnはｎ番目の車速パルスの検出時刻、ｔn-1はｎ−１番目の車速パルスの検出時刻、Ｅ（ｔ）は時刻ｔにおけるジャイロセンサ２０６からの出力電圧、Ｅｂはゼロ点電圧、ｖｓjは感度補正の試行回数ｊ番目のジャイロセンサ２０６の感度である。
【００３５】
また、補正値算出部３３１は、点Ｐａ〜Ｐｃまでの間で、自車移動履歴を算出する機能を有する。具体的には、下記式（２）に示すように、車速パルスごとの車両１０１の位置座標を算出する。
【００３６】
（ｘn，ｙn）＝（ｘn-1＋ｒi・ｃｏｓθn，ｙn-1＋ｒi・ｓｉｎθn）
・・・・・・（２）
【００３７】
ただし、ｘnはｎ番目の車速パルス検出時の車両１０１のＸ座標値、ｘn-1はｎ−１番目の車速パルス検出時の車両１０１のＸ座標値、ｙnはｎ番目の車速パルス検出時の車両１０１のＹ座標値、ｙn-1はｎ−１番目の車速パルス検出時の車両１０１のＹ座標値、である。ｒiは移動距離補正の試行回数ｉ番目のときの車両移動距離である。
【００３８】
また、補正値算出部３３１は、上記式（２）で算出された自車移動履歴の正誤判定値を算出する機能を有する。具体的には、下記式（３）に示すように、移動距離補正の試行回数ｉごとに正誤判定の評価値を算出する。
【００３９】
【数２】
【００４０】
ただし、Ｌｓｔijは試行回数ｉ番目の正誤判定値、ｘaは点Ｐａで位置情報を受け取っ
たときの車両１０１のＸ座標値、ｘbは点Ｐｂで位置情報を受け取ったときの車両１０１のＸ座標値、ｘcは点Ｐｃで位置情報を受け取ったときの車両１０１のＸ座標値、ｙaは点Ｐａで位置情報を受け取ったときの車両１０１のＹ座標値、ｙbは点Ｐｂで位置情報を受け取ったときの車両１０１のＹ座標値、ｙcは点Ｐｃで位置情報を受け取ったときの車両１０１のＹ座標値である。
【００４１】
また、ＸAは点ＰＡで位置情報を受け取ったときの車両１０１のＸ座標値、ＸBは点ＰＢで位置情報を受け取ったときの車両１０１のＸ座標値、ＸCは点ＰＣで位置情報を受け取ったときの車両１０１のＸ座標値、ＹAは点ＰＡで位置情報を受け取ったときの車両１０１のＹ座標値、ＹBは点ＰＢで位置情報を受け取ったときの車両１０１のＹ座標値、ＹCは点ＰＣで位置情報を受け取ったときの車両１０１のＹ座標値である。
【００４２】
Ｌｓｔijは、車両１０１軌跡Ｖａｂにおける点Ｐａ〜Ｐｂ間距離と点ＰＡ，ＰＢの２点間距離ＬＡＢとの差分、および車両１０１軌跡Ｖｂｃにおける点Ｐｂ〜Ｐｃ間距離と点ＰＢ，ＰＣの２点間距離ＬＢＣとの差分の総和である。このＬｓｔijを可能な限り小さくしたときの移動距離ｒiと感度ｖｓjとを求め、１車速パルスあたりの最適な移動距離ｒおよび感度ｖｓとする。そして、前回求めた最適な移動距離ｒ＿ｏｌｄおよび感度ｖｓ＿ｏｌｄとの差分をもとめ、移動距離補正値Δｒおよび感度補正値Δｖｓとする。
【００４３】
Δｒ＝ｒ−ｒ＿ｏｌｄ・・・・・・・・（４）
Δｖｓ＝ｖｓ−ｖｓ＿ｏｌｄ・・・・・（５）
【００４４】
また、補正値算出部３３１は、移動距離補正値Δｒおよび感度補正値Δｖｓのほか、ゼロ点電圧補正値ΔＥｂも算出する機能を有する。具体的には、たとえば、車両１０１が停車してから発車するまでの間のジャイロセンサ２０６からの出力電圧の平均値をゼロ点電圧Ｅｂとする。そして、前回算出したゼロ点電圧Ｅｂ＿ｏｌｄとの差分をとることで、ゼロ点電圧補正値ΔＥｂがもとめられる。
【００４５】
ΔＥｂ＝Ｅｂ−Ｅｂ＿ｏｌｄ・・・・・・・・（６）
【００４６】
初期方位算出部３３３は、車両１０１の初期方位を算出する機能を有する。初期方位とは、車両１０１が地点Ｐｃを位置（到達または通過）したときの北を基準とした車両１０１の絶対方位である。初期方位は、以下のように求める。
【００４７】
図４は、初期方位の導出方法を示す説明図である。図４の（Ａ）において、三角形△ＡＢＣは、図１に示した地点ＰＡ〜ＰＣを頂点とする図形であり、三角形△ａｂｃは、図１に示した地点Ｐａ〜Ｐｃを頂点とする図形である。点Ｇは三角形△ＡＢＣの重心であり、点ｇは三角形△ａｂｃの重心である。（Ａ）の状態から三角形△ａｂｃを平行移動し、点ｇを点Ｇに一致させる。
【００４８】
（Ｂ）は、重心Ｇ，ｇが一致した状態を示す。つぎに、重心ｇを中心として三角形△ａｂｃを回転角φ分回転させる。回転角φは（Ｂ）のように平行移動により重心ｇ，Ｇが一致した状態を基準とする。回転角φは０°≦φ≦３６０°であり、所定量Δφずつ加算されていく。この状態が（Ｃ）である。この回転前に、頂点Ｐａ，ＰＡ間距離ａＡ、頂点Ｐｂ，ＰＢ間距離ｂＢ、頂点Ｐｃ，ＰＣ間距離ｃＣの和を算出する。
【００４９】
（Ｄ）は回転後の状態を示している。回転角φの回転後においても、頂点Ｐａ，ＰＡ間距離ａＡ、頂点Ｐｂ，ＰＢ間距離ｂＢ、頂点Ｐｃ，ＰＣ間距離ｃＣの和を算出して、回転前の距離ａＡ〜ｃＣの和と比較する。回転後の和の方が小さければ、回転後の三角形△ａｂｃおよび三角形△ＡＢＣの重複度が回転前よりも高いこととなる。
【００５０】
したがって、回転後のほうが小さいときの回転角φを保存する。そして、最終的に得られた回転角φと地点Ｐｃに到達する移動軌跡Ｖｂｃの最終ベクトルｖendにおける移動回転角とを加算することで初期方位が算出される。
【００５１】
相対位置算出部３３２は、車両１０１の相対位置を算出する機能を有する。具体的には、車速パルスを１パルス検出するごとに移動距離ｒおよび移動回転角θを移動履歴として保存する。保存される移動距離ｒおよび移動回転角θは、補正値Δｒ，Δｖｓ，ΔＥｂにより補正された値である。これにより、図１に示した車両１０１の移動軌跡Ｖａｂ，Ｖｂｃを得ることができる。
【００５２】
絶対位置算出部３３４は、相対位置算出部３３２から得られた移動距離ｒおよび移動回転角θを初期方位を用いて絶対方位に変換する機能を有する。これにより、地点Ｐｃでの車両１０１の詳細な緯度Ｌａｔおよび経度Ｌｏｎと車両方位Θが得られる。したがって、車両１０１は地点Ｐｃからの自律測位の精度が向上することとなる。
【００５３】
出力部３０４は、補正処理部３０３で得られた車両１０１の緯度および経度、車両方位を出力する機能を有する。具体的には、たとえば、緯度および経度や車両方位を示すアイコンを表示画面に表示する。また、緯度および経度により表示画面に表示された地図上に現在位置を表示する。
【００５４】
（自律測位処理）
図５は、本実施の形態にかかる自律測位装置２００の自律測位処理手順を示すフローチャートである。図５において、連続不更新カウンタｈをｈ＝０にセットする（ステップＳ５０１）。連続不更新カウンタｈとは、移動距離補正値Δｒおよび感度補正値Δｖｓを更新しなかった回数を示す。回数が多いほど補正がなされていないことを示す。そして、出力電圧変動値Ｅｖを算出する（ステップＳ５０２）。
【００５５】
Ｅｖ＝Σ｜Ｅ（ｔ）−Ｅｂ｜・・・（７）
【００５６】
出力電圧変動値Ｅｖは、ジャイロセンサ２０６からの出力電圧Ｅ（ｔ）とゼロ点電圧Ｅｂとの差分を、車両１０１が連続通過した３地点ＰＡ〜ＰＣにおいて時間積分した値である。そして、Ｅｖ≧αであるか否かを判断する（ステップＳ５０３）。αは出力電圧変動値Ｅｖのしきい値である。Ｅｖ≧αである場合（ステップＳ５０３：Ｙｅｓ）、ジャイロ温度変動値Ｃｖを算出する（ステップＳ５０４）。
【００５７】
Ｃｖ＝Σ｜Ｃ（ｔ）−Ｃ’（ｔ）｜・・・（８）
【００５８】
Ｃ（ｔ）は温度センサから得られる検出温度、Ｃ’（ｔ）は温度センサから得られた地点ＰＡ通過時の検出温度、ジャイロ温度変動値Ｃｖは、Ｃ（ｔ）とＣ’（ｔ）との差分を、車両１０１が連続通過した３地点ＰＡ〜ＰＣにおいて時間積分した値である。
【００５９】
そして、Ｃｖ≦βであるか否かを判断する（ステップＳ５０５）。βはジャイロ温度変動値Ｃｖのしきい値である。Ｃｖ≦βである場合（ステップＳ５０５：Ｙｅｓ）、補正処理部３０３による補正処理を実行する（ステップＳ５０６）。このあと、連続不更新カウンタｈをｈ＝０にリセットして（ステップＳ５０７）、ステップＳ５１０に移行する。
【００６０】
一方、ステップＳ５０３において、Ｅｖ≧αでない場合（ステップＳ５０３：Ｎｏ）、または、ステップＳ５０５において、Ｃｖ≦βでない場合（ステップＳ５０５：Ｎｏ）、今回の移動距離補正値Δｒおよび感度補正値ΔｖｓをそれぞれΔｒ＝０、Δｖｓ＝０とする（ステップＳ５０８）。すなわち、両補正値を算出しないため、移動距離ｒおよび感度ｖｓは補正されないこととなる。したがって、連続不更新カウンタｈをインクリメントし（ステップＳ５０９）、ステップＳ５１０に移行する。
【００６１】
そして、出力部３０４による出力処理を実行する（ステップＳ５１０）。なお、出力処理においては、連続不更新カウンタｈの大きさにより、出力結果（絶対位置および絶対方位）を視覚的（強調表示など）に評価することとしてもよい。これにより、ユーザは直感的に補正がおこなわれた出力結果かそうでない出力結果かがわかることとなる。
【００６２】
そして、エンジンがＯＦＦとなったか否かを判断する（ステップＳ５１１）。エンジンＯＦＦでない場合（ステップＳ５１１：Ｎｏ）、ステップＳ５０２に戻る。一方、エンジンＯＦＦの場合（ステップＳ５１１：Ｙｅｓ）、一連の処理を終了する。
【００６３】
（補正処理手順）
図６は、図５に示した補正処理（ステップＳ５０６）の詳細な処理手順を示すフローチャートである。まず、補正値算出部３３１による補正値算出処理（ステップＳ６０１）と相対位置算出部３３２による相対位置算出処理（ステップＳ６０２）とを並列実行する。そして、補正値算出処理（ステップＳ６０１）および相対位置算出処理（ステップＳ６０２）の算出結果を用いて、初期方位算出部３３３による初期方位算出処理を実行し（ステップＳ６０３）、絶対位置算出部３３４による絶対位置算出処理を実行する（ステップＳ６０４）。そして、図５のステップＳ５０２に戻る。
【００６４】
（補正値算出処理手順）
図７は、補正値算出処理（ステップＳ６０１）の詳細な処理手順を示すフローチャートである。まず、移動距離補正値・感度補正値算出処理（ステップＳ７０１）とゼロ点電圧補正値算出処理（ステップＳ７０２）とを並列実行する。そして、ステップＳ６０３に移行する。
【００６５】
（移動距離補正値・感度補正値算出処理手順）
図８は、移動距離補正値・感度補正値算出処理（ステップＳ７０１）の詳細な処理手順を示すフローチャートである。この補正値算出処理では、２つのバッファを用いる。１つは履歴Ｖ１＿ｏｒでもう１つは履歴Ｖ２＿ｏｒである。
【００６６】
履歴Ｖ１＿ｏｒは、現在通過した地点とその直前に通過した地点との間の移動軌跡の情報（車速パルスのＨ（Ｈｉｇｈ）／Ｌ（Ｌｏｗ）およびジャイロセンサ２０６からの出力電圧値Ｅ（ｔ））を１車速パルスごとに記録するバッファである。たとえば、現在位置が地点ＰＢである場合、移動軌跡Ｖａｂの情報を記録し、現在位置が地点ＰＣである場合、移動軌跡Ｖｂｃの情報を記録する。
【００６７】
履歴Ｖ２＿ｏｒは、さらにその前の区間の移動軌跡の情報を記録する。たとえば、現在位置が地点ＰＣである場合、履歴Ｖ１＿ｏｒを空けるために、移動軌跡Ｖａｂの情報を履歴Ｖ１＿ｏｒからコピーする。
【００６８】
まず、位置情報が取得されていない場合（ステップＳ８０１：Ｎｏ）、車速パルスのＨ／Ｌおよびジャイロセンサ２０６からの出力電圧値Ｅ（ｔ）を履歴Ｖ１＿ｏｒに記録する（ステップＳ８０２）。そして、移動距離補正値ΔｒをΔｒ＝０、感度補正値ΔｖｓをΔｖｓ＝０とする（ステップＳ８０３）。このあと、ステップＳ６０３に移行する。
【００６９】
一方、ステップＳ８０１において、位置情報が取得された場合（ステップＳ８０１：Ｙｅｓ）、すなわち、車両１０１が光ビーコンの直下を通過した場合、履歴Ｖ１＿ｏｒおよび履歴Ｖ２＿ｏｒに電圧値Ｅ（ｔ）が記録されているか否かを判断する（ステップＳ８０４）。
【００７０】
両履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒに電圧値Ｅ（ｔ）が記録されていない場合（ステップＳ８０４：Ｎｏ）、すなわち、履歴Ｖ２＿ｏｒに電圧値Ｅ（ｔ）が記録されていない場合、ステップＳ８０６に移行する。一方、両履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒに電圧値Ｅ（ｔ）が記録されている場合（ステップＳ８０４：Ｙｅｓ）、図１に示した地点ＰＣの直下を通過したことになるため、補正値演算処理を実行する（ステップＳ８０５）。そして、履歴Ｖ１＿ｏｒの記録内容を履歴Ｖ２＿ｏｒに上書きし、履歴Ｖ１＿ｏｒを空にする（ステップＳ８０６）。このあと、ステップＳ６０３に移行する。
【００７１】
（補正値演算処理手順）
図９および図１０は、補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャートである。まず、図９において、車速パルス１パルスあたりの移動距離ｒをｒ＝ｒ＿ｍｉｎとし、車速パルス１パルスあたりの感度ｖｓをｖｓ＝ｖｓ＿ｍｉｎとし、上記式（３）で示した評価値Ｌｓｔijを下限値Ｌｓｔij＿ｍｉｎをＬｓｔ＿ｍｉｎ＝∞とする（ステップＳ９０１）。ｒ＝ｒ＿ｍｉｎは移動距離ｒの下限値である。ｖｓ＿ｍｉｎは感度ｖｓの下限値である。
【００７２】
そして、入力用履歴Ｖ＿ｏｒとして履歴Ｖ１＿ｏｒを設定する（ステップＳ９０２）。このあと、車両移動距離算出処理（ステップＳ９０３）および車両移動方位算出処理（ステップＳ９０４）を実行する。
【００７３】
簡単に説明すると、車両移動距離算出処理（ステップＳ９０３）では、車速パルス１パルスあたりのデフォルトの車両移動距離ｒをそのまま用いたり、車両移動補正値Δｒが算出されている場合にはその車両移動補正値Δｒを加算した値をあらたな車両移動距離ｒとする。
【００７４】
また、車両移動方位算出処理（ステップＳ９０４）では、上記式（１）を用いて、車速パルス１パルスあたりの移動回転角θnを算出する。車両移動距離算出処理（ステップＳ９０３）および車両移動方位算出処理（ステップＳ９０４）の詳細なフローチャートについては後述する。
【００７５】
車速パルス１パルスあたりの移動距離ｒおよび移動回転角θが求まると、移動距離ｒおよび移動回転角θをベクトル変換して、ベクトルｖを得る（ステップＳ９０５）。そして、上記式（２）を用いて、このベクトルｖを位置座標（ｘn，ｙn）に変換し（ステップＳ９０６）、バッファに蓄積する（ステップＳ９０７）。
【００７６】
このあと、入力用履歴Ｖ＿ｏｒがすべて処理されたか否かを判断する（ステップＳ９０８）。すなわち、記録されているすべての電圧値Ｅ（ｔ）について車両移動方位算出処理（ステップＳ９０４）が実行されたか否かを判断する。処理済みでない場合（ステップＳ９０８：Ｎｏ）、ステップＳ９０３に戻り、つぎの車速パルスについて移動距離ｒおよび移動回転角θを求める。
【００７７】
一方、処理済みの場合（ステップＳ９０８：Ｙｅｓ）、連続通過したビーコンから得られた２つの位置情報からビーコン間距離を算出する（ステップＳ９０９）。たとえば、入力用履歴Ｖ＿ｏｒに移動軌跡Ｖｂｃの情報が記録されている場合には、すでに取得済みの地点ＰＢ，ＰＣの位置情報（緯度経度情報）を読み出し、この２点間距離Ｌｂを算出する。
【００７８】
つぎに、ステップＳ９０７においてバッファ内に蓄積された位置座標群から対象となる移動軌跡の始点終点間距離Ｌｖを算出する（ステップＳ９１０）。たとえば、入力用履歴Ｖ＿ｏｒに移動軌跡Ｖｂｃの情報が記録されている場合には、移動軌跡Ｖｂｃの始点となる地点Ｐｂの位置座標と終点となる地点Ｐｃの位置座標を読み出し、移動軌跡Ｖｂｃの始点終点間距離Ｌｖを算出する。そして、図１０のステップＳ１００１に移行する。
【００７９】
また、図１０では距離差バッファＬ♯（♯＝１，２）を用いる。距離差バッファＬ♯とは、入力用履歴Ｖ＿ｏｒについての始点終点間距離Ｌｖから２点間距離Ｌｂを引いた値を記憶するバッファである。本フローチャートでは、いずれの距離差であっても一旦、距離差バッファＬ２に記録し、履歴Ｖ２＿ｏｒでないと判明した時点で距離差バッファＬ１にコピーすることとする。すなわち、式（３）の右辺の第１項が距離差バッファＬ２に記録された値となり、第２項が距離差バッファＬ１に記録された値となる。なお、距離差バッファＬ♯に記録された値もＬ♯とする。
【００８０】
まず、図１０において、距離差バッファＬ２に、始点終点間距離Ｌｖから２点間距離Ｌｂを引いた値（Ｌｖ−Ｌｂ）を記録し（ステップＳ１００１）、現在の入力用履歴Ｖ＿ｏｒが履歴Ｖ２＿ｏｒであるか否か（Ｖ＿ｏｒ＝Ｖ２＿ｏｒ）を判断する（ステップＳ１００２）。
【００８１】
Ｖ＿ｏｒ＝Ｖ２＿ｏｒでない場合（ステップＳ１００２：Ｎｏ）、入力用履歴Ｖ＿ｏｒを履歴Ｖ２＿ｏｒとし（ステップＳ１００３）、位置座標が蓄積されたバッファをクリアする（ステップＳ１００４）。そして、距離差バッファＬ２に記録された値を距離差バッファＬ１にコピーする（ステップＳ１００５）。このあと、図９の車両移動距離算出処理（ステップＳ９０３）に戻る。
【００８２】
一方、Ｖ＿ｏｒ＝Ｖ２＿ｏｒである場合、評価値Ｌｓｔij（＝Ｌ１＋Ｌ２）を算出する（ステップＳ１００６）。そして、評価値ＬｓｔijがＬｓｔij＜Ｌｓｔ＿ｍｉｎであるか否かを判断する（ステップＳ１００７）。
【００８３】
Ｌｓｔij＜Ｌｓｔ＿ｍｉｎでない場合（ステップＳ１００７：Ｎｏ）、ステップＳ１０１０に移行する。一方、Ｌｓｔij＜Ｌｓｔ＿ｍｉｎである場合（ステップＳ１００７：Ｙｅｓ）、Ｌｓｔ＿ｍｉｎをＬｓｔijに更新する（ステップＳ１００８）。そして、このときの移動距離ｒおよび感度ｖｓを保存する（ステップＳ１００９）。このあと、ｖｓ＞ｖｓ＿ｍａｘであるか否かを判断する（ステップＳ１０１０）。ｖｓ＿ｍａｘは感度ｖｓの上限値である。
【００８４】
ｖｓ＞ｖｓ＿ｍａｘでない場合（ステップＳ１０１０：Ｎｏ）、感度ｖｓを所定量加算して、試行回数ｊをインクリメントする（ステップＳ１０１１）。そして、ステップＳ９０２にもどる。
【００８５】
一方、ｖｓ＞ｖｓ＿ｍａｘである場合（ステップＳ１０１０：Ｙｅｓ）、ｒ＜ｒ＿ｍａｘであるか否かを判断する（ステップＳ１０１２）。ｒ＿ｍａｘは移動距離ｒの上限値である。ｒ＞ｒ＿ｍａｘでない場合（ステップＳ１０１２：Ｎｏ）、移動距離ｒを所定量加算して、試行回数ｉをインクリメントする（ステップＳ１０１３）。そして、ステップＳ９０２にもどる。
【００８６】
一方、ｒ＞ｒ＿ｍａｘである場合（ステップＳ１０１２：Ｙｅｓ）、前回検出された車速パルスにおける移動距離ｒ＿ｏｌｄと今回の移動距離ｒとを用いて、移動距離補正値Δｒ（＝ｒ−ｒ＿ｏｌｄ）を算出する。同様に、前回検出された車速パルスにおける感度ｖｓ＿ｏｌｄと今回の感度ｖｓとを用いて、感度補正値Δｖｓ（＝ｖｓ−ｖｓ＿ｏｌｄ）を算出する（ステップＳ１０１４）。そして、今回の移動距離ｒおよび感度ｖｓをつぎの車速パルス検出時に用いるため、それぞれ移動距離ｒ＿ｏｌｄ、感度ｖｓ＿ｏｌｄとして保存する（ステップＳ１０１５）。そして、ステップＳ８０６に移行する。
【００８７】
なお、履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒごとにバッファに蓄積された位置座標は、初期方位算出処理（ステップＳ６０３）で利用するため、履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒに退避させておく。
【００８８】
（車両移動距離算出処理手順）
図１１は、車両移動距離算出処理（ステップＳ９０３）の詳細な処理手順を示すフローチャートである。まず、車速パルス１パルスあたりの車両移動距離ｒを読み込み（ステップＳ１１０１）、車両移動距離ｒに値が設定済みであるか否かを判断する（ステップＳ１１０２）。
【００８９】
車両移動距離ｒに値が設定済みでない場合（ステップＳ１１０２：Ｎｏ）、初期設定値（たとえば、３９．２５［ｃｍ］）をｒとする（ステップＳ１１０３）。そして、ステップＳ９０４に移行する。一方、値が設定済みである場合（ステップＳ１１０２：Ｙｅｓ）、補正結果を反映する（ステップＳ１１０４）。すなわち、車両移動距離ｒに車両移動距離Δｒを加算してあらたな車両移動距離ｒとする。そして、ステップＳ９０４に移行する。
【００９０】
（車両移動方位算出処理手順）
図１２および図１３は、車両移動方位算出処理（ステップＳ９０４）の詳細な処理手順を示すフローチャートである。まず、感度ｖｓを読み込み（ステップＳ１２０１）、感度ｖｓに値が設定済みか否かを判断する（ステップＳ１２０２）。値が設定済みでない場合（ステップＳ１２０２：Ｎｏ）、初期設定値（たとえば、２５［ｍＶ］）をｖｓとする（ステップＳ１２０３）。
【００９１】
そして、ステップＳ１２０５に移行する。一方、値が設定済みである場合（ステップＳ１２０２：Ｙｅｓ）、補正結果を反映する（ステップＳ１２０４）。すなわち、感度ｖｓに感度補正値Δｖｓを加算してあらたな感度ｖｓとする。そして、ステップＳ１２０５に移行する。
【００９２】
また、ゼロ点電圧Ｅｂを読み込み（ステップＳ１２０５）、ゼロ点電圧Ｅｂに値が設定済みか否かを判断する（ステップＳ１２０６）。値が設定済みでない場合（ステップＳ１２０６：Ｎｏ）、初期設定値（たとえば、２．５［Ｖ］）をＥｂとする（ステップＳ１２０７）。そして、ステップＳ１２０９に移行する。一方、値が設定済みである場合（ステップＳ１２０６：Ｙｅｓ）、補正結果を反映する（ステップＳ１２０８）。すなわち、ゼロ点電圧Ｅｂにゼロ点電圧補正値ΔＥｂを加算してあらたなゼロ点電圧Ｅｂとする。そして、ステップＳ１２０９に移行する。
【００９３】
また、車速パルスのＨ／Ｌを入力するとともに（ステップＳ１２０９）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１２１０）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１２１１）。このあと、図１３のステップＳ１３０１に移行する。
【００９４】
図１３において、ステップＳ１３０１では、車速パルスがＨであるか否かを判断する（ステップＳ１３０１）。Ｈである場合（ステップＳ１３０１：Ｙｅｓ）、そのあとに検出される車速パルスのＨ／Ｌを入力するとともに（ステップＳ１３０２）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１３０３）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１３０４）。
【００９５】
そして、ステップＳ１３０５において、車速パルスがＬであるか否かを判断する（ステップＳ１３０５）。Ｌでない場合（ステップＳ１３０５：Ｎｏ）、ステップＳ１３０２に戻る。一方、Ｌである場合（ステップＳ１３０５：Ｙｅｓ）、そのあとに検出される車速パルスのＨ／Ｌを入力するとともに（ステップＳ１３０６）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１３０７）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１３０８）。
【００９６】
そして、ステップＳ１３０９において、車速パルスがＨであるか否かを判断する（ステップＳ１３０９）。Ｈでない場合（ステップＳ１３０９：Ｎｏ）、ステップＳ１３０６に戻る。一方、Ｈである場合（ステップＳ１３０９：Ｙｅｓ）、ステップＳ１３１８に移行する。
【００９７】
また、ステップＳ１３０１において、Ｌである場合（ステップＳ１３０１：Ｎｏ）、そのあとに検出される車速パルスのＨ／Ｌを入力するとともに（ステップＳ１３１０）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１３１１）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１３１２）。
【００９８】
そして、ステップＳ１３１３において、車速パルスがＨであるか否かを判断する（ステップＳ１３１３）。Ｈでない場合（ステップＳ１３１３：Ｎｏ）、ステップＳ１３１０に戻る。一方、Ｈである場合（ステップＳ１３１３：Ｙｅｓ）、そのあとに検出される車速パルスのＨ／Ｌを入力するとともに（ステップＳ１３１４）、ジャイロセンサ２０６の出力電圧Ｅ（ｔ）を入力し（ステップＳ１３１５）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１３１６）。
【００９９】
そして、ステップＳ１３１７において、車速パルスがＬであるか否かを判断する（ステップＳ１３１７）。Ｌでない場合（ステップＳ１３１７：Ｎｏ）、ステップＳ１３１４に戻る。一方、Ｌである場合（ステップＳ１３１７：Ｙｅｓ）、ステップＳ１３１８に移行する。これにより、車速パルス１パルスあたりのジャイロセンサ２０６からの出力電圧Ｅ（ｔ）を蓄積することができる。
【０１００】
そして、ステップＳ１３１８において、バッファに蓄積された出力電圧Ｅ（ｔ）を読み出し、上記式（１）を用いて移動回転角θを算出する（ステップＳ１３１８）。そして、出力電圧が蓄積されたバッファをクリアして（ステップＳ１３１９）、ステップＳ９０５に移行する。
【０１０１】
（ゼロ点電圧補正値算出処理手順）
図１４は、ゼロ点電圧補正の動作概要を示す波形図である。図１４において、横軸は時刻であり縦軸は電圧である。まず、車速パルスの変化が一定時間なかった場合に車両１０１が停車していると判断し、停車中においてブレーキがＯＦＦになったことを検出した場合に発車と判断する。ゼロ点電圧の値は、この停車から発車までの間におけるジャイロセンサ２０６からの出力電圧の平均値を算出し、この平均値をゼロ点電圧Ｅｂとして用いる。
【０１０２】
図１５は、ゼロ点電圧補正値算出処理（ステップＳ７０２）の詳細な処理手順を示すフローチャートである。まず、車速パルス不動判定用評価値ＰｂをＮＵＬＬとする（ステップＳ１５０１）。そして、タイマを起動する（ステップＳ１５０２）。このあと、車速パルスのＨ／Ｌを入力し（ステップＳ１５０３）、Ｈ／Ｌが評価値Ｐｂと同一であるか否かを判断する（ステップＳ１５０４）。
【０１０３】
同一でない場合（ステップＳ１５０４：Ｎｏ）、このときの車速パルスのＨ／Ｌを評価値Ｐｂにコピーして（ステップＳ１５０５）、タイマを再起動し（ステップＳ１５０６）、ステップＳ１５０３に戻る。一方、ステップＳ１５０４において、同一である場合（ステップＳ１５０４：Ｙｅｓ）、タイマ計測結果Ｔが一定値以上であるか否かを判断する（ステップＳ１５０７）。
【０１０４】
タイマ計測結果Ｔが一定値以上でない場合（ステップＳ１５０７：Ｎｏ）、ステップＳ１５０３に戻る。一方、タイマ計測結果Ｔが一定値以上である場合（ステップＳ１５０７：Ｙｅｓ）、ジャイロセンサ２０６からの出力電圧Ｅ（ｔ）を入力して（ステップＳ１５０８）、出力電圧Ｅ（ｔ）を時系列にバッファに蓄積する（ステップＳ１５０９）。そして、ブレーキ信号を入力し（ステップＳ１５１０）、ブレーキ信号がＯＦＦであるか否かを判断する（ステップＳ１５１１）。
【０１０５】
ＯＦＦでない場合（ステップＳ１５１１：Ｎｏ）、ステップＳ１５０８に戻る。一方、ＯＦＦである場合（ステップＳ１５１１：Ｙｅｓ）、バッファに蓄積された出力電圧Ｅ（ｔ）とタイマ計測結果Ｔから出力電圧Ｅ（ｔ）の平均値Ｅａｖを算出する（ステップＳ１５１２）。そして、算出された平均値Ｅａｖをゼロ点電圧Ｅｂとする（ステップＳ１５１３）。
【０１０６】
このあと、ゼロ点電圧Ｅｂの補正値ΔＥｂ（＝Ｅｂ−Ｅｂ＿ｏｌｄ）を算出する（ステップＳ１５１４）。そして、次回に用いるため、今回算出されたゼロ点電圧Ｅｂをゼロ点電圧Ｅｂ＿ｏｌｄとして保存する（ステップＳ１５１５）。そして、ステップＳ６０３に移行する。
【０１０７】
（相対位置算出処理手順）
図１６は、相対位置算出処理（ステップＳ６０２）の詳細な処理手順を示すフローチャートである。まず、車両移動距離算出処理を実行する（ステップＳ１６０１）。この処理は、図１１のフローチャートで示した処理と同一処理であるため、説明を省略する。つぎに、車両移動方位算出処理を実行する（ステップＳ１６０２）。この処理は、図１２および図１３のフローチャートで示した処理と同一処理であるため、説明を省略する。
【０１０８】
そして、これらから得られた車速パルス１パルスあたりの車両移動距離ｒおよび移動回転角θをバッファに蓄積する（ステップＳ１６０３）。つぎに、位置情報が取得されたか否かを判断する（ステップＳ１６０４）。位置情報が取得されていない場合（ステップＳ１６０４：Ｎｏ）、ステップＳ６０４に移行する。一方、位置情報が取得された場合（ステップＳ１６０４：Ｙｅｓ）、自車移動履歴Ｗ１に車両移動距離ｒおよび移動回転角θが記録されているか否かを判断する（ステップＳ１６０５）。
【０１０９】
自車移動履歴Ｗ１とは、現在通過した地点とその直前に通過した地点との間において蓄積された車両移動距離ｒおよび移動回転角θを移動履歴情報として記録するバッファである。たとえば、現在位置が地点ＰＢである場合、移動軌跡Ｖａｂの移動履歴情報（車両移動距離ｒおよび移動回転角θ）を記録し、現在位置が地点ＰＣである場合、移動軌跡Ｖｂｃの移動履歴情報（車両移動距離ｒおよび移動回転角θ）を記録する。
【０１１０】
また、自車移動履歴Ｗ２は、さらにその前の区間の移動軌跡の移動履歴情報（車両移動距離ｒおよび移動回転角θ）を記録する。たとえば、現在位置が地点ＰＣである場合、自車移動履歴Ｗ１を空けるために、移動履歴Ｖａｂの移動履歴情報（車両移動距離ｒおよび移動回転角θ）を自車移動履歴Ｗ１から自車移動履歴Ｗ２にコピーする。これにより、自車移動履歴Ｗ１に、移動履歴Ｖｂｃの移動履歴情報を記録することができる。
【０１１１】
そして、ステップＳ１６０５において、自車移動履歴Ｗ１に移動履歴情報が記録されていない場合（ステップＳ１６０５：Ｎｏ）、ステップＳ１６０７に移行する。一方、記録されている場合（ステップＳ１６０５：Ｙｅｓ）、自車移動履歴Ｗ１に記録されている移動履歴情報を自車移動履歴Ｗ２に上書きする（ステップＳ１６０６）。そして、ステップＳ１６０３でバッファに蓄積された車両移動距離ｒおよび移動回転角θを移動履歴情報として自車移動履歴Ｗ１に記録する（ステップＳ１６０７）。そして、バッファをクリアして（ステップＳ１６０８）、初期方位算出処理（ステップＳ６０３）に移行する。
【０１１２】
（初期方位算出処理手順）
図１７および図１８は、初期方位算出処理（ステップＳ６０３）の詳細な処理手順を示すフローチャートである。まず、履歴Ｖ１＿ｏｒ，Ｖ２＿ｏｒに位置座標が記録されているか否かを判断する（ステップＳ１７０１）。記録されていない場合（ステップＳ１７０１：Ｎｏ）、ステップＳ６０４に移行する。
【０１１３】
一方、記録されている場合（ステップＳ１７０１：Ｙｅｓ）、移動軌跡Ｖａｂの始点座標として履歴Ｖ２＿ｏｒの始点座標を抽出し、移動軌跡Ｖｂｃの始点座標として履歴Ｖ１＿ｏｒの始点座標を抽出し、移動軌跡Ｖｂｃの終点座標として履歴Ｖ１＿ｏｒの終点座標を抽出する（ステップＳ１７０２）。移動軌跡Ｖａｂの始点座標が頂点Ｐａの座標となり、移動軌跡Ｖｂｃの始点座標が頂点Ｐｂの座標となり、移動軌跡Ｖｂｃの終点座標が頂点Ｐｃの座標となる。そして、これらを頂点とする三角形△ａｂｃの重心ｇを算出する（ステップＳ１７０３）。
【０１１４】
また、ビーコンＰＡ〜ＰＣからダウンリンクされたそれぞれのビーコンＰＡ〜ＰＣの位置座標を抽出し（ステップＳ１７０４）、それぞれを頂点とする三角形△ＡＢＣの重心Ｇを算出する（ステップＳ１７０５）。このあと、重心ｇが重心Ｇと一致するように、三角形ａｂｃを平行移動させる座標変換をおこなう（ステップＳ１７０６）。
【０１１５】
つぎに、回転量φの初期値としてφ＝０、回転評価値Ｌｓｔ’の下限値Ｌｓｔ’＿ｍｉｎをＬｓｔ’＿ｍｉｎ＝∞とする（ステップＳ１７０７）。そして、重心ｇを中心として三角形△ａｂｃを回転量φだけ回転させる座標変換をおこなう（ステップＳ１７０８）。この回転後の頂点Ｐａ，ＰＡ間距離、頂点Ｐｂ，ＰＢ間距離、頂点Ｐｃ，ＰＣ間距離を算出し、その和を算出し、回転評価値Ｌｓｔ’とする（ステップＳ１７０９）。そして、図１８のステップＳ１８０１に移行する。
【０１１６】
図１８において、回転評価値Ｌｓｔ’が下限値Ｌｓｔ’＿ｍｉｎに対して、Ｌｓｔ’＜Ｌｓｔ’＿ｍｉｎであるか否かを判断する（ステップＳ１８０１）。Ｌｓｔ’＜Ｌｓｔ’＿ｍｉｎでない場合（ステップＳ１８０１：Ｎｏ）、ステップＳ１８０４に移行する。一方、Ｌｓｔ’＜Ｌｓｔ’＿ｍｉｎである場合（ステップＳ１８０１：Ｙｅｓ）、Ｌｓｔ’＿ｍｉｎをＬｓｔ’に更新することで、下限値Ｌｓｔ’＿ｍｉｎを引き下げる（ステップＳ１８０２）。そして、このときの回転量φをバッファに上書き保存し（ステップＳ１８０３）、ステップＳ１８０４に移行する。
【０１１７】
ステップＳ１８０４において、回転量φがφ＞３６０°となったか否かを判断する（ステップＳ１８０４）。すなわち、三角形△ａｂｃが１回転したか否かを判断する。φ＞３６０°でない場合（ステップＳ１８０４：Ｎｏ）、回転量φを所定量インクリメントして（ステップＳ１８０５）、ステップＳ１７０８に戻る。
【０１１８】
一方、φ＞３６０°となった場合（ステップＳ１８０４：Ｙｅｓ）、移動軌跡Ｖｂｃの最終ベクトルｖend（図４の（Ｄ）を参照）の成分（ｒ，θend）から、θendを相対進行方位として抽出する（ステップＳ１８０６）。そして、最終的に保存された回転量φと相対進行方位θendとを加算することで初期方位Ｄを算出する（ステップＳ１８０７）。そして、絶対位置算出処理（ステップＳ６０４）に移行する。
【０１１９】
（絶対位置算出処理）
図１９は、絶対位置算出処理（ステップＳ６０４）の詳細な処理手順を示すフローチャートである。まず、車両方位Θ（＝Ｄ＋θ）を算出する（ステップＳ１９０１）。Ｄは初期方位であり、θは相対位置算出処理で得られた移動回転角θである。つぎに、初期方位Ｄを考慮した車両移動ベクトルを算出する（ステップＳ１９０２）。
【０１２０】
すなわち、このベクトルは、移動距離をｒ、移動回転角をΘとし、移動軌跡Ｖｂｃの最終ベクトルｖendのつぎに出現するベクトルである。これにより、地点Ｐｃからの絶対的な進行方位が特定される。このベクトルを式（２）と同様の手法により位置座標に変換し（ステップＳ１９０３）、その位置座標を緯度ＬＡＴおよび経度ＬＯＮに変換することで（ステップＳ１９０４）、車両１０１の絶対位置（ＬＡＴ，ＬＯＮ）および絶対方位Θを得ることができる。
【０１２１】
このように、上述した実施の形態では、高精度かつ低負荷の自律測位を実現することができる。また、上述した実施の形態では、移動距離ｒ、感度ｖｓおよびゼロ点電圧Ｅｂの３つの補正値を算出することとしているが、少なくともいずれか１つの補正値を算出することとしてもよい。
【０１２２】
例えば、図１における移動軌跡Ｖａｂと、地点ＰＡ、ＰＢの２地点の位置座標のみを用いて感度ｖｓの補正値を本実施の形態により算出し、移動距離ｒの補正値を他の方法（たとえば、特開２００６−３４３２１９など）を用いて算出することでも実現可能である。具体的には、図８を図２０、図９を図２１、図１０を図２２に置き換え、初期方位算出部３３３を他の方法（地磁気センサなど）を用いることでも実現可能である。また、いずれの補正値も算出せず、初期方位算出処理を実行してもよい。なお、以下に説明する図２０〜図２２において、図８〜図１０に示した処理と同一処理については同一ステップ番号を付し、その説明を省略する。
【０１２３】
図２０は、感度ｖｓのみを算出する場合の移動距離補正値・感度補正値算出処理（ステップＳ７０１）の詳細な処理手順を示すフローチャートである。まず、位置情報が取得されていない場合（ステップＳ８０１：Ｎｏ）、車速パルスのＨ／Ｌおよびジャイロセンサ２０６からの出力電圧値Ｅ（ｔ）を履歴Ｖ１＿ｏｒに記録する（ステップＳ８０２）。そして、移動距離補正値ΔｒをΔｒ＝０、感度補正値ΔｖｓをΔｖｓ＝０とする（ステップＳ８０３）。このあと、ステップＳ６０３に移行する。
【０１２４】
一方、ステップＳ８０１において、位置情報が取得された場合（ステップＳ８０１：Ｙｅｓ）、すなわち、車両１０１が光ビーコンの直下を通過した場合、図１に示した地点ＰＣの直下を通過したことになるため、補正値演算処理を実行する（ステップＳ８０５）。すなわち、図８と違い、ステップＳ８０４の処理は実行しない。そして、履歴Ｖ１＿ｏｒの記録内容を履歴Ｖ２＿ｏｒに上書きし、履歴Ｖ１＿ｏｒを空にする（ステップＳ８０６）。このあと、ステップＳ６０３に移行する。
【０１２５】
図２１および図２２は、感度ｖｓのみを算出する場合の補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャートである。まず、図２１において、車速パルス１パルスあたりの感度ｖｓをｖｓ＝ｖｓ＿ｍｉｎとし、上記式（３）で示した評価値Ｌｓｔijを下限値Ｌｓｔij＿ｍｉｎをＬｓｔ＿ｍｉｎ＝∞とする（ステップＳ２１０１）。ｖｓ＿ｍｉｎは感度ｖｓの下限値である。なお、車速パルス１パルスあたりの移動距離ｒの設定は他の方法により得られているため不要である。これ以降、ステップＳ９０２〜ステップＳ９１０を実行して、図２２のステップＳ２２０１に移行する。
【０１２６】
まず、図１０において、距離差バッファＬ２に、始点終点間距離Ｌｖから２点間距離Ｌｂを引いた値（Ｌｖ−Ｌｂ）を評価値Ｌｓｔijとして記録し（ステップＳ２２０１）、評価値ＬｓｔijがＬｓｔij＜Ｌｓｔ＿ｍｉｎであるか否かを判断する（ステップＳ１００７）。
【０１２７】
Ｌｓｔij＜Ｌｓｔ＿ｍｉｎでない場合（ステップＳ１００７：Ｎｏ）、ステップＳ１０１０に移行する。一方、Ｌｓｔij＜Ｌｓｔ＿ｍｉｎである場合（ステップＳ１００７：Ｙｅｓ）、Ｌｓｔ＿ｍｉｎをＬｓｔijに更新する（ステップＳ１００８）。そして、このときの感度ｖｓを保存する（ステップＳ２２０２）。このあと、ｖｓ＞ｖｓ＿ｍａｘであるか否かを判断する（ステップＳ１０１０）。ｖｓ＿ｍａｘは感度ｖｓの上限値である。
【０１２８】
ｖｓ＞ｖｓ＿ｍａｘでない場合（ステップＳ１０１０：Ｎｏ）、感度ｖｓを所定量加算して、試行回数ｊをインクリメントする（ステップＳ１０１１）。そして、図２１のステップＳ９０２にもどる。
【０１２９】
一方、ｖｓ＞ｖｓ＿ｍａｘである場合（ステップＳ１０１０：Ｙｅｓ）、補正済みの移動距離ｒを入力し（ステップＳ２２０３）、前回入力された移動距離ｒ＿ｏｌｄと今回入力された移動距離ｒとを用いて、移動距離補正値Δｒ（＝ｒ−ｒ＿ｏｌｄ）を算出する。同様に、前回検出された車速パルスにおける感度ｖｓ＿ｏｌｄと今回の感度ｖｓとを用いて、感度補正値Δｖｓ（＝ｖｓ−ｖｓ＿ｏｌｄ）を算出する（ステップＳ１０１４）。そして、今回の移動距離ｒおよび感度ｖｓをつぎの車速パルス検出時に用いるため、それぞれ移動距離ｒ＿ｏｌｄ、感度ｖｓ＿ｏｌｄとして保存する（ステップＳ１０１５）。そして、ステップＳ８０６に移行する。
【０１３０】
このように、感度ｖｓの補正値Δｖｓのみを算出することにより、ジャイロセンサ２０６の感度が高くなり、移動回転角θの検出精度が向上することとなる。
【０１３１】
なお、本実施の形態で説明した自律測位方法は、予め用意されたプログラムをパーソナル・コンピュータやワークステーション等のコンピュータで実行することにより実現することができる。このプログラムは、ハードディスク、フレキシブルディスク、ＣＤ−ＲＯＭ、ＭＯ、ＤＶＤ等のコンピュータで読み取り可能な記録媒体に記録され、コンピュータによって記録媒体から読み出されることによって実行される。またこのプログラムは、インターネット等のネットワークを介して配布することが可能としてもよい。
【０１３２】
上述した実施の形態に関し、さらに以下の付記を開示する。
【０１３３】
（付記１）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が通過する複数の地点の位置情報を取得する取得手段、
前記複数の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出する補正値算出手段、
前記補正値算出手段によって算出された補正値を前記移動距離に加算することにより、前記複数の地点のうち最後に通過した地点からの移動距離を算出する補正処理手段、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの移動距離を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【０１３４】
（付記２）前記補正値算出手段は、
前記判断手段により前記出力電圧の変動が所定電圧未満であると判断された場合、前記移動距離の補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの移動距離を出力することを特徴とする付記１に記載の自律測位プログラム。
【０１３５】
（付記３）前記判断手段は、
前記ジャイロセンサの温度を検出する温度センサからの前記複数の地点間における検出温度の変動が所定温度以下であるか否かを判断し、
前記補正値算出手段は、
前記判断手段によって判断された判断結果に基づいて、前記移動距離の補正値を算出することを特徴とする付記１または２に記載の自律測位プログラム。
【０１３６】
（付記４）前記補正値算出手段は、
前記判断手段により前記検出温度の変動が所定温度より高いと判断された場合、前記移動距離の補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの移動距離を出力することを特徴とする付記３に記載の自律測位プログラム。
【０１３７】
（付記５）前記補正値算出手段は、
前記判断結果に基づいて、前記複数の地点の位置情報と、前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出し、
前記補正処理手段は、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出し、
前記出力手段は、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする付記１に記載の自律測位プログラム。
【０１３８】
（付記６）速度センサからの車速パルスごとに検出される移動体の移動距離とジャイロセンサからの出力電圧に応じた前記車速パルスごとに算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が連続通過する複数の地点の位置情報を取得する取得手段、
前記複数の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出する補正値算出手段、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出する補正処理手段、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【０１３９】
（付記７）前記補正値算出手段は、
前記判断結果に基づいて、前記移動体の挙動により前記出力電圧の比較対象となるゼロ点電圧を算出するとともに、前回算出されたゼロ点電圧との差をとることにより、前記ゼロ点電圧の補正値を算出し、
前記補正処理手段は、
前記ゼロ点電圧の補正値により補正された補正後のゼロ点電圧と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出し、
前記出力手段は、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする付記５または６に記載の自律測位プログラム。
【０１４０】
（付記８）前記補正値算出手段は、
前記判断手段により前記出力電圧の変動が所定電圧未満であると判断された場合、前記補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする付記５〜７のいずれか一つに記載の自律測位プログラム。
【０１４１】
（付記９）前記判断手段は、
前記ジャイロセンサの温度を検出する温度センサからの前記複数の地点間における検出温度の変動が所定温度以下であるか否かを判断し、
前記補正値算出手段は、
前記判断手段によって判断された判断結果に基づいて、前記補正値を算出することを特徴とする付記５〜８のいずれか一つに記載の自律測位プログラム。
【０１４２】
（付記１０）前記補正値算出手段は、
前記判断手段により前記検出温度の変動が所定温度より高いと判断された場合、前記補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする付記９に記載の自律測位プログラム。
【０１４３】
（付記１１）前記コンピュータを、
前記複数の地点は任意の第１〜第３の地点であって、該第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理により算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理と、を実行し、当該回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とにより、前記移動体が前記第３の地点に到達したときの初期方位を算出する初期方位算出手段、
前記初期方位算出手段によって算出された初期方位と前記第３の地点からの移動距離および回転角とにより、前記移動体の絶対位置および絶対方位を算出する絶対位置・絶対方位算出手段、として機能させ、
前記出力手段は、
前記絶対位置・絶対方位算出手段によって算出された前記移動体の絶対位置および絶対方位を出力することを特徴とする付記１に記載の自律測位プログラム。
【０１４４】
（付記１２）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が第１〜第３の地点を通過する際に位置情報を取得する取得手段、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
判断された判断結果に基づいて、前記第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理によって算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理とを、前記判断手段によって判断された判断結果に基づいて実行し、前記回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とに基づいて、前記移動体が前記第３の地点に到達したときの初期方位を算出する初期方位算出手段、
前記初期方位算出手段によって算出された初期方位と前記第３の地点からの移動距離および回転角とに基づいて、前記移動体の絶対位置および絶対方位を算出する絶対位置・絶対方位算出手段、
前記絶対位置・絶対方位算出手段によって算出された前記移動体の絶対位置および絶対方位を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【０１４５】
（付記１３）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行する自律測位装置であって、
前記移動体が第１〜第３の地点を通過する際に位置情報を取得する取得手段と、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段と、
前記判断手段によって判断された判断結果に基づいて、前記第１〜第３の地点の位置情報と、前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出する補正値算出手段と、
前記補正値算出手段によって算出された補正値を前記移動距離に加算することにより、前記第３の地点からの移動距離を算出する補正処理手段と、
前記補正処理手段によって得られた前記第３の地点からの移動距離を出力する出力手段と、
を備えることを特徴とする自律測位装置。
【０１４６】
（付記１４）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行する自律測位装置であって、
前記移動体が通過する複数の地点の位置情報を取得する取得手段と、
前記複数の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段と、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出する補正値算出手段と、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出する補正処理手段と、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力する出力手段と、
を備えることを特徴とする自律測位装置。
【０１４７】
（付記１５）速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行する自律測位装置であって、
前記移動体が通過する第１〜第３の地点の位置情報を取得する取得手段と、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段と、
判断された判断結果に基づいて、前記第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理によって算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理とを、前記判断手段によって判断された判断結果に基づいて実行し、前記回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とに基づいて、前記移動体が前記第３の地点に到達したときの初期方位を算出する初期方位算出手段と、
前記初期方位算出手段によって算出された初期方位と前記第３の地点からの移動距離および回転角とに基づいて、前記移動体の絶対位置および絶対方位を算出する絶対位置・絶対方位算出手段と、
前記絶対位置・絶対方位算出手段によって算出された前記移動体の絶対位置および絶対方位を出力する出力手段と、
を備えることを特徴とする自律測位装置。
【図面の簡単な説明】
【０１４８】
【図１】車両の移動軌跡を示す説明図である。
【図２】自律測位装置のハードウェア構成を示すブロック図である。
【図３】自律測位装置の機能的構成を示すブロック図である。
【図４】初期方位の導出方法を示す説明図である。
【図５】本実施の形態にかかる自律測位装置の自律測位処理手順を示すフローチャートである。
【図６】図５に示した補正処理（ステップＳ５０６）の詳細な処理手順を示すフローチャートである。
【図７】補正値算出処理（ステップＳ６０１）の詳細な処理手順を示すフローチャートである。
【図８】移動距離補正値・感度補正値算出処理（ステップＳ７０１）の詳細な処理手順を示すフローチャートである。
【図９】補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャート（前半）である。
【図１０】補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャート（後半）である。
【図１１】車両移動距離算出処理（ステップＳ９０３）の詳細な処理手順を示すフローチャートである。
【図１２】車両移動方位算出処理（ステップＳ９０４）の詳細な処理手順を示すフローチャート（前半）である。
【図１３】車両移動方位算出処理（ステップＳ９０４）の詳細な処理手順を示すフローチャート（後半）である。
【図１４】ゼロ点電圧補正の動作概要を示す波形図である。
【図１５】ゼロ点電圧補正値算出処理（ステップＳ７０２）の詳細な処理手順を示すフローチャートである。
【図１６】相対位置算出処理（ステップＳ６０２）の詳細な処理手順を示すフローチャートである。
【図１７】初期方位算出処理（ステップＳ６０３）の詳細な処理手順を示すフローチャート（前半）である。
【図１８】初期方位算出処理（ステップＳ６０３）の詳細な処理手順を示すフローチャート（後半）である。
【図１９】絶対位置算出処理（ステップＳ６０４）の詳細な処理手順を示すフローチャートである。
【図２０】感度ｖｓのみを算出する場合の移動距離補正値・感度補正値算出処理（ステップＳ７０１）の詳細な処理手順を示すフローチャートである。
【図２１】感度ｖｓのみを算出する場合の補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャート（前半）である。
【図２２】感度ｖｓのみを算出する場合の補正値演算処理（ステップＳ８０５）の詳細な処理手順を示すフローチャート（後半）である。
【符号の説明】
【０１４９】
１０１ 車両
２００ 自律測位装置
３０１ 取得部
３０２ 判断部
３０３ 補正処理部
３０４ 出力部
３３１ 補正値算出部
３３２ 相対位置算出部
３３３ 初期方位算出部
３３４ 絶対位置算出部
【特許請求の範囲】
【請求項１】
速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が通過する複数の地点の位置情報を取得する取得手段、
前記複数の地点間における前記出力電圧の変動が所定電圧以上であるか否かを判断する判断手段、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出する補正値算出手段、
前記補正値算出手段によって算出された補正値を前記移動距離に加算することにより、前記複数の地点のうち最後に通過した地点からの移動距離を算出する補正処理手段、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの移動距離を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【請求項２】
前記補正値算出手段は、
前記判断手段により前記出力電圧の変動が所定電圧未満であると判断された場合、前記移動距離の補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの移動距離を出力することを特徴とする請求項１に記載の自律測位プログラム。
【請求項３】
前記判断手段は、
前記ジャイロセンサの温度を検出する温度センサからの前記複数の地点間における検出温度の変動が所定温度以下以上であるか否かを判断し、
前記補正値算出手段は、
前記判断手段によって判断された判断結果に基づいて、前記移動距離の補正値を算出することを特徴とする請求項１または２に記載の自律測位プログラム。
【請求項４】
前記補正値算出手段は、
前記判断手段により前記検出温度の変動が所定温度より高いと判断された場合、前記移動距離の補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの移動距離を出力することを特徴とする請求項３に記載の自律測位プログラム。
【請求項５】
前記補正値算出手段は、
前記判断結果に基づいて、前記複数の地点の位置情報と、前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出し、
前記補正処理手段は、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出し、
前記出力手段は、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする請求項１に記載の自律測位プログラム。
【請求項６】
速度センサからの車速パルスごとに検出される移動体の移動距離とジャイロセンサからの出力電圧に応じた前記車速パルスごとに算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が連続通過する複数の地点の位置情報を取得する取得手段、
前記複数の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出する補正値算出手段、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出する補正処理手段、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【請求項７】
速度センサからの車速パルスごとに検出される移動体の移動距離とジャイロセンサからの出力電圧に応じた前記車速パルスごとに算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が第１〜第３の地点を通過する際に位置情報を取得する手段、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
前記判断された判断結果に基づいて、前記第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理によって算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理とを、前記判断手段によって判断された判断結果に基づいて実行し、前記回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とに基づいて、前記移動体が前記第３の地点に到達したときの初期方位を算出する初期方位算出手段、
前記初期方位算出手段によって算出された初期方位と前記第３の地点からの移動距離および回転角とに基づいて、前記移動体の絶対位置および絶対方位を算出する絶対位置・絶対方位算出手段、
前記絶対位置・絶対方位算出手段によって算出された前記移動体の絶対位置および絶対方位を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【請求項８】
速度センサからの車速パルスごとに検出される移動体の移動距離とジャイロセンサからの出力電圧に応じた前記車速パルスごとに算出される回転角とにより前記移動体の自律測位を実行する自律測位装置であって、
前記移動体が第１〜第３の地点を通過する際に位置情報を取得する手段と、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段と、
前記判断手段によって判断された判断結果に基づいて、前記第１〜第３の地点の位置情報と、前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出する補正値算出手段と、
前記補正値算出手段によって算出された補正値を前記移動距離に加算することにより、前記第３の地点からの移動距離を算出する補正処理手段と、
前記補正処理手段によって得られた前記第３の地点からの移動距離を出力する出力手段と、
を備えることを特徴とする自律測位装置。
【請求項１】
速度センサからの車速パルスに基づいて算出される移動体の移動距離とジャイロセンサからの出力電圧に応じて算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が通過する複数の地点の位置情報を取得する取得手段、
前記複数の地点間における前記出力電圧の変動が所定電圧以上であるか否かを判断する判断手段、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出する補正値算出手段、
前記補正値算出手段によって算出された補正値を前記移動距離に加算することにより、前記複数の地点のうち最後に通過した地点からの移動距離を算出する補正処理手段、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの移動距離を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【請求項２】
前記補正値算出手段は、
前記判断手段により前記出力電圧の変動が所定電圧未満であると判断された場合、前記移動距離の補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの移動距離を出力することを特徴とする請求項１に記載の自律測位プログラム。
【請求項３】
前記判断手段は、
前記ジャイロセンサの温度を検出する温度センサからの前記複数の地点間における検出温度の変動が所定温度以下以上であるか否かを判断し、
前記補正値算出手段は、
前記判断手段によって判断された判断結果に基づいて、前記移動距離の補正値を算出することを特徴とする請求項１または２に記載の自律測位プログラム。
【請求項４】
前記補正値算出手段は、
前記判断手段により前記検出温度の変動が所定温度より高いと判断された場合、前記移動距離の補正値を算出せず、
前記出力手段は、
前記補正値による補正がされていない前記複数の地点のうち最後に通過した地点からの移動距離を出力することを特徴とする請求項３に記載の自律測位プログラム。
【請求項５】
前記補正値算出手段は、
前記判断結果に基づいて、前記複数の地点の位置情報と、前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出し、
前記補正処理手段は、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出し、
前記出力手段は、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力することを特徴とする請求項１に記載の自律測位プログラム。
【請求項６】
速度センサからの車速パルスごとに検出される移動体の移動距離とジャイロセンサからの出力電圧に応じた前記車速パルスごとに算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が連続通過する複数の地点の位置情報を取得する取得手段、
前記複数の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
前記判断手段によって判断された判断結果に基づいて、前記複数の地点の位置情報と、前記移動体が前記複数の地点を通過したときの前記複数の地点の前記自律測位による位置情報とにより、前記ジャイロセンサから得られる前記回転角に関する感度の補正値を算出する補正値算出手段、
前記補正値算出手段によって算出された前記感度の補正値を前記感度に加算し、当該加算後の感度と前記出力電圧とに基づいて、前記複数の地点のうち最後に通過した地点からの回転角を算出する補正処理手段、
前記補正処理手段によって得られた前記複数の地点のうち最後に通過した地点からの回転角を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【請求項７】
速度センサからの車速パルスごとに検出される移動体の移動距離とジャイロセンサからの出力電圧に応じた前記車速パルスごとに算出される回転角とにより前記移動体の自律測位を実行するコンピュータを、
前記移動体が第１〜第３の地点を通過する際に位置情報を取得する手段、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段、
前記判断された判断結果に基づいて、前記第１〜第３の地点の位置情報に基づく第１の三角形の重心と前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報に基づく第２の三角形の重心とを算出する重心算出処理と、当該重心算出処理によって算出された重心どうしが一致するように、前記第２の三角形を前記第１の三角形に対して平行移動させる座標変換を実行する座標変換処理と、当該座標変換後において前記第２の三角形をその重心を回転中心として回転させたときの前記第１の地点どうしの距離、前記第２の地点どうしの距離、および前記第３の地点どうしの距離の和に基づいて、当該和が最小となる回転量を抽出する回転処理とを、前記判断手段によって判断された判断結果に基づいて実行し、前記回転処理によって抽出された回転量と前記移動体が前記第３の地点に到達したときの回転角とに基づいて、前記移動体が前記第３の地点に到達したときの初期方位を算出する初期方位算出手段、
前記初期方位算出手段によって算出された初期方位と前記第３の地点からの移動距離および回転角とに基づいて、前記移動体の絶対位置および絶対方位を算出する絶対位置・絶対方位算出手段、
前記絶対位置・絶対方位算出手段によって算出された前記移動体の絶対位置および絶対方位を出力する出力手段、
として機能させることを特徴とする自律測位プログラム。
【請求項８】
速度センサからの車速パルスごとに検出される移動体の移動距離とジャイロセンサからの出力電圧に応じた前記車速パルスごとに算出される回転角とにより前記移動体の自律測位を実行する自律測位装置であって、
前記移動体が第１〜第３の地点を通過する際に位置情報を取得する手段と、
前記第１〜第３の地点間における前記出力電圧の変動が所定電圧未満であるか否かを判断する判断手段と、
前記判断手段によって判断された判断結果に基づいて、前記第１〜第３の地点の位置情報と、前記移動体が前記第１〜第３の地点を通過したときの前記第１〜第３の地点の前記自律測位による位置情報とにより、前記移動距離の補正値を算出する補正値算出手段と、
前記補正値算出手段によって算出された補正値を前記移動距離に加算することにより、前記第３の地点からの移動距離を算出する補正処理手段と、
前記補正処理手段によって得られた前記第３の地点からの移動距離を出力する出力手段と、
を備えることを特徴とする自律測位装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図１４】
【図１５】
【図１６】
【図１７】
【図１８】
【図１９】
【図２０】
【図２１】
【図２２】
【公開番号】特開２０１０−１２７９１９（Ｐ２０１０−１２７９１９Ａ）
【公開日】平成２２年６月１０日（２０１０．６．１０）
【国際特許分類】
【出願番号】特願２００８−３０６７８５（Ｐ２００８−３０６７８５）
【出願日】平成２０年１２月１日（２００８．１２．１）
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
【公開日】平成２２年６月１０日（２０１０．６．１０）
【国際特許分類】
【出願日】平成２０年１２月１日（２００８．１２．１）
【出願人】（０００００５２２３）富士通株式会社 (25,993)
【Ｆターム（参考）】
[ Back to top ]