位置算出方法及び位置算出装置
【課題】慣性航法を利用した位置算出の正確性を向上させるための新たな手法を提案すること。
【解決手段】自動車に設置されたジャイロセンサーの検出結果を用いて、移動体である自動車の姿勢及び移動方向が検出される。そして、検出された姿勢に基づいて、自動車に設置された加速度センサーの検出結果が、相対座標系から絶対座標系に座標変換され、座標変換後の検出結果を用いて、速度ベクトルが算出・更新される。そして、算出された速度ベクトルが検出された移動方向を用いて補正され、補正された速度ベクトルを用いて、自動車の位置が算出・更新される。
【解決手段】自動車に設置されたジャイロセンサーの検出結果を用いて、移動体である自動車の姿勢及び移動方向が検出される。そして、検出された姿勢に基づいて、自動車に設置された加速度センサーの検出結果が、相対座標系から絶対座標系に座標変換され、座標変換後の検出結果を用いて、速度ベクトルが算出・更新される。そして、算出された速度ベクトルが検出された移動方向を用いて補正され、補正された速度ベクトルを用いて、自動車の位置が算出・更新される。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、位置算出方法及び位置算出装置に関する。
【背景技術】
【0002】
測位用信号を利用した測位システムとしては、GPS(Global Positioning System)が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。GPSでは、複数のGPS衛星の位置や各GPS衛星から位置算出装置までの擬似距離等の情報に基づいて位置算出装置の位置座標と時計誤差とを求める位置算出計算を行う。
【0003】
測位用信号を利用した位置算出では、様々な誤差要因によって位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献1には、移動体の速度ベクトルを用いた慣性航法演算を行うことにより、GPSを利用して算出した位置を補正する技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開平8−68651号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
従来から用いられている慣性航法演算では、例えば加速度センサーを用いるのであれば、検出された加速度ベクトルを積分することで速度ベクトルを算出し、前回位置に今回の速度ベクトル分の移動量を加算することで今回位置を算出する手法がとられる。ここで「ベクトル」とは、方向及び大きさを表す便宜上の用語である。方向及び大きさを表す表現としては、例えば直交座標系や球座標系における各軸の値としてもよいことは勿論であるが、何れの表記法をとったとしても「ベクトル」と等価であることは自明である。従って、本明細書においては、方向及び大きさの両方を表す用語として「ベクトル」を用いる。
【0006】
しかし、加速度ベクトルが1回検出される毎に積分して速度ベクトルを1回算出し、その速度ベクトルの1回の算出毎に位置を算出(より正確には算定位置を更新)するというわけではない。加速度ベクトルから速度ベクトルを求め、求めた速度ベクトルから位置を算出するという一連の処理には、積分の他に、複数のベクトルを足し合わせる積算という処理が含まれているのである。すなわち、連続して検出された複数の加速度ベクトルを積分したベクトル(次式(1))を積算することで1つの速度ベクトル(次式(2))を算出したり、算出した複数の速度ベクトルを積算することで前回位置から今回位置を算出(算定位置を更新)したりする。
【数1】
【数2】
【0007】
例えば、複数の加速度ベクトルから1つの速度ベクトルを算出する場合について説明する。実際には微小な時間ではあるが、加速度ベクトルが検出されてから次の検出までの間(検出時間間隔)は、加速度ベクトルを一定とみなす。連続して検出された複数の加速度ベクトルそれぞれを検出時間間隔で積分し、積分したそれぞれの値を、所定の単位期間に亘って積算する(足し合わせる)。これにより、単位期間の速度ベクトルを求めるのである。例えば、ナビゲーションシステムにおいては、1秒間に1回の位置算出(位置更新)がなされるが、1秒の間には数多くの加速度検出が行われている。
【0008】
このように従来行っていた加速度ベクトルの検出から位置算出に至るまでの一連の処理には、次のような問題があった。すなわち、短い検出時間間隔で検出される加速度ベクトルを積分及び積算しながら速度ベクトルを逐次的に求めていくため、時間経過に伴って速度ベクトルに含まれる誤差が累積的に増加するという問題である。当然ではあるが、速度ベクトルに含まれる誤差が増加すると、位置算出の正確性の低下を招くことになる。
【0009】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、慣性航法を利用した位置算出の正確性を向上させるための新たな手法を提案することにある。
【課題を解決するための手段】
【0010】
以上の課題を解決するための第1の形態は、移動体の位置を算出する方法であって、前記移動体の移動方向を検出することと、前記移動体に設置された加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求めることと、前記速度ベクトルを前記検出された移動方向を用いて補正することと、前記補正された速度ベクトルを用いて前記移動体の算定位置を求めることで前記移動体の位置を算出することと、を含む方法である。
【0011】
また、他の形態として、移動体に設置されて当該移動体の位置を算出する位置算出装置であって、加速度センサーと、前記移動体の移動方向を検出する移動方向検出部と、前記加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求める速度ベクトル算出部と、前記速度ベクトル算出部により算出された速度ベクトルを前記移動方向検出部により検出された移動方向を用いて補正する速度ベクトル補正部と、前記速度ベクトル補正部により補正された速度ベクトルを用いて前記移動体の算定位置を求めることで前記移動体の位置を算出する位置算出部と、を備えた位置算出装置を構成してもよい。
【0012】
この第1の形態等によれば、移動体に設置された加速度センサーの検出結果を用いて移動体の速度ベクトルを求める。但し、求めた速度ベクトルをそのまま位置算出に利用するわけではない。求めた速度ベクトルを移動体の移動方向を用いて補正した上で、当該補正した速度ベクトルを用いて算定位置を求める。
【0013】
検出された複数の加速度ベクトルを積算して移動体の速度ベクトルを求める場合には、時間経過に伴って誤差が累積していくという問題がある。しかし、第1の形態等によれば、求めた移動体の速度ベクトルが移動体の移動方向を用いて補正されるため、積算による誤差を除去することができる。この結果、慣性航法を利用した位置算出の正確性の向上に資することとなる。
【0014】
また、第2の形態として、第1の形態の方法であって、前記補正することは、前記速度ベクトルの向きを、前記検出された移動方向に補正することを含む、方法を構成してもよい。
【0015】
加速度センサーは移動体に設置されているため、加速度センサーの移動体に対する相対的な向きは固定されている。そこで、この第2の形態のように、求めた速度ベクトルの向きを移動体の移動方向に補正することで、移動体の移動方向に沿った適切な速度ベクトルに補正することができる。
【0016】
また、第3の形態として、第1又は第2の形態の方法であって、前記移動体が回転したことを検出することを更に含み、前記補正することは、前記移動体が回転した場合に前記速度ベクトルの大きさを変更することを含む、方法を構成してもよい。
【0017】
この第3の形態によれば、移動体が回転した場合に速度ベクトルの大きさを変更することで、移動体の回転時の運動に応じた適切な速度ベクトルに補正することができる。
【0018】
第4の形態は、移動体の位置を算出する方法であって、前記移動体の移動方向を検出することと、前記移動体に設置された加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求めることと、前記速度ベクトルを用いて前記移動体の算定位置を求めることと、前記算定位置を前記検出された移動方向を用いて補正することと、を含む方法である。
【0019】
また、他の形態として、移動体に設置されて当該移動体の位置を算出する位置算出装置であって、加速度センサーと、前記移動体の移動方向を検出する移動方向検出部と、前記加速度センサーの検出結果を用いて前記移動体の速度ベクトルを算出する速度ベクトル算出部と、前記速度ベクトル算出部により算出された速度ベクトルを用いて前記移動体の算定位置を算出する位置算出部と、前記位置算出部により算出された算定位置を前記検出された移動方向を用いて補正する位置補正部と、を備えた位置算出装置を構成してもよい。
【0020】
この第4の形態等によれば、移動体に設置された加速度センサーの検出結果を用いて移動体の速度ベクトルを求め、当該速度ベクトルを用いて算定位置を求める。そして、算定位置を移動体の移動方向を用いて補正する。この場合も、第1の形態等と同様に、慣性航法を利用した位置算出の正確性を向上させることができる。
【0021】
また、第5の形態として、第1〜第4の何れかの形態の方法であって、前記移動体に設置されたジャイロセンサーの検出結果を用いて前記移動体の算定姿勢を求めることと、前記算定姿勢を用いて、前記移動体に対する前記加速度センサーの相対的な検出方向を絶対方向に座標変換することで、前記加速度センサーの検出結果を前記絶対方向に座標変換することと、を含み、前記速度ベクトルを求めることは、前記座標変換した後の検出結果を用いて速度ベクトルを求めることである、方法を構成してもよい。
【0022】
この第5の形態によれば、移動体に設置されたジャイロセンサーの検出結果を用いて移動体の算定姿勢を求める。そして、当該算定姿勢を用いて、移動体に対する加速度センサーの相対的な検出方向を絶対方向に座標変換することで、加速度センサーの検出結果を絶対方向に座標変換する。そして、座標変換した後の加速度センサーの検出結果を用いて速度ベクトルを求める。これにより、例えば、加速度センサーの相対的な検出方向ではなく、ジャイロセンサーが検出した姿勢に基づき加速度センサーの検出方向を補正した絶対方向の速度ベクトルを求めることが可能となる。
【0023】
また、第6の形態として、第1〜第5の何れかの形態の方法であって、前記移動体は自動車である、方法を構成してもよい。
【図面の簡単な説明】
【0024】
【図1】位置算出システムのシステム構成及び座標系の説明図。
【図2】カーナビゲーション装置の機能構成の一例を示すブロック図。
【図3】処理部を機能ブロックとして表した概念図。
【図4】(A)は直進時における速度ベクトル補正の説明図。(B)は回転時における速度ベクトル補正の説明図。
【図5】センサーデータのデータ構成の一例を示す図。
【図6】速度ベクトルデータのデータ構成の一例を示す図。
【図7】補正速度ベクトルデータのデータ構成の一例を示す図。
【図8】算定位置データのデータ構成の一例を示す図。
【図9】ナビゲーション処理の流れを示すフローチャート。
【図10】速度ベクトル算出処理の流れを示すフローチャート。
【図11】速度ベクトル補正処理の流れを示すフローチャート。
【図12】従来の手法により位置算出を行った実験結果の一例を示す図。
【図13】実施形態の手法により位置算出を行った実験結果の一例を示す図。
【図14】実施形態の手法により位置算出を行った実験結果の一例を示す図。
【図15】変形例において処理部を機能ブロックとして表した概念図。
【図16】位置補正の原理の説明図。
【図17】変形例における第2ナビゲーション処理の流れを示すフローチャート。
【図18】(A)及び(B)は回転時速度と補正係数との対応関係の一例を示すグラフ。
【発明を実施するための形態】
【0025】
以下、図面を参照して、移動体に設置されて当該移動体の位置を算出する位置算出装置及び位置算出システムの実施形態について説明する。なお、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【0026】
1.システム構成
図1は、本実施形態における位置算出システム1のシステム構成及び座標系の説明図である。位置算出システム1は、移動体である四輪自動車(以下、単に「自動車」と称す。)に、位置算出装置を具備した電子機器の一種であるカーナビゲーション装置2が搭載されている。
【0027】
カーナビゲーション装置2には、加速度センサー及びジャイロセンサーを具備するセンサーユニットであるIMU60が搭載されている。IMU60は、自動車に対する相対的な座標系における加速度及び角速度を検出して出力するように設置・構成されている。本実施形態では、この相対的な座標系が機体座標系(Body Frame)として知られるB座標系と一致するような姿勢でカーナビゲーション装置2が自動車に設置・固定されているものとして説明する。B座標系は、車両に対して前方を正とする前後方向をXB軸(ロール軸)、右方を正とする左右方向をYB軸(ピッチ軸)、鉛直下方を正とする上下方向をZB軸(ヨー軸)とする三次元直交座標系である。
【0028】
カーナビゲーション装置2は、GPS(Global Positioning System)を利用した位置算出演算と、IMU60の検出結果を利用した慣性航法演算との2種類の演算を行って位置を算出する。本実施形態では、位置算出を行う座標系を、絶対座標系の一種であり、NED(North East Down)座標系として知られるL座標系とする。L座標系は、北方を正とする南北方向をXL軸、東方を正とする東西方向をYL軸、鉛直下方を正とする高度方向をZL軸とする三次元直交座標系である。
【0029】
カーナビゲーション装置2は、ジャイロセンサーの検出結果に基づいて、B座標系からL座標系への座標変換行列を算出し、当該座標変換行列を用いて、加速度センサーのB座標系(相対座標系)における検出結果を、L座標系(絶対座標系)における検出結果に座標変換する。座標変換の際には、加速度センサーの検出結果のうち、加速度の方向を座標変換し、加速度の大きさは維持する。言い換えると、加速度センサーの相対的な検出方向を、絶対座標系における方向(絶対方向)に座標変換する。
【0030】
そして、L座標系における加速度センサーの検出結果及びジャイロセンサーの検出結果を用いて、慣性航法演算を行って自動車の位置を逐次的に算出(算定位置を更新)し、算出した位置(算定位置)をプロットしたナビゲーション画面を表示部に表示させるナビゲーション処理を行う。なお、本実施形態では、慣性航法演算を用いて算出した位置を、後述するGPS位置算出部20により算出された位置と区別するために「算定位置」と称する。
【0031】
2.機能構成
図2は、カーナビゲーション装置2の機能構成の一例を示すブロック図である。カーナビゲーション装置2は、GPSアンテナ10と、GPS位置算出部20と、処理部30と、操作部40と、表示部50と、IMU(Inertial Measurement Unit)60と、記憶部70とを備えて構成される。
【0032】
GPSアンテナ10は、測位用衛星の一種であるGPS衛星から発信されているGPS衛星信号を含むRF(Radio Frequency)信号を受信するアンテナであり、受信信号をGPS位置算出部20に出力する。GPS衛星信号は、拡散符号の一種であるCA(Coarse and Acquisition)コードによって、スペクトラム拡散方式として知られるCDMA(Code Division Multiple Access)方式によって変調された1.57542[GHz]の通信信号である。CAコードは、コード長1023チップを1PNフレームとする繰返し周期1msの擬似ランダム雑音符号であり、GPS衛星毎に異なるものである。
【0033】
GPS位置算出部20は、GPSアンテナ10から出力された信号に基づいて自動車(カーナビゲーション装置2)の位置を計測する回路或いは装置であり、いわゆるGPS受信装置に相当する機能ブロックである。図示を省略するが、GPS位置算出部20は、RF受信回路部と、ベースバンド処理回路部とを備えて構成される。なお、RF受信回路部と、ベースバンド処理回路部とは、それぞれ別のLSI(Large Scale Integration)として製造することも、1チップとして製造することも可能である。
【0034】
ベースバンド処理回路部は、RF受信回路部から出力された受信信号に対して相関処理等を行ってGPS衛星信号を捕捉・追尾し、GPS衛星信号から取り出した衛星軌道データや時刻データ等に基づいて、所定の位置算出計算を行って自動車の位置(位置座標)を算出する。そして、算出した位置をGPSデータ727として、処理部30に出力する。
【0035】
処理部30は、記憶部70に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置2の各部を統括的に制御する制御装置であり、CPU(Central Processing Unit)等のプロセッサーを有して構成される。処理部30は、IMU60の検出結果を利用した慣性航法演算処理を行って自動車の位置(位置座標)を算出する。そして、GPS位置算出部20により算出された位置(GPS算出位置)及び処理部30が算出した位置(慣性航法演算位置)をもとに、表示部50に現在位置を指し示した地図を表示させる処理を行う。
【0036】
図3は、処理部30を機能ブロックとして表した概念図である。処理部30は、姿勢算出部31と、座標変換部32と、速度ベクトル算出部33と、速度ベクトル補正部34と、位置算出部35とを機能部として有する。
【0037】
姿勢算出部31は、ジャイロセンサー63により検出されたB座標系における3軸の加速度を積分処理(積分及び積算)することで、自動車の姿勢角を算出する。姿勢算出部31は、算出した姿勢角を算定姿勢として、座標変換部32及び速度ベクトル補正部34に出力する。自動車の姿勢がわかれば、自動車の移動方向もわかる。そのため、姿勢算出部31は、自動車の移動方向を検出する移動方向検出部であるとも言える。
【0038】
座標変換部32は、姿勢算出部31から入力した算定姿勢に基づいて、IMU60のB座標系における検出結果をL座標系における検出結果に変換する座標変換処理を行う。具体的には、姿勢算出部31から入力したB座標系における3軸の姿勢角を用いて、B座標系からL座標系への座標変換行列を算出し、当該座標変換行列を用いて加速度センサー61の検出結果を変換する。そして、変換したL座標系における3軸の加速度をカーナビゲーション装置2の加速度ベクトルとして、速度ベクトル算出部33に出力する。座標変換部32は、加速度ベクトル算出部であるとも言える。
【0039】
速度ベクトル算出部33は、座標変換部32から入力した加速度ベクトルを積分処理することでカーナビゲーション装置2の速度ベクトルを算出し、算出した速度ベクトルを速度ベクトル補正部34に出力する。
【0040】
速度ベクトル補正部34は、速度ベクトル算出部33から入力した速度ベクトルを、姿勢算出部31から入力した算定姿勢に基づいて補正する。そして、補正した速度ベクトルを補正速度ベクトルとして、位置算出部35に出力する。
【0041】
位置算出部35は、速度ベクトル補正部34から入力した補正速度ベクトルを用いて、カーナビゲーション装置2の位置を算出する。そして、算出した位置を算定位置として、表示部50に出力する。
【0042】
図4を参照して、本実施形態における速度ベクトル補正の原理について説明する。説明を分かり易くするため、ここではL座標系の東西方向(YL軸)及び南北方向(XL軸)でなる2次元平面において速度ベクトルを補正する場合について図示・説明する。図4(A)は自動車の直進時における速度ベクトルの補正の説明図であり、図4(B)は自動車の回転(旋回)時における速度ベクトルの補正の説明図である。各図において、横軸はYL軸を示しており、縦軸はXL軸を示している。
【0043】
L座標系における加速度の東西方向成分及び南北方向成分を積分処理すると、速度の東西方向成分及び南北方向成分が得られる。図4において、YL軸上に記載した矢印が速度の東西方向成分を示しており、XL軸上に記載した矢印が速度の南北方向成分を示している。速度の東西方向成分及び南北方向成分を合成することで、大きさが|V|で、向きがθ’である速度ベクトルが得られる。すなわち、動径を|V|、偏角をθ’とする円座標で速度ベクトルが定義される。但し、図4では、速度ベクトルの向きを、速度ベクトルとYL軸との成す角度で表現している。上記の速度ベクトルが、速度ベクトル算出部33により算出される速度ベクトルである。
【0044】
速度ベクトルは、座標変換部32から出力されるL座標系における加速度ベクトルを積分処理することで求められる。より具体的には、所定の単位時間の間(単位期間)に検出された複数の加速度ベクトルを積分及び積算して当該単位期間における速度ベクトルの変位を算出し、前回(最新)の速度ベクトルに加算(積算)することで、速度ベクトルを新たに算出する。なお、単位期間における速度ベクトルの変化に相当するベクトル(式(1)のdvで表されるベクトル)のことを、便宜的に、前回の速度ベクトルに対する今回の速度ベクトルの「変位」と称する。また、この単位期間には複数の検出時間間隔が含まれる(つまり、単位期間中に複数の加速度ベクトルが検出される。)。
【0045】
このように加速度ベクトルを積分及び積算して速度ベクトルを逐次的に求めていく必要があるため、加速度ベクトルに誤差があると、求められる速度ベクトルに誤差が累積していくという問題がある。速度の東西方向成分及び速度の南北方向成分が正確に求まらなければ、図4に示す速度ベクトルの大きさ及び向きが正確に求まらず、誤差を含み得ることになる。
【0046】
そこで、速度ベクトルの大きさ及び向きを次のように補正する。先ず、速度ベクトルの向きθ’を、姿勢算出部31により算出されたカーナビゲーション装置2の移動方向θに補正する。速度ベクトルの向きθ’は、加速度ベクトルの積分処理を経て間接的に求められる向きであるのに対し、移動方向θは、センサーの検出結果から直接的に求められる自動車の向きである。そのため、補正速度ベクトルの向きは、自動車の移動方向に沿った正しい向きとなる。
【0047】
次に、速度ベクトルの大きさ|V|を|V|×αに補正する。ここで、「α」は速度ベクトルの補正係数である。本実施形態では、補正係数「α」を、自動車の直進時と回転時とで可変に設定して速度ベクトルを補正する。具体的には、自動車の直進時には、図4(A)に示すように補正係数「α=1」として、速度ベクトルの大きさをそのままにする。この場合、速度ベクトルの大きさと補正速度ベクトルの大きさとは同じとなる。
【0048】
一方、自動車の回転時には、図4(B)に示すように「α<1」として、速度ベクトルの大きさが小さくなるように補正する。この場合、補正速度ベクトルの大きさは速度ベクトルの大きさよりも小さくなる。
【0049】
実験結果で後述するが、本願発明者が種々の条件で実験を行った結果、補正係数「α」を「1」よりも僅かに小さな値とすることで、速度ベクトルの補正の効果が表れ、位置算出の正確性の向上に資することがわかった。例えば、自動車が曲がり角で曲がる場合に、速度ベクトルの大きさをそのままにして位置算出を行うと、実際には自動車は曲がっているのに対し、算出される位置は真っ直ぐに行き過ぎてしまい、追従性が悪くなるという問題がある。かかる問題に対処するため、本実施形態では、自動車の回転時には、速度ベクトルの大きさをやや小さく見積もって位置算出を行うことにする。なお、自動車の回転には、自動車の右折、左折、転回等、1周より短い回転が含まれる。
【0050】
図4では説明を分かり易くするために2次元平面で考えたが、3次元空間に拡張することは容易である。この場合は、東西南北の2次元平面における自動車の向きに加えて、上下方向の向きを考慮して同様の補正を行えばよい。すなわち、動径を|V|、偏角を(θ’,φ’)とする球座標で速度ベクトルを定義する。そして、速度ベクトルの向きを姿勢算出部31により算出された姿勢角(θ,φ)に補正し、速度ベクトルの大きさを|V|×αに補正するようにする。
【0051】
図2の機能ブロックの説明に戻って、操作部40は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部30に出力する。この操作部40の操作により、目的地の入力等の各種指示入力がなされる。
【0052】
表示部50は、LCD(Liquid Crystal Display)等により構成され、処理部30から入力される表示信号に基づいた各種表示を行う表示装置である。表示部50には、ナビゲーション画面等が表示される。
【0053】
IMU60は、例えば、加速度センサー61やジャイロセンサー63を備え、予めセンサーに対応付けて定められたセンサー座標系の直交3軸それぞれの軸方向の加速度及び各軸の軸回りの角速度を検出可能に構成される。なお、加速度センサー61及びジャイロセンサー63は、それぞれが独立したセンサーであってもよいし、一体型のセンサーであってもよい。
【0054】
記憶部70は、ROM(Read Only Memory)やフラッシュROM、RAM(Random Access Memory)等の記憶装置によって構成され、カーナビゲーション装置2のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【0055】
少なくとも、処理部30と、加速度センサー61と、を含んで本実施形態の位置算出装置が構成される。なお、位置算出装置は、GPSアンテナ10、GPS位置算出部20、操作部40、表示部50、ジャイロセンサー63、記憶部70等のその他の構成を含んでいてもよい。
【0056】
3.データ構成
図2に示すように、記憶部70には、プログラムとして、処理部30により読み出され、ナビゲーション処理(図9参照)として実行されるナビゲーションプログラム711が記憶されている。また、ナビゲーションプログラム711には、速度ベクトル算出処理(図10参照)として実行される速度ベクトル算出プログラム7111と、速度ベクトル補正処理(図11参照)として実行される速度ベクトル補正プログラム7113とがサブルーチンとして含まれている。
【0057】
ナビゲーション処理とは、処理部30が、速度ベクトル算出処理及び速度ベクトル補正処理を行って、慣性航法演算を行うための補正速度ベクトルを求め、当該補正速度ベクトルを用いて、自動車(カーナビゲーション装置2)の位置を算定する処理である。そして、算定位置をプロットしたナビゲーション画面を生成して、表示部50に表示させる。
【0058】
速度ベクトル算出処理とは、速度ベクトル算出部33が、座標変換部32によりB座標系からL座標系に座標変換された加速度ベクトルを積分処理することで、自動車の速度ベクトルを算出する処理である。
【0059】
また、速度ベクトル補正処理とは、速度ベクトル補正部34が、速度ベクトル算出部33により算出された速度ベクトルを、姿勢算出部31により算出された自動車の移動方向を用いて補正する処理である。これらの処理については、フローチャートを用いて詳細に後述する。
【0060】
また、記憶部70には、データとして、センサーデータ721と、速度ベクトルデータ723と、補正速度ベクトルデータ725と、GPSデータ727と、出力位置データ729と、回転時補正係数731とが記憶される。
【0061】
センサーデータ721は、IMU60を構成する加速度センサー61及びジャイロセンサー63によってそれぞれ検出された加速度及び角速度が時系列に記憶されたデータであり、そのデータ構成例を図5に示す。センサーデータ721には、IMU60の検出時刻7211と対応付けて、3次元の加速度7213と、3次元の角速度7215とが時系列に記憶される。
【0062】
速度ベクトルデータ723は、速度ベクトル算出処理により算出された速度ベクトルのデータであり、そのデータ構成例を図6に示す。速度ベクトルデータ723には、速度ベクトルの算出時刻7231と対応付けて、直交座標系であるL座標系で考えた場合の速度ベクトルのベクトル成分7233が時系列に記憶される。センサーデータ721の所定時間分の検出時刻「t1,t2,t3,・・・」の加速度のデータによって、算出時刻「T1」の速度ベクトルが算出される。
【0063】
補正速度ベクトルデータ725は、速度ベクトル補正処理で求められた補正速度ベクトルのデータであり、そのデータ構成例を図7に示す。補正速度ベクトルデータ725には、速度ベクトルの算出時刻7231と対応付けて、球座標系で考えた場合の補正速度ベクトルの大きさ7253及び向き7255と、直交座標系であるL座標系で考えた場合の補正速度ベクトルのベクトル成分7257とが時系列に記憶される。
【0064】
補正速度ベクトルの大きさ7253は、速度ベクトルデータ723の速度ベクトルのベクトル成分7233の二乗和の平方根によって算出される。すなわち、|V|=(Vx2+Vy2+Vz2)1/2である。一方、速度ベクトルの向き7255は、姿勢算出部31により算出・決定された自動車の算定姿勢(θ,φ)である。
【0065】
GPSデータ727は、GPS位置算出部20により算出・決定された位置(以下、「GPS算定位置」と称す。)が時系列に記憶されたデータである。GPSデータ727は、所定時間間隔(例えば10分間隔)で更新される。
【0066】
出力位置データ729は、表示部50に表示された出力位置のデータであり、そのデータ構成例を図8に示す。出力位置データ729には、速度ベクトルの算出時刻7231と対応付けて、出力位置7293と、測位種別7295とが記憶される。
【0067】
測位種別7295は、出力位置7293が、GPS位置算出部20により算出された位置と、位置算出部35により算出された位置との何れの位置であるかを示すフラグである。GPS位置算出部20により算出された位置である場合は「GPS」、位置算出部35により算出された位置である場合は「慣性航法」がそれぞれ記憶される。
【0068】
回転時補正係数731は、自動車の回転を検出した場合に速度ベクトルの大きさ7253を補正するための係数のデータである。詳細は後述する。
【0069】
4.処理の流れ
図9は、記憶部70に記憶されているナビゲーションプログラム711が処理部30により読み出されて実行されることで、カーナビゲーション装置2において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、IMU60により検出された加速度及び角速度が、記憶部70のセンサーデータ721に随時記憶されるものとする。
【0070】
最初に、処理部30は、初期設定を行う(ステップA1)。具体的には、慣性航法演算に使用する各パラメーターを初期化する。次いで、処理部30は、記憶部70に記憶されている速度ベクトル算出プログラム7111を読み出して実行することで、速度ベクトル算出処理を行う(ステップA3)。
【0071】
図10は、速度ベクトル算出処理の流れを示すフローチャートである。
最初に、姿勢算出部31は、ジャイロセンサー63の検出結果を所定期間に亘って積分して、当該所定期間分の姿勢角変化量を算出する(ステップB1)。そして、姿勢算出部31は、前回(最新)の姿勢角と、算出した姿勢角変化量とを用いて、姿勢角を新たに算出(積算)して、新たに算出した姿勢角で前回の姿勢角を更新する(ステップB3)。
【0072】
次いで、座標変換部32は、最新の姿勢角に基づいて、B座標系からL座標系への座標変換行列を算出する(ステップB5)。なお、B座標系からL座標系への座標変換行列については従来公知であるため、ここでは数式等を用いた詳細な説明を省略する。
【0073】
次いで、座標変換部32は、加速度センサー61のB座標系における3軸の検出結果を、ステップB5で算出した座標変換行列を用いてL座標系における3軸の検出結果に座標変換する(ステップB7)。これにより、L座標系における3軸の加速度ベクトルが求まる。そして、座標変換部32は、ステップB7で求めた加速度ベクトルを所定期間に亘って積分して、当該所定期間における速度ベクトルの変位を算出する(ステップB9)。
【0074】
次いで、速度ベクトル算出部33は、前回(最新)の速度ベクトルと、ステップB9で算出した速度ベクトルの変位とを用いて速度ベクトルを新たに算出して、記憶部70の速度ベクトルデータ723を更新する(ステップB11)。具体的には、前回(最新)の速度ベクトルに、ステップB9で算出した速度ベクトルの変位を足し合わせる(積算する)ことで、速度ベクトルを新たに算出する。そして、速度ベクトル算出部33は、速度ベクトル算出処理を終了する。
【0075】
図9のナビゲーション処理に戻って、速度ベクトル算出処理を行った後、速度ベクトル補正部34は、記憶部70に記憶されている速度ベクトル補正プログラム7113を読み出して実行することで、速度ベクトル補正処理を行う(ステップA5)。
【0076】
図11は、速度ベクトル補正処理の流れを示すフローチャートである。
先ず、速度ベクトル補正部34は、速度ベクトル算出処理で算出した速度ベクトルの向き7255を、ステップB3で算出した姿勢角から求められる自動車の移動方向に補正して、記憶部70の補正速度ベクトルデータ725を更新する(ステップC1)。
【0077】
次いで、速度ベクトル補正部34は、自動車の回転(旋回)判定を行う(ステップC3)。具体的には、例えばステップB3で算出した姿勢角の前回の姿勢角からの時間変化に基づいて、自動車が回転中であるか否かを判定する。なお、ジャイロセンサー63により検出された角速度そのものを用いて回転判定を行うこととしてもよい。
【0078】
自動車の回転を検出した場合は(ステップC5;Yes)、速度ベクトル補正部34は、記憶部70に記憶された回転時の補正係数として予め定められた回転時補正係数731(α<1)を用いて速度ベクトルの大きさ7253を補正して、補正速度ベクトルデータ725を更新する(ステップC7)。そして、速度ベクトル補正部34は、速度ベクトル補正処理を終了する。
【0079】
また、自動車の回転を検出しなかった場合は(ステップC5;No)、速度ベクトル補正部34は、速度ベクトルの大きさを補正することなく、速度ベクトル補正処理を終了する。
【0080】
図9のナビゲーション処理に戻って、速度ベクトル補正処理を行った後、位置算出部35は、前回(最新)の出力位置と、速度ベクトル補正処理で求めた補正速度ベクトルとを用いて自動車の位置を新たに算出し、記憶部70の出力位置データ729に記憶させる(ステップA7)。
【0081】
次いで、位置算出部35は、GPS位置算出部20からGPSデータ727を取得したか否かを判定し(ステップA9)、取得したと判定した場合は(ステップA9;Yes)、出力位置のリセットタイミングであるか否かを判定する(ステップA11)。
【0082】
出力位置のリセットタイミングは、種々のタイミングを設定することができる。例えば、自動車が停止したタイミングとすることができる。自動車の停止判定は、例えば速度ベクトルの大きさに基づいて判定することが可能である。すなわち、速度ベクトルの大きさが「0」に近い値として予め定められた所定の閾値以下である場合に、自動車は停止していると判定することができる。
【0083】
また、例えば、自動車がトンネル内などのGPS測位が不可能な環境から、トンネルの外などのGPS測位が可能な環境に変化したタイミング(GPS衛星信号の捕捉ができなくなってから捕捉ができるようになったタイミング)をリセットタイミングとしてもよい。他には、例えば所定時間が経過する毎(例えば30分毎)のタイミングをリセットタイミングとしてもよい。
【0084】
出力位置のリセットタイミングであると判定した場合は(ステップA11;Yes)、位置算出部35は、ステップA7で求めた出力位置を、GPS位置算出部20から取得したGPSデータ727のGPS算出位置でリセットする(ステップA13)。具体的には、ステップA7で求めた出力位置を、GPS算出位置に変更する。
【0085】
一方、ステップA9においてGPSデータを取得できなかったと判定した場合(ステップA9;No)、又は、ステップA11においてリセットタイミングではないと判定した場合は(ステップA11;No)、位置算出部35は、ステップA15へと処理を移行する。
【0086】
次いで、位置算出部35は、最新の出力位置(算定位置)で表示部50のナビゲーション画面の表示を更新する(ステップA15)。そして、位置算出部35は、位置算出を終了するか否かを判定し(ステップA17)、まだ終了しないと判定した場合は(ステップA17;No)、ステップA3に戻る。また、終了すると判定した場合は(ステップA17;Yes)、ナビゲーション処理を終了する。
【0087】
5.実験結果
図12〜図14を参照して、従来の手法と本実施形態の手法とのそれぞれを適用して位置算出を行った場合の実験結果について説明する。カーナビゲーション装置を搭載した自動車を、予め定められた周回経路に沿って2周させ、所定時間間隔で慣性航法演算を行って求めた算定位置を東西南北の2次元平面に順次にプロットする実験を行った。図12〜図14において、横軸は東西方向、縦軸は南北方向をそれぞれ示している。また、点線が実際の経路を示しており、太実線が算定位置の軌跡を示している。
【0088】
図12は、従来の手法により、速度ベクトルの補正を行わずに位置算出を行った実験結果の一例を示す図である。この結果を見ると、1周目の初期の間は実際の経路に沿った算定位置の軌跡が得られているが、1周目の途中から算定位置が実際の経路からずれていき、2周目には実際の経路から大きく外れていることがわかる。
【0089】
図13は、本実施形態の手法により、速度ベクトルの補正を行って位置算出を行った実験結果の一例を示す図である。ここでは、自動車の回転時における速度ベクトルの大きさの補正を行わずに、速度ベクトルの補正係数を常時「α=1」として実験を行った。この結果を見ると、実際の経路と比較するとやはり誤差が生じているものの、図12の従来の結果と比べると、実際の経路に近い算定位置の軌跡が得られていることがわかる。しかし、よく見ると、曲がり角の部分において算定位置が行き過ぎて外側にオーバーしており、曲がり角における追従性が低下していることがわかる。
【0090】
図14は、本実施形態の手法により、速度ベクトルの補正を行って位置算出を行った実験結果の一例を示す図である。ここでは、自動車の回転時における速度ベクトルの大きさの補正を行うこととし、回転時には補正係数「α」を0.9以上1.0未満の一定値として実験を行った。この結果を見ると、2周目においても、算定位置の軌跡が実際の経路とほぼ一致しており、良好な結果が得られていることがわかる。また、曲がり角の部分に着目すると、算定位置が外側に外れることもなく、実際の経路に沿った追従性の良い軌跡が得られており、本実施形態の手法の有効性が実証された。
【0091】
6.作用効果
本実施形態によれば、自動車に設置されたジャイロセンサー63の検出結果を用いて、移動体である自動車の姿勢及び移動方向が検出される。そして、検出された姿勢に基づいて、自動車に設置された加速度センサー61の検出結果が、相対座標系から絶対座標系に座標変換され、座標変換後の検出結果を用いて、速度ベクトルが算出・更新される。そして、算出された速度ベクトルが検出された移動方向を用いて補正され、補正された速度ベクトルを用いて、自動車の位置が算出・更新される。
【0092】
速度ベクトルは、加速度ベクトルを積分処理することで求められる。この際、加速度ベクトルを積分及び積算していく処理を行うために、求められる速度ベクトルには時間経過に伴って誤差が累積していくという問題がある。しかし、算出された速度ベクトルの向きを、他の検出方法により検出された自動車の移動方向に補正することにすれば、速度ベクトルの向きは自動車の移動方向に沿った正確な向きとなる。そして、補正された速度ベクトルを用いて自動車の位置を算出することで、算定位置の正確性を向上させることができる。
【0093】
また、自動車の直進時には、速度ベクトルの大きさをそのままとし、自動車の回転時には、速度ベクトルの大きさを僅かに小さく補正する。例えば、自動車が曲がり角で曲がる場合に、速度ベクトルの大きさをそのままにして位置算出を行うと、算出される位置の実際の位置に対する追従性が悪くなり、次第に乖離していくという問題がある。そのため、自動車の回転時に速度ベクトルの大きさをやや小さく見積もって位置算出を行うことで、算定位置の正確性を向上させることができる。
【0094】
7.変形例
7−1.電子機器
上述した実施形態では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【0095】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、PDA(Personal Digital Assistant)といった他の電子機器についても本発明を同様に適用して、当該電子機器の位置算出を実現することが可能である。
【0096】
7−2.衛星測位システム
また、上述した実施形態では、衛星測位システムとしてGPSを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムであってもよいことは勿論である。
【0097】
7−3.位置の補正
上述した実施形態では、自動車の移動方向に基づいて速度ベクトルを補正し、補正速度ベクトルを用いて位置算出を行うものとして説明したが、次のようにしてもよい。すなわち、速度ベクトルを補正せずに、速度ベクトルをそのまま用いて位置算出を行う。そして、算出された位置を自動車の移動方向に基づいて補正する。
【0098】
図15は、この場合における処理部30を機能ブロックとして表した概念図である。なお、図3に示す処理部30と同一の構成要素については同一の符号を付して説明を省略し、図3とは異なる機能ブロックを中心に説明する。図15の処理部30では、姿勢算出部31により算出・決定された算定姿勢(移動方向)が、座標変換部32及び位置補正部36に出力される。
【0099】
また、速度ベクトル算出部33により算出された速度ベクトルは、そのまま位置算出部35に出力される。そして、当該速度ベクトルを用いて、位置算出部35により位置が算出される。位置算出部35により算出された位置は、位置補正部36に出力される。そして、位置補正部36は、姿勢算出部31から入力した算定姿勢(移動方向)に基づいて、位置算出部35から入力した位置を補正する。
【0100】
図16は、位置補正の原理の説明図である。黒丸で示す時刻「T−1」における出力位置と、時刻「T」における速度ベクトル(向きθ’)とを用いて、白丸で示す時刻「T」における算定位置を求めることができる。すなわち、黒丸を速度ベクトルの向き「θ’」に単位時間分シフトさせた位置が白丸である。
【0101】
このとき、時刻「T−1」における出力位置と、時刻「T」における算定位置との間の距離「L」を、速度ベクトルを用いて算出する。そして、時刻「T−1」における出力位置を、姿勢算出部31から取得した移動体の移動方向「θ」に距離「L」だけシフトさせることで、二重丸で示す時刻「T」における補正位置を求める。出力位置のリセットタイミングでなければ、この補正位置が時刻「T」における出力位置となる。
【0102】
図17は、この場合に処理部30が実行する第2ナビゲーション処理の流れを示すフローチャートである。なお、図9のナビゲーション処理と同一のステップについては同一の符号を付して説明を省略し、ナビゲーション処理とは異なるステップを中心に説明する。
【0103】
第2ナビゲーション処理では、ステップA3において速度ベクトル算出処理を行った後、位置算出部35が、前回(最新)の出力位置と、算出した速度ベクトルとを用いて、自動車の位置を新たに算出して更新する(ステップD5)。そして、位置補正部36が、位置補正処理を行う(ステップD7)。位置補正処理では、図16で説明した位置補正の原理に従って位置を補正する。
【0104】
7−4.速度ベクトルの向きの補正
上述した実施形態では、速度ベクトルの向きを、検出した移動体の移動方向に補正するものとして説明したが、検出した移動方向を用いた補正であればよく、この補正方法に限られるわけではない。
【0105】
例えば、速度ベクトルの向きと、検出した移動体の移動方向との平均処理を行って速度ベクトルの向きを補正することとしてもよい。この場合は、ジャイロセンサー等のセンサーによって直接的に検出した移動体の移動方向の方が信頼性が高いため、例えば、検出した移動体の移動方向の重みを高くし(例えば0.8)、速度ベクトルの向きの重みを低くして(例えば0.2)、加重平均計算によって速度ベクトルを補正するなどすれば適切である。
【0106】
7−5.速度ベクトルの大きさの補正
上述した実施形態では、移動体の回転検出時には速度ベクトルの大きさを一律に小さく変更するものとして説明したが、次のように速度ベクトルの大きさを変更することとしてもよい。
【0107】
例えば、移動体の回転が最初に検出されたとき(回転開始時)における移動体の速度(以下、「回転開始時速度」と称す。)を記録しておく。そして、移動体の回転時に、その回転時における移動体の速度(以下、「回転時速度」と称す。)を、記録しておいた回転開始時速度と比較する。そして、その比較結果に基づいて速度ベクトルの大きさを変更する。具体的には、回転時速度が回転開始時速度よりも小さければ、速度ベクトルの大きさを大きく変更する(α>1)。一方、回転時速度が回転開始時速度よりも大きければ、速度ベクトルの大きさを小さく変更する(α<1)。
【0108】
回転時速度が回転開始時速度よりも小さい場合は、移動体はゆっくりと回転しているものと考えられる。この場合は、速度ベクトルの大きさをやや大きく見積もって位置算出を行うことで、算出される位置が移動体の実際の位置にうまく追従するように調整する。一方、回転時速度が回転開始時速度よりも大きい場合は、移動体は急回転しているものと考えられる。この場合は、速度ベクトルの大きさをやや小さく見積もって位置算出を行う。
【0109】
7−6.速度ベクトルの補正係数の設定
上記の速度ベクトルの大きさの補正とも関連するが、補正係数「α」を次のように設定してもよい。すなわち、移動体の回転速度(或いは角速度)の大きさに基づいて補正係数「α」を設定する。移動体の回転速度は、移動体が四輪自動車である場合は、例えばZB軸(ヨー軸)周りの回転速度とし、移動体が二輪自動車である場合は、例えばXB軸(ロール軸)周りの回転速度とするなどが考えられる。なお、回転速度は、ジャイロセンサー63により検出される角速度と等価である。すなわち、回転速度に「2π(rad)」を乗算した値が角速度である。
【0110】
図18は、この場合の補正係数設定の原理を説明するための図である。図18には、自動車の回転速度と、補正係数「α」との対応関係を示すグラフを示している。補正係数「α」を小さくし過ぎると、位置算出の正確性が低下することが実験により明らかとなった。そのため、補正係数「α」の下限値を予め設定しておくことが適切である。そして、例えば図18(A)に示すように、下限値まで低下しない範囲で、回転速度が大きくなるにつれて補正係数「α」を徐々に減衰させるようにする。
【0111】
また、例えば図18(B)に示すように、回転速度の閾値を予め設定しておく。そして、回転速度が閾値に達するまでは補正係数「α」を一律に「1」とし(α=1)、回転速度が閾値を超えると、補正係数「α」を段階的に減少させることとしてもよい。
【0112】
7−7.移動方向の検出
上述した実施形態では、ジャイロセンサーの検出結果を用いて移動体の姿勢を算出することで移動体の移動方向を検出するものとして説明した。しかし、信頼性の高い移動方向が得られるのであれば、他の検出手段によって移動方向を検出することとしてもよい。例えば、地磁気センサー等の方位センサーによって移動方向を検出してもよい。
【0113】
7−8.出力位置
上述した実施形態では、所定のリセットタイミングにおいて、GPSにより算出された位置で算定位置をリセットするものとして説明したが、GPSを利用して算出した位置(GPS算出位置)と、本実施形態の慣性航法演算により算出した位置(慣性航法演算位置)とを併用して、最終的な出力位置を算出・決定することとしてもよい。例えば、GPS算出位置と慣性航法演算位置との平均処理を行うフィルターや、確率理論に基づく推定方法の一種であるカルマンフィルター等のフィルター処理を行って出力位置を決定してもよい。
【符号の説明】
【0114】
1 位置算出システム、 2 カーナビゲーション装置、 10 GPSアンテナ、 20 GPS位置算出部、 30 処理部、 31 姿勢算出部、 32 座標変換部、 33 速度ベクトル算出部、 34 速度ベクトル補正部、 35 位置算出部、 36 位置補正部、 40 操作部、 50 表示部、 60 IMU、 61 加速度センサー、 63 ジャイロセンサー、 70 記憶部
【技術分野】
【0001】
本発明は、位置算出方法及び位置算出装置に関する。
【背景技術】
【0002】
測位用信号を利用した測位システムとしては、GPS(Global Positioning System)が広く知られており、携帯型電話機やカーナビゲーション装置等に内蔵された位置算出装置に利用されている。GPSでは、複数のGPS衛星の位置や各GPS衛星から位置算出装置までの擬似距離等の情報に基づいて位置算出装置の位置座標と時計誤差とを求める位置算出計算を行う。
【0003】
測位用信号を利用した位置算出では、様々な誤差要因によって位置算出の正確性が低下するという問題があり、位置算出の正確性を向上させるための様々な技術が考案されている。例えば、特許文献1には、移動体の速度ベクトルを用いた慣性航法演算を行うことにより、GPSを利用して算出した位置を補正する技術が開示されている。
【先行技術文献】
【特許文献】
【0004】
【特許文献1】特開平8−68651号公報
【発明の概要】
【発明が解決しようとする課題】
【0005】
従来から用いられている慣性航法演算では、例えば加速度センサーを用いるのであれば、検出された加速度ベクトルを積分することで速度ベクトルを算出し、前回位置に今回の速度ベクトル分の移動量を加算することで今回位置を算出する手法がとられる。ここで「ベクトル」とは、方向及び大きさを表す便宜上の用語である。方向及び大きさを表す表現としては、例えば直交座標系や球座標系における各軸の値としてもよいことは勿論であるが、何れの表記法をとったとしても「ベクトル」と等価であることは自明である。従って、本明細書においては、方向及び大きさの両方を表す用語として「ベクトル」を用いる。
【0006】
しかし、加速度ベクトルが1回検出される毎に積分して速度ベクトルを1回算出し、その速度ベクトルの1回の算出毎に位置を算出(より正確には算定位置を更新)するというわけではない。加速度ベクトルから速度ベクトルを求め、求めた速度ベクトルから位置を算出するという一連の処理には、積分の他に、複数のベクトルを足し合わせる積算という処理が含まれているのである。すなわち、連続して検出された複数の加速度ベクトルを積分したベクトル(次式(1))を積算することで1つの速度ベクトル(次式(2))を算出したり、算出した複数の速度ベクトルを積算することで前回位置から今回位置を算出(算定位置を更新)したりする。
【数1】
【数2】
【0007】
例えば、複数の加速度ベクトルから1つの速度ベクトルを算出する場合について説明する。実際には微小な時間ではあるが、加速度ベクトルが検出されてから次の検出までの間(検出時間間隔)は、加速度ベクトルを一定とみなす。連続して検出された複数の加速度ベクトルそれぞれを検出時間間隔で積分し、積分したそれぞれの値を、所定の単位期間に亘って積算する(足し合わせる)。これにより、単位期間の速度ベクトルを求めるのである。例えば、ナビゲーションシステムにおいては、1秒間に1回の位置算出(位置更新)がなされるが、1秒の間には数多くの加速度検出が行われている。
【0008】
このように従来行っていた加速度ベクトルの検出から位置算出に至るまでの一連の処理には、次のような問題があった。すなわち、短い検出時間間隔で検出される加速度ベクトルを積分及び積算しながら速度ベクトルを逐次的に求めていくため、時間経過に伴って速度ベクトルに含まれる誤差が累積的に増加するという問題である。当然ではあるが、速度ベクトルに含まれる誤差が増加すると、位置算出の正確性の低下を招くことになる。
【0009】
本発明は上述した課題に鑑みて為されたものであり、その目的とするところは、慣性航法を利用した位置算出の正確性を向上させるための新たな手法を提案することにある。
【課題を解決するための手段】
【0010】
以上の課題を解決するための第1の形態は、移動体の位置を算出する方法であって、前記移動体の移動方向を検出することと、前記移動体に設置された加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求めることと、前記速度ベクトルを前記検出された移動方向を用いて補正することと、前記補正された速度ベクトルを用いて前記移動体の算定位置を求めることで前記移動体の位置を算出することと、を含む方法である。
【0011】
また、他の形態として、移動体に設置されて当該移動体の位置を算出する位置算出装置であって、加速度センサーと、前記移動体の移動方向を検出する移動方向検出部と、前記加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求める速度ベクトル算出部と、前記速度ベクトル算出部により算出された速度ベクトルを前記移動方向検出部により検出された移動方向を用いて補正する速度ベクトル補正部と、前記速度ベクトル補正部により補正された速度ベクトルを用いて前記移動体の算定位置を求めることで前記移動体の位置を算出する位置算出部と、を備えた位置算出装置を構成してもよい。
【0012】
この第1の形態等によれば、移動体に設置された加速度センサーの検出結果を用いて移動体の速度ベクトルを求める。但し、求めた速度ベクトルをそのまま位置算出に利用するわけではない。求めた速度ベクトルを移動体の移動方向を用いて補正した上で、当該補正した速度ベクトルを用いて算定位置を求める。
【0013】
検出された複数の加速度ベクトルを積算して移動体の速度ベクトルを求める場合には、時間経過に伴って誤差が累積していくという問題がある。しかし、第1の形態等によれば、求めた移動体の速度ベクトルが移動体の移動方向を用いて補正されるため、積算による誤差を除去することができる。この結果、慣性航法を利用した位置算出の正確性の向上に資することとなる。
【0014】
また、第2の形態として、第1の形態の方法であって、前記補正することは、前記速度ベクトルの向きを、前記検出された移動方向に補正することを含む、方法を構成してもよい。
【0015】
加速度センサーは移動体に設置されているため、加速度センサーの移動体に対する相対的な向きは固定されている。そこで、この第2の形態のように、求めた速度ベクトルの向きを移動体の移動方向に補正することで、移動体の移動方向に沿った適切な速度ベクトルに補正することができる。
【0016】
また、第3の形態として、第1又は第2の形態の方法であって、前記移動体が回転したことを検出することを更に含み、前記補正することは、前記移動体が回転した場合に前記速度ベクトルの大きさを変更することを含む、方法を構成してもよい。
【0017】
この第3の形態によれば、移動体が回転した場合に速度ベクトルの大きさを変更することで、移動体の回転時の運動に応じた適切な速度ベクトルに補正することができる。
【0018】
第4の形態は、移動体の位置を算出する方法であって、前記移動体の移動方向を検出することと、前記移動体に設置された加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求めることと、前記速度ベクトルを用いて前記移動体の算定位置を求めることと、前記算定位置を前記検出された移動方向を用いて補正することと、を含む方法である。
【0019】
また、他の形態として、移動体に設置されて当該移動体の位置を算出する位置算出装置であって、加速度センサーと、前記移動体の移動方向を検出する移動方向検出部と、前記加速度センサーの検出結果を用いて前記移動体の速度ベクトルを算出する速度ベクトル算出部と、前記速度ベクトル算出部により算出された速度ベクトルを用いて前記移動体の算定位置を算出する位置算出部と、前記位置算出部により算出された算定位置を前記検出された移動方向を用いて補正する位置補正部と、を備えた位置算出装置を構成してもよい。
【0020】
この第4の形態等によれば、移動体に設置された加速度センサーの検出結果を用いて移動体の速度ベクトルを求め、当該速度ベクトルを用いて算定位置を求める。そして、算定位置を移動体の移動方向を用いて補正する。この場合も、第1の形態等と同様に、慣性航法を利用した位置算出の正確性を向上させることができる。
【0021】
また、第5の形態として、第1〜第4の何れかの形態の方法であって、前記移動体に設置されたジャイロセンサーの検出結果を用いて前記移動体の算定姿勢を求めることと、前記算定姿勢を用いて、前記移動体に対する前記加速度センサーの相対的な検出方向を絶対方向に座標変換することで、前記加速度センサーの検出結果を前記絶対方向に座標変換することと、を含み、前記速度ベクトルを求めることは、前記座標変換した後の検出結果を用いて速度ベクトルを求めることである、方法を構成してもよい。
【0022】
この第5の形態によれば、移動体に設置されたジャイロセンサーの検出結果を用いて移動体の算定姿勢を求める。そして、当該算定姿勢を用いて、移動体に対する加速度センサーの相対的な検出方向を絶対方向に座標変換することで、加速度センサーの検出結果を絶対方向に座標変換する。そして、座標変換した後の加速度センサーの検出結果を用いて速度ベクトルを求める。これにより、例えば、加速度センサーの相対的な検出方向ではなく、ジャイロセンサーが検出した姿勢に基づき加速度センサーの検出方向を補正した絶対方向の速度ベクトルを求めることが可能となる。
【0023】
また、第6の形態として、第1〜第5の何れかの形態の方法であって、前記移動体は自動車である、方法を構成してもよい。
【図面の簡単な説明】
【0024】
【図1】位置算出システムのシステム構成及び座標系の説明図。
【図2】カーナビゲーション装置の機能構成の一例を示すブロック図。
【図3】処理部を機能ブロックとして表した概念図。
【図4】(A)は直進時における速度ベクトル補正の説明図。(B)は回転時における速度ベクトル補正の説明図。
【図5】センサーデータのデータ構成の一例を示す図。
【図6】速度ベクトルデータのデータ構成の一例を示す図。
【図7】補正速度ベクトルデータのデータ構成の一例を示す図。
【図8】算定位置データのデータ構成の一例を示す図。
【図9】ナビゲーション処理の流れを示すフローチャート。
【図10】速度ベクトル算出処理の流れを示すフローチャート。
【図11】速度ベクトル補正処理の流れを示すフローチャート。
【図12】従来の手法により位置算出を行った実験結果の一例を示す図。
【図13】実施形態の手法により位置算出を行った実験結果の一例を示す図。
【図14】実施形態の手法により位置算出を行った実験結果の一例を示す図。
【図15】変形例において処理部を機能ブロックとして表した概念図。
【図16】位置補正の原理の説明図。
【図17】変形例における第2ナビゲーション処理の流れを示すフローチャート。
【図18】(A)及び(B)は回転時速度と補正係数との対応関係の一例を示すグラフ。
【発明を実施するための形態】
【0025】
以下、図面を参照して、移動体に設置されて当該移動体の位置を算出する位置算出装置及び位置算出システムの実施形態について説明する。なお、本発明を適用可能な実施形態が以下説明する実施形態に限定されるわけでないことは勿論である。
【0026】
1.システム構成
図1は、本実施形態における位置算出システム1のシステム構成及び座標系の説明図である。位置算出システム1は、移動体である四輪自動車(以下、単に「自動車」と称す。)に、位置算出装置を具備した電子機器の一種であるカーナビゲーション装置2が搭載されている。
【0027】
カーナビゲーション装置2には、加速度センサー及びジャイロセンサーを具備するセンサーユニットであるIMU60が搭載されている。IMU60は、自動車に対する相対的な座標系における加速度及び角速度を検出して出力するように設置・構成されている。本実施形態では、この相対的な座標系が機体座標系(Body Frame)として知られるB座標系と一致するような姿勢でカーナビゲーション装置2が自動車に設置・固定されているものとして説明する。B座標系は、車両に対して前方を正とする前後方向をXB軸(ロール軸)、右方を正とする左右方向をYB軸(ピッチ軸)、鉛直下方を正とする上下方向をZB軸(ヨー軸)とする三次元直交座標系である。
【0028】
カーナビゲーション装置2は、GPS(Global Positioning System)を利用した位置算出演算と、IMU60の検出結果を利用した慣性航法演算との2種類の演算を行って位置を算出する。本実施形態では、位置算出を行う座標系を、絶対座標系の一種であり、NED(North East Down)座標系として知られるL座標系とする。L座標系は、北方を正とする南北方向をXL軸、東方を正とする東西方向をYL軸、鉛直下方を正とする高度方向をZL軸とする三次元直交座標系である。
【0029】
カーナビゲーション装置2は、ジャイロセンサーの検出結果に基づいて、B座標系からL座標系への座標変換行列を算出し、当該座標変換行列を用いて、加速度センサーのB座標系(相対座標系)における検出結果を、L座標系(絶対座標系)における検出結果に座標変換する。座標変換の際には、加速度センサーの検出結果のうち、加速度の方向を座標変換し、加速度の大きさは維持する。言い換えると、加速度センサーの相対的な検出方向を、絶対座標系における方向(絶対方向)に座標変換する。
【0030】
そして、L座標系における加速度センサーの検出結果及びジャイロセンサーの検出結果を用いて、慣性航法演算を行って自動車の位置を逐次的に算出(算定位置を更新)し、算出した位置(算定位置)をプロットしたナビゲーション画面を表示部に表示させるナビゲーション処理を行う。なお、本実施形態では、慣性航法演算を用いて算出した位置を、後述するGPS位置算出部20により算出された位置と区別するために「算定位置」と称する。
【0031】
2.機能構成
図2は、カーナビゲーション装置2の機能構成の一例を示すブロック図である。カーナビゲーション装置2は、GPSアンテナ10と、GPS位置算出部20と、処理部30と、操作部40と、表示部50と、IMU(Inertial Measurement Unit)60と、記憶部70とを備えて構成される。
【0032】
GPSアンテナ10は、測位用衛星の一種であるGPS衛星から発信されているGPS衛星信号を含むRF(Radio Frequency)信号を受信するアンテナであり、受信信号をGPS位置算出部20に出力する。GPS衛星信号は、拡散符号の一種であるCA(Coarse and Acquisition)コードによって、スペクトラム拡散方式として知られるCDMA(Code Division Multiple Access)方式によって変調された1.57542[GHz]の通信信号である。CAコードは、コード長1023チップを1PNフレームとする繰返し周期1msの擬似ランダム雑音符号であり、GPS衛星毎に異なるものである。
【0033】
GPS位置算出部20は、GPSアンテナ10から出力された信号に基づいて自動車(カーナビゲーション装置2)の位置を計測する回路或いは装置であり、いわゆるGPS受信装置に相当する機能ブロックである。図示を省略するが、GPS位置算出部20は、RF受信回路部と、ベースバンド処理回路部とを備えて構成される。なお、RF受信回路部と、ベースバンド処理回路部とは、それぞれ別のLSI(Large Scale Integration)として製造することも、1チップとして製造することも可能である。
【0034】
ベースバンド処理回路部は、RF受信回路部から出力された受信信号に対して相関処理等を行ってGPS衛星信号を捕捉・追尾し、GPS衛星信号から取り出した衛星軌道データや時刻データ等に基づいて、所定の位置算出計算を行って自動車の位置(位置座標)を算出する。そして、算出した位置をGPSデータ727として、処理部30に出力する。
【0035】
処理部30は、記憶部70に記憶されているシステムプログラム等の各種プログラムに従ってカーナビゲーション装置2の各部を統括的に制御する制御装置であり、CPU(Central Processing Unit)等のプロセッサーを有して構成される。処理部30は、IMU60の検出結果を利用した慣性航法演算処理を行って自動車の位置(位置座標)を算出する。そして、GPS位置算出部20により算出された位置(GPS算出位置)及び処理部30が算出した位置(慣性航法演算位置)をもとに、表示部50に現在位置を指し示した地図を表示させる処理を行う。
【0036】
図3は、処理部30を機能ブロックとして表した概念図である。処理部30は、姿勢算出部31と、座標変換部32と、速度ベクトル算出部33と、速度ベクトル補正部34と、位置算出部35とを機能部として有する。
【0037】
姿勢算出部31は、ジャイロセンサー63により検出されたB座標系における3軸の加速度を積分処理(積分及び積算)することで、自動車の姿勢角を算出する。姿勢算出部31は、算出した姿勢角を算定姿勢として、座標変換部32及び速度ベクトル補正部34に出力する。自動車の姿勢がわかれば、自動車の移動方向もわかる。そのため、姿勢算出部31は、自動車の移動方向を検出する移動方向検出部であるとも言える。
【0038】
座標変換部32は、姿勢算出部31から入力した算定姿勢に基づいて、IMU60のB座標系における検出結果をL座標系における検出結果に変換する座標変換処理を行う。具体的には、姿勢算出部31から入力したB座標系における3軸の姿勢角を用いて、B座標系からL座標系への座標変換行列を算出し、当該座標変換行列を用いて加速度センサー61の検出結果を変換する。そして、変換したL座標系における3軸の加速度をカーナビゲーション装置2の加速度ベクトルとして、速度ベクトル算出部33に出力する。座標変換部32は、加速度ベクトル算出部であるとも言える。
【0039】
速度ベクトル算出部33は、座標変換部32から入力した加速度ベクトルを積分処理することでカーナビゲーション装置2の速度ベクトルを算出し、算出した速度ベクトルを速度ベクトル補正部34に出力する。
【0040】
速度ベクトル補正部34は、速度ベクトル算出部33から入力した速度ベクトルを、姿勢算出部31から入力した算定姿勢に基づいて補正する。そして、補正した速度ベクトルを補正速度ベクトルとして、位置算出部35に出力する。
【0041】
位置算出部35は、速度ベクトル補正部34から入力した補正速度ベクトルを用いて、カーナビゲーション装置2の位置を算出する。そして、算出した位置を算定位置として、表示部50に出力する。
【0042】
図4を参照して、本実施形態における速度ベクトル補正の原理について説明する。説明を分かり易くするため、ここではL座標系の東西方向(YL軸)及び南北方向(XL軸)でなる2次元平面において速度ベクトルを補正する場合について図示・説明する。図4(A)は自動車の直進時における速度ベクトルの補正の説明図であり、図4(B)は自動車の回転(旋回)時における速度ベクトルの補正の説明図である。各図において、横軸はYL軸を示しており、縦軸はXL軸を示している。
【0043】
L座標系における加速度の東西方向成分及び南北方向成分を積分処理すると、速度の東西方向成分及び南北方向成分が得られる。図4において、YL軸上に記載した矢印が速度の東西方向成分を示しており、XL軸上に記載した矢印が速度の南北方向成分を示している。速度の東西方向成分及び南北方向成分を合成することで、大きさが|V|で、向きがθ’である速度ベクトルが得られる。すなわち、動径を|V|、偏角をθ’とする円座標で速度ベクトルが定義される。但し、図4では、速度ベクトルの向きを、速度ベクトルとYL軸との成す角度で表現している。上記の速度ベクトルが、速度ベクトル算出部33により算出される速度ベクトルである。
【0044】
速度ベクトルは、座標変換部32から出力されるL座標系における加速度ベクトルを積分処理することで求められる。より具体的には、所定の単位時間の間(単位期間)に検出された複数の加速度ベクトルを積分及び積算して当該単位期間における速度ベクトルの変位を算出し、前回(最新)の速度ベクトルに加算(積算)することで、速度ベクトルを新たに算出する。なお、単位期間における速度ベクトルの変化に相当するベクトル(式(1)のdvで表されるベクトル)のことを、便宜的に、前回の速度ベクトルに対する今回の速度ベクトルの「変位」と称する。また、この単位期間には複数の検出時間間隔が含まれる(つまり、単位期間中に複数の加速度ベクトルが検出される。)。
【0045】
このように加速度ベクトルを積分及び積算して速度ベクトルを逐次的に求めていく必要があるため、加速度ベクトルに誤差があると、求められる速度ベクトルに誤差が累積していくという問題がある。速度の東西方向成分及び速度の南北方向成分が正確に求まらなければ、図4に示す速度ベクトルの大きさ及び向きが正確に求まらず、誤差を含み得ることになる。
【0046】
そこで、速度ベクトルの大きさ及び向きを次のように補正する。先ず、速度ベクトルの向きθ’を、姿勢算出部31により算出されたカーナビゲーション装置2の移動方向θに補正する。速度ベクトルの向きθ’は、加速度ベクトルの積分処理を経て間接的に求められる向きであるのに対し、移動方向θは、センサーの検出結果から直接的に求められる自動車の向きである。そのため、補正速度ベクトルの向きは、自動車の移動方向に沿った正しい向きとなる。
【0047】
次に、速度ベクトルの大きさ|V|を|V|×αに補正する。ここで、「α」は速度ベクトルの補正係数である。本実施形態では、補正係数「α」を、自動車の直進時と回転時とで可変に設定して速度ベクトルを補正する。具体的には、自動車の直進時には、図4(A)に示すように補正係数「α=1」として、速度ベクトルの大きさをそのままにする。この場合、速度ベクトルの大きさと補正速度ベクトルの大きさとは同じとなる。
【0048】
一方、自動車の回転時には、図4(B)に示すように「α<1」として、速度ベクトルの大きさが小さくなるように補正する。この場合、補正速度ベクトルの大きさは速度ベクトルの大きさよりも小さくなる。
【0049】
実験結果で後述するが、本願発明者が種々の条件で実験を行った結果、補正係数「α」を「1」よりも僅かに小さな値とすることで、速度ベクトルの補正の効果が表れ、位置算出の正確性の向上に資することがわかった。例えば、自動車が曲がり角で曲がる場合に、速度ベクトルの大きさをそのままにして位置算出を行うと、実際には自動車は曲がっているのに対し、算出される位置は真っ直ぐに行き過ぎてしまい、追従性が悪くなるという問題がある。かかる問題に対処するため、本実施形態では、自動車の回転時には、速度ベクトルの大きさをやや小さく見積もって位置算出を行うことにする。なお、自動車の回転には、自動車の右折、左折、転回等、1周より短い回転が含まれる。
【0050】
図4では説明を分かり易くするために2次元平面で考えたが、3次元空間に拡張することは容易である。この場合は、東西南北の2次元平面における自動車の向きに加えて、上下方向の向きを考慮して同様の補正を行えばよい。すなわち、動径を|V|、偏角を(θ’,φ’)とする球座標で速度ベクトルを定義する。そして、速度ベクトルの向きを姿勢算出部31により算出された姿勢角(θ,φ)に補正し、速度ベクトルの大きさを|V|×αに補正するようにする。
【0051】
図2の機能ブロックの説明に戻って、操作部40は、例えばタッチパネルやボタンスイッチ等により構成される入力装置であり、押下されたキーやボタンの信号を処理部30に出力する。この操作部40の操作により、目的地の入力等の各種指示入力がなされる。
【0052】
表示部50は、LCD(Liquid Crystal Display)等により構成され、処理部30から入力される表示信号に基づいた各種表示を行う表示装置である。表示部50には、ナビゲーション画面等が表示される。
【0053】
IMU60は、例えば、加速度センサー61やジャイロセンサー63を備え、予めセンサーに対応付けて定められたセンサー座標系の直交3軸それぞれの軸方向の加速度及び各軸の軸回りの角速度を検出可能に構成される。なお、加速度センサー61及びジャイロセンサー63は、それぞれが独立したセンサーであってもよいし、一体型のセンサーであってもよい。
【0054】
記憶部70は、ROM(Read Only Memory)やフラッシュROM、RAM(Random Access Memory)等の記憶装置によって構成され、カーナビゲーション装置2のシステムプログラムや、ナビゲーション機能等の各種機能を実現するための各種プログラム、データ等を記憶している。また、各種処理の処理中データ、処理結果などを一時的に記憶するワークエリアを有する。
【0055】
少なくとも、処理部30と、加速度センサー61と、を含んで本実施形態の位置算出装置が構成される。なお、位置算出装置は、GPSアンテナ10、GPS位置算出部20、操作部40、表示部50、ジャイロセンサー63、記憶部70等のその他の構成を含んでいてもよい。
【0056】
3.データ構成
図2に示すように、記憶部70には、プログラムとして、処理部30により読み出され、ナビゲーション処理(図9参照)として実行されるナビゲーションプログラム711が記憶されている。また、ナビゲーションプログラム711には、速度ベクトル算出処理(図10参照)として実行される速度ベクトル算出プログラム7111と、速度ベクトル補正処理(図11参照)として実行される速度ベクトル補正プログラム7113とがサブルーチンとして含まれている。
【0057】
ナビゲーション処理とは、処理部30が、速度ベクトル算出処理及び速度ベクトル補正処理を行って、慣性航法演算を行うための補正速度ベクトルを求め、当該補正速度ベクトルを用いて、自動車(カーナビゲーション装置2)の位置を算定する処理である。そして、算定位置をプロットしたナビゲーション画面を生成して、表示部50に表示させる。
【0058】
速度ベクトル算出処理とは、速度ベクトル算出部33が、座標変換部32によりB座標系からL座標系に座標変換された加速度ベクトルを積分処理することで、自動車の速度ベクトルを算出する処理である。
【0059】
また、速度ベクトル補正処理とは、速度ベクトル補正部34が、速度ベクトル算出部33により算出された速度ベクトルを、姿勢算出部31により算出された自動車の移動方向を用いて補正する処理である。これらの処理については、フローチャートを用いて詳細に後述する。
【0060】
また、記憶部70には、データとして、センサーデータ721と、速度ベクトルデータ723と、補正速度ベクトルデータ725と、GPSデータ727と、出力位置データ729と、回転時補正係数731とが記憶される。
【0061】
センサーデータ721は、IMU60を構成する加速度センサー61及びジャイロセンサー63によってそれぞれ検出された加速度及び角速度が時系列に記憶されたデータであり、そのデータ構成例を図5に示す。センサーデータ721には、IMU60の検出時刻7211と対応付けて、3次元の加速度7213と、3次元の角速度7215とが時系列に記憶される。
【0062】
速度ベクトルデータ723は、速度ベクトル算出処理により算出された速度ベクトルのデータであり、そのデータ構成例を図6に示す。速度ベクトルデータ723には、速度ベクトルの算出時刻7231と対応付けて、直交座標系であるL座標系で考えた場合の速度ベクトルのベクトル成分7233が時系列に記憶される。センサーデータ721の所定時間分の検出時刻「t1,t2,t3,・・・」の加速度のデータによって、算出時刻「T1」の速度ベクトルが算出される。
【0063】
補正速度ベクトルデータ725は、速度ベクトル補正処理で求められた補正速度ベクトルのデータであり、そのデータ構成例を図7に示す。補正速度ベクトルデータ725には、速度ベクトルの算出時刻7231と対応付けて、球座標系で考えた場合の補正速度ベクトルの大きさ7253及び向き7255と、直交座標系であるL座標系で考えた場合の補正速度ベクトルのベクトル成分7257とが時系列に記憶される。
【0064】
補正速度ベクトルの大きさ7253は、速度ベクトルデータ723の速度ベクトルのベクトル成分7233の二乗和の平方根によって算出される。すなわち、|V|=(Vx2+Vy2+Vz2)1/2である。一方、速度ベクトルの向き7255は、姿勢算出部31により算出・決定された自動車の算定姿勢(θ,φ)である。
【0065】
GPSデータ727は、GPS位置算出部20により算出・決定された位置(以下、「GPS算定位置」と称す。)が時系列に記憶されたデータである。GPSデータ727は、所定時間間隔(例えば10分間隔)で更新される。
【0066】
出力位置データ729は、表示部50に表示された出力位置のデータであり、そのデータ構成例を図8に示す。出力位置データ729には、速度ベクトルの算出時刻7231と対応付けて、出力位置7293と、測位種別7295とが記憶される。
【0067】
測位種別7295は、出力位置7293が、GPS位置算出部20により算出された位置と、位置算出部35により算出された位置との何れの位置であるかを示すフラグである。GPS位置算出部20により算出された位置である場合は「GPS」、位置算出部35により算出された位置である場合は「慣性航法」がそれぞれ記憶される。
【0068】
回転時補正係数731は、自動車の回転を検出した場合に速度ベクトルの大きさ7253を補正するための係数のデータである。詳細は後述する。
【0069】
4.処理の流れ
図9は、記憶部70に記憶されているナビゲーションプログラム711が処理部30により読み出されて実行されることで、カーナビゲーション装置2において実行されるナビゲーション処理の流れを示すフローチャートである。以下のナビゲーション処理では、IMU60により検出された加速度及び角速度が、記憶部70のセンサーデータ721に随時記憶されるものとする。
【0070】
最初に、処理部30は、初期設定を行う(ステップA1)。具体的には、慣性航法演算に使用する各パラメーターを初期化する。次いで、処理部30は、記憶部70に記憶されている速度ベクトル算出プログラム7111を読み出して実行することで、速度ベクトル算出処理を行う(ステップA3)。
【0071】
図10は、速度ベクトル算出処理の流れを示すフローチャートである。
最初に、姿勢算出部31は、ジャイロセンサー63の検出結果を所定期間に亘って積分して、当該所定期間分の姿勢角変化量を算出する(ステップB1)。そして、姿勢算出部31は、前回(最新)の姿勢角と、算出した姿勢角変化量とを用いて、姿勢角を新たに算出(積算)して、新たに算出した姿勢角で前回の姿勢角を更新する(ステップB3)。
【0072】
次いで、座標変換部32は、最新の姿勢角に基づいて、B座標系からL座標系への座標変換行列を算出する(ステップB5)。なお、B座標系からL座標系への座標変換行列については従来公知であるため、ここでは数式等を用いた詳細な説明を省略する。
【0073】
次いで、座標変換部32は、加速度センサー61のB座標系における3軸の検出結果を、ステップB5で算出した座標変換行列を用いてL座標系における3軸の検出結果に座標変換する(ステップB7)。これにより、L座標系における3軸の加速度ベクトルが求まる。そして、座標変換部32は、ステップB7で求めた加速度ベクトルを所定期間に亘って積分して、当該所定期間における速度ベクトルの変位を算出する(ステップB9)。
【0074】
次いで、速度ベクトル算出部33は、前回(最新)の速度ベクトルと、ステップB9で算出した速度ベクトルの変位とを用いて速度ベクトルを新たに算出して、記憶部70の速度ベクトルデータ723を更新する(ステップB11)。具体的には、前回(最新)の速度ベクトルに、ステップB9で算出した速度ベクトルの変位を足し合わせる(積算する)ことで、速度ベクトルを新たに算出する。そして、速度ベクトル算出部33は、速度ベクトル算出処理を終了する。
【0075】
図9のナビゲーション処理に戻って、速度ベクトル算出処理を行った後、速度ベクトル補正部34は、記憶部70に記憶されている速度ベクトル補正プログラム7113を読み出して実行することで、速度ベクトル補正処理を行う(ステップA5)。
【0076】
図11は、速度ベクトル補正処理の流れを示すフローチャートである。
先ず、速度ベクトル補正部34は、速度ベクトル算出処理で算出した速度ベクトルの向き7255を、ステップB3で算出した姿勢角から求められる自動車の移動方向に補正して、記憶部70の補正速度ベクトルデータ725を更新する(ステップC1)。
【0077】
次いで、速度ベクトル補正部34は、自動車の回転(旋回)判定を行う(ステップC3)。具体的には、例えばステップB3で算出した姿勢角の前回の姿勢角からの時間変化に基づいて、自動車が回転中であるか否かを判定する。なお、ジャイロセンサー63により検出された角速度そのものを用いて回転判定を行うこととしてもよい。
【0078】
自動車の回転を検出した場合は(ステップC5;Yes)、速度ベクトル補正部34は、記憶部70に記憶された回転時の補正係数として予め定められた回転時補正係数731(α<1)を用いて速度ベクトルの大きさ7253を補正して、補正速度ベクトルデータ725を更新する(ステップC7)。そして、速度ベクトル補正部34は、速度ベクトル補正処理を終了する。
【0079】
また、自動車の回転を検出しなかった場合は(ステップC5;No)、速度ベクトル補正部34は、速度ベクトルの大きさを補正することなく、速度ベクトル補正処理を終了する。
【0080】
図9のナビゲーション処理に戻って、速度ベクトル補正処理を行った後、位置算出部35は、前回(最新)の出力位置と、速度ベクトル補正処理で求めた補正速度ベクトルとを用いて自動車の位置を新たに算出し、記憶部70の出力位置データ729に記憶させる(ステップA7)。
【0081】
次いで、位置算出部35は、GPS位置算出部20からGPSデータ727を取得したか否かを判定し(ステップA9)、取得したと判定した場合は(ステップA9;Yes)、出力位置のリセットタイミングであるか否かを判定する(ステップA11)。
【0082】
出力位置のリセットタイミングは、種々のタイミングを設定することができる。例えば、自動車が停止したタイミングとすることができる。自動車の停止判定は、例えば速度ベクトルの大きさに基づいて判定することが可能である。すなわち、速度ベクトルの大きさが「0」に近い値として予め定められた所定の閾値以下である場合に、自動車は停止していると判定することができる。
【0083】
また、例えば、自動車がトンネル内などのGPS測位が不可能な環境から、トンネルの外などのGPS測位が可能な環境に変化したタイミング(GPS衛星信号の捕捉ができなくなってから捕捉ができるようになったタイミング)をリセットタイミングとしてもよい。他には、例えば所定時間が経過する毎(例えば30分毎)のタイミングをリセットタイミングとしてもよい。
【0084】
出力位置のリセットタイミングであると判定した場合は(ステップA11;Yes)、位置算出部35は、ステップA7で求めた出力位置を、GPS位置算出部20から取得したGPSデータ727のGPS算出位置でリセットする(ステップA13)。具体的には、ステップA7で求めた出力位置を、GPS算出位置に変更する。
【0085】
一方、ステップA9においてGPSデータを取得できなかったと判定した場合(ステップA9;No)、又は、ステップA11においてリセットタイミングではないと判定した場合は(ステップA11;No)、位置算出部35は、ステップA15へと処理を移行する。
【0086】
次いで、位置算出部35は、最新の出力位置(算定位置)で表示部50のナビゲーション画面の表示を更新する(ステップA15)。そして、位置算出部35は、位置算出を終了するか否かを判定し(ステップA17)、まだ終了しないと判定した場合は(ステップA17;No)、ステップA3に戻る。また、終了すると判定した場合は(ステップA17;Yes)、ナビゲーション処理を終了する。
【0087】
5.実験結果
図12〜図14を参照して、従来の手法と本実施形態の手法とのそれぞれを適用して位置算出を行った場合の実験結果について説明する。カーナビゲーション装置を搭載した自動車を、予め定められた周回経路に沿って2周させ、所定時間間隔で慣性航法演算を行って求めた算定位置を東西南北の2次元平面に順次にプロットする実験を行った。図12〜図14において、横軸は東西方向、縦軸は南北方向をそれぞれ示している。また、点線が実際の経路を示しており、太実線が算定位置の軌跡を示している。
【0088】
図12は、従来の手法により、速度ベクトルの補正を行わずに位置算出を行った実験結果の一例を示す図である。この結果を見ると、1周目の初期の間は実際の経路に沿った算定位置の軌跡が得られているが、1周目の途中から算定位置が実際の経路からずれていき、2周目には実際の経路から大きく外れていることがわかる。
【0089】
図13は、本実施形態の手法により、速度ベクトルの補正を行って位置算出を行った実験結果の一例を示す図である。ここでは、自動車の回転時における速度ベクトルの大きさの補正を行わずに、速度ベクトルの補正係数を常時「α=1」として実験を行った。この結果を見ると、実際の経路と比較するとやはり誤差が生じているものの、図12の従来の結果と比べると、実際の経路に近い算定位置の軌跡が得られていることがわかる。しかし、よく見ると、曲がり角の部分において算定位置が行き過ぎて外側にオーバーしており、曲がり角における追従性が低下していることがわかる。
【0090】
図14は、本実施形態の手法により、速度ベクトルの補正を行って位置算出を行った実験結果の一例を示す図である。ここでは、自動車の回転時における速度ベクトルの大きさの補正を行うこととし、回転時には補正係数「α」を0.9以上1.0未満の一定値として実験を行った。この結果を見ると、2周目においても、算定位置の軌跡が実際の経路とほぼ一致しており、良好な結果が得られていることがわかる。また、曲がり角の部分に着目すると、算定位置が外側に外れることもなく、実際の経路に沿った追従性の良い軌跡が得られており、本実施形態の手法の有効性が実証された。
【0091】
6.作用効果
本実施形態によれば、自動車に設置されたジャイロセンサー63の検出結果を用いて、移動体である自動車の姿勢及び移動方向が検出される。そして、検出された姿勢に基づいて、自動車に設置された加速度センサー61の検出結果が、相対座標系から絶対座標系に座標変換され、座標変換後の検出結果を用いて、速度ベクトルが算出・更新される。そして、算出された速度ベクトルが検出された移動方向を用いて補正され、補正された速度ベクトルを用いて、自動車の位置が算出・更新される。
【0092】
速度ベクトルは、加速度ベクトルを積分処理することで求められる。この際、加速度ベクトルを積分及び積算していく処理を行うために、求められる速度ベクトルには時間経過に伴って誤差が累積していくという問題がある。しかし、算出された速度ベクトルの向きを、他の検出方法により検出された自動車の移動方向に補正することにすれば、速度ベクトルの向きは自動車の移動方向に沿った正確な向きとなる。そして、補正された速度ベクトルを用いて自動車の位置を算出することで、算定位置の正確性を向上させることができる。
【0093】
また、自動車の直進時には、速度ベクトルの大きさをそのままとし、自動車の回転時には、速度ベクトルの大きさを僅かに小さく補正する。例えば、自動車が曲がり角で曲がる場合に、速度ベクトルの大きさをそのままにして位置算出を行うと、算出される位置の実際の位置に対する追従性が悪くなり、次第に乖離していくという問題がある。そのため、自動車の回転時に速度ベクトルの大きさをやや小さく見積もって位置算出を行うことで、算定位置の正確性を向上させることができる。
【0094】
7.変形例
7−1.電子機器
上述した実施形態では、四輪自動車に搭載するナビゲーション装置に本発明を適用した場合を例に挙げて説明したが、本発明を適用可能な電子機器はこれに限られるわけではない。例えば、二輪自動車に搭載するナビゲーション装置に適用してもよいし、携帯型ナビゲーション装置に適用することとしてもよい。
【0095】
また、ナビゲーション以外の用途の電子機器についても本発明を同様に適用可能であることは勿論である。例えば、携帯型電話機やパソコン、PDA(Personal Digital Assistant)といった他の電子機器についても本発明を同様に適用して、当該電子機器の位置算出を実現することが可能である。
【0096】
7−2.衛星測位システム
また、上述した実施形態では、衛星測位システムとしてGPSを例に挙げて説明したが、WAAS(Wide Area Augmentation System)、QZSS(Quasi Zenith Satellite System)、GLONASS(GLObal NAvigation Satellite System)、GALILEO等の他の衛星測位システムであってもよいことは勿論である。
【0097】
7−3.位置の補正
上述した実施形態では、自動車の移動方向に基づいて速度ベクトルを補正し、補正速度ベクトルを用いて位置算出を行うものとして説明したが、次のようにしてもよい。すなわち、速度ベクトルを補正せずに、速度ベクトルをそのまま用いて位置算出を行う。そして、算出された位置を自動車の移動方向に基づいて補正する。
【0098】
図15は、この場合における処理部30を機能ブロックとして表した概念図である。なお、図3に示す処理部30と同一の構成要素については同一の符号を付して説明を省略し、図3とは異なる機能ブロックを中心に説明する。図15の処理部30では、姿勢算出部31により算出・決定された算定姿勢(移動方向)が、座標変換部32及び位置補正部36に出力される。
【0099】
また、速度ベクトル算出部33により算出された速度ベクトルは、そのまま位置算出部35に出力される。そして、当該速度ベクトルを用いて、位置算出部35により位置が算出される。位置算出部35により算出された位置は、位置補正部36に出力される。そして、位置補正部36は、姿勢算出部31から入力した算定姿勢(移動方向)に基づいて、位置算出部35から入力した位置を補正する。
【0100】
図16は、位置補正の原理の説明図である。黒丸で示す時刻「T−1」における出力位置と、時刻「T」における速度ベクトル(向きθ’)とを用いて、白丸で示す時刻「T」における算定位置を求めることができる。すなわち、黒丸を速度ベクトルの向き「θ’」に単位時間分シフトさせた位置が白丸である。
【0101】
このとき、時刻「T−1」における出力位置と、時刻「T」における算定位置との間の距離「L」を、速度ベクトルを用いて算出する。そして、時刻「T−1」における出力位置を、姿勢算出部31から取得した移動体の移動方向「θ」に距離「L」だけシフトさせることで、二重丸で示す時刻「T」における補正位置を求める。出力位置のリセットタイミングでなければ、この補正位置が時刻「T」における出力位置となる。
【0102】
図17は、この場合に処理部30が実行する第2ナビゲーション処理の流れを示すフローチャートである。なお、図9のナビゲーション処理と同一のステップについては同一の符号を付して説明を省略し、ナビゲーション処理とは異なるステップを中心に説明する。
【0103】
第2ナビゲーション処理では、ステップA3において速度ベクトル算出処理を行った後、位置算出部35が、前回(最新)の出力位置と、算出した速度ベクトルとを用いて、自動車の位置を新たに算出して更新する(ステップD5)。そして、位置補正部36が、位置補正処理を行う(ステップD7)。位置補正処理では、図16で説明した位置補正の原理に従って位置を補正する。
【0104】
7−4.速度ベクトルの向きの補正
上述した実施形態では、速度ベクトルの向きを、検出した移動体の移動方向に補正するものとして説明したが、検出した移動方向を用いた補正であればよく、この補正方法に限られるわけではない。
【0105】
例えば、速度ベクトルの向きと、検出した移動体の移動方向との平均処理を行って速度ベクトルの向きを補正することとしてもよい。この場合は、ジャイロセンサー等のセンサーによって直接的に検出した移動体の移動方向の方が信頼性が高いため、例えば、検出した移動体の移動方向の重みを高くし(例えば0.8)、速度ベクトルの向きの重みを低くして(例えば0.2)、加重平均計算によって速度ベクトルを補正するなどすれば適切である。
【0106】
7−5.速度ベクトルの大きさの補正
上述した実施形態では、移動体の回転検出時には速度ベクトルの大きさを一律に小さく変更するものとして説明したが、次のように速度ベクトルの大きさを変更することとしてもよい。
【0107】
例えば、移動体の回転が最初に検出されたとき(回転開始時)における移動体の速度(以下、「回転開始時速度」と称す。)を記録しておく。そして、移動体の回転時に、その回転時における移動体の速度(以下、「回転時速度」と称す。)を、記録しておいた回転開始時速度と比較する。そして、その比較結果に基づいて速度ベクトルの大きさを変更する。具体的には、回転時速度が回転開始時速度よりも小さければ、速度ベクトルの大きさを大きく変更する(α>1)。一方、回転時速度が回転開始時速度よりも大きければ、速度ベクトルの大きさを小さく変更する(α<1)。
【0108】
回転時速度が回転開始時速度よりも小さい場合は、移動体はゆっくりと回転しているものと考えられる。この場合は、速度ベクトルの大きさをやや大きく見積もって位置算出を行うことで、算出される位置が移動体の実際の位置にうまく追従するように調整する。一方、回転時速度が回転開始時速度よりも大きい場合は、移動体は急回転しているものと考えられる。この場合は、速度ベクトルの大きさをやや小さく見積もって位置算出を行う。
【0109】
7−6.速度ベクトルの補正係数の設定
上記の速度ベクトルの大きさの補正とも関連するが、補正係数「α」を次のように設定してもよい。すなわち、移動体の回転速度(或いは角速度)の大きさに基づいて補正係数「α」を設定する。移動体の回転速度は、移動体が四輪自動車である場合は、例えばZB軸(ヨー軸)周りの回転速度とし、移動体が二輪自動車である場合は、例えばXB軸(ロール軸)周りの回転速度とするなどが考えられる。なお、回転速度は、ジャイロセンサー63により検出される角速度と等価である。すなわち、回転速度に「2π(rad)」を乗算した値が角速度である。
【0110】
図18は、この場合の補正係数設定の原理を説明するための図である。図18には、自動車の回転速度と、補正係数「α」との対応関係を示すグラフを示している。補正係数「α」を小さくし過ぎると、位置算出の正確性が低下することが実験により明らかとなった。そのため、補正係数「α」の下限値を予め設定しておくことが適切である。そして、例えば図18(A)に示すように、下限値まで低下しない範囲で、回転速度が大きくなるにつれて補正係数「α」を徐々に減衰させるようにする。
【0111】
また、例えば図18(B)に示すように、回転速度の閾値を予め設定しておく。そして、回転速度が閾値に達するまでは補正係数「α」を一律に「1」とし(α=1)、回転速度が閾値を超えると、補正係数「α」を段階的に減少させることとしてもよい。
【0112】
7−7.移動方向の検出
上述した実施形態では、ジャイロセンサーの検出結果を用いて移動体の姿勢を算出することで移動体の移動方向を検出するものとして説明した。しかし、信頼性の高い移動方向が得られるのであれば、他の検出手段によって移動方向を検出することとしてもよい。例えば、地磁気センサー等の方位センサーによって移動方向を検出してもよい。
【0113】
7−8.出力位置
上述した実施形態では、所定のリセットタイミングにおいて、GPSにより算出された位置で算定位置をリセットするものとして説明したが、GPSを利用して算出した位置(GPS算出位置)と、本実施形態の慣性航法演算により算出した位置(慣性航法演算位置)とを併用して、最終的な出力位置を算出・決定することとしてもよい。例えば、GPS算出位置と慣性航法演算位置との平均処理を行うフィルターや、確率理論に基づく推定方法の一種であるカルマンフィルター等のフィルター処理を行って出力位置を決定してもよい。
【符号の説明】
【0114】
1 位置算出システム、 2 カーナビゲーション装置、 10 GPSアンテナ、 20 GPS位置算出部、 30 処理部、 31 姿勢算出部、 32 座標変換部、 33 速度ベクトル算出部、 34 速度ベクトル補正部、 35 位置算出部、 36 位置補正部、 40 操作部、 50 表示部、 60 IMU、 61 加速度センサー、 63 ジャイロセンサー、 70 記憶部
【特許請求の範囲】
【請求項1】
移動体の位置を算出する方法であって、
前記移動体の移動方向を検出することと、
前記移動体に設置された加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求めることと、
前記速度ベクトルを前記検出された移動方向を用いて補正することと、
前記補正された速度ベクトルを用いて前記移動体の算定位置を求めることで前記移動体の位置を算出することと、
を含む方法。
【請求項2】
前記補正することは、前記速度ベクトルの向きを、前記検出された移動方向に補正することを含む、
請求項1に記載の方法。
【請求項3】
前記移動体が回転したことを検出することを更に含み、
前記補正することは、前記移動体が回転した場合に前記速度ベクトルの大きさを変更することを含む、
請求項1又は2に記載の方法。
【請求項4】
移動体の位置を算出する方法であって、
前記移動体の移動方向を検出することと、
前記移動体に設置された加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求めることと、
前記速度ベクトルを用いて前記移動体の算定位置を求めることと、
前記算定位置を前記検出された移動方向を用いて補正することと、
を含む方法。
【請求項5】
前記移動体に設置されたジャイロセンサーの検出結果を用いて前記移動体の算定姿勢を求めることと、
前記算定姿勢を用いて、前記移動体に対する前記加速度センサーの相対的な検出方向を絶対方向に座標変換することで、前記加速度センサーの検出結果を前記絶対方向に座標変換することと、
を含み、
前記速度ベクトルを求めることは、前記座標変換した後の検出結果を用いて速度ベクトルを求めることである、
請求項1〜4の何れか一項に記載の方法。
【請求項6】
前記移動体は自動車である、
請求項1〜5の何れか一項に記載の方法。
【請求項7】
移動体に設置されて当該移動体の位置を算出する位置算出装置であって、
加速度センサーと、
前記移動体の移動方向を検出する移動方向検出部と、
前記加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求める速度ベクトル算出部と、
前記速度ベクトル算出部により算出された速度ベクトルを前記移動方向検出部により検出された移動方向を用いて補正する速度ベクトル補正部と、
前記速度ベクトル補正部により補正された速度ベクトルを用いて前記移動体の位置を求めることで前記移動体の位置を算出する位置算出部と、
を備えた位置算出装置。
【請求項8】
移動体に設置されて当該移動体の位置を算出する位置算出装置であって、
加速度センサーと、
前記移動体の移動方向を検出する移動方向検出部と、
前記加速度センサーの検出結果を用いて前記移動体の速度ベクトルを算出する速度ベクトル算出部と、
前記速度ベクトル算出部により算出された速度ベクトルを用いて前記移動体の算定位置を算出する位置算出部と、
前記位置算出部により算出された算定位置を前記検出された移動方向を用いて補正する位置補正部と、
を備えた位置算出装置。
【請求項1】
移動体の位置を算出する方法であって、
前記移動体の移動方向を検出することと、
前記移動体に設置された加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求めることと、
前記速度ベクトルを前記検出された移動方向を用いて補正することと、
前記補正された速度ベクトルを用いて前記移動体の算定位置を求めることで前記移動体の位置を算出することと、
を含む方法。
【請求項2】
前記補正することは、前記速度ベクトルの向きを、前記検出された移動方向に補正することを含む、
請求項1に記載の方法。
【請求項3】
前記移動体が回転したことを検出することを更に含み、
前記補正することは、前記移動体が回転した場合に前記速度ベクトルの大きさを変更することを含む、
請求項1又は2に記載の方法。
【請求項4】
移動体の位置を算出する方法であって、
前記移動体の移動方向を検出することと、
前記移動体に設置された加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求めることと、
前記速度ベクトルを用いて前記移動体の算定位置を求めることと、
前記算定位置を前記検出された移動方向を用いて補正することと、
を含む方法。
【請求項5】
前記移動体に設置されたジャイロセンサーの検出結果を用いて前記移動体の算定姿勢を求めることと、
前記算定姿勢を用いて、前記移動体に対する前記加速度センサーの相対的な検出方向を絶対方向に座標変換することで、前記加速度センサーの検出結果を前記絶対方向に座標変換することと、
を含み、
前記速度ベクトルを求めることは、前記座標変換した後の検出結果を用いて速度ベクトルを求めることである、
請求項1〜4の何れか一項に記載の方法。
【請求項6】
前記移動体は自動車である、
請求項1〜5の何れか一項に記載の方法。
【請求項7】
移動体に設置されて当該移動体の位置を算出する位置算出装置であって、
加速度センサーと、
前記移動体の移動方向を検出する移動方向検出部と、
前記加速度センサーの検出結果を用いて前記移動体の速度ベクトルを求める速度ベクトル算出部と、
前記速度ベクトル算出部により算出された速度ベクトルを前記移動方向検出部により検出された移動方向を用いて補正する速度ベクトル補正部と、
前記速度ベクトル補正部により補正された速度ベクトルを用いて前記移動体の位置を求めることで前記移動体の位置を算出する位置算出部と、
を備えた位置算出装置。
【請求項8】
移動体に設置されて当該移動体の位置を算出する位置算出装置であって、
加速度センサーと、
前記移動体の移動方向を検出する移動方向検出部と、
前記加速度センサーの検出結果を用いて前記移動体の速度ベクトルを算出する速度ベクトル算出部と、
前記速度ベクトル算出部により算出された速度ベクトルを用いて前記移動体の算定位置を算出する位置算出部と、
前記位置算出部により算出された算定位置を前記検出された移動方向を用いて補正する位置補正部と、
を備えた位置算出装置。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【公開番号】特開2011−220844(P2011−220844A)
【公開日】平成23年11月4日(2011.11.4)
【国際特許分類】
【出願番号】特願2010−90557(P2010−90557)
【出願日】平成22年4月9日(2010.4.9)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
【公開日】平成23年11月4日(2011.11.4)
【国際特許分類】
【出願日】平成22年4月9日(2010.4.9)
【出願人】(000002369)セイコーエプソン株式会社 (51,324)
【Fターム(参考)】
[ Back to top ]