JP6031402B2

JP6031402B2 - 慣性航法システム、移動体端末、慣性航法装置、及びプログラム

Info

Publication number: JP6031402B2
Application number: JP2013090554A
Authority: JP
Inventors: 竜也岩瀬; 柴崎　亮介; 亮介柴崎
Original assignee: University of Tokyo NUC; Toyota Central R&D Labs Inc
Current assignee: University of Tokyo NUC; Toyota Central R&D Labs Inc
Priority date: 2013-04-23
Filing date: 2013-04-23
Publication date: 2016-11-24
Anticipated expiration: 2033-04-23
Also published as: JP2014215086A

Description

本発明は、慣性航法システム、移動体端末、慣性航法装置、及びプログラムに関する。

従来、地磁気センサのキャリブレーションにより方位誤差を補正する方法が知られている（例えば、特許文献１参照）。

また、高精度に携帯端末間の距離を測定できるＵＷＢ（Ultra Wide Band）を利用し、慣性航法誤差を補正する方法が知られている（例えば、非特許文献１参照）。

特許第４０９３９８２号

"ACCURATE AND RELIABLE SOLDIER AND FIRST RESPONDER INDOORPOSITIONING: MULTISENSOR SYSTEMS AND COOPERATIVE LOCALIZATION" Stromback(2011)

しかしながら、特許文献１記載の技術では、地磁気センサのキャリブレーションが不完全であるため、方位誤差を補正しきれない、という問題がある。

また、非特許文献１記載の技術では、ＵＷＢを利用しているため、コストが高い、という問題がある。ＷｉｆｉやＢｌｕｅｔｏｏｔｈ（登録商標）などの安価なセンサを用いて、非特許文献１と同様の方法により、慣性航法誤差を補正することも考えられるが、測距誤差が問題となる。

本発明は、上記の問題点を解決するためになされたもので、高価なセンサを用いることなく、移動体端末の位置を精度良く推定することができる慣性航法システム、移動体端末、慣性航法装置、及びプログラムを提供することを目的とする。

上記目的を達成するために、本発明の慣性航法システムは、自端末の絶対位置を示す絶対位置情報、他の移動体端末との間で近接通信を行った時刻及び通信相手を示す近接通信情報、並びに前記自端末に搭載された慣性航法センサにより検出された慣性航法情報を取得する取得手段、前記取得手段によって取得された前記絶対位置情報を初期位置とし、前記取得手段により取得された前記慣性航法情報の時系列データに基づいて、前記自端末の慣性航法位置の軌跡を推定する軌跡推定手段、前記軌跡推定手段によって推定された前記慣性航法位置の軌跡から、前記自端末の移動方向が変化した位置を各々抽出する抽出手段、前記抽出手段によって抽出された位置を関節とし、前記慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、前記取得手段により取得された近接通信情報が示す他の移動体端末との間で近接通信を行った時刻に対応する前記自端末の慣性航法位置を通信位置として前記近接通信情報と共に記録した前記ネットワークモデルを生成するモデル生成手段、及び前記モデル生成手段によって生成された前記ネットワークモデルを送信する送信手段を含む複数の移動体端末と、前記複数の前記移動体端末から各々送信された前記ネットワークモデルを受信する受信手段、前記受信手段によって受信された前記移動体端末毎の前記ネットワークモデルに基づいて、前記移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定する角度推定手段、及び前記角度推定手段によって推定された前記移動体端末毎の前記ネットワークモデルの各関節の角度に基づいて、前記移動体端末毎の慣性航法位置の各々を補正する補正手段を含む慣性航法装置と、を含んで構成されている。

本発明の慣性航法システムによれば、複数の移動体端末の各々において、取得手段によって、自端末の絶対位置を示す絶対位置情報、他の移動体端末との間で近接通信を行った時刻及び通信相手を示す近接通信情報、並びに前記自端末に搭載された慣性航法センサにより検出された慣性航法情報を取得する。軌跡推定手段によって、前記取得手段によって取得された前記絶対位置情報を初期位置とし、前記取得手段により取得された前記慣性航法情報の時系列データに基づいて、前記自端末の慣性航法位置の軌跡を推定する。抽出手段によって、前記軌跡推定手段によって推定された前記慣性航法位置の軌跡から、前記自端末の移動方向が変化した位置を各々抽出する。

そして、モデル生成手段によって、前記抽出手段によって抽出された位置を関節とし、前記慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、前記取得手段により取得された近接通信情報が示す他の移動体端末との間で近接通信を行った時刻に対応する前記自端末の慣性航法位置を通信位置として前記近接通信情報と共に記録した前記ネットワークモデルを生成する。送信手段によって、前記モデル生成手段によって生成された前記ネットワークモデルを送信する。

そして、慣性航法装置において、受信手段によって、前記複数の前記移動体端末から各々送信された前記ネットワークモデルを受信する。角度推定手段によって、前記受信手段によって受信された前記移動体端末毎の前記ネットワークモデルに基づいて、前記移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定する。

そして、補正手段によって、前記角度推定手段によって推定された前記移動体端末毎の前記ネットワークモデルの各関節の角度に基づいて、前記移動体端末毎の慣性航法位置の各々を補正する。

このように、移動体端末の各々について、絶対位置情報を初期位置とし、慣性航法情報の時系列データに基づいて推定される慣性航法位置の軌跡から、自端末の移動方向が変化した位置を抽出して、抽出された位置を関節とし、他の移動体端末との間で近接通信を行った位置を記録したネットワークモデルを生成し、移動体端末間での近接通信を行った通信位置が移動体端末間で対応していることを拘束条件として、移動体端末毎のネットワークモデルの各関節の角度を推定することにより、高価なセンサを用いることなく、移動体端末の位置を精度良く推定することができる。

