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

位置算出方法及び位置算出装置 Download PDF

Info

Publication number
JP2010117148A
JP2010117148A JP2008288529A JP2008288529A JP2010117148A JP 2010117148 A JP2010117148 A JP 2010117148A JP 2008288529 A JP2008288529 A JP 2008288529A JP 2008288529 A JP2008288529 A JP 2008288529A JP 2010117148 A JP2010117148 A JP 2010117148A
Authority
JP
Japan
Prior art keywords
value
moving
sensor
state
moving body
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Withdrawn
Application number
JP2008288529A
Other languages
English (en)
Inventor
Shunichi Mizuochi
俊一 水落
Shigeru Imafuku
茂 今福
Kenji Onda
健至 恩田
Kumar Anand
アナン クマー
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Seiko Epson Corp
Original Assignee
Seiko Epson Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Seiko Epson Corp filed Critical Seiko Epson Corp
Priority to JP2008288529A priority Critical patent/JP2010117148A/ja
Priority to US12/613,154 priority patent/US8134496B2/en
Publication of JP2010117148A publication Critical patent/JP2010117148A/ja
Withdrawn legal-status Critical Current

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01SRADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42Determining position
    • G01S19/48Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)
  • Traffic Control Systems (AREA)
  • Mobile Radio Communication Systems (AREA)

Abstract

【課題】カルマンフィルタ処理を用いた位置算出の正確性を向上させること。
【解決手段】GPS衛星からのGPS衛星信号に基づいて、最小二乗法を用いた位置算出処理を行って移動体の位置を算出する。そして、加速度センサ71、ジャイロセンサ73及び方位センサ75を含むセンサ部70の検出結果に基づいて移動体の移動状態を判定し、判定した移動状態に応じて、カルマンフィルタを用いた位置算出処理で用いられる誤差パラメータ(観測誤差共分散行列「R」)及び時変パラメータ(システムノイズ行列「Q」)の値を変更する。そして、算出した位置を、カルマンフィルタ位置算出処理を行って補正する。
【選択図】図1

Description

