(57)【特許請求の範囲】
【請求項１】
車両に固定された複数のセンサを用いて該車両の運動を検出することで上記車両の現在位置を計測するようにした航法装置において、
上記車両の進行方向に対する横方向の加速度を検出する第１の加速度センサと、
上記車両の進行方向の加速度を検出する第２の加速度センサと、
上記車両の進行方向に対する水平面内の方向の角速度を検出する角速度センサと、
上記第１の加速度センサによる検出結果と上記角速度センサによる検出結果に基づき上記第２の加速度センサの零点補正値を計算する零点補正値計算手段と、
上記第２の加速度センサの零点補正値を求める際に、上記零点補正値計算手段による零点補正値を用いるか、上記第２の加速度センサによる検出結果から求められる零点補正値を用いるかを選択する選択手段と、
上記第１の加速度センサによる検出結果と上記角速度センサによる検出結果とに基づき上記車両の進行方向の速度を計算する速度計算手段と
を有し、
上記選択手段による選択は、上記第１の加速度センサによる加速度および上記角速度センサによる角速度が所定値を越えている場合に行う
ことを特徴とする航法装置。
【請求項２】
車両に固定された複数のセンサを用いて該車両の運動を検出することで上記車両の現在位置を計測するようにした航法装置において、
上記車両の進行方向に対する横方向の加速度を検出する第１の加速度センサと、
上記車両の進行方向の加速度を検出する第２の加速度センサと、
上記車両の進行方向に対する水平面内の方向の角速度を検出する角速度センサと、
上記第１の加速度センサによる検出結果と上記角速度センサによる検出結果に基づき上記第２の加速度センサの零点補正値を計算する零点補正値計算手段と、
上記第１の加速度センサによる検出結果と上記角速度センサによる検出結果とに基づき上記車両の進行方向の速度を計算する速度計算手段と、
上記車両の進行方向の速度を求める際に、上記第２の加速度センサによる検出結果を用いるか、上記第１の加速度センサと上記角速度センサによる検出結果を用いるかを選択し、上記第２の加速度センサの検出結果を用いる場合には上記零点補正値計算手段による零点補正値を採用し、上記第１の加速度センサと上記角速度センサによる検出結果を用いる場合には上記速度計算手段を採用する選択手段と
を有し、
上記選択手段による選択は、上記第１の加速度センサによる加速度および上記角速度センサによる角速度が所定値を越えている場合に行う
ことを特徴とする航法装置。
【請求項３】
車両に固定された複数のセンサを用いて該車両の運動を検出することで上記車両の現在位置を計測するようにした位置計測方法において、
上記車両の進行方向に対する横方向の加速度を第１の加速度センサにより検出するステップと、
上記車両の進行方向の加速度を第２の加速度センサにより検出するステップと、
上記車両の進行方向に対する水平面内の方向の角速度を角速度センサにより検出するステップと、
上記第１の加速度センサによる検出結果と上記角速度センサによる検出結果に基づき上記第２の加速度センサの零点補正値を計算する零点補正値計算のステップと、
上記第２の加速度センサの零点補正値を求める際に、上記零点補正値計算のステップによる零点補正値を用いるか、上記第２の加速度センサによる検出結果から求められる零点補正値を用いるかを選択する選択のステップと、
上記第１の加速度センサによる検出結果と上記角速度センサによる検出結果とに基づき上記車両の進行方向の速度を計算する速度計算ステップと
を有し、
上記選択のステップによる選択は、上記第１の加速度センサによる加速度および上記角速度センサによる角速度が所定値を越えている場合に行う
ことを特徴とする位置計測方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
この発明は、加速度センサと角速度センサを用いて現在位置を推測して自律航法を行う航法装置および位置計測方法に関する。
【０００２】
【従来の技術】
現在、車載され、車両の位置や進行方向などを通知して、運転の補助を行うような航法装置、所謂カーナビゲーション装置が普及している。このカーナビゲーション装置には、ＧＰＳ(Global Positioning System) などの電波による測位システムを用いて利用して車両の絶対位置を求めるものと、各種センサを用いて車両の進行方向や速度などを知り、車両の位置を相対的に求める自律航法によるものとがある。
【０００３】
自律航法では、車両の前後方向の加速度を検知する加速度センサと、左右の旋回（回転）を検知する角速度センサが用いられる。図１０は、従来技術による、自律航法を用いた現在位置の推測のための計算処理の一例を示す。なお、以下の記述では、車両の運動に関する方向を、図１１に示されるように定義する。すなわち、前後，左右，上下に関する運動方向をＸ軸，Ｙ軸，およびＺ軸でそれぞれ表す。また、Ｘ軸に対する回転をロール（ｒｏｌｌ），Ｙ軸に対する回転をピッチ（ｐｉｔｃｈ），およびＺ軸に対する回転をヨー（ｙａｗ）とする。
【０００４】
このような方向定義において、車両に、Ｘ軸方向の加速度を検知するＸ軸加速度センサおよびヨー方向の角速度を検知する角速度センサが設けられる。先ず、ステップＳ１００で得られるＸ軸加速度センサの出力Ｖｘ〔Ｖ：ｖｏｌｔａｇｅ〕に対して、零点出力処理計算（オフセット処理計算）を行い、Ｘ軸方向の加速度αｘ〔ｍ／ｓｓ〕を得る（ステップＳ１０１）。なお、〔〕内の記述は、単位系を表す。そして、αｘ〔ｍ／ｓｓ〕を積分することで、Ｘ軸方向の速度ｖｘ〔ｍ／ｓ〕が求められ（ステップＳ１０２）、このｖｘ〔ｍ／ｓ〕を再度積分することで、Ｘ軸方向の移動距離ｌｘ〔ｍ〕が求められる（ステップＳ１０３）。
【０００５】
一方、ヨー方向においては、ステップＳ１０４で得られる角速度センサの出力Ｖｙａｗ〔Ｖ〕に対して、零点出力処理計算（オフセット処理計算）を行い、ヨー方向の角速度ωｙａｗ〔ｄｅｇ／ｓ〕を得る（ステップＳ１０５）。このωｙａｗ〔ｄｅｇ／ｓ〕を積分することで、ヨー方向の方位θｙａｗ〔ｄｅｇ〕が求められる（ステップＳ１０６）。そして、上述のＸ軸方向の移動距離ｌｘ〔ｍ〕とヨー方向の方位θｙａｗ〔ｄｅｇ〕とから、現在位置を推測することができる（ステップＳ１０７）。すなわち、前回計測された位置に対して、方位θｙａｗ〔ｄｅｇ〕に距離ｌｘ〔ｍ〕だけ移動すれば、最新の位置を求めることができる。
【０００６】
【発明が解決しようとする課題】
ところで、車両が勾配のある坂を移動するような場合には、Ｘ軸方向の、すなわち前後方向の加速度に対して、重力加速度が加わることになる。したがって、Ｘ軸方向の加速度センサの出力値Ｖｘが重力加速度の影響を受けてしまい、結果的に、移動距離ｌｘの計算誤差が非常に大きくなる。そのため、ステップＳ１０７での現在位置の計算結果が真の値から著しく違ったものになってしまうという問題点があった。
【０００７】
また、従来の方法では、上述したように、Ｘ軸方向の加速度センサの出力Ｖｘに対して、ステップＳ１０２およびステップＳ１０３と、２回の積分を行って移動距離ｌｘを求めている。そのため、Ｘ軸の加速度センサの出力Ｖｘに小さな誤差が含まれていても、最終的な移動距離ｌｘとしては、大きな誤差となって計算されてしまうという問題点があった。
【０００８】
これらの問題点を解決するために、この自律航法とＧＰＳによる方法とを併用して用いることも考えられる。すなわち、例えば自律航法で得られた位置情報や速度情報に対して、定期的にＧＰＳにより得られた位置情報などで補正をかける。しかしながら、ＧＰＳなどのような電波を用いたシステムでは、測位衛星から情報を送信するために、例えば約１．５７５ＧＨｚといった周波数の電波が用いられる。したがって、トンネル内や建物の陰、あるいは高架の下などの、測位衛星からの電波が補足できない状況では、測位を行うことができず、有効ではない。そのため、自律航法による誤差の補正に全く効果が得られないという問題点があった。
【０００９】
したがって、この発明の目的は、重力加速度の影響を受けないと共に、少ない積分回数で現在の位置算出を行い、より高精度な現在位置情報が得られる航法装置および位置計測方法を提供することにある。
【００１０】
【課題を解決するための手段】
この発明は、上述した課題を解決するために、車両に固定された複数のセンサを用いて車両の運動を検出することで車両の現在位置を計測するようにした航法装置において、車両の進行方向に対する横方向の加速度を検出する第１の加速度センサと、車両の進行方向の加速度を検出する第２の加速度センサと、車両の進行方向に対する水平面内の方向の角速度を検出する角速度センサと、第１の加速度センサによる検出結果と角速度センサによる検出結果に基づき第２の加速度センサの零点補正値を計算する零点補正値計算手段と、第２の加速度センサの零点補正値を求める際に、零点補正値計算手段による零点補正値を用いるか、第２の加速度センサによる検出結果から求められる零点補正値を用いるかを選択する選択手段と、第１の加速度センサによる検出結果と角速度センサによる検出結果とに基づき車両の進行方向の速度を計算する速度計算手段とを有し、選択手段による選択は、第１の加速度センサによる加速度および角速度センサによる角速度が所定値を越えている場合に行うことを特徴とする航法装置である。
また、この発明は、車両に固定された複数のセンサを用いて車両の運動を検出することで車両の現在位置を計測するようにした航法装置において、車両の進行方向に対する横方向の加速度を検出する第１の加速度センサと、車両の進行方向の加速度を検出する第２の加速度センサと、車両の進行方向に対する水平面内の方向の角速度を検出する角速度センサと、第１の加速度センサによる検出結果と角速度センサによる検出結果に基づき第２の加速度センサの零点補正値を計算する零点補正値計算手段と、第１の加速度センサによる検出結果と角速度センサによる検出結果とに基づき車両の進行方向の速度を計算する速度計算手段と、車両の進行方向の速度を求める際に、第２の加速度センサによる検出結果を用いるか、第１の加速度センサと角速度センサによる検出結果を用いるかを選択し、第２の加速度センサの検出結果を用いる場合には零点補正値計算手段による零点補正値を採用し、第１の加速度センサと角速度センサによる検出結果を用いる場合には速度計算手段を採用する選択手段とを有し、選択手段による選択は、第１の加速度センサによる加速度および角速度センサによる角速度が所定値を越えている場合に行うことを特徴とする航法装置である。
また、この発明は、車両に固定された複数のセンサを用いて車両の運動を検出することで車両の現在位置を計測するようにした位置計測方法において、車両の進行方向に対する横方向の加速度を第１の加速度センサにより検出するステップと、車両の進行方向の加速度を第２の加速度センサにより検出するステップと、車両の進行方向に対する水平面内の方向の角速度を角速度センサにより検出するステップと、第１の加速度センサによる検出結果と角速度センサによる検出結果に基づき第２の加速度センサの零点補正値を計算する零点補正値計算のステップと、第２の加速度センサの零点補正値を求める際に、零点補正値計算のステップによる零点補正値を用いるか、第２の加速度センサによる検出結果から求められる零点補正値を用いるかを選択する選択のステップと、第１の加速度センサによる検出結果と角速度センサによる検出結果とに基づき車両の進行方向の速度を計算する速度計算ステップとを有し、選択のステップによる選択は、第１の加速度センサによる加速度および角速度センサによる角速度が所定値を越えている場合に行うことを特徴とする位置計測方法である。
【００１１】
上述したように、この発明は、車両に固定されたＹ軸方向の加速度センサとヨー（ｙａｗ）方向の角速度センサの検出結果から、Ｘ軸方向の速度を計算するようにされているため、Ｘ軸方向の速度の計算の際に、坂道などでも重力加速度の影響を受けることがない。
【００１２】
【発明の実施の形態】
以下、この発明の実施の第１の形態について説明する。この発明では、車両の横方向（Ｙ軸方向）の加速度を検出するＹ軸加速度センサと、車両のヨー方向、すなわち水平面内での方向変位を検出する角速度センサとを用いて、車両の前後方向（Ｘ軸方向）の速度を求める。Ｙ軸方向の加速度と角速度とによってＸ軸方向の速度を計算するため、重力加速度の影響を排除でき、より正確に速度を求めることができる。こうして求められた速度と、角速度センサによって得られた方向変位とで、車両の位置を精度良く求めることができる。
【００１３】
図１は、この実施の第１の形態に適用できる航法装置の構成の一例を示す。航法装置１は、本体２とセンサ部３とからなり、例えばＬＣＤ(Liquid Crystal Display)および表示制御部などからなる表示部４、スピーカ５が接続される。航法装置１は、例えば赤外線信号を用いたリモートコントロールコマンダ（以下、リモコンと略称する）６によって操作できる。
【００１４】
航法装置本体１は、この例では、２つのＣＰＵ１０，１１を有する。メインＣＰＵ１０は、データやプログラムなどが格納されるメモリ１２が接続され、主にプログラムの実行を行う。サブＣＰＵ１１は、周辺制御装置１３が接続され、メインＣＰＵ１０からの命令により周辺装置の制御を行う。周辺制御装置１３には、周辺装置として、表示部４，Ｄ／Ａコンバータ１４，およびＣＤ−ＲＯＭドライブ１５が接続される。Ｄ／Ａコンバータ１４の出力は、図示されないアンプを介してスピーカ５に供給される。
【００１５】
サブＣＰＵ１１には、センサ部３からのセンサ出力が供給される。センサ部３は、加速度センサ２０と角速度センサ２１とからなる。加速度センサ２０は、進行方向に対して横方向の、Ｙ軸方向の加速度を検知する。また、角速度センサ２１は、水平面内のヨー角速度を検知する。センサ部３は、例えば車両に対して、各センサ２０および２１のそれぞれが所定の方向の運動を検知するように、固定して用いられる。各センサ２０および２１それぞれの検知結果は、電圧値として出力される。例えば、サブＣＰＵ１１は、Ａ／Ｄコンバータを内蔵し、電圧値として供給された検知結果をディジタルデータに変換して取り込む。
【００１６】
なお、なお、航法装置本体２には、例えば赤外線信号を受信するリモコン受光部１６が設けられる。ユーザは、リモコン６によって航法装置１を操作することができる。
【００１７】
このような構成において、航法装置１は、例えば現在位置周辺の地図画像を表示部４に表示させることができる。センサ部３の各センサ２０，２１の検知出力が航法装置本体２のサブＣＰＵ１１に供給される。サブＣＰＵ１１では、供給された検知出力に基づき入力演算を行い、演算結果のデータが周辺制御装置１３を介してメインＣＰＵ１０に供給される。メインＣＰＵ１０では、供給されたこのデータに基づき現在位置を求めるための位置演算が行われる。位置演算の結果得られた現在位置情報は、メモリ１２に格納されると共に、周辺制御装置１３に供給される。
【００１８】
ところで、ＣＤ−ＲＯＭドライブ１５には、所定領域の地図データが格納されたＣＤ−ＲＯＭ１７がセットされている。周辺制御装置１３では、ＣＤ−ＲＯＭドライブ１５に対して、供給された現在位置情報に基づき、ＣＤ−ＲＯＭ１７から現在位置周辺の所定領域の地図データを読み込むように、命令が出される。この命令に基づきＣＤ−ＲＯＭ１７から読み出された地図データは、周辺制御装置１３に送られる。
【００１９】
この地図データは、周辺制御装置１３において表示部４で表示可能なように変換され、表示部４に供給される。また、現在位置情報も、同様にして表示部４に供給される。地図データによって、表示部４に対して現在位置周辺の地図画像が表示され、現在位置情報によって、現在位置周辺の地図に対して現在の位置がマークされる。
【００２０】
また、この航法装置１では、表示部４に対して現在位置周辺の所定領域以外の地図画像を表示させることができる。例えば、表示部４に所定領域が表示されている地図画像をスクロールさせて他の領域を表示させたり、表示されている地図画像の縮尺を変更することができる。
【００２１】
表示部４には、既に、上述の手順によって現在位置周辺の所定領域の地図画像が表示されており、この地図画像をスクロールさせ他の領域を表示させるものとする。例えばユーザのリモコン６の操作により、地図画像のスクロールの命令がリモコン６から送信される。この信号がリモコン受光部１６によって受信され、赤外線信号がデコードされ所定のデータに変換され、サブＣＰＵ１１に供給される。
【００２２】
この受信データは、サブＣＰＵ１１から周辺制御装置１３を介してメインＣＰＵ１０に供給される。この受信データは、メインＣＰＵ１０で解読され、地図画像のスクロールなどの所定の命令とされる。この命令は、周辺制御装置１３を介してサブＣＰＵ１１に供給される。この命令に基づき、サブＣＰＵ１１からＣＤ−ＲＯＭドライブ１５に対して、ＣＤ−ＲＯＭ１７から指定された領域の地図データが読み出される。読み出された地図データは、周辺制御装置１３を介して表示部４に供給される。こうして、リモコン６によって指定された領域の地図画像が表示部４に表示される。地図画像の縮尺の変更も、同様にしてなされる。
【００２３】
次に、この実施の第１の形態による現在位置の算出方法について説明する。図２は、この実施の第１の形態による現在位置の推測のための計算処理の一例を示す。先ず、Ｙ軸方向の加速度を検知する加速度センサ２０と、ヨー方向の角速度を検出する角速度センサ２１からの出力がサブＣＰＵ１１に供給され、サブＣＰＵ１１で入力演算を施され、メインＣＰＵ１０で位置演算が行われる。このとき、最初に、加速度センサ２０および角速度センサ２１の出力データが電圧〔Ｖ〕から、加速度〔ｍ／ｓｓ〕および角速度〔ｄｅｇ／ｓ〕に、それぞれ変換される。
【００２４】
すなわち、先ず、ステップＳ１０で得られるＹ軸加速度センサ２０の出力Ｖｙ〔Ｖ〕に対して零点出力処理計算が行われる。加速度センサ２０からの出力値が電圧Ｖｙ〔Ｖ〕、加速度センサ２０の零点電圧（センサ静止時の電圧）をＶｙ＿ｏｆｆｓｅｔ〔Ｖ〕，およびセンサ２０の感度をＳｙ〔Ｖ／ｍ／ｓｓ〕とすると、Ｙ軸方向の加速度αｙ〔ｍ／ｓｓ〕は、
αｙ＝（Ｖｙ−Ｖｙ＿ｏｆｆｓｅｔ）×（１／Ｓｙ）  ・・（１）
このように求められる（ステップＳ１１）。
【００２５】
一方、ヨー方向においては、ステップＳ１２で得られる角速度センサ２１の出力Ｖｙａｗ〔Ｖ〕に対して、零点出力処理計算が行われる。角速度センサ２１からの出力値が電圧Ｖｙａｗ〔Ｖ〕，角速度センサ２１の零点電圧をＶｙａｗ＿ｏｆｆｓｅｔ〔Ｖ〕，およびセンサ２１の感度をＳｙａｗ〔Ｖ／ｄｅｇ／ｓ〕とすると、角速度ωｙａｗ〔ｄｅｇ／ｓ〕は、
ωｙａｗ＝（Ｖｙａｗ−Ｖｙａｗ＿ｏｆｆｓｅｔ）×（１／Ｓｙａｗ）  ・・（２）
このように求められる（ステップＳ１３）。
【００２６】
ここで、例えば車両が半径ｒで以て回転運動をしている場合を考える。図３は、この場合の、車両３０に加わる各力の関係を概略的に示す。車両が半径ｒの曲線上を、接線方向を進行方向として、速度ｖｘ〔ｍ／ｓ〕で運動しているとする。このとき、車両３０には、遠心力Ｆがはたらく。遠心力Ｆは、良く知られているように、
Ｆ＝（ｍ×ｖｘ＿ｒｏｔ2 ）／ｒ    ・・（３）
このように表される。なお、速度ｖｘ＿ｒｏｔは、速度ｖｘの接線方向の成分である。
【００２７】
一方、これを、Ｙ軸方向の加速度センサ２０で検出される加速度αｙ〔ｍ／ｓｓ〕で表すと、
Ｆ＝ｍ×αｙ    ・・（４）
このようになる。
【００２８】
したがって、上述の式（３）とこの式（４）とから、Ｙ軸方向の加速度αｙは、
αｙ＝ｖｘ＿ｒｏｔ2 ／ｒ    ・・（５）
このように表される。
【００２９】
また、車両３０の回転半径ｒ〔ｍ〕と車両３０の角速度ωｙａｗ〔ｄｅｇ／ｓ〕と、接線方向の速度ｖｘ＿ｒｏｔ〔ｍ／ｓ〕との間には、Ｙ軸方向への滑り（移動）が無い条件の下で、
ｖｘ＿ｒｏｔ＝ｒ×ωｙａｗ×（π／１８０）    ・・（６）
このような関係がある。
【００３０】
上述の式（５）と式（６）から半径ｒを消去し、ｖｘ＿ｒｏｔを求めると、
ｖｘ＿ｒｏｔ＝（αｙ／ωｙａｗ）×（１８０／π）    ・・（７）
このようになる。この式（７）は、Ｙ軸方向の加速度とヨー方向の角速度とから車両の前後方向の速度が求められることを示している。
【００３１】
ステップＳ１１およびステップＳ１３とでそれぞれ得られた、Ｙ軸方向の加速度αｙ〔ｍ／ｓｓ〕およびヨー方向の角速度ωｙａｗ〔ｄｅｇ／ｓ〕とから、上述の式（７）に基づき、ステップＳ１５でＸ軸方向の速度ｖｘ＿ｒｏｔ〔ｍ／ｓ〕が求められる。この速度ｖｘ＿ｒｏｔをステップＳ１６でＸ軸方向の速度ｖｘ〔ｍ／ｓ〕に代入し、ステップＳ１７で速度ｖｘを積分して移動距離ｌｘ〔ｍ〕が求められる。
【００３２】
一方、ステップＳ１３で求められたヨー方向の角速度ωｙａｗがステップＳ１４で積分され、ヨー方向の方位θｙａｗ〔ｄｅｇ〕が求められる。ステップＳ１８では、ステップＳ１７で求められたＸ軸方向の移動距離ｌｘと、ステップＳ１４で求められたヨー方向の方位θｙａｗ〔ｄｅｇ〕とから、現在位置ＰＯＳが推測される。現在位置ＰＯＳは、例えば緯度，経度情報として得られる。
【００３３】
上述したが、一般的に、Ｘ軸方向の加速度センサから得られるＸ軸加速度αｘは、水平な場所では問題無いが、例えば坂道などで車両のＸ軸が水平面からある角度を持った場合には、重力加速度の影響を受けてしまう。このため、坂道での走行時には、Ｘ軸加速度αｘに、重力加速度による誤差が含まれてしまい、Ｘ軸加速度を積分して求めるＸ軸速度ｖｘ，Ｘ軸距離ｌｘには、さらに積分による誤差も加わり、正確な位置演算を行うことが困難になる。
【００３４】
この実施の第１の形態では、Ｙ軸方向の加速度αｙと、ヨー方向の角速度ωｙａｗとからＸ軸速度ｖｘを求めている。これにより、重力加速度の影響を取り除いたＸ軸速度を求めることができる。したがって、Ｘ軸距離の算出値の精度が上がり、結果的に、現在位置の予測を精度良く行うことができる。
【００３５】
次に、この発明の実施の第２の形態について説明する。上述した実施の第１の形態では、式（７）によるＸ軸方向の速度ｖｘ＿ｒｏｔの算出は、車両の進行方向に対して直角方向あるいは法線方向にはたらく力に基づきなされている。そのため、車両が直進のみの移動の場合、Ｘ軸方向の速度を算出することができない。そこで、この第２の形態では、上述の図１に示した構成のセンサ部３において、Ｙ軸方向の加速度センサ２０およびヨー方向の角速度センサ２１に対して、さらに、Ｘ軸方向の加速度センサ２０’（図示しない）を加える。Ｘ軸方向の加速度センサ２０’も、他のセンサと同様、電圧値Ｖｘ〔Ｖ〕として出力が得られる。加速度センサ２０’の出力は、他のセンサ２０，２１と同様に、サブＣＰＵ１１に供給される。
【００３６】
そして、この実施の第２の形態においては、直線運動のみの場合には、Ｘ軸方向の加速度センサ２０’から求められたＸ軸方向の速度ｖｘ〔ｍ／ｓ〕を位置計算に用いる。坂道などによる重力加速度の影響を取り除く場合には、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとを用いる。
【００３７】
図４は、この実施の第２の形態による現在位置の推測のための計算処理の一例を示す。先ず、この実施の第２の形態による航法装置を搭載した例えば車両が、ある加速度で直線運動を行う（前方あるいは後方に移動する）と、Ｘ軸方向の加速度センサ２０’に加速度が検出される。検出された加速度は、電圧Ｖｘ〔Ｖ〕として求められる（ステップＳ２０）。
【００３８】
Ｘ軸方向の加速度センサ２０’からの出力値が電圧Ｖｘ〔Ｖ〕、加速度センサ２０’の零点電圧をＶｘ＿ｏｆｆｓｅｔ〔Ｖ〕，およびセンサ２０’の感度をＳｘ〔Ｖ／ｍ／ｓｓ〕とすると、Ｘ軸方向の加速度αｘ〔ｍ／ｓｓ〕は、
αｘ＝（Ｖｘ−Ｖｘ＿ｏｆｆｓｅｔ）×（１／Ｓｘ）  ・・（８）
このように求められる（ステップＳ２１）。
【００３９】
一方、Ｙ軸方向の加速度センサ２０およびヨー方向の角速度センサ２１では、上述の実施の第１の形態と同様に、式（１）〜式（７）などに基づき、Ｘ軸の速度ｖｘ＿ｒｏｔ〔ｍ／ｓ〕およびヨー方向の方位θｙａｗ〔ｄｅｇ〕とが求められる。
【００４０】
すなわち、加速度センサ２０の出力Ｖｙ〔Ｖ〕から（ステップＳ２２）、式（１）に基づきＹ軸方向の加速度αｙ〔ｍ／ｓｓ〕が求められ（ステップＳ２３）ると共に、角速度センサ２１の出力Ｖｙａｗ〔Ｖ〕から（ステップＳ２５）、式（２）に基づきヨー方向の角速度ωｙａｗ〔ｄｅｇ／ｓ〕が求められる。式（７）に基づき、加速度αｙと角速度ωｙａｗとからＸ軸方向の速度ｖｘ＿ｒｏｔ〔ｍ／ｓ〕が、求められる（ステップＳ２４）。また、角速度ωｙａｗを積分することで、ヨー方向の方位θｙａｗ〔ｄｅｇ〕が求められる（ステップＳ２７）。
【００４１】
ところで、この実施の第２の形態においては、ステップＳ２８でＸ軸方向の速度を求めるために、上述のステップＳ２１で得られたＸ軸加速度αｘ〔ｍ／ｓｓ〕と、ステップＳ２４で得られたＸ軸方向の速度ｖｘ＿ｒｏｔとを、選択して用いる。すなわち、車両が直線運動を行っている場合には、Ｘ軸加速度αｘを用い、回転運動を行っている場合には、速度ｖｘ＿ｒｏｔを用いるように選択する。
【００４２】
図５は、この、選択の処理の一例を示すフローチャートである。先ず、最初のステップＳ４０で、Ｘ軸方向の加速度センサ２０’による加速度αｘが求められ、次に、Ｙ軸方向の加速度センサ２０による加速度αｙが求められる（ステップＳ４１）。さらに、ステップＳ４２で、ヨー方向の角速度センサ２１による角速度ωｙａｗが求められる。なお、ステップＳ４０，Ｓ４１，およびＳ４２は、順序が前後してもよい。
【００４３】
次のステップＳ４３で、この航法装置が搭載されている車両が回転運動をしているかどうかが判断される。この判断は、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとが、所定の値を越えているかどうかでなされる。若し、回転運動をしているとされれば、処理はステップＳ４４に移行し、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとでＸ軸方向の速度ｖｘ＿ｒｏｔが求められ、次のステップＳ４５で、この速度ｖｘ＿ｒｏｔが速度ｖｘに代入される。
【００４４】
一方、ステップＳ４３で、回転運動ではないと判断されたら、Ｘ軸方向の加速度センサ２０’により求められたＸ方向の加速度αｘに基づき、速度ｖｘが求められる。
【００４５】
図４の説明に戻り、こうしてＸ方向の速度ｖｘが求められたら、この速度ｖｘがステップＳ２９で積分されＸ方向の移動距離ｌｘ〔ｍ〕とされ、次のステップ３０で、この距離ｌｘと、上述のステップＳ２７で求められたヨー方向の方位θを用いて、現在位置ＰＯＳが推測される。
【００４６】
このように、この実施の第２の形態を用いることで、回転運動時には重力加速度の影響を取り除いたＸ軸方向の加速度を求めることができると共に、直線運動時のみの場合でも、Ｘ軸方向の加速度センサ２０’によりＸ軸方向の加速度を求めることができる。したがって、Ｘ軸方向の速度および移動距離を、精度良く求めることができる。
【００４７】
次に、この発明の実施の第３の形態について説明する。一般的に、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔは、停止時などでセンサ出力が無いと想定される場合の出力電圧に設定される。このとき、例えば車両が水平な場所で停止して零点電圧を設定し、その後、坂道などを走行した場合に、問題が生じる。
【００４８】
すなわち、坂道では、重力加速度の影響により、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが変化しているにもかかわらず、水平時の零点電圧が使用されているため、検出結果の加速度αｘに大きな誤差が含まれてしまうのである。その結果、Ｘ軸方向の速度ｖｘや移動距離ｌｘに対して、さらに積分による誤差が加わり、最終的な現在位置ＰＯＳが大きくずれたものとなる。勿論、坂道で停車しているときに零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔを設定し、水平な場所を走行する場合でも、同様である。
【００４９】
そこで、この実施の第３の形態では、センサ部３にＸ軸方向の加速度センサ２０’が加えられた、上述の実施の第２の形態と同一の構成が用いられ、例えば坂道による重力加速度の影響を取り除く目的で、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとから、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔを求める。これにより、Ｘ軸方向の加速度αｘを精度良く計算する。
【００５０】
図６は、この実施の第３の形態による現在位置の推測のための計算処理の一例を示す。上述と同様に、ステップＳ５０およびＳ５１で、式（１）により、Ｙ軸方向の加速度センサ２０でＹ軸方向の加速度αｙが求められ、ステップＳ５２およびＳ５３で、式（２）により、ヨー方向の角速度センサ２１でヨー方向の角速度ωｙａｗが求められる。さらに、式（７）により、これら加速度αｙおよび角速度ωｙａｗとから、Ｘ軸方向の速度ｖｘ＿ｒｏｔが求められる（ステップＳ５４）。また、角速度ωｙａｗが積分されヨー方向の方位θｙａｗが求められる（ステップＳ５５）。
【００５１】
この実施の第３の形態では、ステップＳ５４で求められたＸ軸方向の速度ｖｘ＿ｒｏｔを用いて、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔ〔Ｖ〕を求める。すなわち、Ｘ軸方向の加速度をαｘ＿ｒｏｔ〔ｍ／ｓｓ〕とすると、加速度αｘ＿ｒｏｔとＸ軸方向の速度ｖｘ＿ｒｏｔとの関係は、次式（９）で表される。
【００５２】
【数１】
0003975548.tif 000002
【００５３】
また、このときのＸ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔは、
αｘ＿ｒｏｔ＝（Ｖｘ−Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔ）×（１／Ｓｘ）  ・・（１０）
と表される。したがって、式（９）および式（１０）とから、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔは、ステップＳ５６で、次式（１１）から求められる。
【００５４】
【数２】
0003975548.tif 000003
【００５５】
こうして得られた零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔは、後述する選択のフローチャートの判断に基づき、加速度センサ２０’の零点電圧Ｖｘ＿ｏｆｆｓｅｔに代入される。
【００５６】
一方、Ｘ軸方向の加速度センサ２０’では、ステップＳ５７でＸ軸方向の加速度が検出され、電圧Ｖｘ〔Ｖ〕が出力される。そして、この電圧Ｖｘと加速度センサ２０’の零点電圧Ｖｘ＿ｏｆｆｓｅｔとから、上述の式（８）によって、Ｘ軸方向の加速度αｘ〔ｍ／ｓｓ〕が求められる（ステップＳ５８）。
【００５７】
そして、この加速度αｘを積分してＸ軸方向の速度ｖｘ〔ｍ／ｓ〕を求め（ステップＳ５９）、さらにこの速度ｖｘを積分してＸ軸方向の移動距離ｌｘ〔ｍ〕が求められる（ステップＳ６０）。そして、ステップＳ５５で求められるヨー方向の方位θｙａｗと移動距離ｌｘとから、現在位置ＰＯＳが求められる（ステップＳ６１）。
【００５８】
なお、この実施の第３の形態でも、上述の第２の形態と同様に、零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔを求めるために、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとを用いているので、直線運動しかしていない場合には、零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔを求めることができない。そのため、運動が回転運動であるかどうかを判断して、加速度センサ２０’の零点電圧Ｖｘ＿ｏｆｆｓｅｔとして、加速度αｙと角速度ωｙａｗとから求めた零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔを用いるかどうかを選択する。
【００５９】
図７は、この、選択の処理の一例を示すフローチャートである。先ず、最初のステップＳ７０で、Ｘ軸方向の加速度センサ２０’による加速度αｘが求められ、次に、Ｙ軸方向の加速度センサ２０による加速度αｙが求められる（ステップＳ７１）。さらに、ステップＳ７２で、ヨー方向の角速度センサ２１による角速度ωｙａｗが求められる。なお、ステップＳ７０，Ｓ７１，およびＳ７２は、順序が前後してもよい。
【００６０】
次のステップＳ７３で、この航法装置が搭載されている車両が回転運動をしているかどうかが判断される。この判断は、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとが、所定の値を越えているかどうかでなされる。若し、回転運動をしているとされれば、処理はステップＳ７４に移行し、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとでＸ軸方向の速度ｖｘ＿ｒｏｔが求められ、次のステップＳ７５で、式（１１）により、速度ｖｘ＿ｒｏｔから零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが求められる（ステップＳ７５）。
【００６１】
零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが求められたら、次のステップＳ７６で、この零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔがＶｘ＿ｏｆｆｓｅｔに代入される。そして、ステップＳ７７で、式（８）により、Ｘ軸方向の加速度αｘが求められる。
【００６２】
一方、ステップＳ７３で、回転運動ではないと判断されたら、零点電圧Ｖｘ＿ｏｆｆｓｅｔは、停止時に求められた零点電圧Ｖｘ＿ｏｆｆｓｅｔや回転運動時に求められた零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが用いられる。これは、例えば直前の計測によって得られた値をメモリ１２に記憶させておき、これを必要に応じて読み出すようにすることで、実現される。
【００６３】
このように、この実施の第３の形態を用いることで、回転運動時には、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔを重力加速度の影響を取り除き求めることができる。したがって、Ｘ軸方向の加速度，速度および移動距離を、精度良く求めることができる。
【００６４】
次に、この発明の実施の第４の形態について説明する。上述の実施の第２および第３の形態でも既に述べたが、Ｘ軸方向の加速度センサから得られるＸ軸加速度αｘは、車両のＸ軸が水平面からある角度を持った場合には、重力加速度の影響により誤差が含まれ、このＸ軸加速度αｘを積分して求めるＸ軸速度ｖｘ，Ｘ軸距離ｌｘには、さらに積分による誤差も加わり、正確な位置演算を行うことが困難になる。
【００６５】
また、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔも、坂道などの車両のＸ軸が水平面からある角度を有する場合には、重力加速度の影響により、誤差が含まれる。そのため、加速度センサ２０’の検出結果である加速度αｘに大きな誤差が含まれてしまい、Ｘ軸方向の速度ｖｘや移動距離ｌｘに対して、さらに積分による誤差が加わり、最終的な現在位置ＰＯＳが大きくずれたものとなる。
【００６６】
そこで、この実施の第４の形態では、センサ部３にＸ軸方向の加速度センサ２０’が加えられた、上述の実施の第２および第３の形態と同一の構成を用い、Ｙ軸方向の加速度センサ２０およびヨー方向の角速度センサ２１とのセンサ出力から、Ｘ軸方向の速度ｖｘ＿ｒｏｔを求めると共に、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔを求める。
【００６７】
図８は、この実施の第４の形態による現在位置の推測のための計算処理の一例を示す。上述と同様に、ステップＳ８０およびＳ８１で、式（１）により、Ｙ軸方向の加速度センサ２０でＹ軸方向の加速度αｙが求められ、ステップＳ８２およびＳ８３で、式（２）により、ヨー方向の角速度センサ２１でヨー方向の角速度ωｙａｗが求められる。
【００６８】
さらに、式（７）により、加速度αｙおよび角速度ωｙａｗとから、Ｘ軸方向の速度ｖｘ＿ｒｏｔが求められる（ステップＳ８４）。そして、式（１１）により、Ｘ軸方向の加速度センサ２０’の零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが求められる（ステップＳ８５）。また、角速度ωｙａｗが積分されヨー方向の方位θｙａｗが求められる（ステップＳ８６）。
【００６９】
この実施の第４の形態では、直線運動を行っているか回転運動を行っているかで、現在位置ＰＯＳの算出にＸ軸方向の加速度センサ２０’による検出結果を用いるかどうかが選択される。直線運動であれば加速度センサ２０’による検出結果を用いられるとされ、ステップＳ８５で求められた零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが加速度センサ２０’の零点電圧Ｖｘ＿ｏｆｆｓｅｔとして用いられると共に、ステップＳ８４で求められた速度ｖｘ＿ｒｏｔがＸ軸方向の速度ｖｘに代入される。
【００７０】
図９は、この、選択の処理の一例を示すフローチャートである。先ず、最初のステップＳ１００で、Ｘ軸方向の加速度センサ２０’による加速度αｘが求められ、次に、Ｙ軸方向の加速度センサ２０による加速度αｙが求められる（ステップＳ１０１）。さらに、ステップＳ１０２で、ヨー方向の角速度センサ２１による角速度ωｙａｗが求められる。なお、ステップＳ１００，Ｓ１０１，およびＳ１０２は、順序が前後してもよい。
【００７１】
次のステップＳ１０３で、回転運動をしているかどうかが判断される。この判断は、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとが、所定の値を越えているかどうかでなされる。若し、回転運動をしているとされれば、処理はステップＳ１０４に移行し、式（７）により、Ｙ軸方向の加速度αｙとヨー方向の角速度ωｙａｗとでＸ軸方向の速度ｖｘ＿ｒｏｔが求められ、次のステップＳ１０５で速度ｖｘ＿ｒｏｔがＸ軸方向の速度ｖｘに代入される。
【００７２】
そして、式（１１）により、速度ｖｘ＿ｒｏｔから零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが求められ（ステップＳ１０６）、ステップＳ１０７で、零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが加速度センサ２０’の零点電圧Ｖｘ＿ｏｆｆｓｅｔに代入される。
【００７３】
一方、ステップＳ１０３で、回転運動ではないと判断されたら、零点電圧Ｖｘ＿ｏｆｆｓｅｔは、停止時に求められた零点電圧Ｖｘ＿ｏｆｆｓｅｔや回転運動時に求められた零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔが用いられる。
【００７４】
図８の説明に戻り、ステップＳ８７で、Ｘ軸方向の加速度センサ２０’からの出力値Ｖｘが得られ、次のステップＳ８８で、この出力値Ｖｘと、上述の図９のフローチャートに従い求められた零点電圧Ｖｘ＿ｏｆｆｓｅｔとから、式（８）によりＸ軸方向の加速度αｘが求められる。
【００７５】
そして、ステップＳ８９では、ステップＳ８８で求められた加速度αｘと、上述のステップＳ８４で求められたＸ軸方向の速度ｖｘ＿ｒｏｔとから、図９の選択に従って、Ｘ軸方向の速度ｖｘが求められる。すなわち、直線運動を行っているときには、ステップＳ８８で求められた加速度αｘに基づき速度ｖｘが求められる。一方、回転運動を行っているときには、ステップＳ８４で求められた速度ｖｘ＿ｒｏｔが速度ｖｘに代入される。
【００７６】
こうしてＸ軸方向の速度ｖｘが求められたなら、ステップＳ９０でこの速度ｖｘが積分され移動距離ｌｘが求められ、次のステップＳ９１で、上述のステップＳ８６で得られたヨー方向の方位θｙａｗとこの移動距離ｌｘとから、現在位置ＰＯＳが求められる。
【００７７】
なお、この実施の第４の形態および上述の第２の形態では、Ｘ軸方向の速度ｖｘを、零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔで補正された、Ｘ軸方向の加速度センサ２０’による計算結果を使わずに、Ｙ軸方向の加速度センサ２０による加速度αｙおよびヨー方向の角速度センサ２１による角速度ωｙａｗによる結果である、Ｘ軸方向の速度ｖｘ＿ｒｏｔを直接的に用いている。
【００７８】
これは、零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔで補正することによって得られる加速度αｘ＿ｒｏｔは、上述した式（９）のように、速度ｖｘ＿ｒｏｔを微分することで求められる。Ｘ軸方向の速度ｖｘは、この速度ｖｘ＿ｒｏｔを積分し、式（１２）のように求められる。
【００７９】
【数３】
0003975548.tif 000004
【００８０】
この式（１２）から分かるように、速度ｖｘは、Ｙ軸方向の加速度αｙおよびヨー方向の角速度ωｙａｗから求められた速度ｖｘ＿ｒｏｔを微分し、さらに積分して得られることになる。そのため、計算結果に対して、場合によっては大きな誤差が生じてしまうことになる。したがって、図９に示したように、回転運動時には、Ｘ軸方向の速度ｖｘ＿ｒｏｔを用い、直線運動時には、回転運動時に求められた零点電圧Ｖｘ＿ｒｏｔ＿ｏｆｆｓｅｔを用いて補正したＸ軸方向の速度ｖｘを用いて位置演算を行う。これにより、より精度の高い位置演算を行うことができる。
【００８１】
このように、この実施の第４の形態を用いることで、回転運動時に重力加速度の影響を取り除いたＸ軸方向の速度ｖｘを求めることができると共に、零点電圧Ｖｘ＿ｏｆｆｓｅｔに補正をかけることができるようになる。したがって、Ｘ軸方向の加速度，速度および移動距離を、常に精度良く求めることができる。
【００８２】
【発明の効果】
以上説明したように、この発明によれば、Ｙ軸方向の加速度αｙと、ヨー方向の角速度ωｙａｗとからＸ軸方向の速度ｖｘを求めているため、坂道などの重力加速度の影響を顕著に受けるような場合でも、進行方向の速度を正確に求めることができるという効果がある。また、それにより、結果として現在位置を精度良く推測することができる効果がある。
【００８３】
また、この発明の実施の形態によれば、回転運動であるかどうかの判断に基づき、Ｘ軸方向の速度ｖｘを、Ｘ軸方向の加速度センサを用いて求めるか、Ｙ軸方向の加速度センサおよびヨー方向の角速度センサを用いて求めるかを選択している。そのため、坂道などの重力加速度の影響を顕著に受けるような場合でも、進行方向の加速度センサの零点電圧を正しく求めることができ、進行方向の加速度を精度良く求めることができるという効果がある。また、それにより、結果として現在位置を精度良く推測することができる効果がある。
【００８４】
さらに、この発明の実施の形態によれば、停止しているときのみならず回転運動を行っているときにも、進行方向の加速度センサの零点補正がなされているため、センサの零点補正の精度が向上できる効果がある。また、結果として、進行方向の加速度の計算精度が向上し、現在位置を精度良く推測することができる効果がある。
【００８５】
さらにまた、この発明を用いることで、車両の現在位置をより正確に求めることができるようになり、例えば所定の目的地を設定した場合、目的地の確認を精度良く行うことができ、目的地へ効率よく到達することができる効果がある。
【図面の簡単な説明】
【図１】実施の第１の形態に適用できる航法装置の構成の一例を示すブロック図である。
【図２】実施の第１の形態による現在位置の推測のための一例の計算処理を説明するための図である。
【図３】車両が半径ｒで以て回転運動をしている場合の、車両に加わる各力の関係を概略的に示す略線図である。
【図４】実施の第２の形態による現在位置の推測のための一例の計算処理を説明するための図である。
【図５】回転運動および直線運動による選択の処理の一例を示すフローチャートである。
【図６】実施の第３の形態による現在位置の推測のための一例の計算処理を説明するための図である。
【図７】回転運動および直線運動による選択の処理の一例を示すフローチャートである。
【図８】実施の第４の形態による現在位置の推測のための一例の計算処理を説明するための図である。
【図９】回転運動および直線運動による選択の処理の一例を示すフローチャートである。
【図１０】従来技術による自律航法を用いた現在位置の推測のための一例の計算処理を説明するための図である。
【図１１】車両の運動に関する方向の定義を説明するための図である。
【符号の説明】
１・・・航法装置、２・・・航法装置本体、３・・・センサ部、４・・・表示部、１０・・・メインＣＰＵ、１１・・・サブＣＰＵ、１２・・・メモリ、２０・・・Ｙ軸方向の加速度を検出する加速度センサ、２１・・・ヨー方向の角速度を検出する角速度センサ
【図１】
0003975548.tif 000005
【図２】
0003975548.tif 000006
【図３】
0003975548.tif 000007
【図４】
0003975548.tif 000008
【図５】
0003975548.tif 000009
【図６】
0003975548.tif 000010
【図７】
0003975548.tif 000011
【図８】
0003975548.tif 000012
【図９】
0003975548.tif 000013
【図１０】
0003975548.tif 000014
【図１１】
0003975548.tif 000015