本発明に係る移動体端末は、自端末の絶対位置を示す絶対位置情報、他の移動体端末との間で近接通信を行った時刻及び通信相手を示す近接通信情報、並びに前記自端末に搭載された慣性航法センサにより検出された慣性航法情報を取得する取得手段と、前記取得手段によって取得された前記絶対位置情報を初期位置とし、前記取得手段により取得された前記慣性航法情報の時系列データに基づいて、前記自端末の慣性航法位置の軌跡を推定する軌跡推定手段と、前記軌跡推定手段によって推定された前記慣性航法位置の軌跡から、前記自端末の移動方向が変化した位置を各々抽出する抽出手段と、前記抽出手段によって抽出された位置を関節とし、前記慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、前記取得手段により取得された近接通信情報が示す他の移動体端末との間で近接通信を行った時刻に対応する前記自端末の慣性航法位置を通信位置として前記近接通信情報と共に記録した前記ネットワークモデルを生成するモデル生成手段と、前記モデル生成手段によって生成された前記ネットワークモデルと、他の移動体端末の前記ネットワークモデルとに基づいて、前記移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として推定された前記ネットワークモデルの各関節の角度に基づいて、前記自端末の慣性航法位置の各々を補正する補正手段と、を含んで構成されている。

本発明に係る慣性航法装置は、移動体端末の絶対位置情報を初期位置とし、慣性航法センサにより検出された慣性航法情報の時系列データに基づいて推定された前記移動体端末の慣性航法位置の軌跡から抽出された、前記移動体端末の移動方向が変化した位置の各々を関節とし、前記移動体端末の慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、他の移動体端末との間で近接通信を行った時刻に対応する前記移動体端末の慣性航法位置を通信位置として、前記近接通信を行った時刻及び通信相手を示す近接通信情報と共に記録した前記ネットワークモデルを、複数の前記移動体端末について各々取得する手段と、前記取得手段によって取得された前記移動体端末毎の前記ネットワークモデルに基づいて、移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定する角度推定手段と、前記角度推定手段によって推定された前記移動体端末毎の前記ネットワークモデルの各関節の角度に基づいて、前記移動体端末毎の慣性航法位置の各々を補正する補正手段と、を含んで構成されている。

上記の角度推定手段は、前記ネットワークモデルが表す慣性航法位置の軌跡の初期位置が、前記取得した絶対位置情報であること、前記関節間の距離が、前記慣性航法位置の軌跡に基づいて得られる距離であること、及び前記通信位置が、前記慣性航法位置の軌跡上に存在することを更に拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定するようにすることができる。

上記の移動体端末は、前記取得手段によって取得した近接通信情報に基づいて、他の移動体端末との間で近接通信を行った通信位置を抽出する通信位置抽出手段を更に含み、前記モデル生成手段は、前記通信位置抽出手段によって抽出された通信位置を前記近接通信情報と共に記録した前記ネットワークモデルを生成するようにすることができる。

上記の軌跡推定手段は、前記取得手段により新たな絶対位置情報が取得される毎に、該新たな絶対位置情報を用いて、前記慣性航法位置の軌跡を推定するようにすることができる。

本発明に係るプログラムは、コンピュータを、自端末の絶対位置を示す絶対位置情報、他の移動体端末との間で近接通信を行った時刻及び通信相手を示す近接通信情報、並びに前記自端末に搭載された慣性航法センサにより検出された慣性航法情報を取得する取得手段、前記取得手段によって取得された前記絶対位置情報を初期位置とし、前記取得手段により取得された前記慣性航法情報の時系列データに基づいて、前記自端末の慣性航法位置の軌跡を推定する軌跡推定手段、前記軌跡推定手段によって推定された前記慣性航法位置の軌跡から、前記自端末の移動方向が変化した位置を各々抽出する抽出手段、前記抽出手段によって抽出された位置を関節とし、前記慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、前記取得手段により取得された近接通信情報が示す他の移動体端末との間で近接通信を行った時刻に対応する前記自端末の慣性航法位置を通信位置として前記近接通信情報と共に記録した前記ネットワークモデルを生成するモデル生成手段、及び前記モデル生成手段によって生成された前記ネットワークモデルと、他の移動体端末の前記ネットワークモデルとに基づいて、前記移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として推定された前記ネットワークモデルの各関節の角度に基づいて、前記自端末の慣性航法位置の各々を補正する補正手段として機能させるためのプログラムである。

本発明に係るプログラムは、コンピュータを、移動体端末の絶対位置情報を初期位置とし、慣性航法センサにより検出された慣性航法情報の時系列データに基づいて推定された前記移動体端末の慣性航法位置の軌跡から抽出された、前記移動体端末の移動方向が変化した位置の各々を関節とし、前記移動体端末の慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、他の移動体端末との間で近接通信を行った時刻に対応する前記移動体端末の慣性航法位置を通信位置として、前記近接通信を行った時刻及び通信相手を示す近接通信情報と共に記録した前記ネットワークモデルを、複数の前記移動体端末について各々取得する取得手段、前記取得手段によって取得された前記移動体端末毎の前記ネットワークモデルに基づいて、移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定する角度推定手段、及び前記角度推定手段によって推定された前記移動体端末毎の前記ネットワークモデルの各関節の角度に基づいて、前記移動体端末毎の慣性航法位置の各々を補正する補正手段として機能させるためのプログラムである。

なお、本発明のプログラムを記憶する記憶媒体は、特に限定されず、ハードディスクであってもよいし、ＲＯＭであってもよい。また、ＣＤ−ＲＯＭやＤＶＤディスク、光磁気ディスクやＩＣカードであってもよい。更にまた、該プログラムを、ネットワークに接続されたサーバ等からダウンロードするようにしてもよい。