本発明は、位置算出方法及び位置算出装置に関する。
測位用信号を利用した測位システムとしては、GPS(Global Positioning System)が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。GPSでは、複数のGPS衛星の位置や各GPS衛星から自機までの擬似距離等の情報に基づいて自機の位置を示す3次元の座標値と時計誤差とを求める位置算出演算を行う。
位置算出演算としては、最小二乗法を用いた位置算出演算や、カルマンフィルタを用いた位置算出演算が知られている。このカルマンフィルタに関連する技術として、例えば特許文献1には、速度センサ及び方位センサにより検出された位置算出装置の移動速度及び方位をカルマンフィルタの入力として用いて、位置算出装置の状態を推定する技術が開示されている。
米国特許第4680715号公報
従来のカルマンフィルタを用いた位置算出演算では、固定的なパラメータ値を用いて計算を行うことが一般的であった。しかし、携帯型電話機やカーナビゲーション装置といった電子機器を想定した場合、携帯型電話機を所持したユーザやカーナビゲーション装置を搭載した自動車は移動するため、位置算出装置の移動状態も、停止したり、旋回したり、加速したり、減速したりと常に変化する。
そのため、固定的なパラメータ値を用いて位置算出演算を行ったのでは、位置算出演算により得られた算出位置が実際の位置に対して時間的に遅れる“位置遅れ”や、逆に算出位置が時間的に進む“位置進み”、算出位置が実際の位置から大きくずれる“位置飛び”といった現象が発生し、位置算出の正確性が低下するという問題があった。
本発明は、上述した課題に鑑みて為されたものであり、カルマンフィルタ処理を用いた位置算出の正確性を向上させることを目的としている。
以上の課題を解決するための第1の発明は、測位用衛星からの測位用信号に基づいて移動体の位置を算出することと、加速度センサ、角速度センサ及び方位センサのうちの少なくとも1つを含むセンサ部の検出結果に基づいて前記移動体の移動状態を判定することと、前記判定された移動状態に応じて、所定のカルマンフィルタ処理で用いられる誤差パラメータの値を変更することと、前記算出された位置を前記カルマンフィルタ処理で補正することと、を含む位置算出方法である。
また、他の発明として、測位用衛星からの測位用信号に基づいて移動体の位置を算出する算出部と、加速度センサ、角速度センサ及び方位センサのうちの少なくとも1つを含むセンサ部の検出結果に基づいて前記移動体の移動状態を判定する判定部と、前記判定部により判定された移動状態に応じて、所定のカルマンフィルタ処理で用いられる誤差パラメータの値を変更するパラメータ値変更部と、前記算出部により算出された位置を前記カルマンフィルタ処理で補正するカルマンフィルタ処理部と、を備える位置算出装置を構成してもよい。
この第1の発明等によれば、測位用衛星からの測位用信号に基づいて移動体の位置を算出する。一方、センサ部の検出結果に基づいて移動体の移動状態を判定し、判定した移動状態に応じて、カルマンフィルタ処理で用いられる誤差パラメータの値を変更する。そして、算出した位置をカルマンフィルタ処理で補正する。
加速度センサ、角速度センサ、方位センサといった各種センサの検出結果に基づいて移動体の移動状態を判定し、判定した移動状態に応じて誤差パラメータの値を変更してカルマンフィルタ処理を行うことで、移動体の移動状態に応じた適切な位置算出を実現することが可能となり、位置算出の正確性が向上する。
また、第2の発明として、第1の発明の位置算出方法であって、前記センサ部には、少なくとも加速度センサが含まれ、前記移動状態の判定は、前記移動体が停止状態にあるか否かを判定することを含み、前記測位用衛星からの測位用信号に基づいて前記移動体の移動速度を算出することを更に含み、前記誤差パラメータには、前記算出された移動速度に対する誤差の大きさを表す速度誤差パラメータが含まれ、前記誤差パラメータの値の変更は、前記移動体が停止状態にあると判定されたか否かに応じて前記速度誤差パラメータの値を変更することを含む位置算出方法を構成してもよい。
この第2の発明によれば、センサ部には、少なくとも加速度センサが含まれ、移動体が停止状態にあるか否かを判定する。そして、測位用衛星からの測位用信号に基づいて移動体の移動速度を算出し、移動体が停止状態にあると判定されたか否かに応じて、移動速度に対する誤差の大きさを表す速度誤差パラメータの値を変更する。尚、移動体が停止状態にあると判定した場合は、測位用信号に基づいて算出した移動体の移動速度に対する誤差は小さいものと推定して、停止状態にないと判定した場合に比べて速度誤差パラメータの値を小さな値に変更すればより好適である。
また、第3の発明として、第1の発明の位置算出方法であって、前記センサ部には、少なくとも加速度センサが含まれ、前記移動状態の判定は、前記移動体が停止状態にあるか否かを判定することを含み、前記測位用衛星からの測位用信号に基づいて前記移動体の移動速度を算出することを更に含み、前記誤差パラメータには、前記算出された移動速度の時間変化の許容度を表す速度時変パラメータが含まれ、前記誤差パラメータの値の変更は、前記移動体が停止状態にあると判定されたか否かに応じて前記速度時変パラメータの値を変更することを含む位置算出方法を構成してもよい。
この第3の発明によれば、センサ部には、少なくとも加速度センサが含まれ、移動体が停止状態にあるか否かを判定する。そして、測位用衛星からの測位用信号に基づいて移動体の移動速度を算出し、移動体が停止状態にあると判定されたか否かに応じて、算出された移動速度の時間変化の許容度を表す速度時変パラメータの値を変更する。尚、移動体が停止状態にあると判定した場合は、移動体の移動速度の時間変化を大きくするために、停止状態にないと判定した場合に比べて速度時変パラメータの値を大きな値に変更すればより好適である。
また、第4の発明として、第1〜第3の何れかの発明の位置算出方法であって、前記センサ部には、少なくとも角速度センサ及び方位センサが含まれ、前記移動状態の判定は、前記移動体が旋回状態にあるか否かを判定することを含み、前記誤差パラメータには、前記方位センサの検出結果に対する誤差の大きさを表す方位誤差パラメータが含まれ、前記誤差パラメータの値の変更は、前記移動体が旋回状態にあると判定されたか否かに応じて前記方位誤差パラメータの値を変更することを含む位置算出方法を構成してもよい。
この第4の発明によれば、センサ部には、少なくとも角速度センサ及び方位センサが含まれ、移動体が旋回状態にあるか否かを判定する。そして、移動体が旋回状態にあると判定されたか否かに応じて、方位センサの検出結果に対する誤差の大きさを表す方位誤差パラメータの値を変更する。尚、移動体が旋回状態にあると判定した場合は、方位センサにより検出された方位に対する誤差は小さいものと推定して、旋回状態にないと判定した場合に比べて方位誤差パラメータの値を小さな値に変更すればより好適である。
また、第5の発明として、第1〜第3の何れかの発明の位置算出方法であって、前記センサ部には、少なくとも角速度センサ及び方位センサが含まれ、前記移動状態の判定は、前記移動体が旋回状態にあるか否かを判定することを含み、前記誤差パラメータには、前記移動体の移動方向の時間変化の許容度を表す方向時変パラメータが含まれ、前記誤差パラメータの値の変更は、前記移動体が旋回状態にあると判定されたか否かに応じて前記方向時変パラメータの値を変更することを含む位置算出方法を構成してもよい。
この第5の発明によれば、センサ部には、少なくとも角速度センサ及び方位センサが含まれ、移動体が旋回状態にあるか否かを判定する。そして、移動体が旋回状態にあると判定されたか否かに応じて、移動体の移動方向の時間変化の許容度を表す方向時変パラメータの値を変更する。尚、移動体が旋回状態にあると判定した場合は、移動体の方位の時間変化を大きくするために、旋回状態にないと判定した場合に比べて方位時変パラメータの値を大きな値に変更すればより好適である。
また、第6の発明として、第1の発明の位置算出方法であって、前記センサ部には、少なくとも加速度センサ及び方位センサが含まれ、前記移動状態の判定は、前記移動体が急加速状態にあるか否かを判定することを含み、前記測位用衛星からの測位用信号に基づいて前記移動体の移動速度を算出することを更に含み、前記誤差パラメータには、前記算出された移動速度に対する誤差の大きさを表す速度誤差パラメータが含まれ、前記誤差パラメータの値の変更は、前記移動体が急加速状態にあると判定されたか否かに応じて前記速度誤差パラメータの値を変更することを含む位置算出方法を構成してもよい。
この第6の発明によれば、センサ部には、少なくとも加速度センサ及び方位センサが含まれ、移動体が急加速状態にあるか否かを判定する。そして、移動体が急加速状態にあると判定されたか否かに応じて、測位用信号に基づいて算出した移動体の移動速度に対する誤差の大きさを表す速度誤差パラメータの値を変更する。尚、移動体が急加速状態にあると判定した場合は、移動速度に対する誤差は大きいものと推定して、急加速状態にないと判定した場合に比べて速度誤差パラメータの値を大きな値に変更すればより好適である。
また、第7の発明として、第1の発明の位置算出方法であって、前記センサ部には、少なくとも加速度センサ及び方位センサが含まれ、前記移動状態の判定は、前記移動体が急加速状態にあるか否かを判定することを含み、前記測位用衛星からの測位用信号に基づいて前記移動体の移動速度を算出することを更に含み、前記誤差パラメータには、前記算出された移動速度の時間変化の許容度を表す速度時変パラメータが含まれ、前記誤差パラメータの値の変更は、前記移動体が急加速状態にあると判定されたか否かに応じて前記速度時変パラメータの値を変更することを含む位置算出方法を構成してもよい。
この第7の発明によれば、センサ部には、少なくとも加速度センサ及び方位センサが含まれ、移動体が急加速状態にあるか否かを判定する。そして、移動体が急加速状態にあると判定されたか否かに応じて、測位用信号に基づいて算出した移動体の移動速度の時間変化の許容度を表す速度時変パラメータの値を変更する。尚、移動体が急加速状態にあると判定した場合は、移動体の移動速度の時間変化を大きくするために、急加速状態にないと判定した場合に比べて速度時変パラメータの値を大きな値に変更すればより好適である。
以下、図面を参照して、位置算出装置を備えた電子機器の一種である携帯型電話機に本発明を適用した場合の実施形態について説明する。また、携帯型電話機は、ユーザによって携帯されて移動したり、自動車等に配置設定されて移動する。このため、本実施形態では、携帯型電話機を保持或いは搭載した人や物のことを“移動体”として説明し、携帯型電話機が検出する各種方向を移動体の方向として説明する。尚、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
1.概略構成
図1及び図2は、携帯型電話機1の概略外観図である。携帯型電話機1は、表示部60が蓋部に一体化されて構成された折り畳み式の携帯電話機であり、蓋部を開いた状態において、携帯型電話機1のユーザが表示面を時計回り或いは反時計回りに回転させるように蓋部を回転させることで、表示部60を縦向き及び横向きの何れかの姿勢にして画面を見ることができるように構成されている。携帯型電話機1は、操作部50を介したユーザ操作に従って、当該携帯型電話機1自身の位置を算出する位置算出演算を行い、その算出位置を表示部60に表示させる。
蓋部には、加速度センサ71、ジャイロセンサ73及び方位センサ75を含むセンサ部70が内蔵配置されている。加速度センサ71は3軸の加速度センサであり、例えば表示部60を縦向きの姿勢(図1の姿勢)とした場合に、表示部60に向かって前後方向が「x軸」、上下方向が「y軸」、左右方向が「z軸」となるように右手系の直交3軸座標系(ローカル座標系)が対応付けられている。また、ジャイロセンサ73は3軸の軸回りの角速度を検出可能な角速度センサであり、加速度センサ71と同一の座標系が対応付けられている。
携帯型電話機1は、センサ部70の検出結果を用いて、ファジィ(Fuzzy)推定演算に基づいて移動体である携帯型電話機1自身の移動状態(停止状態、旋回状態、加減速状態等)を判定する。この際、表示部60が縦向き及び横向きの何れの姿勢であるかに応じて、地球に対するセンサ部70の座標系の向きが変化する。そのため、本実施形態では、停止状態にあると判定された際に、センサ部70に対応付けられた検出軸のうち、移動体である携帯型電話機1自身の重力方向の軸(以下、「重力軸」と称す。)及び正面方向の軸(以下、「正面軸」と称す。)がどの軸に対応するかを判断する。そして、その判断結果を用いて各軸を定義して、移動体の旋回状態及び加減速状態を判定する。すなわち、各軸の判断は、携帯型電話機1の相対姿勢を判断することと等価である。
尚、以下の説明では、正面軸を「X軸」、重力軸を「Z軸」、正面軸及び重力軸に直交する軸を「Y軸」とし、携帯型電話機1に対応付けられたローカル座標系における3軸「x軸」、「y軸」及び「z軸」と区別して説明する。
2.機能構成
図3は、携帯型電話機1の機能構成を示すブロック図である。携帯型電話機1は、GPSアンテナ10と、GPS受信部20と、ホストCPU(Central Processing Unit)40と、操作部50と、表示部60と、センサ部70と、ROM(Read Only Memory)80と、RAM(Random Access Memory)90と、携帯電話用アンテナ100と、携帯電話用無線通信回路部110とを備えて構成される。
GPSアンテナ10は、GPS衛星から発信されているGPS衛星信号を含むRF(Radio Frequency)信号を受信するアンテナであり、受信した信号をGPS受信部20に出力する。尚、GPS衛星信号は、衛星毎に異なる拡散符号の一種であるPRN(Pseudo Random Noise)コードで直接スペクトラム拡散方式により変調された1.57542[GHz]の通信信号である。PRNコードは、コード長1023チップを1PNフレームとする繰返し周期1msの擬似ランダム雑音符号である。
GPS受信部20は、GPSアンテナ10から出力された信号に基づいて携帯型電話機1の位置を計測する位置算出回路であり、いわゆるGPS受信機に相当する機能ブロックである。GPS受信部20は、RF(Radio Frequency)受信回路部21と、ベースバンド処理回路部30とを備えて構成される。尚、RF受信回路部21と、ベースバンド処理回路部30とは、それぞれ別のLSI(Large Scale Integration)として製造することも、1チップとして製造することも可能である。
RF受信回路部21は、RF信号の処理回路ブロックであり、所定の発振信号を分周或いは逓倍することで、RF信号乗算用の発振信号を生成する。そして、生成した発振信号を、GPSアンテナ10から出力されたRF信号に乗算することで、RF信号を中間周波数の信号(以下、「IF(Intermediate Frequency)信号」と称す。)にダウンコンバートし、IF信号を増幅等した後、A/D変換器でデジタル信号に変換して、ベースバンド処理回路部30に出力する。
ベースバンド処理回路部30は、RF受信回路部21から出力されたIF信号に対して相関処理等を行ってGPS衛星信号を捕捉・抽出し、データを復号して航法メッセージや時刻情報等を取り出す回路部である。ベースバンド処理回路部30は、演算制御部31と、ROM35と、RAM37とを備えて構成される。また、演算制御部31は、メジャメント取得演算部33を備えて構成される。
メジャメント取得演算部33は、RF受信回路部21から出力された受信信号(IF信号)から、GPS衛星信号の捕捉・追尾を行う回路部であり、相関演算部331を備えて構成されている。メジャメント取得演算部33は、捕捉・追尾したGPS衛星信号の受信周波数やコード位相等の情報を取得し、メジャメント実測値としてホストCPU40に出力する。
相関演算部331は、受信信号に含まれるPRNコードとレプリカコードとの相関を、例えばFFT演算を用いて算出し積算する相関演算処理を行って、GPS衛星信号を捕捉する。レプリカコードとは、擬似的に発生させた捕捉しようとするGPS衛星信号に含まれるPRNコードを模擬した信号である。
捕捉しようとするGPS衛星信号が間違いなければ、そのGPS衛星信号に含まれるPRNコードとレプリカコードとは一致し(捕捉成功)、間違っていれば一致しない(捕捉失敗)。そのため、算出された相関値のピークを判定することによってGPS衛星信号の捕捉が成功したか否かを判定でき、レプリカコードを次々に変更して、同じ受信信号との相関演算を行うことで、GPS衛星信号を捕捉することが可能となる。
また、相関演算部331は、上述した相関演算処理を、レプリカコードの発生信号の周波数及び位相を変更しつつ行っている。レプリカコードの発生信号の周波数と受信信号の周波数とが一致し、且つ、PRNコードとレプリカコードとの位相が一致した場合に、相関値が最大となる。
より具体的には、捕捉対象のGPS衛星信号に応じた所定の周波数及び位相の範囲をサーチ範囲として設定する。そして、このサーチ範囲内で、PRNコードの開始位置(コード位相)を検出するための位相方向の相関演算と、周波数を検出するための周波数方向の相関演算とを行う。サーチ範囲は、例えば、周波数についてはGPS衛星信号の搬送波周波数である1.57542[GHz]を中心とする所定の周波数掃引範囲、位相についてはPRNコードのチップ長である1023チップのコード位相範囲内に定められる。
ホストCPU40は、ROM80に記憶されているシステムプログラム等の各種プログラムに従って携帯型電話機1の各部を統括的に制御するプロセッサである。また、ホストCPU40は、位置算出処理を行って携帯型電話機1の位置を計測して表示部60に表示させる出力位置を決定し、当該出力位置をプロットしたナビゲーション画面を生成して表示部60に表示させる。
操作部50は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号をホストCPU40に出力する。この操作部50の操作により、目的地の入力やナビゲーション画面の表示要求等の各種指示入力がなされる。
表示部60は、LCD(Liquid Crystal Display)等により構成され、ホストCPU40から入力される表示信号に基づいた各種表示を行う表示装置である。表示部60には、ナビゲーション画面や時刻情報等が表示される。
センサ部70は、移動体の移動状態を検出するための各種センサで構成されており、例えば、加速度センサ71と、ジャイロセンサ73と、方位センサ75とを備えている。
加速度センサ71は、直交3軸の加速度を検出するセンサであり、歪みゲージ式や圧電式の何れであってもよく、またMEMS(Micro Electro Mechanical Systems)センサであってもよい。
ジャイロセンサ73は、直交3軸の角速度を検出するセンサであり、加速度センサ71と軸方向が同一となるように配置設定されている。尚、加速度センサ71とジャイロセンサ73とは一体型のセンサであってもよい。
また、方位センサ75は、例えば磁場の強さによって抵抗値やインピーダンス値が増減する素子等で構成される2軸の地磁気センサであり、移動体の方位角を検出する。
ROM80は、読み取り専用の不揮発性の記憶装置であり、ホストCPU40が携帯型電話機1を制御するためのシステムプログラムや、ナビゲーション機能を実現するための各種プログラムやデータ等を記憶している。
RAM90は、読み書き可能な揮発性の記憶装置であり、ホストCPU40により実行されるシステムプログラム、各種処理プログラム、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを形成している。
携帯電話用アンテナ100は、携帯型電話機1の通信サービス事業者が設置した無線基地局との間で携帯電話用無線信号の送受信を行うアンテナである。
携帯電話用無線通信回路部110は、RF変換回路、ベースバンド処理回路等によって構成される携帯電話の通信回路部であり、携帯電話用無線信号の変調・復調等を行うことで、通話やメールの送受信等を実現する。
3.データ構成
図4は、ROM80に格納されたデータの一例を示す図である。ROM80には、ホストCPU40により読み出され、メイン処理(図18参照)として実行されるメインプログラム800と、停止判定用データ810と、旋回判定用データ820とが記憶されている。
また、メインプログラム800には、停止判定処理(図19参照)として実行される停止判定プログラム801と、旋回判定処理(図20参照)として実行される旋回判定プログラム802と、加減速判定処理(図21参照)として実行される加減速判定プログラム803と、位置算出処理(図22参照)として実行される位置算出プログラム804と、LS(Least Squared)位置算出処理(図23参照)として実行されるLS位置算出プログラム805と、KF(Kalman Filter)位置算出処理(図24参照)として実行されるKF位置算出プログラム806と、KFパラメータ設定処理(図25参照)として実行されるKFパラメータ設定プログラム807とがサブルーチンとして含まれている。
メイン処理とは、ホストCPU40が、停止判定処理、旋回判定処理及び加減速判定処理を行って移動体の移動状態を判定するとともに、判定した移動状態に応じて可変に設定したKFパラメータを利用した位置算出処理を行うことで、移動体の位置を算出して出力する処理である。
停止判定処理とは、ホストCPU40が、加速度センサ71の検出結果を用いて、ファジィ推定(ファジィ理論)に基づいて移動体が停止状態にあるか否かを判定する処理である。旋回判定処理とは、ホストCPU40が、ジャイロセンサ73の検出結果を用いて、ファジィ推定に基づいて移動体が旋回状態にあるか否かを判定する処理である。また、加減速判定処理とは、ホストCPU40が、加速度センサ71の検出結果を用いて移動体が加減速状態にあるか否かを判定する処理である。
位置算出処理とは、ホストCPU40が、最小二乗法を用いた位置算出処理(LS位置算出処理)を行って暫定的な算出位置を求め、当該算出位置を基礎値としてカルマンフィルタを用いた位置算出処理(KF位置算出処理)を行うことで、最終的に表示部60に出力させる位置を決定する処理である。
位置算出処理において、ホストCPU40は、LS位置算出処理を行ってECEF(Earth Centered Earth Fixed)座標系における移動体の位置ベクトル及び移動速度ベクトルを求める。そして、このECEF座標系における処理結果に対して所定の座標変換演算を行うことで、ENU(East North Up)座標系における移動体の位置及び移動速度を算出する。そして、算出した位置及び移動速度と、方位センサ75により検出された方位及びこの方位の時間変化量である時間毎方位とを観測情報とするKF位置算出処理を行うことで、ENU座標系における移動体の位置を出力位置として求める。
ECEF座標系は、地球中心地球固定座標系として知られ、地球の中心(地球重心)を原点とし、地球自転軸をZE軸、グリニジ子午面と赤道面とが交わる軸をXE軸、これらの2軸と直交する軸をYE軸とする3軸直交座標系である。また、ENU座標系は、東北上座標系として知られ、地表面付近のある点を原点として、天頂方向(地表面に対する垂直線の上方向)をU軸、東方向をE軸、北方向をN軸とする3軸直交座標系である。
ENU座標系における移動体の位置、移動速度及び方位は、ECEF座標系において求めた移動体の位置ベクトル及び移動速度ベクトルに対して公知の座標変換演算(行列演算)を行うことで求めることができる。尚、本明細書では、「移動速度」というときは方位(移動方向)を含まないスカラー量を表すものとし、「移動速度ベクトル」というときは方位を含むベクトル量を表すものとする。
KFパラメータ設定処理とは、ホストCPU40が、停止判定処理、旋回判定処理及び加減速判定処理の処理結果に基づいて、KF位置算出処理において使用する観測誤差共分散行列「R」やシステムノイズ行列「Q」といったパラメータの値を設定・変更する処理である。これらの処理については、フローチャートを用いて詳細に後述する。
停止判定用データ810は、停止判定処理においてホストCPU40が停止判定を行う際に使用するデータであり、停止判定用ファジィルール811と、停止判定用メンバシップ関数813と、停止判定用判定結果テーブル815とがこれに含まれる。
図6は、停止判定用ファジィルール811のデータ構成の一例を示す図である。停止判定用ファジィルール811には、ルールの番号8111と、「もし〜なら(if〜)」に相当する部分である前件部8113と、「・・・する(then・・・)」に相当する部分である後件部8115とが対応付けて記憶されている。
前件部8113には、加速度センサ71により検出された3軸の加速度それぞれについて、加速度の変化量(例えば1ミリ秒の間における変化量)を1秒分積算した積算値(以下、x軸方向の加速度の積算値を「AJx」、y軸方向の加速度の積算値を「AJy」、z軸方向の加速度の積算値を「AJz」と表す。)と、「高い(移動可能性)」、「中間(不定)」、「低い(停止可能性)」の3つの基準との組合せが条件として定められている。
また、後件部8115には、移動体が移動していることを示す「移動」、移動しているか停止しているかが不明であることを示す「不定」、停止していることを示す「停止」の何れかが前件部8113と対応付けて記憶されている。
例えば、「AJx」、「AJy」及び「AJz」が全て「中間」である場合は、4番目のルールに適合するため「移動」と判定され、「AJx」が「中間」で、「AJy」、「AJz」が「低い」である場合は、7番目のルールに適合するため「不定」と判定される。また、「AJx」、「AJy」及び「AJz」が全て「低い」である場合は、11番目のルールに適合するため「停止」と判定される。
図7は、停止判定用メンバシップ関数813の一例を示す図である。停止判定用メンバシップ関数813には、停止判定用ファジィルール811の前件部8113に対応するメンバシップ関数と、後件部8115に対応するメンバシップ関数とが含まれる。前件部8113のメンバシップ関数は、横軸を「AJx」、縦軸を「適合度」とするx軸用の関数と、横軸を「AJy」、縦軸を「適合度」とするy軸用の関数と、横軸を「AJz」、縦軸を「適合度」とするz軸用の関数とを有している。
これらの各関数は、前件部8113の「低い(停止可能性)」に対応する検出値適合範囲と、「中間(不定)」に対応する検出値適合範囲と、「高い(移動可能性)」に対応する検出値適合範囲との3つの適合範囲が合成された全体としてW型の形状となっている。何れの関数においても、「高い(移動可能性)」の検出値適合範囲が最も広く設定されている。また、適合度は、「0」〜「1」の範囲の値として定められている。前件部8113のメンバシップ関数は、入力値としての「AJx」、「AJy」及び「AJz」のファジィ化を行うために利用される。
後件部8115のメンバシップ関数は、横軸を「出力」、縦軸を「集積値」とする関数であり、図7では、後件部8115の「停止」に対応する三角形の山と、「不定」に対応する三角形の山と、「移動」に対応する三角形の山との3つの山として表される。また、集積値は、「0」〜「1」の範囲の値として定められている。
ファジィ推定では、後件部8115に定められた条件(「停止」、「不定」及び「移動」)の集合(ファジィ集合)の重心を求めることによって最終的な出力値を決定する「重心法」と呼ばれる非ファジィ化の手法が一般的に用いられている。本実施形態でも、この重心法を用いて非ファジィ化を行う。後件部8115のメンバシップ関数は、この非ファジィ化を行うために利用される。
図8は、停止判定用判定結果テーブル815のテーブル構成の一例を示す図である。停止判定用判定結果テーブル815には、条件の番号8151と、条件8153と、判定結果8155とが対応付けて記憶されている。条件8153は、非ファジィ化により求められた出力値が満たすべき条件として定められた条件である。また、判定結果8155は、移動体の移動状態の最終的な判定結果であり、「移動」又は「停止」が記憶されている。
例えば、非ファジィ化により求めた出力値が「0.95以上」である場合は、移動体は移動状態にあると判定される。また、出力値が「0.05以下」である場合は、移動体は停止状態にあると判定される。
図9〜図11を参照して、ファジィ推定に基づく停止判定の具体例を説明する。ここでは、「AJx=10、AJy=11、AJz=9」である場合について説明する。
先ず、「AJx」、「AJy」及び「AJz」それぞれについて、前件部8113のメンバシップ関数を用いて適合度を判定する。具体的には、前件部のメンバシップ関数において、「AJx=10」、「AJy=11」及び「AJz=9」が、それぞれ「低い」、「中間」及び「高い」の何れの検出値適合範囲において値をとるかを判定する。
図9では、「AJx」については、「低い」の検出値適合範囲において「0.23」の値をとるが、「中間」、「高い」の検出値適合範囲においては値をとらない。この場合、「AJx」の適合度は「低い=0.23,中間=0,高い=0」となる。同様に「AJy」、「AJz」についても適合度を判定すると、「AJy」については、「低い=0.1,中間=0.25,高い=0」となり、「AJz」については、「低い=0.24,中間=0,高い=0」となる。
次に、停止判定用ファジィルール811の前件部8113を参照して、先に判定した適合度が何れの番号8111のルールに該当するかを判定する。具体的には、「AJx」は「低い」において値をとり、「AJy」は「低い」及び「中間」において値をとり、「AJz」は「低い」において値をとるため、「AJx=低い、AJy=中間、AJz=低い」と、「AJx=低い、AJy=低い、AJz=低い」の2つの組合せに対応するルールに適合する。すなわち、図10に示すように、9番目と11番目のルールに適合することになる。
このとき、適合したルールにおける「AJx」、「AJy」及び「AJz」の適合度のうちの最小値を選択して、当該ルールにおける候補値とする。その結果、9番目のルールの候補値は「0.23」、11番目のルールの候補値は「0.1」となる。また、他のルールの候補値は全て「0」となる。
候補値が決まったら、後件部8115における「移動」、「不定」及び「停止」の3つの条件それぞれについて、候補値の中から最大値を抽出して、当該条件における集積値に決定する。その結果、集積値は、「移動=0,不定=0.23,停止=0.1」となる。その後、決定した集積値に対して非ファジィ化を行って出力値を決定する。ここでは、重心法を用いて出力値を決定する。重心法には公知の算出演算が存在する。本実施形態では、その公知の算出演算に従って重心を算出するが、重心法の概念を簡単に説明する。
先ず、後件部8115のメンバシップ関数において、対応する条件の山を対応する集積値で切り取る。具体的には、図11に示すように、「停止」の集積値は「0.1」であるため、「停止」の山を縦軸の「0.1」のラインで切り取ることにより台形T1を得る。同様に、「不定」の集積値は「0.23」であるため、「不定」の山を縦軸の「0.23」のラインで切り取ることにより台形T2を得る。また、「移動」の集積値は「0」であるため、「移動」の山を縦軸の「0」のラインで切り取ることにより線分Sを得る。
次いで、台形T1、台形T2及び線分Sが描かれた位置をそのままとして、台形T1、台形T2及び線分Sを1つの図形と見立てた全体の重心を求める。その結果、図11の×印で示した部分が重心として求められ、この重心に対応する横軸の値は「0.35」であるため、出力値は「0.35」となる。
出力値が求まったら、図8の停止判定用判定結果テーブル815を参照し、出力値が何れの条件8153に適合するかを判定する。その結果、3番目と4番目の条件に適合することになり、前回の判定結果が移動であった場合は、最終的な判定結果8155は「移動」となり、前回の判定結果が停止であった場合は、最終的な判定結果8155は「停止」となる。
停止判定処理を行った結果、停止状態にあると判定された場合には、ホストCPU40は、重力軸判定処理と正面軸判定処理とを行って、センサ部70に対応付けられた「x軸」、「y軸」及び「z軸」の3軸うち、重力軸に対応する軸(Z軸)と、正面軸に対応する軸(X軸)とがそれぞれどの軸に対応しているかを判定する。
重力軸判定処理では、加速度センサ71により検出された3軸の加速度のうち値が最大となった軸を特定する。そして、他の2軸の加速度が、特定した軸の加速度の15%以下の値となっているかを判定し、この条件を満たす場合は、特定した軸を重力軸(Z軸)と判定する。また、条件を満たさないと判定した場合は、重力軸の判定は不可とし、前回重力軸であると判定した軸を重力軸(Z軸)とみなす。
また、正面軸判定処理では、判定結果が停止状態から移動状態に変化した際に、加速度センサ71により検出された3軸の加速度のうち値が最大となった軸を正面軸(X軸)と判定する。すなわち、移動体が停止している状態から移動し始めた方向を特定して正面軸(X軸)と判定する。
図4の説明に戻って、旋回判定用データ820は、旋回判定処理においてホストCPU40が旋回判定を行うために使用するデータであり、旋回判定用ファジィルール821と、旋回判定用メンバシップ関数823と、旋回判定用判定結果テーブル825とがこれに含まれる。
図12は、旋回判定用ファジィルール821のデータ構成の一例を示す図である。旋回判定用ファジィルール821には、ルールの番号8211と、当該ルールにおける前件部8213と、当該ルールにおける後件部8215とが対応付けて記憶されている。
前件部8213には、ジャイロセンサ73により検出された3軸の角速度のうち、正面軸(X軸)と、重力軸(Z軸)と、X軸及びZ軸に直交する軸(Y軸)とに対応する角速度の絶対値(以下、X軸方向の角速度の絶対値を「GJX」、Y軸方向の角速度の絶対値を「GJY」、Z軸方向の角速度の絶対値を「GJZ」と表す。)それぞれについて、「高い(旋回可能性)」、「中間(不定)」及び「低い(直進可能性)」の3つの基準との組合せが条件として記憶されている。
また、後件部8215には、移動体が旋回状態にあることを示す「旋回」、旋回しているか直進しているかが不明であることを示す「不定」、直進状態にあることを示す「直進」の何れかが前件部8213と対応付けて記憶されている。
例えば、「GJZ」が「高い」である場合は、1番目のルールに適合するため「GJX」、「GJY」の値に関わりなく「旋回」と判定され、「GJX」が「低い」で、「GJY」、「GJZ」が「中間」である場合は、8番目のルールに適合するため「不定」と判定される。また、「GJX」、「GJY」及び「GJZ」が全て「低い」である場合は、11番目のルールに適合するため「直進」と判定される。
図13は、旋回判定用メンバシップ関数823の一例を示す図である。旋回判定用メンバシップ関数823には、旋回判定用ファジィルール821の前件部8213に対応するメンバシップ関数と、後件部8215に対応するメンバシップ関数とが含まれる。前件部8213のメンバシップ関数は、横軸を「GJX」、縦軸を「適合度」とするX軸用の関数と、横軸を「GJY」、縦軸を「適合度」とするY軸用の関数と、横軸を「GJZ」、縦軸を「適合度」とするZ軸用の関数とを含んでいる。
これらの各関数は、前件部8213の「低い(直進可能性)」に対応する適合範囲と、「中間(不定)」に対応する適合範囲と、「高い(旋回可能性)」に対応する適合範囲との3つの適合範囲が合成された全体としてW型の形状となっている。何れの関数においても、「高い(旋回可能性)」の適合範囲が最も広く設定されている。また、適合度は、「0」〜「1」の範囲の値として定められている。
特徴的であることの1つは、各軸のメンバシップ関数について、図13に示した横軸のスケールが異なっている点に表れている。Z軸は重力軸(ヨー軸)であり、移動体はZ軸回りに回転する場合がほとんどであると考えられる。そのため、Z軸回りの角速度については「低い」、「中間」、「高い」の何れのグループに分類されるかを精細に判定することが必要となるのに対し、ほとんど回転しないであろうX軸とY軸回りの角速度については、大雑把な判定で足りるものと考えられる。そのため、Z軸については、他の2軸に比べて横軸のスケールが狭められており、他の2軸のメンバシップ関数に比べて、「高い(旋回可能性)」についてのZ軸の適合範囲が最も広く設定されている。
例えば、X軸回り、Y軸回り、Z軸回りの角速度が同じ「20」という値をとったとしても、図13のメンバシップ関数に従えば、X軸及びY軸については「低い」のグループに分類されるのに対し、Z軸については「高い」のグループに分類されることになる。
尚、これに対して、前述した停止判定処理は、重力軸及び正面軸を特定するために行う処理であり、センサ部70に対応付けられた3軸(x軸、y軸、z軸)と、重力軸(Z軸)及び正面軸(X軸)との対応関係が不明な状態で行う処理であるため、図7に示す通り、適合範囲は各軸ともに略同一である。
後件部8215のメンバシップ関数は、横軸を「出力」、縦軸を「集積値」とする関数であり、図13では、後件部8215の「直進」に対応する山と、「不定」に対応する山と、「旋回」に対応する山との独立した3つの山として表される。また、集積値は、「0」〜「1」の範囲の値として定められている。
図14は、旋回判定用判定結果テーブル825のデータ構成の一例を示す図である。旋回判定用判定結果テーブル825には、条件の番号8251と、条件8253と、判定結果8255とが対応付けて記憶されている。条件8253は、非ファジィ化により求められた出力値が満たすべき条件として定められた条件である。また、判定結果8255は、移動体の旋回状態の最終的な判定結果であり、「旋回」、「直進」又は「不定(前回と同じ判定結果とする)」が記憶されている。
例えば、出力値が「0.95以上」である場合は、移動体は「旋回状態」にあると判定され、出力値が「0.05以下」である場合は、移動体は「直進状態」にあると判定される。また、それ以外の場合は「不定」であり、前回と同じ判定結果とされる。
旋回判定処理における出力値の決定方法は、前述した停止判定処理と同一である。すなわち、入力値である「GJX」、「GJY」及び「GJZ」に対して、前件部8213のメンバシップ関数を用いてファジィ化を行う。次いで、ファジィ化により得られた適合度と旋回判定用ファジィルール821に従って、「GJX」、「GJY」及び「GJZ」それぞれの集積値を算出する。そして、算出した集積値に対して、後件部8215のメンバシップ関数を用いて非ファジ化を行い、得られた重心で表される出力値が旋回判定用判定結果テーブル825の何れの条件8253に適合するかを判定する。
図5は、RAM90に格納されるデータの一例を示す図である。RAM90には、センサデータ901と、メジャメントデータ903と、計測履歴データ905と、KFパラメータデータ907とが記憶される。
図15は、センサデータ901のデータ構成の一例を示す図である。センサデータ901には、検出時刻9011(例えばミリ秒)と対応付けて、加速度センサ71により検出された3次元の加速度9013と、ジャイロセンサ73により検出された3次元の角速度9015と、方位センサ75により検出された方位9017とが対応付けて記憶される。方位9017は、2次元平面における方位角で表される。
例えば、検出時刻「t1」において検出された加速度9013は「(Ax1,Ay1,Az1)」であり、角速度9015は「(ωx1,ωy1,ωz1)」、方位は「Heading1」である。
図16は、メジャメントデータ903のデータ構成の一例を示す図である。メジャメントデータ903は、メジャメント取得演算部33により取得演算されたメジャメント実測値に関するデータであり、捕捉衛星9031と、当該捕捉衛星から受信したGPS衛星信号の受信周波数及びコード位相でなるメジャメント実測値9033とが対応付けて記憶される。例えば、捕捉衛星「S1」についてのメジャメント実測値9033は、受信周波数が「Freq1」、コード位相が「CP1」である。
図17は、計測履歴データ905のデータ構成の一例を示す図である。計測履歴データ905は、ENU座標系における移動体の位置、移動速度についてのデータであり、各位置算出時刻9051について、東方向の位置9052と、北方向の位置9053と、東方向の移動速度9054と、北方向の移動速度9055と、位置算出平面における移動速度9056とが対応付けて記憶される。
東方向の位置9052は、ENU座標系における東西方向の位置座標であり、北方向の位置9053は、ENU座標系における南北方向の位置座標である。また、東方向の移動速度9054は、ENU座標系における東西方向の移動速度であり、北方向の移動速度9055は、ENU座標系における南北方向の移動速度である。
位置算出平面における移動速度9056は、例えばENU座標系において天頂方向の位置座標を“0”とした場合における東西南北の2次元平面(この平面を「位置算出平面」と定義する。)における移動速度である。東方向の移動速度9054と、北方向の移動速度9055とを合成することで、位置算出平面における移動速度9056が算出可能である。
例えば、位置算出時刻「T1」においては、東方向の位置として「EastPos1」、北方向の位置として「NorthPos1」、東方向の移動速度として「EastVel1」、北方向の移動速度として「NorthVel1」、位置算出平面における移動速度として「Speed1」がそれぞれ記憶されている。
KFパラメータデータ907は、KF位置算出処理で用いるカルマンフィルタの各種パラメータの値が記憶されたデータであり、位置算出処理においてホストCPU40により更新される。
4.処理の流れ
図18は、ホストCPU40によりROM80に記憶されているメインプログラム800が読み出されて実行されることで、携帯型電話機1において実行されるメイン処理の流れを示すフローチャートである。メイン処理は、ホストCPU40が、操作部50に位置算出開始指示の操作がなされたことを検出した場合に実行を開始する処理であり、各種アプリケーションの実行といった各種の処理と並行して行われる処理である。
以下の説明において、位置算出演算は1秒毎に行われるものとする。すなわち、位置算出時刻の時間間隔は1秒である。また、「今回」というときは現在演算処理中の位置算出時刻のことを指し、「前回」というときは1つ前の演算処理で算出した位置算出時刻のことを指し、「前々回」というときは2つ前の演算処理で算出した位置算出時刻のことを指すものとする。
尚、携帯型電話機1の電源のON/OFFとGPSの起動/停止とを連動させ、携帯型電話機1の電源投入操作を検出した場合に処理の実行を開始させることにしてもよい。また、特に説明しないが、以下のメイン処理の実行中は、GPSアンテナ10によるRF信号の受信や、RF受信回路部21によるIF信号へのダウンコンバート、メジャメント取得演算部33によるGPS衛星信号の受信周波数やコード位相(メジャメント実測値)の情報の取得・算出等が随時行われている状態にあるものとする。また、ホストCPU40は、1ミリ秒毎にセンサ部70から検出結果を取得し、RAM90のセンサデータ901に蓄積・記憶するものとする。
先ず、ホストCPU40は、ROM80に記憶されている停止判定プログラム801を読み出して実行することで、停止判定処理を行う(ステップA1)。
図19は、停止判定処理の流れを示すフローチャートである。
先ず、ホストCPU40は、各軸について、加速度センサ71の検出結果の変化量を1秒間分積算する(ステップB1)。そして、ホストCPU40は、ROM80の停止判定用メンバシップ関数813に含まれる前件部8113のメンバシップ関数を用いて、ステップB1で算出した積算値に対するファジィ化を実行する(ステップB3)。そして、ホストCPU40は、停止判定用ファジィルール811に従って集積値を求める(ステップB5)。
次いで、ホストCPU40は、停止判定用メンバシップ関数813に含まれる後件部8115のメンバシップ関数を用いて、ステップB5で求めた集積値に対する非ファジィ化を実行する(ステップB7)。そして、停止判定用判定結果テーブル815に従って、非ファジィ化で得られた出力値に基づいて停止判定を行う(ステップB9)。そして、ホストCPU40は、停止判定処理を終了する。
図18のメイン処理に戻って、停止判定処理を行った後、ホストCPU40は、重力軸判定処理を行う(ステップA3)。具体的には、停止判定処理において停止していると判定した場合は、加速度センサ71により検出された3軸の加速度のうち値が最大となった軸を特定する。そして、他の2軸の加速度が、それぞれ特定軸の加速度の15%以下の値となっているかを判定し、この条件を満たす場合は、特定した軸を重力軸(Z軸)と判定する。また、条件を満たさないと判定した場合は、前回重力軸と判定した軸を重力軸(Z軸)と判定する。
次いで、ホストCPU40は、正面軸判定処理を行う(ステップA5)。具体的には、前回の停止判定処理において停止していると判定した場合であって、今回の停止判定処理において移動していると判定した場合に、加速度センサ71により検出された3軸の加速度のうち値が最大となった軸を正面軸(X軸)と判定する。すなわち、移動体が停止している状態から一歩踏み出した方向を特定して正面軸(X軸)と判定する。
その後、ホストCPU40は、ROM80に記憶されている旋回判定プログラム802を読み出して実行することで、旋回判定処理を行う(ステップA7)。
図20は、旋回判定処理の流れを示すフローチャートである。
先ず、ホストCPU40は、「X軸」、「Y軸」及び「Z軸」それぞれについて、ジャイロセンサ73の最新の検出結果(角速度)の絶対値を算出する(ステップC1)。そして、ホストCPU40は、ROM80の旋回判定用メンバシップ関数823に含まれる前件部8213のメンバシップ関数を用いて、ステップC1で算出した角速度の絶対値に対するファジィ化を実行する(ステップC3)。そして、ホストCPU40は、旋回判定用ファジィルール821に従って集積値を求める(ステップC5)。
次いで、ホストCPU40は、旋回判定用メンバシップ関数823に含まれる後件部8215のメンバシップ関数を用いて、ステップC5で求めた集積値に対する非ファジィ化を実行する(ステップC7)。そして、旋回判定用判定結果テーブル825に従って、非ファジィ化で得られた出力値に基づいて旋回判定を行う(ステップC9)。そして、ホストCPU40は、旋回判定処理を終了する。
図18のメイン処理に戻って、旋回判定処理を行った後、ホストCPU40は、ROM80に記憶されている加減速判定プログラム803を読み出して実行することで、加減速判定処理を行う(ステップA9)。
図21は、加減速判定処理の流れを示すフローチャートである。
先ず、ホストCPU40は、加速度センサ71の正面軸(X軸)方向の検出結果を1秒間分積算する(ステップD1)。そして、ホストCPU40は、今回、前回及び前々回の停止判定処理の判定結果が全て停止であったか否かを判定する(ステップD3)。
そして、判定結果が全て停止であったと判定した場合は(ステップD3;Yes)、ホストCPU40は、ステップD1で算出した積算値で停止値を更新する(ステップD5)。また、今回、前回及び前々回の何れかにおいて判定結果が移動であったと判定した場合は(ステップD3;No)、ホストCPU40は、停止値を更新することなく、ステップD7へと処理を移行する。
その後、ホストCPU40は、ステップD1で算出した積算値から現在設定されている停止値を減算して変動値とする(ステップD7)。そして、ホストCPU40は、変動値が「100」を超えているか否かを判定し(ステップD9)、超えていると判定した場合は(ステップD9;Yes)、移動体は「強加速状態」であると判定して(ステップD11)、加減速判定処理を終了する。
また、ステップD9において変動値が「100以下」であると判定した場合は(ステップD9;No)、ホストCPU40は、変動値が「20以上100以下」であるか否かを判定する(ステップD13)。そして、この条件を満たすと判定した場合は(ステップD13;Yes)、移動体は「弱加速状態」であると判定して(ステップD15)、加減速判定処理を終了する。
また、ステップD13において変動値が「20以上100以下」ではないと判定した場合は(ステップD13;No)、ホストCPU40は、変動値が「−100以上−20以下」であるか否かを判定する(ステップD17)。そして、この条件を満たすと判定した場合は(ステップD17;Yes)、移動体は「弱減速状態」であると判定して(ステップD19)、加減速判定処理を終了する。
また、ステップD17において変動値が「−100以上−20以下」ではないと判定した場合は(ステップD17;No)、ホストCPU40は、変動値が「−100」よりも小さいか否かを判定する(ステップD21)。そして、この条件を満たすと判定した場合は(ステップD21;Yes)、移動体は「強減速状態」であると判定して(ステップD23)、加減速判定処理を終了する。
また、ステップD21において変動値が「−100」以上であると判定した場合は(ステップD21;No)、ホストCPU40は、移動体は「加減速無し状態」であると判定して(ステップD25)、加減速判定処理を終了する。
図18のメイン処理に戻って、加減速判定処理を行った後、ホストCPU40は、ROM80に記憶されている位置算出プログラム804を読み出して実行することで、位置算出処理を行う(ステップA11)。
図22は、位置算出処理の流れを示すフローチャートである。
先ず、ホストCPU40は、ROM80に記憶されているLS位置算出プログラム805を読み出して実行することで、LS位置算出処理を行う(ステップE1)。
図23は、LS位置算出処理の流れを示すフローチャートである。
先ず、ホストCPU40は、メジャメント取得演算部33からメジャメント実測値を取得し(ステップF1)、取得したメジャメント実測値に含まれるコード位相を用いて、携帯型電話機1と捕捉衛星間の擬似距離を算出する(ステップF3)。そして、ホストCPU40は、算出した擬似距離を用いて最小二乗法による位置算出演算を行って、ECEF座標系における移動体の位置ベクトルを算出する(ステップF5)。
その後、ホストCPU40は、ステップF1で取得したメジャメント実測値に含まれるGPS衛星信号の受信周波数を用いて、1秒間における受信周波数の変化量を算出する(ステップF7)。そして、ホストCPU40は、算出した受信周波数の変化量を用いて、ECEF座標系における移動体の移動速度ベクトルを算出して(ステップF9)、LS位置算出処理を終了する。
図22の位置算出処理に戻って、LS位置算出処理を行った後、ホストCPU40は、ステップF5で算出したECEF座標系における位置ベクトルに対して所定の座標変換演算を行って、ENU座標系における東方向の位置「EastPos」及び北方向の位置「NorthPos」を算出し、現在の位置算出時刻と対応付けてRAM90の計測履歴データ905に記憶させる(ステップE3)。
同様に、ホストCPU40は、ステップF9で算出したECEF座標系における移動速度ベクトルに対して所定の座標変換演算を行って、ENU座標系における東方向の移動速度「EastVel」及び北方向の移動速度「NorthVel」を算出し、現在の位置算出時刻と対応付けてRAM90の計測履歴データ905に記憶させる(ステップE5)。
そして、ホストCPU40は、ステップE5で算出した東方向の移動速度「EastVel」及び北方向の移動速度「NorthVel」を用いて、次式(1)に従って、位置算出平面における移動体の移動速度「Speed」を算出し、現在の位置算出時刻と対応付けてRAM90の計測履歴データ905に記憶させる(ステップE7)。
Figure 2010117148
次いで、ホストCPU40は、センサデータ901に記憶されている方位に基づいて、次式(2)に従って時間毎方位を算出する(ステップE9)。
Figure 2010117148
但し、「t」は位置算出時刻であり、「dt」は現在の位置算出時刻と1つ前の位置算出時刻との時刻差である。
その後、ホストCPU40は、ROM80に記憶されているKF位置算出プログラム806を読み出して実行することで、KF位置算出処理を行う(ステップE11)。
図24は、KF位置算出処理の流れを示すフローチャートである。
先ず、KF位置算出処理用の状態ベクトル「X」及び誤差共分散行列「P」のKF初期値を設定する(ステップG1)。KF位置算出処理では、ENU座標系における移動体の東方向の位置「EastPos」、北方向の位置「NorthPos」、位置算出平面における移動速度「Speed」、方位「Heading」及び時間毎方位「dHeading」を成分とする、次式(3)に示す5×1の行列で表される状態ベクトル「X」を用いて、予測演算及び補正演算を行う。特徴的であるのは、時間毎方位を状態ベクトルの成分とすることで、方位の演算精度を高めている点である。
Figure 2010117148
また、誤差共分散行列「P」は、状態ベクトル「X」の5次元の各成分の誤差の共分散を示す5×5の行列である。状態ベクトル「X」及び誤差共分散行列「P」の初期値は、例えば1時刻前のKF位置算出処理で求められた状態ベクトル「X」及び誤差共分散行列「P」の補正値「X」及び「P」とすることができる。
次いで、ホストCPU40は、状態ベクトル「X」及び誤差共分散行列「P」について、次式(4)及び(5)に従って予測演算を行い、状態ベクトルの予測値「X」及び誤差共分散行列の予測値「P」を算出する(ステップG3)。この際、状態ベクトル「X」の5次元の各成分については、次式(6)〜(10)に従って予測演算を行う。
Figure 2010117148
Figure 2010117148
Figure 2010117148
Figure 2010117148
Figure 2010117148
Figure 2010117148
Figure 2010117148
但し、各式における添え字の「−」は予測値、「+」は補正値であることを示しており、「T」は転置行列であることを示している。また、「φ」は、状態遷移行列と呼ばれる状態ベクトルの遷移を表す5×5の行列であり、「Q」は、システムノイズ行列と呼ばれるKFモデルに内在する誤差(モデル誤差)の共分散を示す5×5の行列である。状態遷移行列「φ」は予め定められた行列とすることができる。また、システムノイズ行列「Q」は、1時刻前にステップG9のKFパラメータ設定処理で設定された行列とする。
その後、ホストCPU40は、RAM90の計測履歴データ905に記憶されている最新の移動体の東方向の位置「EastPos」、北方向の位置「NorthPos」及び位置算出平面における移動速度「Speed」と、センサデータ901に記憶されている最新の方位「Heading」と、ステップE9で算出した時間毎方位「dHeading」とを成分とする、次式(11)で表される5×1の行列をKF位置算出処理用の観測ベクトル「Z」として設定する(ステップG5)。
Figure 2010117148
また、ホストCPU40は、ステップG3で算出した状態ベクトルの予測値「X」に所定の観測行列「H」を乗算することで、予測された観測ベクトル(以下、「予測観測ベクトル」と称す。)「HX」を算出する(ステップG7)。
観測行列「H」は、状態ベクトルの予測値「X」の各成分を用いて、観測ベクトル「Z」の成分である東方向の位置「EastPos」、北方向の位置「NorthPos」、位置算出平面における移動速度「Speed」、方位「Heading」及び時間毎方位「dHeading」を予測するための行列である。観測行列「H」は5×5の行列であり、状態ベクトルの予測値「X」は5×1の行列であるため、予測観測ベクトル「HX」は5×1の行列として求めることができる。
次いで、ホストCPU40は、ROM80に記憶されているKFパラメータ設定プログラム807を読み出して実行することで、KFパラメータ設定処理を行う(ステップG9)。
図25は、KFパラメータ設定処理の流れを示すフローチャートである。
先ず、ホストCPU40は、停止判定処理における判定結果が「停止」であったか否かを判定する(ステップH1)。そして、「停止」であったと判定した場合は(ステップH1;Yes)、観測誤差共分散行列「R」に、次式(12)で表される「R1」を設定する(ステップH3)。また、システムノイズ行列「Q」に、次式(13)で表される「Q1」を設定する(ステップH5)。
Figure 2010117148
Figure 2010117148
観測誤差共分散行列「R」は、観測ベクトル「Z」に含まれる各成分の誤差の共分散を示す5×5の行列であり、その行と列の並びは、観測ベクトル「Z」の5つの成分(東方向の位置、北方向の位置、移動速度、方位、時間毎方位)にそれぞれ対応している。また、システムノイズ行列「Q」は、KFモデルのモデル誤差の共分散を表す5×5の行列であり、その行と列の並びは、状態ベクトル「X」の5つの成分(東方向の位置、北方向の位置、移動速度、方位、時間毎方位)にそれぞれ対応している。
本実施形態では、観測誤差共分散行列「R」、システムノイズ行列「Q」共に、対角成分のみを正の値とし、他の全ての成分は「0」とする。すなわち、観測誤差共分散行列「R」では、東方向の位置、北方向の位置、移動速度、方位及び時間毎方位の5つの成分について、それぞれの対角成分に対応するパラメータ(以下、「東方向位置誤差パラメータ」、「北方向位置誤差パラメータ」、「速度誤差パラメータ」、「方位誤差パラメータ」及び「時間毎方位誤差パラメータ」と称し、包括的に「誤差パラメータ」と称する。)の値を変更する。誤差が小さいと考えられる成分(信頼性が高い成分)の誤差パラメータは値を小さくし、誤差が大きいと考えられる成分(信頼性が低い成分)の誤差パラメータは値を大きくする。
また、システムノイズ行列「Q」では、東方向の位置、北方向の位置、移動速度、方位及び時間毎方位の5つの成分について、それぞれの対角成分に対応するパラメータ(以下、「東方向位置時変パラメータ」、「北方向位置時変パラメータ」、「速度時変パラメータ」、「方位時変パラメータ」及び「時間毎方位時変パラメータ」と称し、包括的に「時変パラメータ」と称する。)の値を変更する。時間変化の許容度を小さくしたい成分(変動量を小さくしたい成分)の時変パラメータは値を小さくし、許容度を大きくしたい成分(変動量を大きくしたい成分)の時変パラメータは値を大きくする。
移動体が停止状態であると判定した場合は、ステップH9において、観測ベクトル「Z」に含まれる移動速度を「0」に修正することにしている。そのため、移動速度に対する誤差は小さいものと推定し、観測誤差共分散行列「R1」に含まれる速度誤差パラメータの値「c1」が他の移動状態における速度誤差パラメータの値に比べて小さくなるように設定する。また、式(12)においては、例えば「a1=b1>d1>c1>e1」となるように値を設定する。
また、移動速度については時間変化の許容度を大きくするために、システムノイズ行列「Q1」に含まれる速度時変パラメータの値「x1」が他の移動状態における速度時変パラメータの値よりも大きくなるように設定する。また、式(13)においては、例えば「z1<y1<v1=w1<x1」となるように値を設定する。
但し、停止状態では、時間毎方位は限りなく「0」に近いものと考えられるため、時間毎方位誤差パラメータの値「e1」及び時間毎方位時変パラメータの値「z1」が最も小さくなるように調整している。
その後、ホストCPU40は、観測ベクトル「Z」に含まれる東方向の位置、北方向の位置及び方位を、前回の東方向の位置、前回の北方向の位置及び前回の方位にそれぞれ修正する(ステップH7)。また、観測ベクトル「Z」に含まれる移動速度及び時間毎方位を「0」に修正する(ステップH9)。これは、停止判定の場合は、移動体の位置及び方位は前回の位置算出時と同じであると考えられ、また、移動速度及び方位の変化量は「0」であると考えられるためである。その後、ホストCPU40は、KFパラメータ設定処理を終了する。
また、ステップH1において判定結果が「停止」ではなかったと判定した場合は(ステップH1;No)、ホストCPU40は、加減速判定処理における判定結果が「加減速無し」であったか否かを判定する(ステップH11)。そして、「加減速無し」であったと判定した場合は(ステップH11;Yes)、観測誤差共分散行列「R」に次式(14)で表される「R2」を設定する(ステップH13)。また、システムノイズ行列「Q」に次式(15)で表される「Q2」を設定する(ステップH15)。
Figure 2010117148
Figure 2010117148
移動体が加減速無し状態であると判定した場合は、方位及び時間毎方位については誤差が小さいであろうと推定して、式(14)においては、例えば「a2=b2>c2>d2>e2」となるように値を設定する。また、方位及び時間毎方位の時間変化の許容度を小さくするために、式(15)においては、例えば「v2=w2>x2>y2>z2」となるように値を設定する。その後、ホストCPU40は、KFパラメータ設定処理を終了する。
また、ステップH11において判定結果が「加減速無し」ではなかったと判定した場合は(ステップH11;No)、ホストCPU40は、旋回判定処理における判定結果が「旋回」であったか否かを判定する(ステップH17)。そして、「旋回」であったと判定した場合は(ステップH17;Yes)、観測誤差共分散行列「R」に次式(16)で表される「R3」を設定する(ステップH19)。また、システムノイズ行列「Q」に次式(17)で表される「Q3」を設定する(ステップH21)。
Figure 2010117148
Figure 2010117148
移動体が旋回状態にあると判定した場合は、方位及び時間毎方位は誤差が小さいであろうと推定して、観測誤差共分散行列「R3」に含まれる方位誤差パラメータの値「d3」及び時間毎方位誤差パラメータ「e3」が他の移動状態における方位誤差パラメータ及び時間毎方位誤差パラメータの値に比べて小さくなるように設定する。また、式(16)においては、例えば「a3=b3>c3>d3>e3」となるように値を設定する。
また、位置及び方位については時間変化の許容度を大きくするために、システムノイズ行列「Q3」に含まれる位置時変パラメータの値「v3、w3」及び方位時変パラメータの値「y3」が他の移動状態における位置時変パラメータ及び方位時変パラメータの値よりも大きくなるように設定する。また、式(17)においては、例えば「v3=w3>x3=y3>z3」となるように値を設定する。その後、ホストCPU40は、KFパラメータ設定処理を終了する。
また、ステップH17において判定結果が「旋回」ではなかったと判定した場合は(ステップH17;No)、ホストCPU40は、加減速判定処理における判定結果が「強加速或いは強減速」であったか否かを判定する(ステップH23)。そして、「強加速或いは強減速」であったと判定した場合は(ステップH23;Yes)、観測誤差共分散行列「R」に次式(18)で表される「R4」を設定する(ステップH25)。また、システムノイズ行列「Q」に次式(19)で表される「Q4」を設定する(ステップH27)。
Figure 2010117148
Figure 2010117148
移動体が強加減速状態にあると判定した場合は、移動速度に対する誤差は大きいであろうと推定して、観測誤差共分散行列「R4」に含まれる速度誤差パラメータの値「c4」が他の移動状態における速度誤差パラメータの値に比べて大きくなるように設定する。また、式(18)においては、例えば「a4=b4=d4>c4=e4」となるように値を設定する。
また、移動速度については時間変化の許容度を大きくするために、システムノイズ行列「Q4」に含まれる速度時変パラメータの値「x4」が他の移動状態における速度時変パラメータの値よりも大きくなるように設定する。また、式(19)においては、例えば「v4=w4>x4>y4>z4」となるように値を設定する。その後、ホストCPU40は、KFパラメータ設定処理を終了する。
また、ステップH23において判定結果が「強加速或いは強減速」ではなかったと判定した場合は(ステップH23;No)、ホストCPU40は、加減速判定処理における判定結果が「弱加速或いは弱減速」であったか否かを判定する(ステップH29)。そして、「弱加速或いは弱減速」であったと判定した場合は(ステップH29;Yes)、観測誤差共分散行列「R」に次式(20)で表される「R5」を設定する(ステップH31)。また、システムノイズ行列「Q」に次式(21)で表される「Q5」を設定する(ステップH33)。
Figure 2010117148
Figure 2010117148
移動体が弱加減速状態にあると判定した場合は、位置以外の成分は誤差が小さいであろうと推定して、式(20)においては、例えば「a5=b5>c5>d5>e5」となるように値を設定する。また、位置以外の成分の時間変化の許容度を小さくするために、式(21)においては、例えば「v5=w5>x5>y5>z5」となるように値を設定する。その後、ホストCPU40は、KFパラメータ設定処理を終了する。
また、ステップH29において判定結果が「弱加速或いは弱減速」ではなかったと判定した場合は(ステップH29;No)、ホストCPU40は、観測誤差共分散行列「R」に次式(22)で表される「R6」を設定する(ステップH35)。また、システムノイズ行列「Q」に次式(23)で表される「Q6」を設定する(ステップH37)。
Figure 2010117148
Figure 2010117148
移動体の判定結果が「停止」、「加減速無し」、「旋回」、「強加減速」、「弱加減速」の何れでもない場合は、位置の誤差は大きく、方位及び時間毎方位の誤差は小さい(方位センサ75により方位の検出を行っているため)ものと判断して、式(22)においては、例えば「a6=b6>c6>d6>e6」となるように値を設定する。また、位置及び時間毎方位の時間変化の許容度を大きくするために、式(23)においては、例えば「v6=w6=z6>x6>y6」となるように値を設定する。その後、ホストCPU40は、KFパラメータ設定処理を終了する。
図24のKF位置算出処理に戻って、KFパラメータ設定処理を行った後、ホストCPU40は、状態ベクトル「X」及び誤差共分散行列「P」について、次式(24)〜(26)に従って状態ベクトルの補正値「X」及び誤差共分散行列の補正値「P」を算出する補正演算を行う(ステップG11)。
Figure 2010117148
Figure 2010117148
Figure 2010117148
但し、「K」はカルマンゲイン行列と呼ばれる5×5の行列であり、「R」は、KFパラメータ設定処理で設定された5×5の観測誤差共分散行列である。また、「I」は単位行列である。補正演算を行った後、ホストCPU40は、KF位置算出処理を終了する。
図22の位置算出処理に戻って、ホストCPU40は、KF位置算出処理で算出した状態ベクトルの補正値「X」に含まれる東方向の位置「EastPos」及び北方向の位置「NorthPos」で表される位置を出力位置に決定する(ステップE13)。そして、ホストCPU40は、位置算出処理を終了する。
図18のメイン処理に戻って、位置算出処理を行った後、ホストCPU40は、出力位置をプロットしたナビゲーション画面を生成して表示部60に表示させる(ステップA13)。そして、ホストCPU40は、操作部50を介してユーザにより位置算出終了指示がなされたか否かを判定し(ステップA15)、なされなかったと判定した場合は(ステップA15;No)、ステップA1に戻る。また、位置算出終了指示がなされたと判定した場合は(ステップA15;Yes)、メイン処理を終了する。
5.作用効果
本実施形態によれば、GPS衛星からのGPS衛星信号に基づいて、最小二乗法を用いた位置算出処理を行って移動体の位置を算出する。そして、加速度センサ71、ジャイロセンサ73及び方位センサ75を含むセンサ部70の検出結果に基づいて移動体の移動状態を判定し、判定した移動状態に応じて、カルマンフィルタを用いた位置算出処理で用いられる誤差パラメータ(観測誤差共分散行列「R」)及び時変パラメータ(システムノイズ行列「Q」)の値を変更する。そして、算出した位置を、カルマンフィルタ位置算出処理を行って補正する。
各種センサの検出結果に基づいて移動体の移動状態を判定し、判定した移動状態に応じて誤差パラメータ及び時変パラメータの値を変更してカルマンフィルタ位置算出処理を行うことで、移動体の移動状態に応じた適切な位置算出を実現することが可能となり、位置算出の正確性が向上する。
6.変形例
6−1.携帯型ナビゲーションシステム
本発明は、自動車やバイクといった移動体に着脱可能な“携帯ナビ(携帯型ナビゲーションシステム)”に適用することも可能である。
6−2.電子機器
また、本発明は、携帯型電話機の他にも、位置算出装置を備えた電子機器であれば何れの電子機器にも適用可能である。例えば、ノート型パソコンやPDA(Personal Digital Assistant)、カーナビゲーション装置等についても同様に適用可能である。
6−3.衛星測位システム
上述した実施形態では、衛星測位システムとしてGPSを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムであってもよい。
6−4.処理の分化
ホストCPU40が実行する処理の一部又は全部を、ベースバンド処理回路部30の演算制御部31が行う構成としてもよい。例えば、メジャメント取得演算部33により演算されたメジャメント実測値を用いて、演算制御部31が位置算出処理を行う。また、停止判定処理、旋回判定処理、加減速判定処理も含めて、演算制御部31が全ての処理を行うこととしてもよい。
6−5.基礎KF位置算出処理
上述した実施形態では、LS位置算出処理を行ってKF位置算出処理の観測情報とする移動体の位置及び移動速度を算出するものとして説明したが、LS位置算出処理の代わりにカルマンフィルタを用いた位置算出処理を行って観測情報を算出することとしてもよい。この場合は、ROM80にLS位置算出プログラム805の代わりに基礎KF位置算出プログラムを格納しておき、ホストCPU40は、図22の位置算出処理のステップE1において、基礎KF位置算出プログラムを読み出して実行することで基礎KF位置算出処理を行う。
図26は、基礎KF位置算出処理の流れを示すフローチャートである。
先ず、ホストCPU40は、基礎KF位置算出処理用の状態ベクトル「X1」及び誤差共分散行列「P1」の初期値を設定する(ステップJ1)。基礎KF位置算出処理では、状態ベクトル「X1」を、例えば、ECEF座標系における携帯型電話機1の3次元の位置ベクトル、3次元の移動速度ベクトル、クロックバイアス及びクロックドリフトを成分とする8×1の行列とすることができる。
その後、ホストCPU40は、式(4)及び(5)に従って予測演算を行い、状態ベクトルの予測値「X1 」及び誤差共分散行列の予測値「P1 」を算出する(ステップJ3)。そして、ホストCPU40は、メジャメント取得演算部33からGPS衛星信号の受信周波数及びコード位相(メジャメント実測値)を取得して、2×1の観測ベクトル「Z1」とする(ステップJ5)。基礎KF位置算出処理では、受信周波数及びコード位相を観測情報としてカルマンフィルタ処理を行う。
次いで、ホストCPU40は、状態ベクトルの予測値「X1 」と所定の観測行列「H1」とを用いて予測観測ベクトル「H11 」を算出する(ステップJ7)。観測行列「H1」は、状態ベクトルの予測値「X1 」の各成分を用いて、観測ベクトル「Z1」の成分である受信周波数及びコード位相を予測するための行列である。観測行列「H1」は2×8の行列であり、状態ベクトルの予測値「X1 」は8×1の行列であるため、予測観測ベクトル「H11 」は2×1の行列となる。
その後、ホストCPU40は、式(24)〜(26)に従って補正演算を行い、状態ベクトルの予測値「X1 」及び誤差共分散行列の予測値「P1 」を算出する(ステップJ9)。そして、ホストCPU40は、基礎KF位置算出処理を終了する。
基礎KF位置算出処理を終了したら、ホストCPU40は、得られた状態ベクトルの補正値「X1 」に含まれるECEF座標系における位置ベクトル及び移動速度ベクトルに対して座標変換演算を行って、ENU座標系における位置及び移動速度を求める。そして、求めた位置及び移動速度と、方位センサ75により検出された方位及びこの方位を用いて算出した時間毎方位とを観測情報として、図24のKF位置算出処理を行う。
この場合には、基礎KF位置算出処理とKF位置算出処理との2段階のカルマンフィルタ処理を行って出力位置を決定することになる。尚、この他にも、例えば慣性航法演算処理を行ってKF位置算出処理の観測情報を算出することも可能であり、観測情報の算出方法は適宜変更可能である。
6−6.位置飛びの判定
KF位置算出処理において位置飛びを判定し、その判定結果に基づいて、KFパラメータを調整することとしてもよい。具体的には、ROM80にメインプログラム800のサブルーチンとして位置飛び判定プログラムを格納しておく。そして、ホストCPU40は、図24のKF位置算出処理のステップG9においてKFパラメータ設定処理を行った後に、位置飛び判定プログラムを読み出して実行することで位置飛び判定処理を行う。
図27は、位置飛び判定処理の流れを示すフローチャートである。
先ず、ホストCPU40は、図22の位置算出処理のステップE3で算出した東方向の位置及び北方向の位置で表される位置(以下、「LS算出位置」と称す。)と、図24のKF位置算出処理のステップG3の予測演算で求められた状態ベクトルの予測値「X」に含まれる東方向の位置及び北方向の位置で表される位置(以下、「KF予測位置」と称す。)との間の距離を算出する(ステップK1)。
その後、ホストCPU40は、ステップK1で算出したLS算出位置とKF予測位置間の距離が「500m」以上であるか否かを判定し(ステップK3)、「500m」以上であると判定した場合は(ステップK3;Yes)、位置飛びが発生したと判定した上で、位置飛び判定用のカウンタ値が「5」よりも小さいか否かを判定する(ステップK5)。
そして、カウンタ値が「5」よりも小さいと判定した場合は(ステップK5;Yes)、ホストCPU40は、観測ベクトル「Z」と予測観測ベクトル「HX」との差ベクトル「V=Z−HX」に含まれる各成分(東方向の位置、北方向の位置、移動速度、方位及び時間毎方位)を「0」に設定する(ステップK7)。
式(25)からわかるように、差ベクトル「V」が「0」になると右辺第2項が「0」となり、状態ベクトルの予測値「X」がそのまま補正値「X」となる。よって、位置飛びが発生したと判定した場合は、予測演算により求めた予測位置を出力位置に決定することになる。
その後、ホストCPU40は、カウンタ値をインクリメントして(ステップK9)、位置飛び判定処理を終了する。また、ステップK5においてカウンタ値が「5」以上であると判定した場合は(ステップK5;No)、ホストCPU40は、KFパラメータを初期化する(ステップK11)。位置飛びが所定回数以上連続して検出された場合は、KFパラメータが適切な値に設定されていない可能性があるため、KFパラメータを初期化することにしたものである。そして、ホストCPU40は、位置飛び判定処理を終了する。
また、ステップK3においてLS算出位置とKF予測位置間の距離が「500m」未満であると判定した場合は(ステップK3;No)、ホストCPU40は、カウンタ値をリセットして(ステップK13)、位置飛び判定処理を終了する。
6−7.方位の算出
上述した実施形態では、方位センサ75により検出された方位と、この方位の時間変化から求めた時間毎方位とを観測情報として用いるものとして説明したが、LS位置算出処理により求められた移動速度ベクトルを用いて方位を算出することとしてもよい。具体的には、東方向の移動速度「NorthVel」及び北方向の移動速度「EastVel」を用いて、次式(27)に従って位置算出平面における方位「Heading」を算出する。
Figure 2010117148
このようにして移動体の方位を算出する場合は、方位センサ75を携帯型電話機1に搭載しておく必要がなくなる。
携帯型電話機の概略外観図。 携帯型電話機の概略外観図。 携帯型電話機の機能構成を示すブロック図。 ROMに格納されたデータの一例を示す図。 RAMに格納されるデータの一例を示す図。 停止判定用ファジィルールのデータ構成の一例を示す図。 停止判定用メンバシップ関数の一例を示す図。 停止判定用判定結果テーブルのテーブル構成の一例を示す図。 停止判定処理の説明図。 停止判定処理の説明図。 停止判定処理の説明図。 旋回判定用ファジィルールのデータ構成の一例を示す図。 旋回判定用メンバシップ関数の一例を示す図。 旋回判定用判定結果テーブルのテーブル構成の一例を示す図。 センサデータのデータ構成の一例を示す図。 メジャメントデータのデータ構成の一例を示す図。 計測履歴データのデータ構成の一例を示す図。 メイン処理の流れを示すフローチャート。 停止判定処理の流れを示すフローチャート。 旋回判定処理の流れを示すフローチャート。 加減速判定処理の流れを示すフローチャート。 位置算出処理の流れを示すフローチャート。 LS位置算出処理の流れを示すフローチャート。 KF位置算出処理の流れを示すフローチャート。 KFパラメータ設定処理の流れを示すフローチャート。 基礎KF位置算出処理の流れを示すフローチャート。 位置飛び判定処理の流れを示すフローチャート。
符号の説明
1 携帯型電話機、 10 GPSアンテナ、 20 GPS受信部、 21 RF受信回路部、 30 ベースバンド処理回路部、 31 演算制御部、 33 メジャメント取得演算部、 35 ROM、 37 RAM、 40 ホストCPU、 50 操作部、 60 表示部、 70 センサ部、 71 加速度センサ、 73 ジャイロセンサ、 75 方位センサ、 80 ROM、 90 RAM、 100 携帯電話用アンテナ、 110 携帯電話用無線通信回路部

