測位方法、プログラム及び測位装置
【課題】慣性航法用センサの検出結果に含まれる誤差を補正するための新たな手法を提案
し、より正確な現在位置の測位を実現すること。
【解決手段】カーナビゲーション装置１において、カーナビ用ジャイロセンサ６１の検出
結果を積分することで、地球に対する慣性航法用センサ６０の絶対姿勢の推定値である推
定絶対姿勢が算出される。そして、移動体座標系と基準座標系との座標変換行列が推定絶
対姿勢に基づいて算出され、当該座標変換行列を用いて、カーナビ用加速度センサ６３の
検出結果が基準座標系に変換されるとともに、重力加速度の成分が減算されることで、基
準座標系における自動車の移動ベクトルが算出される。そして、この移動ベクトルを用い
て現在位置が測位される。
し、より正確な現在位置の測位を実現すること。
【解決手段】カーナビゲーション装置１において、カーナビ用ジャイロセンサ６１の検出
結果を積分することで、地球に対する慣性航法用センサ６０の絶対姿勢の推定値である推
定絶対姿勢が算出される。そして、移動体座標系と基準座標系との座標変換行列が推定絶
対姿勢に基づいて算出され、当該座標変換行列を用いて、カーナビ用加速度センサ６３の
検出結果が基準座標系に変換されるとともに、重力加速度の成分が減算されることで、基
準座標系における自動車の移動ベクトルが算出される。そして、この移動ベクトルを用い
て現在位置が測位される。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ジャイロセンサ及び加速度センサが一体的に構成された慣性航法用センサが
固定された移動体の現在位置を、前記慣性航法用センサの検出結果を用いて測位する測位
方法等に関する。
【背景技術】
【０００２】
人工衛星を利用した測位システムとしては、ＧＰＳ（Global Positioning System）が
広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された測位装置に利
用されている。ＧＰＳでは、自機の位置を示す３次元の座標値と、時計誤差との４つのパ
ラメータの値を、複数のＧＰＳ衛星の位置や各ＧＰＳ衛星から自機までの擬似距離等の情
報に基づいて求める測位演算を行うことで、自機の現在位置を測位する。
【０００３】
しかし、トンネル内や屋内等、ＧＰＳ衛星信号を受信することができない環境では、Ｇ
ＰＳによる測位を行うことができないため、ジャイロセンサや加速度センサ等の慣性航法
用センサの検出結果を用いた慣性航法演算処理を行って現在位置を測位する技術が広く用
いられている（例えば、特許文献１）。
【特許文献１】特開平１０−１３２５８９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、慣性航法用センサであるジャイロセンサや加速度センサの検出結果は、種々
の誤差要因に起因する誤差を内在しているため、必ずしも移動体の正しい運動状態や移動
状態を示しているとは限らない。例えば、走行路面の傾斜や、加速度センサの取付傾斜の
ため、加速度センサにより検出される加速度には重力加速度の成分が含まれる。
【０００５】
また、ジャイロセンサにより検出される角速度にも誤差が含まれるため、角速度を積分
することで得られる姿勢角には、角速度による誤差が蓄積される。このように不正確な慣
性航法用センサの検出結果を基に慣性航法演算処理を行ったのでは、移動体の正確な位置
を求めることができないという問題がある。
【０００６】
本発明は、上述した課題に鑑みて為されたものであり、その目的とするところは、慣性
航法用センサの検出結果に含まれる誤差を補正するための新たな手法を提案し、より正確
な現在位置の測位を実現することにある。
【課題を解決するための手段】
【０００７】
以上の課題を解決するための第１の発明は、ジャイロセンサ及び加速度センサが一体的
に構成された慣性航法用センサが固定された移動体の現在位置を、前記慣性航法用センサ
の検出結果を用いて測位する測位方法であって、前記ジャイロセンサの検出結果を積分す
ることで、地球に対する前記慣性航法用センサの絶対姿勢の推定値である推定絶対姿勢を
算出することと、前記移動体自身を基準とした座標系である移動体座標系と基準座標系と
の座標変換行列を、前記推定絶対姿勢に基づいて算出することと、前記座標変換行列を用
いて、前記加速度センサの検出結果を前記基準座標系に変換するとともに重力加速度の成
分を減算することで前記基準座標系における前記移動体の移動ベクトルを算出することと
、前記移動ベクトルを用いて現在位置を測位することと、を含む測位方法である。
【０００８】
また、他の発明として、ジャイロセンサ及び加速度センサが一体的に構成された慣性航
法用センサが固定された移動体の現在位置を、前記慣性航法用センサの検出結果を用いて
コンピュータに測位計算させるためのプログラムであって、前記ジャイロセンサの検出結
果を積分することで、地球に対する前記慣性航法用センサの絶対姿勢の推定値である推定
絶対姿勢を算出することと、前記移動体自身を基準とした座標系である移動体座標系と基
準座標系との座標変換行列を、前記推定絶対姿勢に基づいて算出することと、前記座標変
換行列を用いて、前記加速度センサの検出結果を前記基準座標系に変換するとともに重力
加速度の成分を減算することで前記基準座標系における前記移動体の移動ベクトルを算出
することと、前記移動ベクトルを用いて現在位置を測位することと、を前記コンピュータ
に実行させるためのプログラムを構成してもよい。
【０００９】
さらに、他の発明として、ジャイロセンサ及び加速度センサが一体的に構成された慣性
航法用センサが固定された移動体の現在位置を、前記慣性航法用センサの検出結果を用い
て測位する測位装置であって、前記ジャイロセンサの検出結果を積分することで、地球に
対する前記慣性航法用センサの絶対姿勢の推定値である推定絶対姿勢を算出する絶対姿勢
推定部と、前記移動体自身を基準とした座標系である移動体座標系と基準座標系との座標
変換行列を、前記推定絶対姿勢に基づいて算出する行列算出部と、前記座標変換行列を用
いて、前記加速度センサの検出結果を前記基準座標系に変換するとともに重力加速度の成
分を減算することで前記基準座標系における前記移動体の移動ベクトルを算出する移動ベ
クトル算出部と、前記移動ベクトルを用いて現在位置を測位する測位部と、を備えた測位
装置を構成してもよい。
【００１０】
この第１の発明等によれば、ジャイロセンサの検出結果を積分することで、地球に対す
る慣性航法用センサの絶対姿勢の推定値である推定絶対姿勢が算出される。そして、移動
体座標系と基準座標系との座標変換行列が推定絶対姿勢に基づいて算出され、当該座標変
換行列を用いて、加速度センサの検出結果が基準座標系に変換されるとともに、重力加速
度の成分が減算されることで、基準座標系における移動体の移動ベクトルが算出される。
そして、この移動ベクトルを用いて現在位置が測位される。
【００１１】
加速度センサにより検出される加速度は、移動体自身を基準とする移動体座標系におけ
る加速度であるため、重力に起因する誤差を補正するためには、基準座標系における加速
度に変換する必要がある。そこで、第１の発明では、慣性航法用センサの推定絶対姿勢を
基に座標変換行列を算出し、この座標変換行列を用いて、加速度センサにより検出された
加速度を基準座標系における加速度に変換するとともに、この変換後の加速度から重力加
速度を減算することにしている。これにより、地球の重力に起因する誤差が補正された加
速度を得ることができ、その結果、より正確な現在位置の測位を実現し得る。
【００１２】
また、第２の発明として、第１の発明の測位方法であって、前記移動体が停止中又は等
速走行中であることを検出することと、前記移動体が停止中又は等速走行中であることが
検出された場合に、前記加速度センサの検出結果に表れる重力加速度の成分に基づいて、
地球に対する前記慣性航法用センサの絶対姿勢を算出することと、前記算出された絶対姿
勢で前記推定絶対姿勢を補正することと、を含む測位方法を構成してもよい。
【００１３】
この第２の発明によれば、移動体が停止中又は等速走行中であることが検出された場合
に、加速度センサの検出結果に表れる重力加速度の成分に基づいて地球に対する慣性航法
用センサの絶対姿勢が算出され、この算出された絶対姿勢で推定絶対姿勢が補正される。
【００１４】
詳細は後述するが、移動体が停止中又は等速走行中である場合は、加速度センサの検出
結果には重力加速度の成分しか含まれないため、加速度センサの検出結果と、基準座標系
から移動体座標系への変換行列（変換式）とを用いることで、地球に対する慣性航法用セ
ンサの絶対姿勢を算出することができる。そして、この算出した絶対姿勢を用いて、ジャ
イロセンサの検出結果を積分して求めている推定絶対姿勢を補正することで、推定絶対姿
勢に含まれる誤差を適切に補正することが可能となる。
【００１５】
また、第３の発明として、第１又は第２の発明の測位方法であって、測位用衛星から発
信されている測位用信号に基づく所定の測位演算を行って現在位置を断続的に測位するこ
とと、前記測位演算の測位結果から所定期間における前記移動体の水平方向の移動距離を
算出することと、外気圧を検出する気圧センサの検出結果に基づいて、前記所定期間と同
一の期間における前記移動体の高度方向の移動距離を算出することと、前記水平方向の移
動距離と前記高度方向の移動距離とに基づいて、前記移動体が位置する路面の傾斜角を算
出することと、前記加速度センサにより検出された加速度と、前記移動体の姿勢と同一の
姿勢になるように前記移動体に設置された第２の加速度センサにより検出された加速度と
、重力加速度とを用いて、前記慣性航法用センサの前記移動体に対する取付傾斜角を算出
することと、前記路面の傾斜角と前記慣性航法用センサの前記移動体に対する取付傾斜角
とを用いて、前記慣性航法用センサのピッチ角を算出することと、前記慣性航法用センサ
のピッチ角を用いた所定の推定演算を行って前記推定絶対姿勢に含まれる誤差を推定して
前記推定絶対姿勢を補正することと、を含む測位方法を構成してもよい。
【００１６】
この第３の発明によれば、測位用信号に基づく測位演算の測位結果から算出した移動体
の水平方向の移動距離と、気圧センサの検出結果を基に算出した移動体の高度方向の移動
距離とに基づいて、移動体が位置する路面の傾斜角が算出される。また、加速度センサに
より検出された加速度と、移動体と同一の姿勢になるように移動体に設置された第２の加
速度センサにより検出された加速度と、重力加速度とを用いて、慣性航法用センサの移動
体に対する取付傾斜角が算出され、路面の傾斜角と慣性航法用センサの移動体に対する取
付傾斜角とを用いて、慣性航法用センサのピッチ角が算出される。そして、算出された慣
性航法用センサのピッチ角を用いた所定の推定演算が行われて推定絶対姿勢に含まれる誤
差が推定されて推定絶対姿勢が補正される。
【００１７】
加速度センサの検出結果には、移動体に対する慣性航法用センサの取付傾斜角に起因す
る重力加速度の成分が含まれる。例えば、慣性航法用センサを内蔵した測位装置としてナ
ビゲーション装置を想定した場合、ナビゲーション装置は移動体に対して斜めに配置され
る場合があるため、加速度センサにより検出される加速度そのままを移動体の真の加速度
とすることはできない。
【００１８】
そこで、第３の発明では、慣性航法用センサの移動体に対する取付傾斜角を算出した上
で、慣性航法用センサの正しいピッチ角を用いた推定演算を行って推定絶対姿勢に含まれ
る誤差を推定して推定絶対姿勢を補正することにしている。この補正後の推定絶対姿勢は
、取付傾斜角を考慮した慣性航法用センサの正確な姿勢であると考えられるため、この正
確な姿勢を基に算出される座標変換行列もまた正確なものとなる。ゆえに、第１の発明と
相まって、限りなく真値に近い移動体の加速度を求めることが可能となる。
【発明を実施するための最良の形態】
【００１９】
以下、図面を参照して、本発明に好適な実施形態の一例を説明する。尚、以下では、測
位装置として、移動体の一種である自動車に搭載されるカーナビゲーション装置を例に挙
げ、測位システムとしてＧＰＳ（Global Positioning System）を用いた場合について説
明するが、本発明を適用可能な実施形態がこれらに限定されるわけではない。
【００２０】
１．機能構成
図１は、本実施形態におけるカーナビゲーション装置１の機能構成を示すブロック図で
ある。カーナビゲーション装置１は、ＧＰＳアンテナ１０と、ＧＰＳ受信部２０と、ホス
トＣＰＵ（Central Processing Unit）３０と、操作部４０と、表示部５０と、慣性航法
用センサ６０と、気圧センサ７０と、ＲＯＭ（Read Only Memory）８０と、ＲＡＭ（Rand
om Access Memory）９０とを備えて構成される。
【００２１】
ＧＰＳアンテナ１０は、ＧＰＳ衛星から発信されているＧＰＳ衛星信号を含むＲＦ（Ra
dio Frequency）信号を受信するアンテナであり、受信した信号をＧＰＳ受信部２０に出
力する。尚、ＧＰＳ衛星信号は、衛星毎に異なる拡散符号の一種であるＰＲＮ（Pseudo R
andom Noise）コードで直接スペクトラム拡散方式により変調された１．５７５４２［Ｇ
Ｈｚ］の通信信号である。ＰＲＮコードは、コード長１０２３チップを１ＰＮフレームと
する繰返し周期１ｍｓの擬似ランダム雑音符号である。
【００２２】
ＧＰＳ受信部２０は、ＧＰＳアンテナ１０から出力された信号に基づいてカーナビゲー
ション装置１の現在位置を測位する測位回路であり、いわゆるＧＰＳ受信機に相当する機
能ブロックである。ＧＰＳ受信部２０は、ＲＦ（Radio Frequency）受信回路部２１と、
ベースバンド処理回路部２３とを備えて構成される。尚、ＲＦ受信回路部２１と、ベース
バンド処理回路部２３とは、それぞれ別のＬＳＩ（Large Scale Integration）として製
造することも、１チップとして製造することも可能である。
【００２３】
ＲＦ受信回路部２１は、ＲＦ信号の処理回路ブロックであり、所定の局部発振信号を分
周或いは逓倍することで、ＲＦ信号乗算用の発振信号を生成する。そして、生成した発振
信号を、ＧＰＳアンテナ１０から出力されたＲＦ信号に乗算することで、ＲＦ信号を中間
周波数の信号（以下、「ＩＦ（Intermediate Frequency）信号」と称す。）にダウンコン
バートし、ＩＦ信号を増幅等した後、Ａ／Ｄ変換器でデジタル信号に変換して、ベースバ
ンド処理回路部２３に出力する。
【００２４】
ベースバンド処理回路部２３は、ＲＦ受信回路部２１から出力されたＩＦ信号に対して
相関処理等を行ってＧＰＳ衛星信号を捕捉・抽出し、データを復号して航法メッセージや
時刻情報等を取り出して測位演算を行う回路部である。ベースバンド処理回路部２３は、
プロセッサとしてのＣＰＵと、メモリとしてのＲＯＭ及びＲＡＭとを備えて構成される。
尚、測位演算としては、例えば最小二乗法を用いた測位演算等の公知の手法を適用するこ
とができる。
【００２５】
ホストＣＰＵ３０は、ＲＯＭ８０に記憶されているシステムプログラム等の各種プログ
ラムに従ってカーナビゲーション装置１の各部を統括的に制御するプロセッサである。特
に本実施形態では、ホストＣＰＵ３０は、測位プログラム８０３に従って、ナビゲーショ
ン画面に表示させる位置である出力位置を決定する。そして、ナビゲーションプログラム
８０１に従って、出力位置をプロットしたナビゲーション画面を生成して、表示部５０に
表示させる。尚、ナビゲーション画面に表示させる位置としては、出力位置を更にマップ
マッチングすることで、出力位置に最近接する道路上にプロットすることが好適である。
【００２６】
操作部４０は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であ
り、押下されたアイコンやボタンの信号をホストＣＰＵ３０に出力する。この操作部４０
の操作により、目的地の入力や、ナビゲーション画面の表示要求等の各種指示入力がなさ
れる。
【００２７】
表示部５０は、ＬＣＤ（Liquid Crystal Display）等により構成され、ホストＣＰＵ３
０から入力される表示信号に基づいた各種表示を行う表示装置である。表示部５０には、
ナビゲーション画面等が表示される。
【００２８】
慣性航法用センサ６０は、慣性航法演算処理に使用するセンサであり、カーナビ用ジャ
イロセンサ６１及びカーナビ用加速度センサ６３が一体化（パッケージ化）されて、カー
ナビゲーション装置１の本体に内蔵・固定されている。
【００２９】
カーナビ用ジャイロセンサ６１は、直交３軸それぞれの軸周りの角速度を検出すること
でカーナビゲーション装置１の回転を検出するセンサ（角速度センサ）であり、検出した
３軸の角速度をホストＣＰＵ３０に出力する。
【００３０】
カーナビ用加速度センサ６３は、直交３軸それぞれの軸方向の加速度を検出することで
カーナビゲーション装置１の移動状態を検出するセンサであり、検出した３軸の加速度を
ホストＣＰＵ３０に出力する。
【００３１】
気圧センサ７０は、カーナビゲーション装置１の外気圧を検出するセンサであり、検出
した外気圧をホストＣＰＵ３０に出力する。
【００３２】
ＲＯＭ８０は、読み出し専用の記憶装置であり、ホストＣＰＵ３０がカーナビゲーショ
ン装置１を制御するためのシステムプログラムや、ナビゲーション機能を実現するための
各種プログラムやデータ等を記憶している。
【００３３】
ＲＡＭ９０は、読み書き可能な記憶装置であり、ホストＣＰＵ３０により実行されるシ
ステムプログラム、各種処理プログラム、各種処理の処理中データ、処理結果などを一時
的に記憶するワークエリアを形成している。
【００３４】
また、カーナビゲーション装置１とは別に、自動車には加速度センサ１００が搭載され
ており、その検出結果がホストＣＰＵ３０に出力されるように構成されている。加速度セ
ンサ１００は、自動車の移動状態を検出する直交３軸のセンサであり、車体の揺れ制御等
のためのサスペンション制御や前輪及び後輪のトルク配分制御、ＡＢＳ（Anti-lock Brak
e System）制御等を行うために使用され、自動車の制御のために予め自動車に設置されて
いるものである。
【００３５】
ここで、本実施形態で使用する座標系等について説明する。図２に示すように、自動車
の進行方向を「Ｘb軸」（直進方向を正方向）、Ｘb軸正方向に向かって左右方向を「Ｙb
軸」（右方向を正方向）、Ｘb軸に対する高さ方向を「Ｚb軸」（下方向を正方向）とする
右手系直交座標系を「移動体座標系」と定義する。また、北方向を「Ｘn軸」、東方向を
「Ｙn軸」、地球による重力方向を「Ｚn軸」とする右手系直交座標系を「基準座標系」と
定義する。
【００３６】
また、自動車に対する慣性航法用センサ６０の傾斜及び傾斜角のことを、それぞれ「取
付傾斜」及び「取付傾斜角」と称する。また、地球に対する慣性航法用センサ６０の姿勢
及び姿勢角のことを、それぞれ「絶対姿勢」及び「絶対姿勢角」と称し、Ｘb軸周りの回
転角「φ」を「絶対ロール角」、Ｙb軸周りの回転角「θ」を「絶対ピッチ角」、Ｚb軸周
りの回転角「ψ」を「絶対ヨー角」とそれぞれ称する。さらに、カーナビ用ジャイロセン
サ６１の検出結果を積分することで得られる地球に対する慣性航法用センサ６０の絶対姿
勢及び絶対姿勢角のことを、それぞれ「推定絶対姿勢」及び「推定絶対姿勢角」と称する
。
【００３７】
自動車の傾斜及び慣性航法用センサ６０の傾斜に起因して、カーナビ用加速度センサ６
３により検出される加速度には重力加速度の成分が含まれるため、この重力成分を除去し
なければ、自動車の正確な加速度を求めることができない。そこで、本実施形態では、移
動体座標系から基準座標系への座標変換行列を、推定絶対姿勢に基づいて算出する。そし
て、算出した座標変換行列を用いて、カーナビ用加速度センサ６３の検出結果を基準座標
系に変換するとともに、重力加速度の成分を減算することで、地球座標系における自動車
の加速度を求める。
【００３８】
しかし、カーナビ用ジャイロセンサ６１により検出される角速度は誤差を含んでいるた
め、カーナビ用ジャイロセンサ６１の検出結果を積分することで得られる推定絶対姿勢は
、必ずしも絶対姿勢と一致するとは限らない。この問題を解決するため、本実施形態では
、自動車が走行している路面（以下、「走行路面」と称す。）の傾斜角及び取付傾斜角を
算出し、これらを基に慣性航法用センサ６０の絶対ピッチ角を算出する。そして、算出し
た絶対ピッチ角を観測値として用いたカルマンフィルタ処理（推定演算）を行うことで推
定絶対姿勢に含まれる誤差を推定して、推定絶対姿勢を補正する。
【００３９】
図３は、取付傾斜角算出の原理を説明するための図である。ここでは、取付傾斜角を「
α」、走行路面の傾斜角を「β」として説明する。自動車が停止している状態では、カー
ナビ用加速度センサ６３及び加速度センサ１００により検出される自動車の進行方向（Ｘ
b軸方向）に対する加速度は、重力加速度の進行方向に対する成分のみである。
【００４０】
具体的には、重力加速度を「ｇ」とすると、カーナビ用加速度センサ６３及び加速度セ
ンサ１００により検出されるＸb軸方向の加速度「ａx1」及び「ａx2」は、それぞれ次式
（１）及び（２）で与えられる。
【数１】
【数２】
【００４１】
式（１）及び（２）から「β」を消去することで、取付傾斜角「α」は、次式（３）の
ように算出される。
【数３】
【００４２】
２．データ構成
図４は、ＲＯＭ８０に格納されたデータの一例を示す図である。ＲＯＭ８０には、ホス
トＣＰＵ３０により読み出され、ナビゲーション処理として実行されるナビゲーションプ
ログラム８０１と、測位処理（図７参照）として実行される測位プログラム８０３と、ナ
ビゲーション画面を生成するための地図情報のデータである地図データ８０５とが記憶さ
れている。また、測位プログラム８０３には、推定絶対姿勢補正処理（図８参照）として
実行される推定絶対姿勢補正プログラム８０３１がサブルーチンとして含まれている。
【００４３】
ナビゲーション処理とは、ホストＣＰＵ３０が、地図データ８０５に記憶されている地
図情報を用いて、測位処理により決定した出力位置を道路上に補正するマップマッチング
処理を行うとともに、補正後の位置をプロットしたナビゲーション画面を生成して、表示
部５０に表示させる処理である。尚、マップマッチング処理については公知であるため、
詳細な説明を省略する。
【００４４】
測位処理とは、ホストＣＰＵ３０が、ＧＰＳによる測位が可能である場合は、ＧＰＳ測
位処理を行って現在位置を測位し、ＧＰＳによる測位が不可能である場合は、慣性航法演
算処理を行って現在位置を測位する処理である。測位処理については、フローチャートを
用いて詳細に後述する。
【００４５】
推定絶対姿勢補正処理とは、ホストＣＰＵ３０が、取付傾斜角を考慮したカルマンフィ
ルタ処理を行って推定絶対姿勢に含まれる誤差を推定して、推定絶対姿勢を補正する処理
である。取付傾斜角は、自動車の停止時に、カーナビ用加速度センサ６３及び加速度セン
サ１００の検出結果を用いて算出する。推定絶対姿勢補正処理についても、フローチャー
トを用いて詳細に後述する。
【００４６】
図５は、ＲＡＭ９０に格納されるデータの一例を示す図である。ＲＡＭ９０には、計測
履歴データ９０１と、補正用絶対姿勢角データ９０３と、取付傾斜角データ９０５とが記
憶される。
【００４７】
図６は、計測履歴データ９０１のデータ構成の一例を示す図である。計測履歴データ９
０１には、各時刻９０１１毎に、カーナビ用ジャイロセンサ６１により検出された３軸の
角速度９０１２と、カーナビ用加速度センサ６３により検出された３軸の加速度９０１３
と、気圧センサ７０により検出された外気圧９０１４と、角速度９０１２を積分すること
で得られる推定絶対姿勢角９０１５と、ＧＰＳ測位処理又は慣性航法演算処理により求め
られた速度９０１７と、ＧＰＳ測位処理により求められたＧＰＳ測位位置９０１８と、慣
性航法演算処理により求められた慣性航法演算位置９０１９とが対応付けて記憶される。
計測履歴データ９０１は、測位処理においてホストＣＰＵ３０により更新される。
【００４８】
補正用絶対姿勢角データ９０３は、カーナビ用加速度センサ６３の検出結果と重力加速
度とに基づいて算出される絶対姿勢角のデータであり、推定絶対姿勢を補正するために使
用される。補正用絶対姿勢角データ９０３は、推定絶対姿勢補正処理においてホストＣＰ
Ｕ３０により更新される。
【００４９】
取付傾斜角データ９０５は、取付傾斜角「α」のデータであり、推定絶対姿勢補正処理
においてホストＣＰＵ３０により更新される。
【００５０】
３．処理の流れ
図７は、ホストＣＰＵ３０によりＲＯＭ８０に記憶されている測位プログラム８０３が
読み出されて実行されることで、カーナビゲーション装置１において実行される測位処理
の流れを示すフローチャートである。
【００５１】
測位処理は、ホストＣＰＵ３０が、操作部４０に測位開始指示の操作がなされたことを
検出した場合に実行を開始する処理である。尚、カーナビゲーション装置１の電源のＯＮ
／ＯＦＦとＧＰＳの起動／停止とを連動させ、カーナビゲーション装置１の電源投入操作
を検出した場合に測位処理の実行を開始させることにしてもよい。
【００５２】
また、特に説明しないが、以下の測位処理の実行中は、ＧＰＳアンテナ１０によるＲＦ
信号の受信や、ＲＦ受信回路部２１によるＩＦ信号へのダウンコンバート、ベースバンド
処理回路部２３によるＧＰＳ衛星信号の捕捉・追尾等が随時行われている状態にあるもの
とする。また、カーナビ用ジャイロセンサ６１、カーナビ用加速度センサ６３及び気圧セ
ンサ７０の検出結果に従って、ホストＣＰＵ３０により、ＲＡＭ９０の計測履歴データ９
０１は随時更新されるものとする。
【００５３】
先ず、ホストＣＰＵ３０は、ＧＰＳによる測位が可能であるか否かを判定する（ステッ
プＡ１）。具体的には、３次元測位（高度を含む測位）の場合は、ベースバンド処理回路
部２３が現在捕捉しているＧＰＳ衛星（以下、「捕捉衛星」と称す。）の数が「４個以上
」である場合に、ＧＰＳによる測位が可能であると判定する。また、２次元測位（高度を
含まない測位）の場合は、捕捉衛星の数が「３個以上」である場合に、ＧＰＳによる測位
が可能であると判定する。
【００５４】
そして、ＧＰＳによる測位が可能であると判定した場合は（ステップＡ１；Ｙｅｓ）、
ホストＣＰＵ３０は、ＧＰＳ測位処理を行う（ステップＡ３）。具体的には、ホストＣＰ
Ｕ３０は、ベースバンド処理回路部２３のＣＰＵに、捕捉衛星の衛星情報及び時刻情報を
基に、例えば最小二乗法を用いた測位演算を実行させて、カーナビゲーション装置１の速
度及び位置を算出する。そして、算出した速度及び位置（ＧＰＳ測位位置）を、ＲＡＭ９
０の計測履歴データ９０１に記憶させる。
【００５５】
その後、ホストＣＰＵ３０は、ＧＰＳ測位処理で求めたＧＰＳ測位位置を出力位置に決
定して（ステップＡ５）、ステップＡ１７へと処理を移行する。
【００５６】
一方、ステップＡ１においてＧＰＳによる測位が不可能であると判定した場合は（ステ
ップＡ１；Ｎｏ）、ホストＣＰＵ３０は、ＲＡＭ９０の計測履歴データ９０１に記憶され
ている最新の推定絶対姿勢角を用いて、移動体座標系から基準座標系への座標変換行列「
Ｃnb」を算出する（ステップＡ７）。座標変換行列は「Ｃnb」は、絶対ピッチ角「θ」、
絶対ロール角「φ」及び絶対ヨー角「ψ」を変数とする三角関数を成分として含む３×３
の公知の行列である。
【００５７】
そして、ホストＣＰＵ３０は、ステップＡ７で算出した座標変換行列「Ｃnb」と、カー
ナビ用加速度センサ６３の検出結果とを用いて、次式（４）に従って基準座標系における
自動車の加速度を算出する（ステップＡ９）。
【数４】
但し、ベクトル表記の「ａn」、「ａb」、「ｇn」は、それぞれ基準座標系における自
動車の加速度ベクトル、移動体座標系における自動車の加速度ベクトル、基準座標系にお
ける重力加速度ベクトルをそれぞれ示している。
【００５８】
その後、ホストＣＰＵ３０は、ステップＡ９で算出した加速度を基に自動車の速度（単
なる速度ではなく方向を含んだ速度であるため、移動ベクトルと言える。）を算出し（ス
テップＡ１１）、ＲＡＭ９０の計測履歴データ９０１に記憶させる。そして、算出した速
度と、前回（１時刻前）の出力位置とを用いて自動車の位置を算出して慣性航法演算位置
とし（ステップＡ１３）、ＲＡＭ９０の計測履歴データ９０１に記憶させる。そして、算
出した慣性航法演算位置を出力位置に決定する（ステップＡ１５）。
【００５９】
次いで、ホストＣＰＵ３０は、カーナビ用ジャイロセンサ６１の検出結果を積分するこ
とにより推定絶対姿勢角を算出し（ステップＡ１７）、ＲＡＭ９０の計測履歴データ９０
１に記憶させる。そして、ＲＯＭ８０に記憶されている推定絶対姿勢補正プログラム８０
３１を読み出して実行することで、ステップＡ１７で算出した推定絶対姿勢角を補正する
推定絶対姿勢補正処理を行う（ステップＡ１９）。
【００６０】
推定絶対姿勢補正処理を行った後、ホストＣＰＵ３０は、処理を終了するか否かを判定
し（ステップＡ２１）、まだ終了しないと判定した場合は（ステップＡ２１；Ｎｏ）、ス
テップＡ１に戻る。また、終了すると判定した場合は（ステップＡ２１；Ｙｅｓ）、測位
処理を終了する。
【００６１】
図８は、推定絶対姿勢補正処理の流れを示すフローチャートである。
先ず、ホストＣＰＵ３０は、自動車が停止中又は等速走行中であるか否かを判定する（
ステップＢ１）。自動車が停止しているか否かの判定は、例えばカーナビ用ジャイロセン
サ６１の出力値である角速度の分散値が所定の閾値以下であるか否かを判定することで行
う。また、自動車が等速走行中であるか否かを判定は、例えばカーナビ用加速度センサ６
３の出力値である加速度の分散値が所定の閾値以下であるか否かを判定することで行う。
【００６２】
ステップＢ１において自動車が停止中又は等速走行中であると判定した場合は（ステッ
プＢ１；Ｙｅｓ）、ホストＣＰＵ３０は、自動車が停止中であるか否かを判定する（ステ
ップＢ３）。そして、停止中ではない、すなわち等速走行中であると判定した場合は（ス
テップＢ３；Ｎｏ）、ステップＢ７へと処理を移行する。
【００６３】
また、自動車が停止中であると判定した場合は（ステップＢ３；Ｙｅｓ）、ホストＣＰ
Ｕ３０は、カーナビ用加速度センサ６３により検出されたＸb軸方向の加速度「ａx1」と
、加速度センサ１００により検出されたＸb軸方向の加速度「ａx2」とを用いて、式（３
）に従って取付傾斜角「α」を算出する（ステップＢ５）。そして、算出した取付傾斜角
「α」で、ＲＡＭ９０の取付傾斜角データ９０５を更新する。
【００６４】
次いで、ホストＣＰＵ３０は、カーナビ用加速度センサ６３の検出結果を用いて、慣性
航法用センサ６０の絶対ピッチ角「θ」及び絶対ロール角「φ」を算出し（ステップＢ７
）、ＲＡＭ９０の補正用絶対姿勢角データ９０３に記憶させる。
【００６５】
詳細に説明すると、カーナビ用加速度センサ６３により検出された移動体座標系におけ
る自動車の加速度ベクトルをベクトル表記の「ａb」、基準座標系から移動体座標系への
座標変換行列を「Ｃbn」、基準座標系における重力加速度ベクトルをベクトル表記の「ｇ
n」とすると、次式（５）が成立する。
【数５】
【００６６】
式（５）を３次元の各成分「（ａx，ａy，ａz）」について展開すると、次式（６−１
）〜（６−３）のようになる。
【数６】
【００６７】
そして、式（６−１）〜（６−３）から、絶対ピッチ角「θ」及び絶対ロール角「φ」
は、次式（７−１）及び（７−２）のように求められる。
【数７】
【００６８】
次いで、ホストＣＰＵ３０は、ステップＢ７で算出した絶対ピッチ角「θ」及び絶対ロ
ール角「φ」を観測値とするカルマンフィルタ処理を行って、ステップＡ１７で算出して
計測履歴データ９０１に記憶させた推定絶対姿勢角を補正する（ステップＢ９）。具体的
には、推定絶対姿勢角に含まれる誤差を状態ベクトルとし、絶対ピッチ角「θ」及び絶対
ロール角「φ」を外部観測量として、カルマンフィルタの理論に基づく推定演算を行う。
そして、推定された誤差を、カーナビ用ジャイロセンサ６１の検出結果を基に算出した推
定絶対姿勢角に加算することで、推定絶対姿勢角を補正する。
【００６９】
一方、ステップＢ１において自動車が停止中でも等速走行中でもないと判定した場合は
（ステップＢ１；Ｎｏ）、ホストＣＰＵ３０は、今回の測位タイミングがＧＰＳ測位であ
るか否かを判定する（ステップＢ１１）。そして、今回の測位タイミングがＧＰＳ測位で
あると判定した場合は（ステップＢ１１；Ｙｅｓ）、前回の測位タイミングもＧＰＳ測位
であったか否かを判定する（ステップＢ１３）。
【００７０】
そして、前回の測位タイミングもＧＰＳ測位であったと判定した場合は（ステップＢ１
３；Ｙｅｓ）、ホストＣＰＵ３０は、ＲＡＭ９０の計測履歴データ９０１に記憶されてい
る前回のＧＰＳ測位位置と今回のＧＰＳ測位位置とから、前回の測位タイミングと今回の
測位タイミング間における自動車の水平方向の移動距離を算出する（ステップＢ１５）。
また、ホストＣＰＵ３０は、気圧センサ７０の検出結果に基づいて、前回の測位タイミン
グと今回の測位タイミング間における自動車の高度方向の移動距離を算出する（ステップ
Ｂ１７）。
【００７１】
次いで、ホストＣＰＵ３０は、ステップＢ１５で算出した水平方向の移動距離と、ステ
ップＢ１７で算出した高度方向の移動距離とを用いて、走行路面の傾斜角「β」を算出す
る（ステップＢ１９）。そして、ＲＡＭ９０の取付傾斜角データ９０５に記憶されている
取付傾斜角「α」を加味することで絶対ピッチ角「θ」を算出し（ステップＢ２１）、Ｒ
ＡＭ９０の補正用絶対姿勢角データ９０３を更新する。
【００７２】
そして、ホストＣＰＵ３０は、ステップＢ２１で算出した絶対ピッチ角「θ」を観測値
とするカルマンフィルタ処理を行って、ステップＡ１７で算出して計測履歴データ９０１
に記憶させた推定絶対姿勢角を補正する（ステップＢ２３）。具体的には、推定絶対姿勢
角に含まれる誤差を状態ベクトルとし、絶対ピッチ角「θ」を外部観測量として、カルマ
ンフィルタの理論に基づく推定演算を行う。そして、推定された誤差を、カーナビ用ジャ
イロセンサ６１の検出結果を基に算出した推定絶対姿勢角に加算することで、推定絶対姿
勢角を補正する。
【００７３】
ステップＢ９又はＢ２３において推定絶対姿勢角の補正を行った後、ホストＣＰＵ３０
は、推定絶対姿勢補正処理を終了する。また、ステップＢ１１において今回の測位タイミ
ングがＧＰＳ測位ではないと判定した場合（ステップＢ１１；Ｎｏ）、又は、ステップＢ
１３において前回の測位タイミングがＧＰＳ測位ではなかったと判定した場合にも（ステ
ップＢ１３；Ｎｏ）、ホストＣＰＵ３０は、推定絶対姿勢補正処理を終了する。
【００７４】
３．作用効果
本実施形態によれば、カーナビゲーション装置１において、カーナビ用ジャイロセンサ
６１の検出結果を積分することで、地球に対する慣性航法用センサ６０の絶対姿勢の推定
値である推定絶対姿勢が算出される。そして、移動体座標系と基準座標系との座標変換行
列が推定絶対姿勢に基づいて算出され、当該座標変換行列を用いて、カーナビ用加速度セ
ンサ６３の検出結果が基準座標系に変換されるとともに、重力加速度の成分が減算される
ことで、基準座標系における自動車の移動ベクトルが算出される。そして、この移動ベク
トルを用いて現在位置が測位される。
【００７５】
カーナビ用加速度センサ６３により検出される加速度は、自動車自身を基準とする移動
体座標系における加速度であるため、重力加速度の影響を補正するためには、基準座標系
における加速度に変換する必要がある。そこで、本実施形態では、慣性航法用センサ６０
の推定絶対姿勢を基に座標変換行列を算出し、この座標変換行列を用いて、カーナビ用加
速度センサ６１により検出された加速度を基準座標系における加速度に変換するとともに
、この変換後の加速度から重力加速度を減算することにしている。これにより、地球の重
力に起因する成分が補正された加速度を得ることが可能となり、その結果、より正確な現
在位置の測位を実現し得る。
【００７６】
４．変形例
４−１．電子機器
本発明は、測位装置を備えた電子機器であれば何れの電子機器にも適用可能である。例
えば、ノート型パソコンやＰＤＡ（Personal Digital Assistant）等についても同様に適
用可能である。
【００７７】
４−２．移動体
また、移動体は必ずしも自動車に限られるわけではなく、バスや電車等の移動体につい
ても同様に適用可能である。
【００７８】
４−３．衛星測位システム
上述した実施形態では、衛星測位システムとしてＧＰＳを例に挙げて説明したが、ＷＡ
ＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）
、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛
星測位システムであってもよい。
【００７９】
４−４．処理の分化
ホストＣＰＵ３０が行う処理の一部又は全部を、ベースバンド処理回路部２３のＣＰＵ
が行うことにしてもよい。具体的には、例えばベースバンド処理回路部２３のＣＰＵが測
位処理を行う。そして、ホストＣＰＵ３０が、測位処理により求められた出力位置に対し
てマップマッチング処理等を行ってナビゲーション画面を生成し、生成したナビゲーショ
ン画面を表示部５０に表示させるナビゲーション処理を行う。また、ナビゲーション処理
も含めてホストＣＰＵ３０が行う処理の全部をＣＰＵが行うこととしてもよい。
【００８０】
４−５．走行路面の傾斜角の算出
上述した実施形態では、ＧＰＳの測位結果から求められる自動車の水平方向の移動距離
と、気圧センサ７０の検出結果から求められる自動車の高度方向の移動距離とに基づいて
、走行路面の傾斜角「β」を算出するものとして説明したが、気圧センサ７０の検出結果
を用いずに、ＧＰＳの測位結果のみから走行路面の傾斜角「β」を算出することとしても
よい。
【００８１】
すなわち、３次元測位では、自動車の緯度、経度及び高度を求めることができるため、
緯度及び経度の変化から求められる水平方向の移動距離と、高度の変化から求められる高
度方向の移動距離とを用いることで、走行路面の傾斜角「β」を算出することができる。
この場合は、カーナビゲーション装置１に気圧センサ７０を設ける必要はない。
【００８２】
また、上述した実施形態では、１時刻間の移動距離に基づいて路面の傾斜角「β」を算
出するものとして説明したが、これを所定期間（例えば１０時刻分）の移動距離に基づい
て算出することとしてもよいことは勿論である。
【００８３】
４−６．推定絶対姿勢の補正
上述した実施形態では、カルマンフィルタ処理を行って推定絶対姿勢角に含まれる誤差
を推定することで、推定絶対姿勢の補正を行うものとして説明したが、次のようにしても
よい。すなわち、図８の推定絶対姿勢補正処理のステップＢ９において、カルマンフィル
タ処理を行わずに、推定絶対姿勢角に含まれる絶対ピッチ角及び絶対ロール角を、ステッ
プＢ７で算出した絶対ピッチ角「θ」及び絶対ロール角「φ」にそれぞれ置き換える処理
を行う。また、ステップＢ２３においても同様にカルマンフィルタ処理を行わず、推定絶
対姿勢角に含まれる絶対ピッチ角を、ステップＢ２１で算出した絶対ピッチ角「θ」に置
き換える処理を行う。
【図面の簡単な説明】
【００８４】
【図１】カーナビゲーション装置の機能構成を示すブロック図。
【図２】移動体座標系の一例を示す図。
【図３】取付傾斜角算出の原理の説明図。
【図４】ＲＯＭに格納されたデータの一例を示す図。
【図５】ＲＡＭに格納されたデータの一例を示す図。
【図６】計測履歴データのデータ構成の一例を示す図。
【図７】測位処理の流れを示すフローチャート。
【図８】推定絶対姿勢補正処理の流れを示すフローチャート。
【符号の説明】
【００８５】
１ カーナビゲーション装置、 １０ ＧＰＳアンテナ、 ２０ ＧＰＳ受信部、
２１ ＲＦ受信回路部、 ２３ ベースバンド処理回路部、 ３０ ホストＣＰＵ、
４０ 操作部、 ５０ 表示部、 ６０ 慣性航法用センサ、
６１ カーナビ用ジャイロセンサ、 ６３ カーナビ用加速度センサ、
７０ 気圧センサ、 ８０ ＲＯＭ、 ９０ ＲＡＭ、 １００ 加速度センサ
【技術分野】
【０００１】
本発明は、ジャイロセンサ及び加速度センサが一体的に構成された慣性航法用センサが
固定された移動体の現在位置を、前記慣性航法用センサの検出結果を用いて測位する測位
方法等に関する。
【背景技術】
【０００２】
人工衛星を利用した測位システムとしては、ＧＰＳ（Global Positioning System）が
広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された測位装置に利
用されている。ＧＰＳでは、自機の位置を示す３次元の座標値と、時計誤差との４つのパ
ラメータの値を、複数のＧＰＳ衛星の位置や各ＧＰＳ衛星から自機までの擬似距離等の情
報に基づいて求める測位演算を行うことで、自機の現在位置を測位する。
【０００３】
しかし、トンネル内や屋内等、ＧＰＳ衛星信号を受信することができない環境では、Ｇ
ＰＳによる測位を行うことができないため、ジャイロセンサや加速度センサ等の慣性航法
用センサの検出結果を用いた慣性航法演算処理を行って現在位置を測位する技術が広く用
いられている（例えば、特許文献１）。
【特許文献１】特開平１０−１３２５８９号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、慣性航法用センサであるジャイロセンサや加速度センサの検出結果は、種々
の誤差要因に起因する誤差を内在しているため、必ずしも移動体の正しい運動状態や移動
状態を示しているとは限らない。例えば、走行路面の傾斜や、加速度センサの取付傾斜の
ため、加速度センサにより検出される加速度には重力加速度の成分が含まれる。
【０００５】
また、ジャイロセンサにより検出される角速度にも誤差が含まれるため、角速度を積分
することで得られる姿勢角には、角速度による誤差が蓄積される。このように不正確な慣
性航法用センサの検出結果を基に慣性航法演算処理を行ったのでは、移動体の正確な位置
を求めることができないという問題がある。
【０００６】
本発明は、上述した課題に鑑みて為されたものであり、その目的とするところは、慣性
航法用センサの検出結果に含まれる誤差を補正するための新たな手法を提案し、より正確
な現在位置の測位を実現することにある。
【課題を解決するための手段】
【０００７】
以上の課題を解決するための第１の発明は、ジャイロセンサ及び加速度センサが一体的
に構成された慣性航法用センサが固定された移動体の現在位置を、前記慣性航法用センサ
の検出結果を用いて測位する測位方法であって、前記ジャイロセンサの検出結果を積分す
ることで、地球に対する前記慣性航法用センサの絶対姿勢の推定値である推定絶対姿勢を
算出することと、前記移動体自身を基準とした座標系である移動体座標系と基準座標系と
の座標変換行列を、前記推定絶対姿勢に基づいて算出することと、前記座標変換行列を用
いて、前記加速度センサの検出結果を前記基準座標系に変換するとともに重力加速度の成
分を減算することで前記基準座標系における前記移動体の移動ベクトルを算出することと
、前記移動ベクトルを用いて現在位置を測位することと、を含む測位方法である。
【０００８】
また、他の発明として、ジャイロセンサ及び加速度センサが一体的に構成された慣性航
法用センサが固定された移動体の現在位置を、前記慣性航法用センサの検出結果を用いて
コンピュータに測位計算させるためのプログラムであって、前記ジャイロセンサの検出結
果を積分することで、地球に対する前記慣性航法用センサの絶対姿勢の推定値である推定
絶対姿勢を算出することと、前記移動体自身を基準とした座標系である移動体座標系と基
準座標系との座標変換行列を、前記推定絶対姿勢に基づいて算出することと、前記座標変
換行列を用いて、前記加速度センサの検出結果を前記基準座標系に変換するとともに重力
加速度の成分を減算することで前記基準座標系における前記移動体の移動ベクトルを算出
することと、前記移動ベクトルを用いて現在位置を測位することと、を前記コンピュータ
に実行させるためのプログラムを構成してもよい。
【０００９】
さらに、他の発明として、ジャイロセンサ及び加速度センサが一体的に構成された慣性
航法用センサが固定された移動体の現在位置を、前記慣性航法用センサの検出結果を用い
て測位する測位装置であって、前記ジャイロセンサの検出結果を積分することで、地球に
対する前記慣性航法用センサの絶対姿勢の推定値である推定絶対姿勢を算出する絶対姿勢
推定部と、前記移動体自身を基準とした座標系である移動体座標系と基準座標系との座標
変換行列を、前記推定絶対姿勢に基づいて算出する行列算出部と、前記座標変換行列を用
いて、前記加速度センサの検出結果を前記基準座標系に変換するとともに重力加速度の成
分を減算することで前記基準座標系における前記移動体の移動ベクトルを算出する移動ベ
クトル算出部と、前記移動ベクトルを用いて現在位置を測位する測位部と、を備えた測位
装置を構成してもよい。
【００１０】
この第１の発明等によれば、ジャイロセンサの検出結果を積分することで、地球に対す
る慣性航法用センサの絶対姿勢の推定値である推定絶対姿勢が算出される。そして、移動
体座標系と基準座標系との座標変換行列が推定絶対姿勢に基づいて算出され、当該座標変
換行列を用いて、加速度センサの検出結果が基準座標系に変換されるとともに、重力加速
度の成分が減算されることで、基準座標系における移動体の移動ベクトルが算出される。
そして、この移動ベクトルを用いて現在位置が測位される。
【００１１】
加速度センサにより検出される加速度は、移動体自身を基準とする移動体座標系におけ
る加速度であるため、重力に起因する誤差を補正するためには、基準座標系における加速
度に変換する必要がある。そこで、第１の発明では、慣性航法用センサの推定絶対姿勢を
基に座標変換行列を算出し、この座標変換行列を用いて、加速度センサにより検出された
加速度を基準座標系における加速度に変換するとともに、この変換後の加速度から重力加
速度を減算することにしている。これにより、地球の重力に起因する誤差が補正された加
速度を得ることができ、その結果、より正確な現在位置の測位を実現し得る。
【００１２】
また、第２の発明として、第１の発明の測位方法であって、前記移動体が停止中又は等
速走行中であることを検出することと、前記移動体が停止中又は等速走行中であることが
検出された場合に、前記加速度センサの検出結果に表れる重力加速度の成分に基づいて、
地球に対する前記慣性航法用センサの絶対姿勢を算出することと、前記算出された絶対姿
勢で前記推定絶対姿勢を補正することと、を含む測位方法を構成してもよい。
【００１３】
この第２の発明によれば、移動体が停止中又は等速走行中であることが検出された場合
に、加速度センサの検出結果に表れる重力加速度の成分に基づいて地球に対する慣性航法
用センサの絶対姿勢が算出され、この算出された絶対姿勢で推定絶対姿勢が補正される。
【００１４】
詳細は後述するが、移動体が停止中又は等速走行中である場合は、加速度センサの検出
結果には重力加速度の成分しか含まれないため、加速度センサの検出結果と、基準座標系
から移動体座標系への変換行列（変換式）とを用いることで、地球に対する慣性航法用セ
ンサの絶対姿勢を算出することができる。そして、この算出した絶対姿勢を用いて、ジャ
イロセンサの検出結果を積分して求めている推定絶対姿勢を補正することで、推定絶対姿
勢に含まれる誤差を適切に補正することが可能となる。
【００１５】
また、第３の発明として、第１又は第２の発明の測位方法であって、測位用衛星から発
信されている測位用信号に基づく所定の測位演算を行って現在位置を断続的に測位するこ
とと、前記測位演算の測位結果から所定期間における前記移動体の水平方向の移動距離を
算出することと、外気圧を検出する気圧センサの検出結果に基づいて、前記所定期間と同
一の期間における前記移動体の高度方向の移動距離を算出することと、前記水平方向の移
動距離と前記高度方向の移動距離とに基づいて、前記移動体が位置する路面の傾斜角を算
出することと、前記加速度センサにより検出された加速度と、前記移動体の姿勢と同一の
姿勢になるように前記移動体に設置された第２の加速度センサにより検出された加速度と
、重力加速度とを用いて、前記慣性航法用センサの前記移動体に対する取付傾斜角を算出
することと、前記路面の傾斜角と前記慣性航法用センサの前記移動体に対する取付傾斜角
とを用いて、前記慣性航法用センサのピッチ角を算出することと、前記慣性航法用センサ
のピッチ角を用いた所定の推定演算を行って前記推定絶対姿勢に含まれる誤差を推定して
前記推定絶対姿勢を補正することと、を含む測位方法を構成してもよい。
【００１６】
この第３の発明によれば、測位用信号に基づく測位演算の測位結果から算出した移動体
の水平方向の移動距離と、気圧センサの検出結果を基に算出した移動体の高度方向の移動
距離とに基づいて、移動体が位置する路面の傾斜角が算出される。また、加速度センサに
より検出された加速度と、移動体と同一の姿勢になるように移動体に設置された第２の加
速度センサにより検出された加速度と、重力加速度とを用いて、慣性航法用センサの移動
体に対する取付傾斜角が算出され、路面の傾斜角と慣性航法用センサの移動体に対する取
付傾斜角とを用いて、慣性航法用センサのピッチ角が算出される。そして、算出された慣
性航法用センサのピッチ角を用いた所定の推定演算が行われて推定絶対姿勢に含まれる誤
差が推定されて推定絶対姿勢が補正される。
【００１７】
加速度センサの検出結果には、移動体に対する慣性航法用センサの取付傾斜角に起因す
る重力加速度の成分が含まれる。例えば、慣性航法用センサを内蔵した測位装置としてナ
ビゲーション装置を想定した場合、ナビゲーション装置は移動体に対して斜めに配置され
る場合があるため、加速度センサにより検出される加速度そのままを移動体の真の加速度
とすることはできない。
【００１８】
そこで、第３の発明では、慣性航法用センサの移動体に対する取付傾斜角を算出した上
で、慣性航法用センサの正しいピッチ角を用いた推定演算を行って推定絶対姿勢に含まれ
る誤差を推定して推定絶対姿勢を補正することにしている。この補正後の推定絶対姿勢は
、取付傾斜角を考慮した慣性航法用センサの正確な姿勢であると考えられるため、この正
確な姿勢を基に算出される座標変換行列もまた正確なものとなる。ゆえに、第１の発明と
相まって、限りなく真値に近い移動体の加速度を求めることが可能となる。
【発明を実施するための最良の形態】
【００１９】
以下、図面を参照して、本発明に好適な実施形態の一例を説明する。尚、以下では、測
位装置として、移動体の一種である自動車に搭載されるカーナビゲーション装置を例に挙
げ、測位システムとしてＧＰＳ（Global Positioning System）を用いた場合について説
明するが、本発明を適用可能な実施形態がこれらに限定されるわけではない。
【００２０】
１．機能構成
図１は、本実施形態におけるカーナビゲーション装置１の機能構成を示すブロック図で
ある。カーナビゲーション装置１は、ＧＰＳアンテナ１０と、ＧＰＳ受信部２０と、ホス
トＣＰＵ（Central Processing Unit）３０と、操作部４０と、表示部５０と、慣性航法
用センサ６０と、気圧センサ７０と、ＲＯＭ（Read Only Memory）８０と、ＲＡＭ（Rand
om Access Memory）９０とを備えて構成される。
【００２１】
ＧＰＳアンテナ１０は、ＧＰＳ衛星から発信されているＧＰＳ衛星信号を含むＲＦ（Ra
dio Frequency）信号を受信するアンテナであり、受信した信号をＧＰＳ受信部２０に出
力する。尚、ＧＰＳ衛星信号は、衛星毎に異なる拡散符号の一種であるＰＲＮ（Pseudo R
andom Noise）コードで直接スペクトラム拡散方式により変調された１．５７５４２［Ｇ
Ｈｚ］の通信信号である。ＰＲＮコードは、コード長１０２３チップを１ＰＮフレームと
する繰返し周期１ｍｓの擬似ランダム雑音符号である。
【００２２】
ＧＰＳ受信部２０は、ＧＰＳアンテナ１０から出力された信号に基づいてカーナビゲー
ション装置１の現在位置を測位する測位回路であり、いわゆるＧＰＳ受信機に相当する機
能ブロックである。ＧＰＳ受信部２０は、ＲＦ（Radio Frequency）受信回路部２１と、
ベースバンド処理回路部２３とを備えて構成される。尚、ＲＦ受信回路部２１と、ベース
バンド処理回路部２３とは、それぞれ別のＬＳＩ（Large Scale Integration）として製
造することも、１チップとして製造することも可能である。
【００２３】
ＲＦ受信回路部２１は、ＲＦ信号の処理回路ブロックであり、所定の局部発振信号を分
周或いは逓倍することで、ＲＦ信号乗算用の発振信号を生成する。そして、生成した発振
信号を、ＧＰＳアンテナ１０から出力されたＲＦ信号に乗算することで、ＲＦ信号を中間
周波数の信号（以下、「ＩＦ（Intermediate Frequency）信号」と称す。）にダウンコン
バートし、ＩＦ信号を増幅等した後、Ａ／Ｄ変換器でデジタル信号に変換して、ベースバ
ンド処理回路部２３に出力する。
【００２４】
ベースバンド処理回路部２３は、ＲＦ受信回路部２１から出力されたＩＦ信号に対して
相関処理等を行ってＧＰＳ衛星信号を捕捉・抽出し、データを復号して航法メッセージや
時刻情報等を取り出して測位演算を行う回路部である。ベースバンド処理回路部２３は、
プロセッサとしてのＣＰＵと、メモリとしてのＲＯＭ及びＲＡＭとを備えて構成される。
尚、測位演算としては、例えば最小二乗法を用いた測位演算等の公知の手法を適用するこ
とができる。
【００２５】
ホストＣＰＵ３０は、ＲＯＭ８０に記憶されているシステムプログラム等の各種プログ
ラムに従ってカーナビゲーション装置１の各部を統括的に制御するプロセッサである。特
に本実施形態では、ホストＣＰＵ３０は、測位プログラム８０３に従って、ナビゲーショ
ン画面に表示させる位置である出力位置を決定する。そして、ナビゲーションプログラム
８０１に従って、出力位置をプロットしたナビゲーション画面を生成して、表示部５０に
表示させる。尚、ナビゲーション画面に表示させる位置としては、出力位置を更にマップ
マッチングすることで、出力位置に最近接する道路上にプロットすることが好適である。
【００２６】
操作部４０は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であ
り、押下されたアイコンやボタンの信号をホストＣＰＵ３０に出力する。この操作部４０
の操作により、目的地の入力や、ナビゲーション画面の表示要求等の各種指示入力がなさ
れる。
【００２７】
表示部５０は、ＬＣＤ（Liquid Crystal Display）等により構成され、ホストＣＰＵ３
０から入力される表示信号に基づいた各種表示を行う表示装置である。表示部５０には、
ナビゲーション画面等が表示される。
【００２８】
慣性航法用センサ６０は、慣性航法演算処理に使用するセンサであり、カーナビ用ジャ
イロセンサ６１及びカーナビ用加速度センサ６３が一体化（パッケージ化）されて、カー
ナビゲーション装置１の本体に内蔵・固定されている。
【００２９】
カーナビ用ジャイロセンサ６１は、直交３軸それぞれの軸周りの角速度を検出すること
でカーナビゲーション装置１の回転を検出するセンサ（角速度センサ）であり、検出した
３軸の角速度をホストＣＰＵ３０に出力する。
【００３０】
カーナビ用加速度センサ６３は、直交３軸それぞれの軸方向の加速度を検出することで
カーナビゲーション装置１の移動状態を検出するセンサであり、検出した３軸の加速度を
ホストＣＰＵ３０に出力する。
【００３１】
気圧センサ７０は、カーナビゲーション装置１の外気圧を検出するセンサであり、検出
した外気圧をホストＣＰＵ３０に出力する。
【００３２】
ＲＯＭ８０は、読み出し専用の記憶装置であり、ホストＣＰＵ３０がカーナビゲーショ
ン装置１を制御するためのシステムプログラムや、ナビゲーション機能を実現するための
各種プログラムやデータ等を記憶している。
【００３３】
ＲＡＭ９０は、読み書き可能な記憶装置であり、ホストＣＰＵ３０により実行されるシ
ステムプログラム、各種処理プログラム、各種処理の処理中データ、処理結果などを一時
的に記憶するワークエリアを形成している。
【００３４】
また、カーナビゲーション装置１とは別に、自動車には加速度センサ１００が搭載され
ており、その検出結果がホストＣＰＵ３０に出力されるように構成されている。加速度セ
ンサ１００は、自動車の移動状態を検出する直交３軸のセンサであり、車体の揺れ制御等
のためのサスペンション制御や前輪及び後輪のトルク配分制御、ＡＢＳ（Anti-lock Brak
e System）制御等を行うために使用され、自動車の制御のために予め自動車に設置されて
いるものである。
【００３５】
ここで、本実施形態で使用する座標系等について説明する。図２に示すように、自動車
の進行方向を「Ｘb軸」（直進方向を正方向）、Ｘb軸正方向に向かって左右方向を「Ｙb
軸」（右方向を正方向）、Ｘb軸に対する高さ方向を「Ｚb軸」（下方向を正方向）とする
右手系直交座標系を「移動体座標系」と定義する。また、北方向を「Ｘn軸」、東方向を
「Ｙn軸」、地球による重力方向を「Ｚn軸」とする右手系直交座標系を「基準座標系」と
定義する。
【００３６】
また、自動車に対する慣性航法用センサ６０の傾斜及び傾斜角のことを、それぞれ「取
付傾斜」及び「取付傾斜角」と称する。また、地球に対する慣性航法用センサ６０の姿勢
及び姿勢角のことを、それぞれ「絶対姿勢」及び「絶対姿勢角」と称し、Ｘb軸周りの回
転角「φ」を「絶対ロール角」、Ｙb軸周りの回転角「θ」を「絶対ピッチ角」、Ｚb軸周
りの回転角「ψ」を「絶対ヨー角」とそれぞれ称する。さらに、カーナビ用ジャイロセン
サ６１の検出結果を積分することで得られる地球に対する慣性航法用センサ６０の絶対姿
勢及び絶対姿勢角のことを、それぞれ「推定絶対姿勢」及び「推定絶対姿勢角」と称する
。
【００３７】
自動車の傾斜及び慣性航法用センサ６０の傾斜に起因して、カーナビ用加速度センサ６
３により検出される加速度には重力加速度の成分が含まれるため、この重力成分を除去し
なければ、自動車の正確な加速度を求めることができない。そこで、本実施形態では、移
動体座標系から基準座標系への座標変換行列を、推定絶対姿勢に基づいて算出する。そし
て、算出した座標変換行列を用いて、カーナビ用加速度センサ６３の検出結果を基準座標
系に変換するとともに、重力加速度の成分を減算することで、地球座標系における自動車
の加速度を求める。
【００３８】
しかし、カーナビ用ジャイロセンサ６１により検出される角速度は誤差を含んでいるた
め、カーナビ用ジャイロセンサ６１の検出結果を積分することで得られる推定絶対姿勢は
、必ずしも絶対姿勢と一致するとは限らない。この問題を解決するため、本実施形態では
、自動車が走行している路面（以下、「走行路面」と称す。）の傾斜角及び取付傾斜角を
算出し、これらを基に慣性航法用センサ６０の絶対ピッチ角を算出する。そして、算出し
た絶対ピッチ角を観測値として用いたカルマンフィルタ処理（推定演算）を行うことで推
定絶対姿勢に含まれる誤差を推定して、推定絶対姿勢を補正する。
【００３９】
図３は、取付傾斜角算出の原理を説明するための図である。ここでは、取付傾斜角を「
α」、走行路面の傾斜角を「β」として説明する。自動車が停止している状態では、カー
ナビ用加速度センサ６３及び加速度センサ１００により検出される自動車の進行方向（Ｘ
b軸方向）に対する加速度は、重力加速度の進行方向に対する成分のみである。
【００４０】
具体的には、重力加速度を「ｇ」とすると、カーナビ用加速度センサ６３及び加速度セ
ンサ１００により検出されるＸb軸方向の加速度「ａx1」及び「ａx2」は、それぞれ次式
（１）及び（２）で与えられる。
【数１】
【数２】
【００４１】
式（１）及び（２）から「β」を消去することで、取付傾斜角「α」は、次式（３）の
ように算出される。
【数３】
【００４２】
２．データ構成
図４は、ＲＯＭ８０に格納されたデータの一例を示す図である。ＲＯＭ８０には、ホス
トＣＰＵ３０により読み出され、ナビゲーション処理として実行されるナビゲーションプ
ログラム８０１と、測位処理（図７参照）として実行される測位プログラム８０３と、ナ
ビゲーション画面を生成するための地図情報のデータである地図データ８０５とが記憶さ
れている。また、測位プログラム８０３には、推定絶対姿勢補正処理（図８参照）として
実行される推定絶対姿勢補正プログラム８０３１がサブルーチンとして含まれている。
【００４３】
ナビゲーション処理とは、ホストＣＰＵ３０が、地図データ８０５に記憶されている地
図情報を用いて、測位処理により決定した出力位置を道路上に補正するマップマッチング
処理を行うとともに、補正後の位置をプロットしたナビゲーション画面を生成して、表示
部５０に表示させる処理である。尚、マップマッチング処理については公知であるため、
詳細な説明を省略する。
【００４４】
測位処理とは、ホストＣＰＵ３０が、ＧＰＳによる測位が可能である場合は、ＧＰＳ測
位処理を行って現在位置を測位し、ＧＰＳによる測位が不可能である場合は、慣性航法演
算処理を行って現在位置を測位する処理である。測位処理については、フローチャートを
用いて詳細に後述する。
【００４５】
推定絶対姿勢補正処理とは、ホストＣＰＵ３０が、取付傾斜角を考慮したカルマンフィ
ルタ処理を行って推定絶対姿勢に含まれる誤差を推定して、推定絶対姿勢を補正する処理
である。取付傾斜角は、自動車の停止時に、カーナビ用加速度センサ６３及び加速度セン
サ１００の検出結果を用いて算出する。推定絶対姿勢補正処理についても、フローチャー
トを用いて詳細に後述する。
【００４６】
図５は、ＲＡＭ９０に格納されるデータの一例を示す図である。ＲＡＭ９０には、計測
履歴データ９０１と、補正用絶対姿勢角データ９０３と、取付傾斜角データ９０５とが記
憶される。
【００４７】
図６は、計測履歴データ９０１のデータ構成の一例を示す図である。計測履歴データ９
０１には、各時刻９０１１毎に、カーナビ用ジャイロセンサ６１により検出された３軸の
角速度９０１２と、カーナビ用加速度センサ６３により検出された３軸の加速度９０１３
と、気圧センサ７０により検出された外気圧９０１４と、角速度９０１２を積分すること
で得られる推定絶対姿勢角９０１５と、ＧＰＳ測位処理又は慣性航法演算処理により求め
られた速度９０１７と、ＧＰＳ測位処理により求められたＧＰＳ測位位置９０１８と、慣
性航法演算処理により求められた慣性航法演算位置９０１９とが対応付けて記憶される。
計測履歴データ９０１は、測位処理においてホストＣＰＵ３０により更新される。
【００４８】
補正用絶対姿勢角データ９０３は、カーナビ用加速度センサ６３の検出結果と重力加速
度とに基づいて算出される絶対姿勢角のデータであり、推定絶対姿勢を補正するために使
用される。補正用絶対姿勢角データ９０３は、推定絶対姿勢補正処理においてホストＣＰ
Ｕ３０により更新される。
【００４９】
取付傾斜角データ９０５は、取付傾斜角「α」のデータであり、推定絶対姿勢補正処理
においてホストＣＰＵ３０により更新される。
【００５０】
３．処理の流れ
図７は、ホストＣＰＵ３０によりＲＯＭ８０に記憶されている測位プログラム８０３が
読み出されて実行されることで、カーナビゲーション装置１において実行される測位処理
の流れを示すフローチャートである。
【００５１】
測位処理は、ホストＣＰＵ３０が、操作部４０に測位開始指示の操作がなされたことを
検出した場合に実行を開始する処理である。尚、カーナビゲーション装置１の電源のＯＮ
／ＯＦＦとＧＰＳの起動／停止とを連動させ、カーナビゲーション装置１の電源投入操作
を検出した場合に測位処理の実行を開始させることにしてもよい。
【００５２】
また、特に説明しないが、以下の測位処理の実行中は、ＧＰＳアンテナ１０によるＲＦ
信号の受信や、ＲＦ受信回路部２１によるＩＦ信号へのダウンコンバート、ベースバンド
処理回路部２３によるＧＰＳ衛星信号の捕捉・追尾等が随時行われている状態にあるもの
とする。また、カーナビ用ジャイロセンサ６１、カーナビ用加速度センサ６３及び気圧セ
ンサ７０の検出結果に従って、ホストＣＰＵ３０により、ＲＡＭ９０の計測履歴データ９
０１は随時更新されるものとする。
【００５３】
先ず、ホストＣＰＵ３０は、ＧＰＳによる測位が可能であるか否かを判定する（ステッ
プＡ１）。具体的には、３次元測位（高度を含む測位）の場合は、ベースバンド処理回路
部２３が現在捕捉しているＧＰＳ衛星（以下、「捕捉衛星」と称す。）の数が「４個以上
」である場合に、ＧＰＳによる測位が可能であると判定する。また、２次元測位（高度を
含まない測位）の場合は、捕捉衛星の数が「３個以上」である場合に、ＧＰＳによる測位
が可能であると判定する。
【００５４】
そして、ＧＰＳによる測位が可能であると判定した場合は（ステップＡ１；Ｙｅｓ）、
ホストＣＰＵ３０は、ＧＰＳ測位処理を行う（ステップＡ３）。具体的には、ホストＣＰ
Ｕ３０は、ベースバンド処理回路部２３のＣＰＵに、捕捉衛星の衛星情報及び時刻情報を
基に、例えば最小二乗法を用いた測位演算を実行させて、カーナビゲーション装置１の速
度及び位置を算出する。そして、算出した速度及び位置（ＧＰＳ測位位置）を、ＲＡＭ９
０の計測履歴データ９０１に記憶させる。
【００５５】
その後、ホストＣＰＵ３０は、ＧＰＳ測位処理で求めたＧＰＳ測位位置を出力位置に決
定して（ステップＡ５）、ステップＡ１７へと処理を移行する。
【００５６】
一方、ステップＡ１においてＧＰＳによる測位が不可能であると判定した場合は（ステ
ップＡ１；Ｎｏ）、ホストＣＰＵ３０は、ＲＡＭ９０の計測履歴データ９０１に記憶され
ている最新の推定絶対姿勢角を用いて、移動体座標系から基準座標系への座標変換行列「
Ｃnb」を算出する（ステップＡ７）。座標変換行列は「Ｃnb」は、絶対ピッチ角「θ」、
絶対ロール角「φ」及び絶対ヨー角「ψ」を変数とする三角関数を成分として含む３×３
の公知の行列である。
【００５７】
そして、ホストＣＰＵ３０は、ステップＡ７で算出した座標変換行列「Ｃnb」と、カー
ナビ用加速度センサ６３の検出結果とを用いて、次式（４）に従って基準座標系における
自動車の加速度を算出する（ステップＡ９）。
【数４】
但し、ベクトル表記の「ａn」、「ａb」、「ｇn」は、それぞれ基準座標系における自
動車の加速度ベクトル、移動体座標系における自動車の加速度ベクトル、基準座標系にお
ける重力加速度ベクトルをそれぞれ示している。
【００５８】
その後、ホストＣＰＵ３０は、ステップＡ９で算出した加速度を基に自動車の速度（単
なる速度ではなく方向を含んだ速度であるため、移動ベクトルと言える。）を算出し（ス
テップＡ１１）、ＲＡＭ９０の計測履歴データ９０１に記憶させる。そして、算出した速
度と、前回（１時刻前）の出力位置とを用いて自動車の位置を算出して慣性航法演算位置
とし（ステップＡ１３）、ＲＡＭ９０の計測履歴データ９０１に記憶させる。そして、算
出した慣性航法演算位置を出力位置に決定する（ステップＡ１５）。
【００５９】
次いで、ホストＣＰＵ３０は、カーナビ用ジャイロセンサ６１の検出結果を積分するこ
とにより推定絶対姿勢角を算出し（ステップＡ１７）、ＲＡＭ９０の計測履歴データ９０
１に記憶させる。そして、ＲＯＭ８０に記憶されている推定絶対姿勢補正プログラム８０
３１を読み出して実行することで、ステップＡ１７で算出した推定絶対姿勢角を補正する
推定絶対姿勢補正処理を行う（ステップＡ１９）。
【００６０】
推定絶対姿勢補正処理を行った後、ホストＣＰＵ３０は、処理を終了するか否かを判定
し（ステップＡ２１）、まだ終了しないと判定した場合は（ステップＡ２１；Ｎｏ）、ス
テップＡ１に戻る。また、終了すると判定した場合は（ステップＡ２１；Ｙｅｓ）、測位
処理を終了する。
【００６１】
図８は、推定絶対姿勢補正処理の流れを示すフローチャートである。
先ず、ホストＣＰＵ３０は、自動車が停止中又は等速走行中であるか否かを判定する（
ステップＢ１）。自動車が停止しているか否かの判定は、例えばカーナビ用ジャイロセン
サ６１の出力値である角速度の分散値が所定の閾値以下であるか否かを判定することで行
う。また、自動車が等速走行中であるか否かを判定は、例えばカーナビ用加速度センサ６
３の出力値である加速度の分散値が所定の閾値以下であるか否かを判定することで行う。
【００６２】
ステップＢ１において自動車が停止中又は等速走行中であると判定した場合は（ステッ
プＢ１；Ｙｅｓ）、ホストＣＰＵ３０は、自動車が停止中であるか否かを判定する（ステ
ップＢ３）。そして、停止中ではない、すなわち等速走行中であると判定した場合は（ス
テップＢ３；Ｎｏ）、ステップＢ７へと処理を移行する。
【００６３】
また、自動車が停止中であると判定した場合は（ステップＢ３；Ｙｅｓ）、ホストＣＰ
Ｕ３０は、カーナビ用加速度センサ６３により検出されたＸb軸方向の加速度「ａx1」と
、加速度センサ１００により検出されたＸb軸方向の加速度「ａx2」とを用いて、式（３
）に従って取付傾斜角「α」を算出する（ステップＢ５）。そして、算出した取付傾斜角
「α」で、ＲＡＭ９０の取付傾斜角データ９０５を更新する。
【００６４】
次いで、ホストＣＰＵ３０は、カーナビ用加速度センサ６３の検出結果を用いて、慣性
航法用センサ６０の絶対ピッチ角「θ」及び絶対ロール角「φ」を算出し（ステップＢ７
）、ＲＡＭ９０の補正用絶対姿勢角データ９０３に記憶させる。
【００６５】
詳細に説明すると、カーナビ用加速度センサ６３により検出された移動体座標系におけ
る自動車の加速度ベクトルをベクトル表記の「ａb」、基準座標系から移動体座標系への
座標変換行列を「Ｃbn」、基準座標系における重力加速度ベクトルをベクトル表記の「ｇ
n」とすると、次式（５）が成立する。
【数５】
【００６６】
式（５）を３次元の各成分「（ａx，ａy，ａz）」について展開すると、次式（６−１
）〜（６−３）のようになる。
【数６】
【００６７】
そして、式（６−１）〜（６−３）から、絶対ピッチ角「θ」及び絶対ロール角「φ」
は、次式（７−１）及び（７−２）のように求められる。
【数７】
【００６８】
次いで、ホストＣＰＵ３０は、ステップＢ７で算出した絶対ピッチ角「θ」及び絶対ロ
ール角「φ」を観測値とするカルマンフィルタ処理を行って、ステップＡ１７で算出して
計測履歴データ９０１に記憶させた推定絶対姿勢角を補正する（ステップＢ９）。具体的
には、推定絶対姿勢角に含まれる誤差を状態ベクトルとし、絶対ピッチ角「θ」及び絶対
ロール角「φ」を外部観測量として、カルマンフィルタの理論に基づく推定演算を行う。
そして、推定された誤差を、カーナビ用ジャイロセンサ６１の検出結果を基に算出した推
定絶対姿勢角に加算することで、推定絶対姿勢角を補正する。
【００６９】
一方、ステップＢ１において自動車が停止中でも等速走行中でもないと判定した場合は
（ステップＢ１；Ｎｏ）、ホストＣＰＵ３０は、今回の測位タイミングがＧＰＳ測位であ
るか否かを判定する（ステップＢ１１）。そして、今回の測位タイミングがＧＰＳ測位で
あると判定した場合は（ステップＢ１１；Ｙｅｓ）、前回の測位タイミングもＧＰＳ測位
であったか否かを判定する（ステップＢ１３）。
【００７０】
そして、前回の測位タイミングもＧＰＳ測位であったと判定した場合は（ステップＢ１
３；Ｙｅｓ）、ホストＣＰＵ３０は、ＲＡＭ９０の計測履歴データ９０１に記憶されてい
る前回のＧＰＳ測位位置と今回のＧＰＳ測位位置とから、前回の測位タイミングと今回の
測位タイミング間における自動車の水平方向の移動距離を算出する（ステップＢ１５）。
また、ホストＣＰＵ３０は、気圧センサ７０の検出結果に基づいて、前回の測位タイミン
グと今回の測位タイミング間における自動車の高度方向の移動距離を算出する（ステップ
Ｂ１７）。
【００７１】
次いで、ホストＣＰＵ３０は、ステップＢ１５で算出した水平方向の移動距離と、ステ
ップＢ１７で算出した高度方向の移動距離とを用いて、走行路面の傾斜角「β」を算出す
る（ステップＢ１９）。そして、ＲＡＭ９０の取付傾斜角データ９０５に記憶されている
取付傾斜角「α」を加味することで絶対ピッチ角「θ」を算出し（ステップＢ２１）、Ｒ
ＡＭ９０の補正用絶対姿勢角データ９０３を更新する。
【００７２】
そして、ホストＣＰＵ３０は、ステップＢ２１で算出した絶対ピッチ角「θ」を観測値
とするカルマンフィルタ処理を行って、ステップＡ１７で算出して計測履歴データ９０１
に記憶させた推定絶対姿勢角を補正する（ステップＢ２３）。具体的には、推定絶対姿勢
角に含まれる誤差を状態ベクトルとし、絶対ピッチ角「θ」を外部観測量として、カルマ
ンフィルタの理論に基づく推定演算を行う。そして、推定された誤差を、カーナビ用ジャ
イロセンサ６１の検出結果を基に算出した推定絶対姿勢角に加算することで、推定絶対姿
勢角を補正する。
【００７３】
ステップＢ９又はＢ２３において推定絶対姿勢角の補正を行った後、ホストＣＰＵ３０
は、推定絶対姿勢補正処理を終了する。また、ステップＢ１１において今回の測位タイミ
ングがＧＰＳ測位ではないと判定した場合（ステップＢ１１；Ｎｏ）、又は、ステップＢ
１３において前回の測位タイミングがＧＰＳ測位ではなかったと判定した場合にも（ステ
ップＢ１３；Ｎｏ）、ホストＣＰＵ３０は、推定絶対姿勢補正処理を終了する。
【００７４】
３．作用効果
本実施形態によれば、カーナビゲーション装置１において、カーナビ用ジャイロセンサ
６１の検出結果を積分することで、地球に対する慣性航法用センサ６０の絶対姿勢の推定
値である推定絶対姿勢が算出される。そして、移動体座標系と基準座標系との座標変換行
列が推定絶対姿勢に基づいて算出され、当該座標変換行列を用いて、カーナビ用加速度セ
ンサ６３の検出結果が基準座標系に変換されるとともに、重力加速度の成分が減算される
ことで、基準座標系における自動車の移動ベクトルが算出される。そして、この移動ベク
トルを用いて現在位置が測位される。
【００７５】
カーナビ用加速度センサ６３により検出される加速度は、自動車自身を基準とする移動
体座標系における加速度であるため、重力加速度の影響を補正するためには、基準座標系
における加速度に変換する必要がある。そこで、本実施形態では、慣性航法用センサ６０
の推定絶対姿勢を基に座標変換行列を算出し、この座標変換行列を用いて、カーナビ用加
速度センサ６１により検出された加速度を基準座標系における加速度に変換するとともに
、この変換後の加速度から重力加速度を減算することにしている。これにより、地球の重
力に起因する成分が補正された加速度を得ることが可能となり、その結果、より正確な現
在位置の測位を実現し得る。
【００７６】
４．変形例
４−１．電子機器
本発明は、測位装置を備えた電子機器であれば何れの電子機器にも適用可能である。例
えば、ノート型パソコンやＰＤＡ（Personal Digital Assistant）等についても同様に適
用可能である。
【００７７】
４−２．移動体
また、移動体は必ずしも自動車に限られるわけではなく、バスや電車等の移動体につい
ても同様に適用可能である。
【００７８】
４−３．衛星測位システム
上述した実施形態では、衛星測位システムとしてＧＰＳを例に挙げて説明したが、ＷＡ
ＡＳ（Wide Area Augmentation System）、ＱＺＳＳ（Quasi Zenith Satellite System）
、ＧＬＯＮＡＳＳ（GLObal NAvigation Satellite System）、ＧＡＬＩＬＥＯ等の他の衛
星測位システムであってもよい。
【００７９】
４−４．処理の分化
ホストＣＰＵ３０が行う処理の一部又は全部を、ベースバンド処理回路部２３のＣＰＵ
が行うことにしてもよい。具体的には、例えばベースバンド処理回路部２３のＣＰＵが測
位処理を行う。そして、ホストＣＰＵ３０が、測位処理により求められた出力位置に対し
てマップマッチング処理等を行ってナビゲーション画面を生成し、生成したナビゲーショ
ン画面を表示部５０に表示させるナビゲーション処理を行う。また、ナビゲーション処理
も含めてホストＣＰＵ３０が行う処理の全部をＣＰＵが行うこととしてもよい。
【００８０】
４−５．走行路面の傾斜角の算出
上述した実施形態では、ＧＰＳの測位結果から求められる自動車の水平方向の移動距離
と、気圧センサ７０の検出結果から求められる自動車の高度方向の移動距離とに基づいて
、走行路面の傾斜角「β」を算出するものとして説明したが、気圧センサ７０の検出結果
を用いずに、ＧＰＳの測位結果のみから走行路面の傾斜角「β」を算出することとしても
よい。
【００８１】
すなわち、３次元測位では、自動車の緯度、経度及び高度を求めることができるため、
緯度及び経度の変化から求められる水平方向の移動距離と、高度の変化から求められる高
度方向の移動距離とを用いることで、走行路面の傾斜角「β」を算出することができる。
この場合は、カーナビゲーション装置１に気圧センサ７０を設ける必要はない。
【００８２】
また、上述した実施形態では、１時刻間の移動距離に基づいて路面の傾斜角「β」を算
出するものとして説明したが、これを所定期間（例えば１０時刻分）の移動距離に基づい
て算出することとしてもよいことは勿論である。
【００８３】
４−６．推定絶対姿勢の補正
上述した実施形態では、カルマンフィルタ処理を行って推定絶対姿勢角に含まれる誤差
を推定することで、推定絶対姿勢の補正を行うものとして説明したが、次のようにしても
よい。すなわち、図８の推定絶対姿勢補正処理のステップＢ９において、カルマンフィル
タ処理を行わずに、推定絶対姿勢角に含まれる絶対ピッチ角及び絶対ロール角を、ステッ
プＢ７で算出した絶対ピッチ角「θ」及び絶対ロール角「φ」にそれぞれ置き換える処理
を行う。また、ステップＢ２３においても同様にカルマンフィルタ処理を行わず、推定絶
対姿勢角に含まれる絶対ピッチ角を、ステップＢ２１で算出した絶対ピッチ角「θ」に置
き換える処理を行う。
【図面の簡単な説明】
【００８４】
【図１】カーナビゲーション装置の機能構成を示すブロック図。
【図２】移動体座標系の一例を示す図。
【図３】取付傾斜角算出の原理の説明図。
【図４】ＲＯＭに格納されたデータの一例を示す図。
【図５】ＲＡＭに格納されたデータの一例を示す図。
【図６】計測履歴データのデータ構成の一例を示す図。
【図７】測位処理の流れを示すフローチャート。
【図８】推定絶対姿勢補正処理の流れを示すフローチャート。
【符号の説明】
【００８５】
１ カーナビゲーション装置、 １０ ＧＰＳアンテナ、 ２０ ＧＰＳ受信部、
２１ ＲＦ受信回路部、 ２３ ベースバンド処理回路部、 ３０ ホストＣＰＵ、
４０ 操作部、 ５０ 表示部、 ６０ 慣性航法用センサ、
６１ カーナビ用ジャイロセンサ、 ６３ カーナビ用加速度センサ、
７０ 気圧センサ、 ８０ ＲＯＭ、 ９０ ＲＡＭ、 １００ 加速度センサ
【特許請求の範囲】
【請求項１】
ジャイロセンサ及び加速度センサが一体的に構成された慣性航法用センサが固定された
移動体の現在位置を、前記慣性航法用センサの検出結果を用いて測位する測位方法であっ
て、
前記ジャイロセンサの検出結果を積分することで、地球に対する前記慣性航法用センサ
の絶対姿勢の推定値である推定絶対姿勢を算出することと、
前記移動体自身を基準とした座標系である移動体座標系と基準座標系との座標変換行列
を、前記推定絶対姿勢に基づいて算出することと、
前記座標変換行列を用いて、前記加速度センサの検出結果を前記基準座標系に変換する
とともに重力加速度の成分を減算することで前記基準座標系における前記移動体の移動ベ
クトルを算出することと、
前記移動ベクトルを用いて現在位置を測位することと、
を含む測位方法。
【請求項２】
前記移動体が停止中又は等速走行中であることを検出することと、
前記移動体が停止中又は等速走行中であることが検出された場合に、前記加速度センサ
の検出結果に表れる重力加速度の成分に基づいて、地球に対する前記慣性航法用センサの
絶対姿勢を算出することと、
前記算出された絶対姿勢で前記推定絶対姿勢を補正することと、
を含む請求項１に記載の測位方法。
【請求項３】
測位用衛星から発信されている測位用信号に基づく所定の測位演算を行って現在位置を
断続的に測位することと、
前記測位演算の測位結果から所定期間における前記移動体の水平方向の移動距離を算出
することと、
外気圧を検出する気圧センサの検出結果に基づいて、前記所定期間と同一の期間におけ
る前記移動体の高度方向の移動距離を算出することと、
前記水平方向の移動距離と前記高度方向の移動距離とに基づいて、前記移動体が位置す
る路面の傾斜角を算出することと、
前記加速度センサにより検出された加速度と、前記移動体の姿勢と同一の姿勢になるよ
うに前記移動体に設置された第２の加速度センサにより検出された加速度と、重力加速度
とを用いて、前記慣性航法用センサの前記移動体に対する取付傾斜角を算出することと、
前記路面の傾斜角と前記慣性航法用センサの前記移動体に対する取付傾斜角とを用いて
、前記慣性航法用センサのピッチ角を算出することと、
前記慣性航法用センサのピッチ角を用いた所定の推定演算を行って前記推定絶対姿勢に
含まれる誤差を推定して前記推定絶対姿勢を補正することと、
を含む請求項１又は２に記載の測位方法。
【請求項４】
ジャイロセンサ及び加速度センサが一体的に構成された慣性航法用センサが固定された
移動体の現在位置を、前記慣性航法用センサの検出結果を用いてコンピュータに測位計算
させるためのプログラムであって、
前記ジャイロセンサの検出結果を積分することで、地球に対する前記慣性航法用センサ
の絶対姿勢の推定値である推定絶対姿勢を算出することと、
前記移動体自身を基準とした座標系である移動体座標系と基準座標系との座標変換行列
を、前記推定絶対姿勢に基づいて算出することと、
前記座標変換行列を用いて、前記加速度センサの検出結果を前記基準座標系に変換する
とともに重力加速度の成分を減算することで前記基準座標系における前記移動体の移動ベ
クトルを算出することと、
前記移動ベクトルを用いて現在位置を測位することと、
を前記コンピュータに実行させるためのプログラム。
【請求項５】
ジャイロセンサ及び加速度センサが一体的に構成された慣性航法用センサが固定された
移動体の現在位置を、前記慣性航法用センサの検出結果を用いて測位する測位装置であっ
て、
前記ジャイロセンサの検出結果を積分することで、地球に対する前記慣性航法用センサ
の絶対姿勢の推定値である推定絶対姿勢を算出する絶対姿勢推定部と、
前記移動体自身を基準とした座標系である移動体座標系と基準座標系との座標変換行列
を、前記推定絶対姿勢に基づいて算出する行列算出部と、
前記座標変換行列を用いて、前記加速度センサの検出結果を前記基準座標系に変換する
とともに重力加速度の成分を減算することで前記基準座標系における前記移動体の移動ベ
クトルを算出する移動ベクトル算出部と、
前記移動ベクトルを用いて現在位置を測位する測位部と、
を備えた測位装置。
【請求項１】
ジャイロセンサ及び加速度センサが一体的に構成された慣性航法用センサが固定された
移動体の現在位置を、前記慣性航法用センサの検出結果を用いて測位する測位方法であっ
て、
前記ジャイロセンサの検出結果を積分することで、地球に対する前記慣性航法用センサ
の絶対姿勢の推定値である推定絶対姿勢を算出することと、
前記移動体自身を基準とした座標系である移動体座標系と基準座標系との座標変換行列
を、前記推定絶対姿勢に基づいて算出することと、
前記座標変換行列を用いて、前記加速度センサの検出結果を前記基準座標系に変換する
とともに重力加速度の成分を減算することで前記基準座標系における前記移動体の移動ベ
クトルを算出することと、
前記移動ベクトルを用いて現在位置を測位することと、
を含む測位方法。
【請求項２】
前記移動体が停止中又は等速走行中であることを検出することと、
前記移動体が停止中又は等速走行中であることが検出された場合に、前記加速度センサ
の検出結果に表れる重力加速度の成分に基づいて、地球に対する前記慣性航法用センサの
絶対姿勢を算出することと、
前記算出された絶対姿勢で前記推定絶対姿勢を補正することと、
を含む請求項１に記載の測位方法。
【請求項３】
測位用衛星から発信されている測位用信号に基づく所定の測位演算を行って現在位置を
断続的に測位することと、
前記測位演算の測位結果から所定期間における前記移動体の水平方向の移動距離を算出
することと、
外気圧を検出する気圧センサの検出結果に基づいて、前記所定期間と同一の期間におけ
る前記移動体の高度方向の移動距離を算出することと、
前記水平方向の移動距離と前記高度方向の移動距離とに基づいて、前記移動体が位置す
る路面の傾斜角を算出することと、
前記加速度センサにより検出された加速度と、前記移動体の姿勢と同一の姿勢になるよ
うに前記移動体に設置された第２の加速度センサにより検出された加速度と、重力加速度
とを用いて、前記慣性航法用センサの前記移動体に対する取付傾斜角を算出することと、
前記路面の傾斜角と前記慣性航法用センサの前記移動体に対する取付傾斜角とを用いて
、前記慣性航法用センサのピッチ角を算出することと、
前記慣性航法用センサのピッチ角を用いた所定の推定演算を行って前記推定絶対姿勢に
含まれる誤差を推定して前記推定絶対姿勢を補正することと、
を含む請求項１又は２に記載の測位方法。
【請求項４】
ジャイロセンサ及び加速度センサが一体的に構成された慣性航法用センサが固定された
移動体の現在位置を、前記慣性航法用センサの検出結果を用いてコンピュータに測位計算
させるためのプログラムであって、
前記ジャイロセンサの検出結果を積分することで、地球に対する前記慣性航法用センサ
の絶対姿勢の推定値である推定絶対姿勢を算出することと、
前記移動体自身を基準とした座標系である移動体座標系と基準座標系との座標変換行列
を、前記推定絶対姿勢に基づいて算出することと、
前記座標変換行列を用いて、前記加速度センサの検出結果を前記基準座標系に変換する
とともに重力加速度の成分を減算することで前記基準座標系における前記移動体の移動ベ
クトルを算出することと、
前記移動ベクトルを用いて現在位置を測位することと、
を前記コンピュータに実行させるためのプログラム。
【請求項５】
ジャイロセンサ及び加速度センサが一体的に構成された慣性航法用センサが固定された
移動体の現在位置を、前記慣性航法用センサの検出結果を用いて測位する測位装置であっ
て、
前記ジャイロセンサの検出結果を積分することで、地球に対する前記慣性航法用センサ
の絶対姿勢の推定値である推定絶対姿勢を算出する絶対姿勢推定部と、
前記移動体自身を基準とした座標系である移動体座標系と基準座標系との座標変換行列
を、前記推定絶対姿勢に基づいて算出する行列算出部と、
前記座標変換行列を用いて、前記加速度センサの検出結果を前記基準座標系に変換する
とともに重力加速度の成分を減算することで前記基準座標系における前記移動体の移動ベ
クトルを算出する移動ベクトル算出部と、
前記移動ベクトルを用いて現在位置を測位する測位部と、
を備えた測位装置。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【公開番号】特開２００９−２３６５３２（Ｐ２００９−２３６５３２Ａ）
【公開日】平成２１年１０月１５日（２００９．１０．１５）
【国際特許分類】
【出願番号】特願２００８−７９９６９（Ｐ２００８−７９９６９）
【出願日】平成２０年３月２６日（２００８．３．２６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２１年１０月１５日（２００９．１０．１５）
【国際特許分類】
【出願日】平成２０年３月２６日（２００８．３．２６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]