以上説明したように、本発明の慣性航法システム、移動体端末、慣性航法装置、及びプログラムによれば、移動体端末の各々について、絶対位置情報を初期位置とし、慣性航法情報の時系列データに基づいて推定される慣性航法位置の軌跡から、自端末の移動方向が変化した位置を抽出して、抽出された位置を関節とし、他の移動体端末との間で近接通信を行った位置を記録したネットワークモデルを生成し、移動体端末間での近接通信を行った通信位置が移動体端末間で対応していることを拘束条件として、移動体端末毎のネットワークモデルの各関節の角度を推定することにより、高価なセンサを用いることなく、移動体端末の位置を精度良く推定することができる、という効果が得られる。

本発明の第１の実施の形態に係る慣性航法システムの機能的構成を示すブロック図である。（Ａ）近接通信における移動体端末間距離と信号強度との関係を示す図、及び（Ｂ）通信位置を抽出する方法を説明するための図である。関節を抽出する方法を説明するための図である。ネットワークモデルを説明するための図である。（Ａ）各移動体端末で推定されたＩＮＳ軌跡を示す図、及び（Ｂ）慣性航法位置が補正された各移動体端末の軌跡を示す図である。本発明の第１の実施の形態におけるネットワークモデル生成処理ルーチンの内容を示すフローチャートである。本発明の第１の実施の形態におけるネットワーク計算処理ルーチンの内容を示すフローチャートである。本発明の第１の実施の形態におけるモデル補正処理ルーチンの内容を示すフローチャートである。本発明の第２の実施の形態に係る慣性航法システムの機能的構成を示すブロック図である。本発明の第１の実施の形態におけるネットワーク計算処理ルーチンの内容を示すフローチャートである。

以下、図面を参照して本発明の実施の形態を詳細に説明する。

図１に示すように、第１の実施の形態に係る慣性航法システム１０は、複数の移動体端末３０と、複数の移動体端末３０から送信された移動体端末情報を受信して、複数の移動体端末３０の位置を推定する慣性航法装置５０とを備えている。複数の移動体端末３０と慣性航法装置５０とは、インターネットなどのネットワーク６０を介して接続されている。

移動体端末３０は、Ｂｌｕｅｔｏｏｔｈ（登録商標）や赤外線通信などにより近接して存在する他の移動体端末３０との間で近接通信を行う近接通信装置３２と、ＧＰＳ（Global Positioning System）衛星及び基地局から送信されたＧＰＳ情報を受信するＧＰＳ装置３４と、加速度センサ、地磁気センサ、ジャイロセンサ等で構成され、移動体端末３０の移動量及び移動方向を含むＩＮＳ（Inertial Navigation System）情報を取得するＩＮＳ装置３６と、移動体端末情報を慣性航法装置５０へ送信するコンピュータ３８とを含んで構成されている。

このコンピュータ３８は、移動体端末３０全体の制御を司るＣＰＵ、後述するネットワークモデル生成処理ルーチンやモデル補正処理ルーチンを実現するためのプログラム等を記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、記憶手段としてのメモリ、ネットワークインタフェース等を含んで構成されており、機能的には、通信点抽出部４０と、慣性航法推定部４２と、関節抽出部４４と、ネットワークモデル生成部４６と、送受信部４８とを含んだ構成で表すことができる。なお、慣性航法推定部４２は本発明の軌跡推定手段の一例である。

近接通信装置３２は、他の移動体端末３０との間で近接通信を行った際に、近接通信を行った時刻、通信相手、及び受信した信号強度を示す近接通信情報を出力する。

ＧＰＳ装置３４は、受信したＧＰＳ情報に基づいて移動体端末３０の位置を測位し、移動体端末の各時刻における絶対位置情報として出力する。ＧＰＳ情報を受信できない屋内等の区間においては、絶対位置情報を出力しない。

ＩＮＳ装置３６は、加速度センサ、地磁気センサ、ジャイロセンサ等で検出された、移動体端末３０の移動量及び移動方向を含むＩＮＳ情報を出力する。

通信点抽出部４０は、近接通信装置３２により得られた近接通信情報の信号強度に基づいて、信号強度が閾値以上となる近接通信情報を、通信位置のデータとして抽出する。

図２（Ａ）に示すように、Ｂｌｕｅｔｏｏｔｈ（登録商標）や無線ＬＡＮなどの通信では、距離に応じて受信する信号強度が変化する。移動体端末同士がすれ違うときには、信号強度が最大になる時点で最も距離が近くなる。

そこで、通信点抽出部４０は、抽出した、信号強度が閾値以上となる近接通信情報の中から、図２（Ｂ）に示すように、信号強度から推定した距離に基づいて、過去Ｔ秒間の推定距離が最小値となる時点での近接通信情報を、通信位置のデータとして抽出する。

慣性航法推定部４２は、取得した各時刻の絶対位置情報の有無に基づいて、ＧＰＳ受信不可区間に入る直前に移動体端末３０で受信したＧＰＳ情報（最後に受信したＧＰＳ情報）に基づく絶対位置情報を、移動体端末３０の初期位置として設定する。

また、慣性航法推定部４２は、設定した初期位置を基準として、取得したセンサ値及び下記（１）式に示すＩＮＳモデルに基づいて、各移動体端末の各時刻における慣性航法位置（以下、「ＩＮＳ軌跡」ともいう）を推定する。

上記（２）式では、進行距離を歩幅ρxステップ数で求める。ステップ数は加速度センサの垂直成分の波形から、ピークの数を数えて求められる。ｓ_kはｋ番目のステップにおける移動体端末３０のＩＮＳ情報（ＩＮＳ装置３６の観測値）である。また、ρ_kは移動体端末３０を携帯する歩行者のｋ番目のステップの歩幅である。θ_kは移動体端末３０のｋ番目のステップにおける進行方位であり、地磁気センサの観測値である。Ｋは歩行者の歩幅パラメータ（定数）、ａ^max _kは移動体端末３０のｋ番目のステップ付近の加速度最大値、ａ^min _kは移動体端末３０のｋ番目のステップ付近の加速度最小値である。上記（２）式においてパラメータＫが決まるとＩＮＳ軌跡が一意に決まるため、ＩＮＳ軌跡を計算することは、パラメータＫを求める問題になる。