Claims (8)

  1. 測位用衛星からの測位用信号に基づいて移動体の位置を算出することと、
    加速度センサ、角速度センサ及び方位センサのうちの少なくとも1つを含むセンサ部の検出結果に基づいて前記移動体の移動状態を判定することと、
    前記判定された移動状態に応じて、所定のカルマンフィルタ処理で用いられる誤差パラメータの値を変更することと、
    前記算出された位置を前記カルマンフィルタ処理で補正することと、
    を含む位置算出方法。
  2. 前記センサ部には、少なくとも加速度センサが含まれ、
    前記移動状態の判定は、前記移動体が停止状態にあるか否かを判定することを含み、
    前記測位用衛星からの測位用信号に基づいて前記移動体の移動速度を算出することを更に含み、
    前記誤差パラメータには、前記算出された移動速度に対する誤差の大きさを表す速度誤差パラメータが含まれ、
    前記誤差パラメータの値の変更は、前記移動体が停止状態にあると判定されたか否かに応じて前記速度誤差パラメータの値を変更することを含む、
    請求項1に記載の位置算出方法。
  3. 前記センサ部には、少なくとも加速度センサが含まれ、
    前記移動状態の判定は、前記移動体が停止状態にあるか否かを判定することを含み、
    前記測位用衛星からの測位用信号に基づいて前記移動体の移動速度を算出することを更に含み、
    前記誤差パラメータには、前記算出された移動速度の時間変化の許容度を表す速度時変パラメータが含まれ、
    前記誤差パラメータの値の変更は、前記移動体が停止状態にあると判定されたか否かに応じて前記速度時変パラメータの値を変更することを含む、
    請求項1に記載の位置算出方法。
  4. 前記センサ部には、少なくとも角速度センサ及び方位センサが含まれ、
    前記移動状態の判定は、前記移動体が旋回状態にあるか否かを判定することを含み、
    前記誤差パラメータには、前記方位センサの検出結果に対する誤差の大きさを表す方位誤差パラメータが含まれ、
    前記誤差パラメータの値の変更は、前記移動体が旋回状態にあると判定されたか否かに応じて前記方位誤差パラメータの値を変更することを含む、
    請求項1〜3の何れか一項に記載の位置算出方法。
  5. 前記センサ部には、少なくとも角速度センサ及び方位センサが含まれ、
    前記移動状態の判定は、前記移動体が旋回状態にあるか否かを判定することを含み、
    前記誤差パラメータには、前記移動体の移動方向の時間変化の許容度を表す方向時変パラメータが含まれ、
    前記誤差パラメータの値の変更は、前記移動体が旋回状態にあると判定されたか否かに応じて前記方向時変パラメータの値を変更することを含む、
    請求項1〜3の何れか一項に記載の位置算出方法。
  6. 前記センサ部には、少なくとも加速度センサ及び方位センサが含まれ、
    前記移動状態の判定は、前記移動体が急加速状態にあるか否かを判定することを含み、
    前記測位用衛星からの測位用信号に基づいて前記移動体の移動速度を算出することを更に含み、
    前記誤差パラメータには、前記算出された移動速度に対する誤差の大きさを表す速度誤差パラメータが含まれ、
    前記誤差パラメータの値の変更は、前記移動体が急加速状態にあると判定されたか否かに応じて前記速度誤差パラメータの値を変更することを含む、
    請求項1に記載の位置算出方法。
  7. 前記センサ部には、少なくとも加速度センサ及び方位センサが含まれ、
    前記移動状態の判定は、前記移動体が急加速状態にあるか否かを判定することを含み、
    前記測位用衛星からの測位用信号に基づいて前記移動体の移動速度を算出することを更に含み、
    前記誤差パラメータには、前記算出された移動速度の時間変化の許容度を表す速度時変パラメータが含まれ、
    前記誤差パラメータの値の変更は、前記移動体が急加速状態にあると判定されたか否かに応じて前記速度時変パラメータの値を変更することを含む、
    請求項1に記載の位置算出方法。
  8. 測位用衛星からの測位用信号に基づいて移動体の位置を算出する算出部と、
    加速度センサ、角速度センサ及び方位センサのうちの少なくとも1つを含むセンサ部の検出結果に基づいて前記移動体の移動状態を判定する判定部と、
    前記判定部により判定された移動状態に応じて、所定のカルマンフィルタ処理で用いられる誤差パラメータの値を変更するパラメータ値変更部と、
    前記算出部により算出された位置を前記カルマンフィルタ処理で補正するカルマンフィルタ処理部と、
    を備える位置算出装置。
