以下、図面を参照して、位置算出装置を備えた電子機器の一種である携帯型電話機に本発明を適用した場合の実施形態について説明する。また、携帯型電話機は、ユーザーによって携帯されて移動したり、自動車等に配置設定されて移動する。このため、本実施形態では、携帯型電話機を保持或いは搭載した人や物のことを“移動体”として説明し、携帯型電話機が検出する各種方向を移動体の方向として説明する。尚、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
1.機能構成
図1は、携帯型電話機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変換回路、ベースバンド処理回路等によって構成される携帯電話の通信回路部であり、携帯電話用無線信号の変調・復調等を行うことで、通話やメールの送受信等を実現する。
2.データ構成
図2は、ROM80に格納されたデータの一例を示す図である。ROM80には、ホストCPU40により読み出され、メイン処理(図12参照)として実行されるメインプログラム801が記憶されている。また、メインプログラム801には、位置算出処理(図13参照)として実行される位置算出プログラム8011と、LS(Least Squared)位置算出処理として実行されるLS位置算出プログラム8013と、KF(Kalman Filter)位置算出処理(図15参照)として実行されるKF位置算出プログラム8015と、システムノイズ行列設定処理(図14参照)として実行されるシステムノイズ行列設定プログラム8017とがサブルーチンとして含まれている。
メイン処理では、ホストCPU40は、携帯型電話機1の本来の機能である通話やメールの送受信のための処理を行う他、携帯型電話機1の位置を算出する処理を行う。
位置算出処理では、ホストCPU40は、KF位置算出処理の基礎となる位置算出処理として、最小二乗法を用いた位置算出処理(LS位置算出処理)を行って移動体の位置、移動速度及び移動方向を求める。そして、これらの情報を観測情報としてカルマンフィルターを用いた位置算出処理(KF位置算出処理)を行うことで、最終的に表示部60に出力させる位置を決定する。
より具体的には、ホストCPU40は、LS位置算出処理を行ってECEF(Earth Centered Earth Fixed)座標系における移動体の位置、移動速度及び移動方向を求める。そして、このECEF座標系における処理結果に対して所定の座標変換演算を行うことで、ENU(East, North, Up)座標系における移動体の位置、移動速度及び移動方向を算出する。そして、ホストCPU40は、算出した位置、移動速度及び移動方向を観測情報とするKF位置算出処理を行うことで、ENU座標系における移動体の位置を出力位置として求める。
ECEF座標系は、地球中心地球固定座標系として知られ、地球の中心(地球重心)を原点とし、地球自転軸をZ軸、グリニジ子午面と赤道面とが交わる軸をX軸、これらの2軸と直交する軸をY軸とする3軸直交座標系である。また、ENU座標系は、東北上座標系として知られ、地表面付近のある点を原点として、天頂方向(地表面に対する垂直線の上方向)をU軸、東方向をE軸、北方向をN軸とする3軸直交座標系である。
ENU座標系における移動体の位置、移動速度及び移動方向は、ECEF座標系において求めた移動体の位置、移動速度及び移動方向に対して公知の座標変換演算(行列演算)を行うことで求めることができる。
システムノイズ行列設定処理では、ホストCPU40は、位置算出処理において算出されたENU座標系における移動体の移動速度及び移動方向に基づいて、カルマンフィルターにおける誤差パラメーターの一種であるシステムノイズを設定する。
図4は、本実施形態において使用するシステムノイズであるシステムノイズ行列「Q」の一例を示す図である。本実施形態では、移動体の状態を示すベクトルとして、次式(1)に示すような状態ベクトル「X」を用いてKF位置算出処理を行う。
式(1)において、「EastPos」は、ENU座標系における移動体の東西方向の位置座標を示しており、「NorthPos」は、ENU座標系における移動体の南北方向の位置座標を示している。また、「Speed」は、ENU座標系において天頂方向の位置変化がゼロであると仮定した場合における東西南北の2次元平面(以下、この平面を「位置算出平面」と称す。)における移動体の移動速度を示している。また、「Heading」は、位置算出平面における移動体の移動方向を示しており、「dHeading」は、位置算出平面における移動体の移動方向の時間変化である時間毎移動方向を示している。
状態ベクトル「X」を5次元のベクトルとしているため、システムノイズも5次元で考える必要がある。具体的には、状態ベクトル「X」の5次元の成分に対応する5×5の行列で表されるシステムノイズを考える。行と列の並びは、状態ベクトル「X」の5次元の成分にそれぞれ対応している。本実施形態において、システムノイズは行列の形で表されるため「システムノイズ行列」ともいう。
図4のシステムノイズ行列を見ると、対角成分以外の成分は全て「0」であり、対角成分にのみ値が設定されている。具体的には、東西方向の位置に対応する成分には「QEastPos」が設定され、南北方向の位置に対応する成分には「QNorthPos」が設定されている。また、移動速度に対応する成分には「QSpeed」が設定され、移動方向に対応する成分には「QHeading」が設定され、時間毎移動方向に対応する成分には「QdHeading」が設定されている。
本実施形態では、「QSpeed」、「QHeading」及び「QdHeading」には固定値を設定するが、「QEastPos」及び「QNorthPos」を移動体の移動方向に基づいて可変に設定する。「QEastPos」及び「QNorthPos」は、移動体の位置変化の許容度を表すパラメーターであるため、これらを包括的に「位置変化許容パラメーター」という。
図5及び図6は、位置変化許容パラメーターの値の算出方法を説明するための図である。これらの図において、横軸は東西方向(右方向が東方向)、縦軸は南北方向(上方向が北方向)を示している。また、実線で囲まれる範囲が、位置変化が許容される範囲(以下、「位置変化許容範囲」と称する。)を示している。
従来は、位置変化許容パラメーターの値を固定値としていた。具体的には、例えば図5に示すように、前回出力位置を中心とする半径「r」の円で囲まれる範囲を位置変化許容範囲とし、移動体の移動方向に関わらず、半径「r」の二乗値を「QEastPos」及び「QNorthPos」としていた。すなわち、「QEastPos=QNorthPos=r2」としていた。KF位置算出処理では、今回の位置計算により算出される位置が位置変化許容範囲内に収まるようにフィルターが作用する。
例えば、移動体が東方向に走行しているような場合を考えると、東方向に沿った位置が算出されるようにフィルターが作用することが望まれる。しかし、図5に示す位置変化許容範囲は、北方向に対しても東方向と同じだけの長さを持っているため、算出位置が北方向に大きくずれて算出される場合がある。これは、算出位置の直進性を低下させる大きな要因となっていた。
そこで、本実施形態では、移動体の移動方向を考慮して位置変化許容範囲を設定し、位置変化許容パラメーターの値を設定する。具体的には、図6に示すように、前回出力位置を中心とし、オーバル状の一種である楕円形状の位置変化許容範囲を設定する。この場合、位置変化許容範囲の形状は、位置変化許容範囲の大きさを決定付ける長さ「L」と、例えば東方向を基準方向とした場合に、基準方向と移動方向との成す角度「θ」とによって定めることができる。
この場合、位置変化許容範囲の東西方向の半径(以下、「東西方向径」と称す。)「LEastPos」は「Lcosθ」となり、位置変化許容範囲の南北方向の半径(以下、「南北方向径」と称す。)「LNorthPos」は「Lsinθ」となる。そして、東西方向径「LEastPos」の二乗値を「QEastPos」として設定し、南北方向径「LNorthPos」の二乗値を「QNorthPos」として設定する。すなわち、「QEastPos=L2 EastPos=L2cos2θ」、「QNorthPos=L2 NorthPos=L2sin2θ」とする。
但し、「θ=0、π」の場合は「QNorthPos=0」となり、「θ=π/2、3π/2」の場合は「QEastPos=0」となるため、楕円形状の位置変化許容範囲を定義することができない。そこで、位置変化許容パラメーター「QEastPos」及び「QNorthPos」に下限値「Qmin」を設定し、位置変化許容パラメーターの値が下限値「Qmin」に満たない場合には、位置変化許容パラメーターの値を下限値「Qmin」に補正する。下限値「Qmin」は、長さ「L」を用いて、例えば「Qmin=(L/10)2」とすることができる。
例えば、移動体が東方向に走行している場合を考える。この場合、「θ=0」であり、「L=3」を設定することにすると、「QEastPos=32cos20=9」となる。また、「QNorthPos=32sin20=0」となるが、これは下限値「Qmin=(3/10)2=0.09」に満たないため、「QNorthPos=0.09」となる。そうすると、位置変化許容範囲は、東西方向に対しては長いが、南北方向に対しては非常に短い楕円形状となる。その結果、東西方向に対しては位置変化が許容されるが南北方向に対しては位置変化が許容されにくくなり、移動方向に沿った位置が算出されるようにフィルターが作用し、算出位置の直進性が向上する。
ここまで位置変化許容パラメーターの値の算出方法について説明したが、本実施形態では、さらに移動体の移動状態に基づいて、位置変化許容範囲(楕円形状)の大きさを変更する。移動体の移動状態としては、例えば「停止」及び「移動」の2つの状態に分けることができる。
この場合、移動体が停止しているか移動しているかの判定は、例えばセンサー部70の検出結果に基づいて行うことができる。例えば、加速度センサー71の検出結果(移動体の加速度)とジャイロセンサー73の検出結果(移動体の回転有無)とを併用することで、移動体の移動状態を適確に把握できる。
その結果、移動体が停止していると判定した場合は、図7に示すように、例えば「L=1」とすることによって、位置変化許容範囲の大きさを小さくする。一方、移動体が移動していると判定した場合は、図8に示すように、例えば「L=4」とすることによって、位置変化許容範囲の大きさを大きくする。これにより、移動体が停止していると考えられる場合は、位置の変化を抑制するようにフィルターを作用させ、逆に、移動体が移動していると考えられる場合は、位置の変化を許容するようにフィルターを作用させることができる。すなわち、移動体の移動状態に応じて適切な位置変化許容パラメーターの値を設定して位置算出を行うことが可能となる。
データ構成の説明に戻って、図3は、RAM90に格納されるデータの一例を示す図である。RAM90には、センサーデータ901と、メジャメントデータ903と、計測履歴データ905と、KFパラメーターデータ907とが記憶される。
図9は、センサーデータ901のデータ構成の一例を示す図である。センサーデータ901には、検出時刻9011(例えばミリ秒)と対応付けて、加速度センサー71により検出された3次元の加速度9013と、ジャイロセンサー73により検出された3次元の角速度9015と、方位センサー75により検出された方位9017とが対応付けて記憶される。方位9017は、2次元平面における方位角で表される。例えば、検出時刻「t1」において検出された加速度9013は「(Ax1,Ay1,Az1)」であり、角速度9015は「(ωx1,ωy1,ωz1)」、方位9017は「h1」である。
図10は、メジャメントデータ903のデータ構成の一例を示す図である。メジャメントデータ903は、メジャメント取得演算部33により取得・演算されたメジャメント観測値に関するデータであり、捕捉衛星9031と、当該捕捉衛星から受信したGPS衛星信号の受信周波数及びコード位相でなるメジャメント観測値9033とが対応付けて記憶される。例えば、捕捉衛星「S1」についてのメジャメント観測値9033は、受信周波数が「f1」、コード位相が「CP1」である。
図11は、計測履歴データ905のデータ構成の一例を示す図である。計測履歴データ905は、ENU座標系における移動体の位置、移動速度及び移動方向についてのデータである。具体的には、各位置算出時刻9051について、東西方向の位置9052と、南北方向の位置9053と、東西方向の移動速度9054と、南北方向の移動速度9055と、位置算出平面における移動速度9056と、位置算出平面における移動方向9057とが対応付けて記憶される。
例えば、位置算出時刻「T1」においては、東西方向の位置として「EastPos1」、南北方向の位置として「NorthPos1」が記憶されている。また、東西方向の移動速度として「EastVel1」、南北方向の移動速度として「NorthVel1」が記憶されている。そして、位置算出平面における移動速度として「Speed1」、位置算出平面における移動方向として「θ1」が記憶されている。
KFパラメーターデータ907は、KF位置算出処理で用いるカルマンフィルターの各種パラメーターの値が記憶されたデータであり、例えば図4に示したようなシステムノイズ行列9071がこれに含まれる。
3.処理の流れ
図12は、ホストCPU40によりROM80に記憶されているメインプログラム801が読み出されて実行されることで、携帯型電話機1において実行されるメイン処理の流れを示すフローチャートである。メイン処理は、ホストCPU40が、操作部50を介してユーザーにより電源投入操作がなされたことを検出した場合に実行を開始する処理である。
特に説明しないが、以下のメイン処理の実行中は、GPSアンテナ10によるRF信号の受信や、RF受信回路部21によるRF信号からIF信号へのダウンコンバージョンが行われ、IF信号がベースバンド処理回路部30に随時出力される状態にあるものとする。また、メジャメント取得演算部33によりGPS衛星信号の受信周波数やコード位相(メジャメント観測値)の取得・演算が随時行われるものとし、ホストCPU40は、メジャメント取得演算部33からメジャメント観測値を取得して、RAM90のメジャメントデータ903に随時記憶させるものとする。また、ホストCPU40は、センサー部70から検出結果を取得し、RAM90のセンサーデータ901に随時記憶させるものとする。
先ず、ホストCPU40は、操作部50を介してユーザーによりなされた指示操作を判定し(ステップA1)、指示操作が通話指示操作であると判定した場合は(ステップA1;通話指示操作)、通話処理を行う(ステップA3)。具体的には、携帯電話用無線通信回路部110に無線基地局との間の基地局通信を行わせ、携帯型電話機1と他機との間の通話を実現する。
また、ステップA1において指示操作がメール送受信指示操作であると判定した場合は(ステップA1;メール送受信指示操作)、ホストCPU40は、メール送受信処理を行う(ステップA5)。具体的には、携帯電話用無線通信回路部110に基地局通信を行わせ、携帯型電話機1と他機との間のメールの送受信を実現する。
また、ステップA1において指示操作が位置算出指示操作であると判定した場合は(ステップA1;位置算出指示操作)、ホストCPU40は、ROM80から位置算出プログラム8011を読み出して実行することで、位置算出処理を行う(ステップA7)。
図13は、位置算出処理の流れを示すフローチャートである。
先ず、ホストCPU40は、KF位置算出処理の基礎となる位置算出処理として、ROM80に記憶されているLS位置算出プログラム8013を読み出して実行することでLS位置算出処理を行う(ステップB1)。
LS位置算出処理では、ホストCPU40は、メジャメントデータ903に記憶されている最新のメジャメント観測値9033に含まれるコード位相を用いて、携帯型電話機1と捕捉衛星間の擬似距離を算出し、算出した擬似距離を用いて最小二乗法を用いた位置算出演算を行って、ECEF座標系における移動体の位置を算出する。また、ホストCPU40は、最新のメジャメント観測値に含まれるGPS衛星信号の受信周波数を用いて、1秒間における受信周波数の変化量を算出し、算出した受信周波数の変化量を用いて、ECEF座標系における移動体の移動速度及び移動方向を算出する。
LS位置算出処理を行った後、ホストCPU40は、LS位置算出処理の処理結果に対して所定の座標変換演算を行うことで、ENU座標系における東西方向の位置「EastPos」、南北方向の位置「NorthPos」、東西方向の移動速度「EastVel」、南北方向の移動速度「NorthVel」を算出し、現在の位置算出時刻と対応付けてRAM90の計測履歴データ905に記憶させる(ステップB3)。
そして、ホストCPU40は、ステップB3で算出した東西方向の移動速度「EastVel」及び南北方向の移動速度「NorthVel」を用いて、次式(2)に従って位置算出平面における移動体の移動速度「Speed」を算出するとともに、次式(3)に従って移動体の移動方向「Heading」を算出し、計測履歴データ905に記憶させる(ステップB5)。
その後、ホストCPU40は、ROM80に記憶されているシステムノイズ行列設定プログラム8017を読み出して実行することで、システムノイズ行列設定処理を行う(ステップB7)。
図14は、システムノイズ行列設定処理の流れを示すフローチャートである。
先ず、ホストCPU40は、RAM90のセンサーデータ901に記憶されているセンサー部70の最新の検出結果に基づいて、移動体の移動状態を判定する(ステップC1)。
その後、ホストCPU40は、判定した移動状態に基づいて、位置変化許容範囲の大きさを決定付ける長さ「L」を決定する(ステップC3)。そして、ホストCPU40は、決定した長さ「L」を用いて、位置変化許容パラメーターの下限値「Qmin」を算出する(ステップC5)。
次いで、ホストCPU40は、長さ「L」と移動体の移動方向「θ」とを用いて、東西方向径「LEastPos」及び南北方向径「LNorthPos」を算出する(ステップC7)。そして、ホストCPU40は、算出した東西方向径「LEastPos」及び南北方向径「LNorthPos」を用いて、位置変化許容パラメーターの値「QEastPos」及び「QNorthPos」を算出する(ステップC9)。
その後、ホストCPU40は、位置変化許容パラメーターの値「QEastPos」及び「QNorthPos」のそれぞれが、ステップC5で算出した下限値「Qmin」未満であるか否かを判定する(ステップC11、C15)。そして、下限値「Qmin」未満であると判定した場合は(ステップC11;Yes、C15;Yes)、位置変化許容パラメーターの値を下限値「Qmin」に補正する(ステップC13、C17)。
次に、ホストCPU40は、位置変化許容パラメーターの値に「QEastPos」及び「QNorthPos」を設定し、位置以外の成分の変化許容パラメーターに所定値を設定することで、5×5のシステムノイズ行列を設定する(ステップC19)。そして、ホストCPU40は、システムノイズ行列設定処理を終了する。
図13の位置算出処理に戻って、システムノイズ行列設定処理を行った後、ホストCPU40は、ROM80に記憶されているKF位置算出プログラム8015を読み出して実行することで、KF位置算出処理を行う(ステップB9)。
図15は、KF位置算出処理の流れを示すフローチャートである。
先ず、KF位置算出処理用の状態ベクトル「X」及び誤差共分散行列「P」のKF初期値を設定する(ステップD1)。KF位置算出処理では、ENU座標系における移動体の東西方向の位置「EastPos」、南北方向の位置「NorthPos」、位置算出平面における移動速度「Speed」、移動方向「Heading」及び時間毎移動方向「dHeading」を成分とする式(1)に示した状態ベクトル「X」を用いて、予測演算及び補正演算を行う。特徴的であるのは、時間毎移動方向を状態ベクトルの成分とすることで、移動方向の演算精度を高めている点である。
また、誤差共分散行列「P」は、状態ベクトル「X」の5次元の各成分の誤差の共分散を示す5×5の行列である。状態ベクトル「X」及び誤差共分散行列「P」の初期値は、例えば1時刻前のKF位置算出処理で求められた状態ベクトル「X」及び誤差共分散行列「P」の補正値「X+」及び「P+」とすることができる。
次いで、ホストCPU40は、状態ベクトル「X」及び誤差共分散行列「P」について、次式(4)及び(5)に従って予測演算を行い、状態ベクトルの予測値「X
−」及び誤差共分散行列の予測値「P
−」を算出する(ステップC3)。
この際、状態ベクトル「X」の5次元の各成分については、次式(6)〜(10)に従って予測演算を行う。
但し、各式における添え字の「−」は予測値、「+」は補正値であることを示しており、「T」は転置行列であることを示している。また、「φ」は、状態遷移行列と呼ばれる状態ベクトルの遷移を表す5×5の行列であり、「Q」は、5×5のシステムノイズ行列である。状態遷移行列「φ」は予め定められた行列とすることができる。また、システムノイズ行列「Q」は、ステップB7のシステムノイズ行列設定処理で設定した行列とする。
その後、ホストCPU40は、RAM90の計測履歴データ905に記憶されている移動体の最新の東西方向の位置「EastPos」、南北方向の位置「NorthPos」、位置算出平面における移動速度「Speed」及び移動方向「Heading」を成分とする次式(11)で表される4×1の行列を、KF位置算出処理における観測ベクトル「Z」として設定する(ステップD5)。
また、ホストCPU40は、ステップD3で算出した状態ベクトルの予測値「X−」に所定の観測行列「H」を乗算することで、予測された観測ベクトル(以下、「予測観測ベクトル」と称す。)「HX−」を算出する(ステップD7)。
観測行列「H」は、状態ベクトルの予測値「X−」の各成分を用いて、観測ベクトル「Z」の成分である東西方向の位置「EastPos」、南北方向の位置「NorthPos」、位置算出平面における移動速度「Speed」及び移動方向「Heading」を予測するための行列である。観測行列「H」は4×5の行列であり、状態ベクトルの予測値「X−」は5×1の行列であるため、予測観測ベクトル「HX−」は4×1の行列となる。
次いで、ホストCPU40は、状態ベクトル「X」及び誤差共分散行列「P」について、次式(12)〜(14)に従って状態ベクトルの補正値「X
+」及び誤差共分散行列の補正値「P
+」を算出する補正演算を行う(ステップD9)。
但し、「K」はカルマンゲイン行列と呼ばれる5×4の行列である。また、「R」は、観測ベクトル「Z」に含まれる各成分の誤差の共分散を示す4×4の行列であり、その行と列の並びは、観測ベクトル「Z」の4つの成分(東西方向の位置、南北方向の位置、移動速度及び移動方向)にそれぞれ対応している。また、「I」は単位行列である。そして、ホストCPU40は、KF位置算出処理を終了する。
図13の位置算出処理に戻って、ホストCPU40は、KF位置算出処理で算出した状態ベクトルの補正値「X+」に含まれる東西方向の位置「EastPos」及び南北方向の位置「NorthPos」で表される位置を出力位置に決定する(ステップB11)。そして、ホストCPU40は、出力位置をプロットしたナビゲーション画面を生成して表示部60に表示させる(ステップB13)。
そして、ホストCPU40は、操作部50を介してユーザーにより位置算出終了指示操作がなされたか否かを判定し(ステップB15)、なされなかったと判定した場合は(ステップB15;No)、ステップB1に戻る。また、位置算出終了指示操作がなされたと判定した場合は(ステップB15;Yes)、位置算出処理を終了する。
図12のメイン処理に戻って、ステップA3〜A7の何れかの処理を行った後、ホストCPU40は、操作部50を介してユーザーにより電源切断指示操作がなされたか否かを判定し(ステップA9)、なされなかったと判定した場合は(ステップA9;No)、ステップA1に戻る。また、電源切断指示操作がなされたと判定した場合は(ステップA9;Yes)、メイン処理を終了する。
4.実験結果
図16は、本実施形態の手法を用いて実際に位置算出を行った実験結果の一例を示す図である。携帯型電話機1を搭載した移動体を道路に沿って南方向から北方向に向かって直進させた後、東方向に右折させた場合における出力位置をプロットした結果を示している。分かり易いように、従来の手法を用いて位置算出を行った結果も併せて示している。丸印が本実施形態の手法で位置算出を行った場合の出力位置を示しており、三角印が従来の手法で位置算出を行った場合の出力位置を示している。
この結果を見ると、従来の手法を用いた場合は、出力位置が道路に対して蛇行しており、出力位置の直進性が悪いことがわかる。一方、本実施形態の手法を用いた場合は、出力位置が道路に対してほぼ真っ直ぐとなっており、出力位置の直進性が向上していることがわかる。
5.作用効果
位置算出装置を具備した携帯型電話機1において、カルマンフィルター処理で用いる位置変化の許容度を表す位置変化許容パラメーターの値を、検出された移動方向に基づいて設定する。そして、設定した位置変化許容パラメーターの値を用いてカルマンフィルター処理を実行して位置を算出する。
カルマンフィルター処理の基礎となる位置算出処理(例えばLS位置算出処理)を行うことで得られた移動方向と、所与の基準方向(例えば東方向)との成す角度を用いて、オーバル状の一種である楕円形状の位置変化許容範囲を定める。そして、定めた位置変化許容範囲の長軸及び短軸の長さを用いて、位置変化許容パラメーターの値を設定する。
移動方向に対しては長く、移動方向以外の方向に対しては短くなるように位置変化許容範囲を設定することで、移動方向以外の方向に算出位置が大きくずれることを防止し、移動方向に沿った位置が算出されるようにすることができる。すなわち、算出位置の直進性を向上させることができる。
また、本実施形態では、センサー部70の検出結果に基づいて、移動体が停止及び移動の何れの移動状態であるかを判定する。そして、判定した移動状態に基づいて、位置変化許容範囲の大きさを変更する。具体的には、移動状態が停止であると判定した場合は、位置変化許容範囲を小さくし、移動状態が移動である場合は、位置変化許容範囲を大きくする。
これにより、移動体が停止していると考えられる場合は、位置の変化を抑制するようにフィルターを作用させ、逆に、移動体が移動していると考えられる場合は、位置の変化を許容するようにフィルターを作用させることができる。すなわち、移動体の移動状態に応じて適切な位置変化許容パラメーターの値を設定して位置算出を行うことが可能となる。
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が行う構成としてもよい。例えば、位置算出処理をホストCPU40が行うのではなく演算制御部31が行う。この場合、演算制御部31は、位置算出処理により求めた出力位置をホストCPU40に出力する。そして、ホストCPU40は、演算制御部31から入力した出力位置を各種アプリケーションに利用する。
6−5.位置変化許容範囲の形状
上述した実施形態では、オーバル状の一例として楕円形状に位置変化許容範囲を定める場合について説明したが、楕円形状の他にも、例えば角丸長方形状や卵型形状に位置変化許容範囲を定めることとしてもよい。
6−6.3次元への拡張
上述した実施形態では、天頂方向の位置変化をゼロとみなして、2次元平面において位置算出を行う場合について説明したが、天頂方向の位置変化も考慮して、3次元空間において位置算出を行うこととしてもよい。
この場合は、携帯型電話機1の状態を示すベクトルとして、式(1)の状態ベクトルの各成分に、天頂方向の位置「UpPos」及び天頂方向の移動速度「UpVel」を加えた次式(15)に示すような7×1の行列で表される状態ベクトル「X」を用いてKF位置算出処理を行うといった方法が考えられる。
図17は、この場合に用いるシステムノイズ行列「Q」の一例を示す図である。システムノイズ行列「Q」は7×7の行列で表され、その行と列の並びは、式(15)の状態ベクトル「X」の7次元の各成分に対応している。対角成分以外の成分は全て「0」であり、対角成分にのみ値が設定されている。天頂方向の位置に対応する成分には「QUpPos」が設定され、天頂方向の移動速度に対応する成分には「QUpVel」が設定されている。この場合は、「QEastPos」、「QNorthPos」及び「QUpPos」の値を移動体の移動方向に基づいて可変に設定する。
図18は、位置変化許容パラメーターの値の算出方法を説明するための図である。この場合は、前回出力位置を中心とする楕円体を考え、楕円体の内部を位置変化許容範囲として設定する。位置変化許容範囲の形状は、位置変化許容範囲の大きさを決定付ける長さ「L」と、例えば天頂方向と移動方向との成す角度「θ」と、東西南北の平面において東方向と移動方向との成す角度「φ」とによって定めることができる。
この場合、東西方向径「LEastPos」は「Lsinθcosφ」となり、南北方向径「LNorthPos」は「Lsinθsinφ」となる。また、位置変化許容範囲の天頂方向の半径(以下、「天頂方向径」と称す。)「LUpPos」は「Lcosθ」となる。そして、東西方向径「LEastPos」の二乗値を「QEastPos」に設定し、南北方向径「LNorthPos」の二乗値を「QNorthPos」に設定し、天領方向径「LUpPos」の二乗値を「QUpPos」に設定する。すなわち、「QEastPos=L2 EastPos=L2sin2θcos2φ」、「QNorthPos=L2 NorthPos=L2sin2θsin2φ」、「QUpPos=L2 UpPos=L2cos2θ」とする。
6−7.基準方向
上述した実施形態では、東方向を基準方向として、東方向と移動方向との成す角度「θ」を用いて位置変化許容範囲(楕円形状)を定義するものとして説明したが、基準方向は適宜設定変更することが可能である。
例えば図19に示すように、前回位置算出した際の移動方向(前回移動方向)を仮想東方向(基準方向)とし、仮想東方向に直交する方向を仮想北方向として定義する。この場合、東方向と仮想東方向との成す角度を「α」とすると、仮想東方向及び仮想北方向で成る座標系(以下、「仮想座標系」と称す。)は、東方向及び北方向で成る座標系(以下、「基準座標系」と称す。)を角度「α」だけ回転させた座標系として定義される。そして、仮想座標系において、仮想東方向と今回算出された移動方向との成す角度「θ」を用いて、位置変化許容範囲(楕円形状)を定義する。
また、例えば図20に示すように、今回の位置算出において算出された移動方向を仮想東方向(基準方向)として、同様に仮想座標系を定義することとしてもよい。この場合は、位置変化許容範囲の長軸方向は必ず移動体の移動方向と一致することになる。従って、移動体の移動方向に対して位置変化の許容度が必ず大きくなり、算出位置の直進性が格段に向上する。尚、これらの位置変化許容範囲の設定方法は、公知の行列演算によって座標系を回転させることで容易に実現することができる。
6−8.位置変化許容範囲の大きさを移動速度に基づいて変更
上述した実施形態では、移動体の移動状態に基づいて位置変化許容範囲の大きさを変更するものとして説明したが、移動体の移動速度に基づいて位置変化許容範囲の大きさを変更することにしてもよい。
この場合は、ROM80にシステムノイズ行列設定プログラム8017の代わりに第2のシステムノイズ行列設定プログラムを格納しておき、ホストCPU40は、図13の位置算出処理のステップB7において、第2のシステムノイズ行列設定プログラムを読み出して実行することで第2のシステムノイズ行列設定処理を行う。
図21は、第2のシステムノイズ行列設定処理の流れを示すフローチャートである。尚、図14のシステムノイズ行列設定処理と同一のステップについては同一の符号を付して説明を省略し、システムノイズ行列設定処理とは異なる部分について説明する。
先ず、ホストCPU40は、図13の位置算出処理のステップB5で算出された位置算出平面における移動速度に基づいて、位置変化許容範囲の長さ「L」を決定する(ステップF1)。具体的には、移動速度が大きいほど、長さ「L」が大きくなるようにする。すなわち、次式(16)に従って長さ「L」を算出する。
但し、「λ」は定数であり、「Speed」は移動速度である。
その後、ホストCPU40は、ステップF1で決定した長さ「L」を用いて、位置変化許容パラメーターの下限値「Qmin」を算出し(ステップC5)、ステップC7以降へと処理を移行する。
6−9.観測情報の算出
上述した実施形態では、KF位置算出処理の基礎となる位置算出処理としてLS位置算出処理を行い、KF位置算出処理の観測情報とする移動体の位置、移動速度及び移動方向を算出するものとして説明したが、観測情報の算出方法は適宜設定変更可能である。例えば、基礎となる位置算出処理として、カルマンフィルターを用いた位置算出処理(以下、「基礎KF位置算出処理」と称す。)を行って観測情報を算出することとしてもよい。
この場合は、ROM80にLS位置算出プログラム8013の代わりに基礎KF位置算出プログラムを格納しておき、ホストCPU40は、図13の位置算出処理のステップB1において、基礎KF位置算出プログラムを読み出して実行することで基礎KF位置算出処理を行う。
図22は、基礎KF位置算出処理の流れを示すフローチャートである。
先ず、ホストCPU40は、基礎KF位置算出処理用の状態ベクトル「X1」及び誤差共分散行列「P1」の初期値を設定する(ステップE1)。基礎KF位置算出処理では、状態ベクトル「X1」を、例えば、ECEF座標系における携帯型電話機1の3次元の位置(x,y,z)、3次元の移動速度ベクトル(u,v,w)、クロックバイアス(b)及びクロックドリフト(d)を成分とする8×1の行列とすることができる。
その後、ホストCPU40は、式(4)及び(5)に従って予測演算を行い、状態ベクトルの予測値「X1 −」及び誤差共分散行列の予測値「P1 −」を算出する(ステップE3)。そして、ホストCPU40は、メジャメント取得演算部33からGPS衛星信号の受信周波数及びコード位相(メジャメント観測値)を取得して、2×1の観測ベクトル「Z1」とする(ステップE5)。基礎KF位置算出処理では、GPS衛星信号の受信周波数及びコード位相を観測情報としてカルマンフィルター処理を行う。
次いで、ホストCPU40は、状態ベクトルの予測値「X1 −」と所定の観測行列「H1」とを用いて予測観測ベクトル「H1X1 −」を算出する(ステップE7)。観測行列「H1」は、状態ベクトルの予測値「X1 −」の各成分を用いて、観測ベクトル「Z1」の成分である受信周波数及びコード位相を予測するための行列である。観測行列「H1」は2×8の行列であり、状態ベクトルの予測値「X1 −」は8×1の行列であるため、予測観測ベクトル「H1X1 −」は2×1の行列となる。
その後、ホストCPU40は、式(12)〜(14)に従って補正演算を行い、状態ベクトルの予測値「X1 +」及び誤差共分散行列の予測値「P1 +」を算出する(ステップE9)。そして、ホストCPU40は、基礎KF位置算出処理を終了する。
基礎KF位置算出処理を行った後、ホストCPU40は、得られた状態ベクトルの補正値「X1 +」に含まれるECEF座標系における位置(x,y,z)及び移動速度(u,v,w)に対して座標変換演算を行って、ENU座標系における位置、移動速度及び移動方向を求める。そして、求めた位置、移動速度及び移動方向を観測情報として、図15のKF位置算出処理を行う。
この場合には、基礎KF位置算出処理とKF位置算出処理との2段階のカルマンフィルター処理を行って出力位置を決定することになる。尚、この他にも、例えば慣性航法演算処理を行ってKF位置算出処理の観測情報を算出することも可能であり、観測情報の算出方法は適宜変更可能である。
6−10.移動状態の種類
上述した実施形態では、移動体の移動状態を「停止」と「移動」の2状態の何れかとし、判定した移動状態に基づいて位置変化許容範囲の形状を変化させるものとして説明したが、移動体の移動状態を3以上の状態としても良い。
例えば、センサー部70の検出結果に基づいて、移動体の移動状態が「停止可能性大」、「停止可能性小」、「移動可能性小」、「移動可能性大」の4状態の何れであるかを判定する。そして、例えば、「停止可能性大」の場合は「L=1」とし、「停止可能性小」及び「移動可能性小」の場合は「L=3」とし、「移動可能性大」の場合は「L=4」とすることで、位置変化許容範囲の大きさを変更する。
6−11.移動方向の検出
上述した実施形態では、LS位置算出処理の処理結果を用いて移動体の移動方向を算出し、KF位置算出処理における観測情報として利用するものとして説明したが、方位センサー75により検出された方位を移動体の移動方向として、KF位置算出処理における観測情報として利用することとしてもよい。