関節抽出部４４は、慣性航法推定部４２によって推定されたＩＮＳ軌跡に基づいて、方位変化が大きい位置を抽出し、抽出した位置を関節として定義する。例えば、図３に示すように、ＩＮＳ軌跡から、方位角変化を算出し、方位角変化から、各時刻について、一定時間の角度変化量を算出する。角度変化量が閾値以上であって、角度変化量が極大となる時刻を抽出し、抽出された時刻に対応する慣性航法位置を抽出し、関節として定義する。

ネットワークモデル生成部４６は、通信点抽出部４０による抽出結果、慣性航法推定部４２によって推定されたＩＮＳ軌跡、及び関節抽出部４４による抽出結果を取得し、取得した各情報に基づいて、ＩＮＳ軌跡を近似するように、抽出された関節間を直線で結んだネットワークモデルであって、抽出された通信位置の時刻に対応する移動体端末３０の慣性航法位置を通信位置として近接通信情報と共に記録したネットワークモデルを生成し、自端末の識別情報を付与して、移動体端末情報として生成する。

ここで、図４に示すように、生成されるネットワークモデルは、出発点、関節、通信位置の各々をノードとして表し、ノード間が直線リンクで結合されている。

ネットワークモデルの各ノードには、慣性航法から求まるノードの位置情報が記録されると共に、ノード番号とノード種別とが付与されている(1:gps, 2:関節, 3:通信点, …)。また、通信位置のノードの各々には、通信相手の端末IDが記録されている。また、ネットワークモデルは、リンク情報(どのノード間が結合されているか、[1,2],[2,3],[3,4]…)を含んでいる。

送受信部４８は、ネットワークモデル生成部４６により生成された、自端末の識別情報（端末ＩＤ）を付したネットワークモデルを含む移動体端末情報を慣性航法装置５０へ送信する。また、慣性航法装置５０で補正されたネットワークモデルの各ノード位置を示す位置情報を受信し、図示しない表示装置等に受信した位置情報を表示するように制御する。

慣性航法装置５０は、慣性航法装置５０全体の制御を司るＣＰＵ、後述するネットワーク計算処理ルーチンを実現するためのプログラムを記憶したＲＯＭ、データを一時的に記憶するＲＡＭ、ＨＤＤ等の記憶装置、及びネットワークインタフェース等を含むコンピュータ５２で構成されている。このコンピュータ５２は、機能的には、送受信部５４と、ネットワーク計算部５６と、ノード位置補正部５８とを含んだ構成で表すことができる。なお、ネットワーク計算部５６は本発明の角度推定手段の一例であり、ノード位置補正部５８本発明の補正手段の一例である。

送受信部５４は、複数の移動体端末３０から送信された移動体端末情報の各々を受信し、移動体端末情報に含まれるネットワークモデルをネットワーク計算部５６へ受け渡す。また、補正した各ネットワークモデルの各ノードの位置情報を各移動体端末３０へ送信する。

ネットワーク計算部５６は、複数の移動体端末３０のネットワークモデルに基づいて、以下の（２）式〜（６）式に示す拘束条件の下、ネットワークモデルの各関節の角度の最適解を求める。

ただし、初期値と変位の関係が、以下の式で表わされる。

また、各記号の説明を以下に示す。

ｍ：移動体端末を識別する番号１〜Ｎ
ｘ_j ^m：移動体端末ｍの関節ｊの位置
θ_j ^m：移動体端末ｍの関節ｊの角度
ｘ_c ^m：移動体端末ｍの通信位置ｃの位置
ｘ_GPS ^m：ＧＰＳにより観測された絶対位置
ｄ_j1-j2 ^m：関節ｊ１、ｊ２の距離

上記（２）式の拘束条件は、各移動体端末のネットワークモデルの初期位置は、ＧＰＳで求めた絶対位置であることを示している。上記（３）式の拘束条件は、関節間の距離は、当該関節の慣性航法位置に基づいて定められることを示している。上記（４）式の拘束条件は、通信位置が、ＩＮＳ軌跡上に存在することを示している。上記（５）式の拘束条件は、近接無線通信を行った移動体端末間で当該移動体端末の通信位置が同一であることを示している。上記（６）式の拘束条件は、地磁気センサに関するオフセット誤差の平均は０となることを示している。

上記（２）式〜（６）式の連立方程式は、以下の（７）式に示す行列方程式の形に書ける。

なお、移動体端末３０が３つ以上ある場合には、移動体端末３０毎に、上記（２）式〜（４）式の方程式が生成され、移動体端末３０の組み合わせ毎に、上記（５）式の方程式が生成される。

ここで、Ｑ_yを、上記（２）式〜（６）式の拘束条件の誤差を表す共分散行列とすると、以下の（８）式に従って、最小二乗法により最適解が求められる。

ネットワーク計算部５６は、複数の移動体端末３０から受信した移動体端末情報のネットワークモデルに基づいて、上記（８）式に従って、最小二乗法により、各ネットワークモデルの各関節の角度の初期値との変位Δθ、各ノード（各関節及び各通信位置）の位置の初期値との変位Δｘの最適解を求める。

ノード位置補正部５８は、ネットワーク計算部５６によって計算された各ネットワークモデルの各ノードの位置の初期値との変位Δｘに基づいて、複数の移動体端末３０から受信した移動体端末情報のネットワークモデルの各ノード位置を補正する。

このようにして、拘束条件を同時に満たす各関節の角度の最適解を計算することにより、ネットワークモデルの各ノードに設定された慣性航法位置が近接通信情報を使って補正される（図５（Ａ）、（Ｂ）参照）。

次に、図６を参照して、第１の実施の形態に係る移動体端末３０において実行されるネットワークモデル生成処理ルーチンについて説明する。