JP2008288529A 2008-11-11 2008-11-11 位置算出方法及び位置算出装置 Withdrawn JP2010117148A (ja)

Priority Applications (2)

Application Number Priority Date Filing Date Title
JP2008288529A JP2010117148A (ja) 2008-11-11 2008-11-11 位置算出方法及び位置算出装置
US12/613,154 US8134496B2 (en) 2008-11-11 2009-11-05 Position calculating method and position calculating device

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
JP2008288529A JP2010117148A (ja) 2008-11-11 2008-11-11 位置算出方法及び位置算出装置

Publications (1)

Publication Number Publication Date
JP2010117148A true JP2010117148A (ja) 2010-05-27

Family

ID=42164713

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2008288529A Withdrawn JP2010117148A (ja) 2008-11-11 2008-11-11 位置算出方法及び位置算出装置

Country Status (2)

Country Link
US (1) US8134496B2 (ja)
JP (1) JP2010117148A (ja)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2012147136A (ja) * 2011-01-07 2012-08-02 Kyocera Corp 無線端末及び移動通信方法
JP2012191456A (ja) * 2011-03-10 2012-10-04 Toshiba Corp 情報配信システム、情報配信方法、およびプログラム
KR101309291B1 (ko) 2010-07-05 2013-09-16 아주대학교산학협력단 실내 측위 시스템 및 그 방법