ステップ１００で、ＧＰＳ装置３４によってＧＰＳ情報を受信したか否かを判定する。ＧＰＳ装置３４によってＧＰＳ情報を受信すると、ステップ１０２へ進む。

ステップ１０２では、上記ステップ１００で受信したＧＰＳ情報に基づく絶対位置情報を、移動体端末３０の初期位置として設定する。

ステップ１０４では、ＧＰＳ装置３４によってＧＰＳ情報を受信したか否かを判定する。ＧＰＳ装置３４によってＧＰＳ情報を受信すると、移動体端末３０の初期位置を新たに設定する必要があると判断し、上記ステップ１０２へ戻る。一方、ＧＰＳ装置３４によってＧＰＳ情報を受信していない場合には、ステップ１０６へ進む。

ステップ１０６では、ＩＮＳ装置３６の加速度センサ、磁気センサ、ジャイロセンサ等で検出されたＩＮＳ情報を取得する。ステップ１０８では、近接通信装置３２によって行われた近接通信の近接通信情報を取得する。

ステップ１１０において、上記ステップ１０６、１０８で所定期間以上のデータを取得したか否かを判定する。所定期間以上のデータを取得していない場合には、ステップ１０４へ戻る。一方、所定期間以上のデータを取得した場合には、ステップ１１２へ進む。

ステップ１１２では、上記ステップ１０２で設定した初期位置を基準として、上記ステップ１０６で取得したＩＮＳ情報及び上記（１）式に示すＩＮＳモデルに基づいて、移動体端末３０の各時刻における慣性航法位置を推定し、ＩＮＳ軌跡を推定する。

次のステップ１１４では、上記ステップ１１２で推定されたＩＮＳ軌跡から、方位変化が大きい位置を関節として抽出する。ステップ１１６では、上記ステップ１０８で取得した近接通信情報の信号強度に基づいて、通信位置を抽出する。

そして、ステップ１１８において、上記ステップ１１２で推定されたＩＮＳ軌跡、上記ステップ１１４で抽出された関節の位置、及び上記ステップ１１６で抽出された通信位置に基づいて、ネットワークモデルを生成する。

このとき、前回のステップ１１８で生成されたネットワークモデルであって、初期位置が同一であるネットワークモデルが存在する場合には、前回のステップ１１６で生成されたネットワークモデルと結合させたネットワークモデルを生成する。また、後述するモデル補正処理ルーチンで、前回生成されたネットワークモデルの各ノード位置が補正されている場合には、当該補正されているネットワークモデルと結合させたネットワークモデルを生成する。

次のステップ１２０では、上記ステップ１１８で生成されたネットワークモデルに、移動体端末３０のＩＤを付して、移動体端末情報として慣性航法装置５０へ送信し、上記ステップ１０４へ戻る。

このように、各移動体端末３０が、上記ネットワークモデル生成処理ルーチンを実行することにより、ＧＰＳ情報を受信した後にＧＰＳ受信不可区間に入った場合に、ＧＰＳ情報に基づく絶対位置を初期位置としたネットワークモデルを随時生成して、慣性航法装置５０へ送信する。

次に、図７を参照して、第１の実施の形態に係る慣性航法装置５０において実行されるネットワーク計算処理ルーチンについて説明する。

ステップ１３０で、送受信部５４が、複数の移動体端末３０から送信された移動体端末情報の各々を受信して、移動体端末情報を収集する。

次に、ステップ１３２で、上記ステップ１３０で収集された移動体端末情報に含まれるネットワークモデルにおける通信位置の有無に基づいて、受信した移動体端末情報に通信位置が含まれるか否かを判定する。通信位置が含まれる場合には、ステップ１３４へ移行し、一方、通信位置が含まれない場合には、ネットワーク計算を行うことができないと判断し、ネットワーク計算処理ルーチンを終了する。

ステップ１３４では、上記ステップ１３０で収集された移動体端末情報に含まれるネットワークモデルについて成立する拘束条件（（２）式〜（６）式）の下、各ネットワークモデルの各ノードの位置の変位及び各関節の角度の変位の最適解を計算する。

次に、ステップ１３６で、上記ステップ１３６で計算された各ノードの位置の変位に基づいて、移動体端末情報に含まれるネットワークモデルの各ノード位置を補正する。ステップ１３８では、上記ステップ１３６で各ノード位置が補正されたネットワークモデルの各々を、当該ネットワークモデルを含む移動体端末情報を送信した移動体端末３０へ送信し、ネットワーク計算処理ルーチンを終了する。

次に、図８を参照して、第１の実施の形態に係る移動体端末３０において実行されるモデル補正処理ルーチンについて説明する。

まず、ステップ１５０において、送受信部４８によって、慣性航法装置５０から送信された、各ノード位置が補正されたネットワークモデルを受信したか否かを判定する。各ノード位置が補正されたネットワークモデルを受信すると、ステップ１５２へ進み、上述したネットワークモデル生成処理ルーチンで生成したネットワークモデルを、上記ステップ１５０で受信した、各ノード位置が補正されたネットワークモデルに更新し、モデル補正処理ルーチンを終了する。

以上説明したように、第１の実施の形態に係る慣性航法システムによれば、移動体端末の各々において、ＧＰＳ情報に基づく絶対位置情報を初期位置とし、ＩＮＳ情報の時系列データに基づいて推定されるＩＮＳ軌跡から、自端末の移動方向が変化した位置を抽出して、抽出された位置を関節とし、他の移動体端末との間で近接通信を行った位置を記録したネットワークモデルを生成する。慣性航法装置は、複数の移動体端末の各々で生成されたネットワークモデルに基づいて、移動体端末間での近接通信を行った通信位置が移動体端末間で同一となることを拘束条件として、移動体端末毎のネットワークモデルの各関節の角度の最適解を推定することにより、高価なセンサを用いることなく、移動体端末の位置を精度良く推定することができる。