Families Citing this family (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
NZ538796A (en) * 2005-03-10 2007-05-31 Brunswick New Technologies Asi Vehicle location and navigation system
JP2008076818A (ja) * 2006-09-22 2008-04-03 Fujitsu Ltd 携帯端末装置
US8727056B2 (en) * 2011-04-01 2014-05-20 Navman Wireless North America Ltd. Systems and methods for generating and using moving violation alerts
JP5742450B2 (ja) * 2011-05-10 2015-07-01 セイコーエプソン株式会社 位置算出方法及び位置算出装置
FI123761B (fi) * 2011-09-16 2013-10-31 Suunto Oy Menetelmä mitattavissa olevan kohdesuureen määrittämiseksi ja vastaava järjestelmä
US9659500B2 (en) 2011-12-05 2017-05-23 Navman Wireless North America Ltd. Safety monitoring in systems of mobile assets
JP5122678B1 (ja) * 2011-12-09 2013-01-16 東京計器株式会社 衛星航法システムを用いた位置出力装置
US9231652B2 (en) * 2012-03-22 2016-01-05 Furuno Electric Co., Ltd. Method and device for searching signal, GNSS signal reception apparatus and information equipment terminal
CN102944889A (zh) * 2012-11-16 2013-02-27 成都西可科技有限公司 一种传感器辅助定位终端进行定位的系统及方法
JP6677533B2 (ja) * 2016-03-01 2020-04-08 クラリオン株式会社 車載装置、及び、推定方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE3418081A1 (de) 1984-05-16 1985-11-21 Teldix Gmbh, 6900 Heidelberg Ortungsverfahren fuer fahrzeuge, insbesondere fuer landfahrzeuge
US6826477B2 (en) * 2001-04-23 2004-11-30 Ecole Polytechnique Federale De Lausanne (Epfl) Pedestrian navigation method and apparatus operative in a dead reckoning mode
JP5398120B2 (ja) * 2007-03-22 2014-01-29 古野電気株式会社 Gps複合航法装置

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101309291B1 (ko) 2010-07-05 2013-09-16 아주대학교산학협력단 실내 측위 시스템 및 그 방법
JP2012147136A (ja) * 2011-01-07 2012-08-02 Kyocera Corp 無線端末及び移動通信方法
JP2012191456A (ja) * 2011-03-10 2012-10-04 Toshiba Corp 情報配信システム、情報配信方法、およびプログラム

Also Published As

Publication number Publication date
US20100117895A1 (en) 2010-05-13
US8134496B2 (en) 2012-03-13

Similar Documents

Publication Publication Date Title
JP2010117147A (ja) 位置算出方法及び位置算出装置
JP2010117148A (ja) 位置算出方法及び位置算出装置
JP5521531B2 (ja) 位置算出方法及び位置算出システム
JP5652049B2 (ja) 位置算出方法及び受信装置
JP5251090B2 (ja) 測位方法、プログラム及び測位装置
JP5270184B2 (ja) 衛星航法/推測航法統合測位装置
JP5034935B2 (ja) 測位方法、プログラム、測位装置及び電子機器
JP2010151459A (ja) 位置算出方法及び位置算出装置
JP4775478B2 (ja) 位置算出方法及び位置算出装置
US8630798B2 (en) Electronic system and method for personal navigation
EP2717014B1 (en) Position calculation method and position calculation device
JP5186873B2 (ja) 測位方法、プログラム、測位装置及び電子機器
JP2008038418A (ja) 測位装置及び初期化方法
JP2010169640A (ja) 位置算出方法及び位置算出装置
JP5119967B2 (ja) 測位方法、プログラム及び測位装置
JP2009222438A (ja) 移動体用測位装置
JP5332333B2 (ja) 測位方法、プログラム及び測位装置
JP2014219340A (ja) オフセット補正方法及びオフセット補正装置
JP5348093B2 (ja) 位置算出方法及び位置算出装置
JP2009115514A (ja) 測位方法、プログラム、測位回路及び電子機器
JP2008215924A (ja) 測位装置、測位方法及びプログラム
JP2011128138A (ja) 位置算出方法及び位置算出システム
JP2013108930A (ja) 慣性航法演算方法及び慣性航法演算装置
JPH07294269A (ja) 車上測位装置
KR20140142610A (ko) 위치 측정 장치 및 방법

Legal Events

Date Code Title Description
A300 Application deemed to be withdrawn because no request for examination was validly filed

Free format text: JAPANESE INTERMEDIATE CODE: A300

Effective date: 20120207