また、屋内に設置するインフラがない場合でも、近接通信を使って慣性航法誤差を補正することで、携帯端末のみで自己位置を精度よく推定することができる。また、測距精度の低いWifi等であっても、距離の近い状況ではある程度の測距精度が保証できる。一方、データ点数が少なくなるが、関節モデルを利用することで自由度を下げることができるため、少ないデータ点数でも効果を得ることができる。

また、ＩＮＳ軌跡をリンク構造であるネットワークモデルで近似し、Wifiを使った協調測位で、ネットワークモデルの各関節の角度を推定する。これによって、地磁気センサのオフセット誤差を補正できる。また、ネットワークモデルの自由度が低いため、精度良い点が少ないWifiデータなどの近接通信情報であっても、移動体端末の位置を精度よく推定することができる。

次に、第２の実施の形態について説明する。なお、第１の実施の形態と同様の構成となる部分については、同一符号を付して説明を省略する。

第２の実施の形態では、慣性航法装置側で、ネットワークモデルを生成している点が、第１の実施の形態と異なっている。

図９に示すように、第２の実施の形態に係る慣性航法システム２１０の移動体端末２３０は、近接通信装置３２と、ＧＰＳ装置３４と、ＩＮＳ装置３６と、移動体端末情報を慣性航法装置２５０へ送信するコンピュータ２３８とを含んで構成されている。

このコンピュータ２３８は、機能的には、制御部２４８と送受信部４８とを含んだ構成で表すことができる。

制御部２４８は、各時刻において、近接通信装置３２から出力された近接通信情報、ＧＰＳ装置３４から出力された絶対位置情報、及びＩＮＳ装置３６から出力されたＩＮＳ情報を取得し、取得した各情報をまとめて、自端末の識別情報を付与した移動体端末情報として生成する。なお、近接通信情報は近接通信が行われた際に出力され、絶対位置情報はＧＰＳ情報が受信された場合に出力されるものであるため、各時刻における移動体端末情報には、近接通信情報及び絶対位置情報が含まれない場合もある。

送受信部４８は、制御部２４８により生成された移動体端末情報を所定時間毎に慣性航法装置２５０へ送信する。また、慣性航法装置２５０で推定された位置を示す位置情報を受信し、図示しない表示装置等に受信した位置情報を表示するように制御する。

慣性航法装置２５０は、機能的には、送受信部５４と、通信点抽出部２４０と、慣性航法推定部２４２と、関節抽出部２４４と、ネットワークモデル生成部２４６と、ネットワーク計算部５６と、ノード位置補正部５８とを含んだ構成で表すことができる。

送受信部５４は、複数の移動体端末２３０から送信された移動体端末情報の各々を受信し、移動体端末情報に含まれる絶対位置情報及びセンサ値を慣性航法推定部２４２へ受け渡し、かつ、移動体端末情報に含まれる近接通信情報を通信点抽出部２４０へ受け渡す。また、各ノードの位置情報を補正した各ネットワークモデルから得られる各移動体端末３０の絶対位置情報を各移動体端末２３０へ送信する。

通信点抽出部２４０は、複数の移動体端末２３０の各々について、当該移動体端末２３０から送信された移動体端末情報に含まれる近接通信情報の信号強度に基づいて、上記の第１の実施の形態における通信点抽出部４０と同様に、信号強度が閾値以上となる近接通信情報を、当該移動体端末２３０の通信位置のデータとして抽出する。

慣性航法推定部２４２は、複数の移動体端末２３０の各々について、当該移動体端末２３０について取得した各時刻の絶対位置情報の有無に基づいて、上記の第１の実施の形態と同様に、ＧＰＳ受信不可区間に入る直前に移動体端末３０で受信したＧＰＳ情報（最後に受信したＧＰＳ情報）に基づく絶対位置情報を、移動体端末２３０の初期位置として設定する。

また、慣性航法推定部２４２は、複数の移動体端末２３０の各々について、設定した初期位置を基準として、取得したセンサ値及び上記（１）式に示すＩＮＳモデルに基づいて、上記の第１の実施の形態と同様に、当該移動体端末２３０の各時刻における慣性航法位置を推定し、当該移動体端末２３０のＩＮＳ軌跡を推定する。

関節抽出部２４４は、複数の移動体端末２３０の各々について、慣性航法推定部２４２によって当該移動体端末について推定されたＩＮＳ軌跡に基づいて、上記の第１の実施の形態と同様に、方位変化が大きい位置を抽出し、抽出した位置を関節として定義する。

ネットワークモデル生成部２４６は、複数の移動体端末２３０の各々について、通信点抽出部４０による当該移動体端末に対する抽出結果、慣性航法推定部４２によって推定された当該移動体端末のＩＮＳ軌跡、及び関節抽出部４４による当該移動体端末に対する抽出結果を取得し、取得した各情報に基づいて、上記の第１の実施の形態と同様に、当該移動体端末２３０のネットワークモデルを生成する。

第２の実施の形態に係る慣性航法装置２５０によって実行されるネットワーク計算処理ルーチンについて図１０を用いて説明する。なお、第１の実施の形態と同様の処理については、同一符号を付して詳細な説明を省略する。

まず、ステップ２６０で、送受信部５４が、一定期間の間、複数の移動体端末２３０から送信された移動体端末情報の各々を受信して、移動端端末情報を収集する。

次に、ステップ２６２で、上記ステップ２６０で収集された移動体端末情報に基づいて、受信した移動体端末情報に近接通信情報が含まれるか否かを判定する。近接通信情報が含まれる場合には、ステップ２６４へ移行し、一方、近接通信情報が含まれない場合には、ネットワーク計算を行うことができないと判断し、ネットワーク計算処理ルーチンを終了する。

ステップ２６４では、上記ステップ２６０で移動体端末情報を受信した複数の移動体端末２３０の各々について、上記ステップ２６０で収集した当該移動体端末２３０の移動体端末情報に含まれる最後のＧＰＳ情報に基づく絶対位置情報を、当該移動体端末２３０の初期位置として設定する。

ステップ２６６では、上記ステップ２６０で移動体端末情報を受信した複数の移動体端末２３０の各々について、上記ステップ２６４で設定した初期位置を基準として、上記ステップ２６０で収集した移動体端末情報に含まれる各センサ値及び上記（１）式に示すＩＮＳモデルに基づいて、当該移動体端末２３０の各時刻における慣性航法位置を推定し、ＩＮＳ軌跡を推定する。

次のステップ２６８では、上記ステップ２６０で移動体端末情報を受信した複数の移動体端末２３０の各々について、上記ステップ２６６で推定された当該移動体端末２３０のＩＮＳ軌跡から、方位変化が大きい位置を関節として抽出する。ステップ２７０では、上記ステップ２６０で移動体端末情報を受信した複数の移動体端末２３０の各々について、上記ステップ２６０で収集した当該移動体端末２３０の移動体端末情報に含まれる近接通信情報の信号強度に基づいて、通信位置を抽出する。

そして、ステップ２７２において、上記ステップ２６０で移動体端末情報を受信した複数の移動体端末２３０の各々について、上記ステップ２６６で推定されたＩＮＳ軌跡、上記ステップ２６８で抽出された関節の位置、及び上記ステップ２７０で抽出された通信位置に基づいて、当該移動体端末２３０のネットワークモデルを生成する。

ステップ１３４では、上記ステップ２７２で生成されたネットワークモデルについて成立する拘束条件（（２）式〜（６）式）の下、各ネットワークモデルの各ノードの位置の変位及び各関節の角度の変位の最適解を計算する。

次に、ステップ１３６で、上記ステップ１３６で計算された各ノードの位置の変位に基づいて、各移動体端末２３０のネットワークモデルの各ノード位置を補正する。ステップ２７４では、上記ステップ２６０で移動体端末情報を受信した複数の移動体端末２３０の各々について、上記ステップ１３６で各ノード位置が補正された当該移動体端末２３０のネットワークモデルに基づいて、当該移動体端末２３０の絶対位置情報を求め、当該移動体端末２３０へ送信し、ネットワーク計算処理ルーチンを終了する。

なお、第２の実施の形態に係る慣性航法システム２１０の他の構成及び作用については、第１の実施の形態と同様であるため、説明を省略する。

以上説明したように、第２の実施の形態に係る慣性航法システムによれば、慣性航法装置において、移動体端末の各々について、ＧＰＳ情報に基づく絶対位置情報を初期位置とし、ＩＮＳ情報の時系列データに基づいて推定されるＩＮＳ軌跡から、自端末の移動方向が変化した位置を抽出して、抽出された位置を関節とし、他の移動体端末との間で近接通信を行った位置を記録したネットワークモデルを生成する。そして、慣性航法装置は、複数の移動体端末の各々で生成されたネットワークモデルに基づいて、移動体端末間での近接通信を行った通信位置が移動体端末間で同一となることを拘束条件として、移動体端末毎のネットワークモデルの各関節の角度の最適解を推定することにより、高価なセンサを用いることなく、移動体端末の位置を精度良く推定することができる。

なお、上記実施の形態では、移動体端末を歩行者が携帯する形態を想定しているが、移動体端末を車両等の移動体に搭載した形態としてもよい。

また、上記実施の形態では、ＧＰＳ受信不可区間に入る直前にＧＰＳ情報に基づく絶対位置を初期位置として設定する場合について説明したが、例えば、建物に入る際に移動体端末に搭載されたＩＣ等に記録されるタグ情報等を初期位置として設定してもよい。

また、慣性航法装置において、各ネットワークモデルの各関節の角度の最適解を計算する場合を例に説明したが、これに限定されるものでない。例えば、移動体端末において、自己のネットワークモデルを生成すると共に、他の移動体端末からネットワークモデルを受信し、移動体端末が、複数のネットワークモデルに基づいて、各ネットワークモデルの各関節の角度の最適解を計算するようにしてもよい。

１０、２１０慣性航法システム
３０、２３０移動体端末
３２近接通信装置
３４ＧＰＳ装置
３６ＩＮＳ装置
３８、２３８コンピュータ
４０、２４０通信点抽出部
４２、２４２慣性航法推定部
４４、２４４関節抽出部
４６、２４６ネットワークモデル生成部
４８送受信部
５０、２５０慣性航法装置
５２コンピュータ
５４送受信部
５６ネットワーク計算部
５８ノード位置補正部
２４８制御部

Claims

自端末の絶対位置を示す絶対位置情報、他の移動体端末との間で近接通信を行った時刻及び通信相手を示す近接通信情報、並びに前記自端末に搭載された慣性航法センサにより検出された慣性航法情報を取得する取得手段、
前記取得手段によって取得された前記絶対位置情報を初期位置とし、前記取得手段により取得された前記慣性航法情報の時系列データに基づいて、前記自端末の慣性航法位置の軌跡を推定する軌跡推定手段、
前記軌跡推定手段によって推定された前記慣性航法位置の軌跡から、前記自端末の移動方向が変化した位置を各々抽出する抽出手段、
前記抽出手段によって抽出された位置を関節とし、前記慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、前記取得手段により取得された近接通信情報が示す他の移動体端末との間で近接通信を行った時刻に対応する前記自端末の慣性航法位置を通信位置として前記近接通信情報と共に記録した前記ネットワークモデルを生成するモデル生成手段、及び
前記モデル生成手段によって生成された前記ネットワークモデルを送信する送信手段
を含む複数の移動体端末と、
前記複数の前記移動体端末から各々送信された前記ネットワークモデルを受信する受信手段、
前記受信手段によって受信された前記移動体端末毎の前記ネットワークモデルに基づいて、前記移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定する角度推定手段、及び
前記角度推定手段によって推定された前記移動体端末毎の前記ネットワークモデルの各関節の角度に基づいて、前記移動体端末毎の慣性航法位置の各々を補正する補正手段
を含む慣性航法装置と、
を含む慣性航法システム。
自端末の絶対位置を示す絶対位置情報、他の移動体端末との間で近接通信を行った時刻及び通信相手を示す近接通信情報、並びに前記自端末に搭載された慣性航法センサにより検出された慣性航法情報を取得する取得手段と、
前記取得手段によって取得された前記絶対位置情報を初期位置とし、前記取得手段により取得された前記慣性航法情報の時系列データに基づいて、前記自端末の慣性航法位置の軌跡を推定する軌跡推定手段と、
前記軌跡推定手段によって推定された前記慣性航法位置の軌跡から、前記自端末の移動方向が変化した位置を各々抽出する抽出手段と、
前記抽出手段によって抽出された位置を関節とし、前記慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、前記取得手段により取得された近接通信情報が示す他の移動体端末との間で近接通信を行った時刻に対応する前記自端末の慣性航法位置を通信位置として前記近接通信情報と共に記録した前記ネットワークモデルを生成するモデル生成手段と、
前記モデル生成手段によって生成された前記ネットワークモデルと、他の移動体端末の前記ネットワークモデルとに基づいて、前記移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として推定された前記ネットワークモデルの各関節の角度に基づいて、前記自端末の慣性航法位置の各々を補正する補正手段と、
を含む移動体端末。
移動体端末の絶対位置情報を初期位置とし、慣性航法センサにより検出された慣性航法情報の時系列データに基づいて推定された前記移動体端末の慣性航法位置の軌跡から抽出された、前記移動体端末の移動方向が変化した位置の各々を関節とし、前記移動体端末の慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、他の移動体端末との間で近接通信を行った時刻に対応する前記移動体端末の慣性航法位置を通信位置として、前記近接通信を行った時刻及び通信相手を示す近接通信情報と共に記録した前記ネットワークモデルを、複数の前記移動体端末について各々取得する手段と、
前記取得手段によって取得された前記移動体端末毎の前記ネットワークモデルに基づいて、移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定する角度推定手段と、
前記角度推定手段によって推定された前記移動体端末毎の前記ネットワークモデルの各関節の角度に基づいて、前記移動体端末毎の慣性航法位置の各々を補正する補正手段と、
を含む慣性航法装置。
前記角度推定手段は、前記ネットワークモデルが表す慣性航法位置の軌跡の初期位置が、前記取得した絶対位置情報であること、前記関節間の距離が、前記関節の前記慣性航法位置に基づいて定められる距離であること、及び前記通信位置が、前記慣性航法位置の軌跡上に存在することを更に拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定する請求項１記載の慣性航法システム。
前記移動体端末は、前記取得手段によって取得した近接通信情報に基づいて、他の移動体端末との間で近接通信を行った通信位置を抽出する通信位置抽出手段を更に含み、
前記モデル生成手段は、前記通信位置抽出手段によって抽出された通信位置を前記近接通信情報と共に記録した前記ネットワークモデルを生成する請求項１又は４記載の慣性航法システム。
前記軌跡推定手段は、前記取得手段により新たな絶対位置情報が取得される毎に、該新たな絶対位置情報を用いて、前記慣性航法位置の軌跡を推定する請求項１、４、又は５記載の慣性航法システム。
コンピュータを、
自端末の絶対位置を示す絶対位置情報、他の移動体端末との間で近接通信を行った時刻及び通信相手を示す近接通信情報、並びに前記自端末に搭載された慣性航法センサにより検出された慣性航法情報を取得する取得手段、
前記取得手段によって取得された前記絶対位置情報を初期位置とし、前記取得手段により取得された前記慣性航法情報の時系列データに基づいて、前記自端末の慣性航法位置の軌跡を推定する軌跡推定手段、
前記軌跡推定手段によって推定された前記慣性航法位置の軌跡から、前記自端末の移動方向が変化した位置を各々抽出する抽出手段、
前記抽出手段によって抽出された位置を関節とし、前記慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、前記取得手段により取得された近接通信情報が示す他の移動体端末との間で近接通信を行った時刻に対応する前記自端末の慣性航法位置を通信位置として前記近接通信情報と共に記録した前記ネットワークモデルを生成するモデル生成手段、及び
前記モデル生成手段によって生成された前記ネットワークモデルと、他の移動体端末の前記ネットワークモデルとに基づいて、前記移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として推定された前記ネットワークモデルの各関節の角度に基づいて、前記自端末の慣性航法位置の各々を補正する補正手段
として機能させるためのプログラム。
コンピュータを、
移動体端末の絶対位置情報を初期位置とし、慣性航法センサにより検出された慣性航法情報の時系列データに基づいて推定された前記移動体端末の慣性航法位置の軌跡から抽出された、前記移動体端末の移動方向が変化した位置の各々を関節とし、前記移動体端末の慣性航法位置の軌跡を近似するように前記関節間を直線で結んだネットワークモデルであって、他の移動体端末との間で近接通信を行った時刻に対応する前記移動体端末の慣性航法位置を通信位置として、前記近接通信を行った時刻及び通信相手を示す近接通信情報と共に記録した前記ネットワークモデルを、複数の前記移動体端末について各々取得する取得手段、
前記取得手段によって取得された前記移動体端末毎の前記ネットワークモデルに基づいて、移動体端末間での近接通信を行った通信位置が前記移動体端末間で対応していることを拘束条件として、前記移動体端末毎の前記ネットワークモデルの各関節の角度を推定する角度推定手段、及び
前記角度推定手段によって推定された前記移動体端末毎の前記ネットワークモデルの各関節の角度に基づいて、前記移動体端末毎の慣性航法位置の各々を補正する補正手段
として機能させるためのプログラム。
コンピュータを、請求項２記載の移動体端末、又は請求項３記載の慣性航法装置を構成する各手段として機能させるためのプログラム。