特許 7602960 姿勢推定装置、姿勢推定方法及び姿勢推定プログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2024-12-11

(45)【発行日】2024-12-19

(54)【発明の名称】姿勢推定装置、姿勢推定方法及び姿勢推定プログラム

(51)【国際特許分類】

   G01S 19/54 20100101AFI20241212BHJP

   G01S 19/47 20100101ALI20241212BHJP

【ＦＩ】

G01S19/54

G01S19/47

【請求項の数】 7

(21)【出願番号】P 2021077731

(22)【出願日】2021-04-30

(65)【公開番号】P2022171212

(43)【公開日】2022-11-11

【審査請求日】2024-02-15

(73)【特許権者】

【識別番号】000003609

【氏名又は名称】株式会社豊田中央研究所

(73)【特許権者】

【識別番号】000004260

【氏名又は名称】株式会社デンソー

(74)【代理人】

【識別番号】110001519

【氏名又は名称】弁理士法人太陽国際特許事務所

(72)【発明者】

【氏名】武山 洪二郎

(72)【発明者】

【氏名】鈴木 徳祥

(72)【発明者】

【氏名】牧戸 知史

(72)【発明者】

【氏名】宝地 卓

(72)【発明者】

【氏名】宮島 朗

(72)【発明者】

【氏名】武藤 勝彦

【審査官】藤田 都志行

(56)【参考文献】

【文献】特開２００７－２２５４０８（ＪＰ，Ａ）

【文献】特開２０１５－０６８７１４（ＪＰ，Ａ）

【文献】特開２０２１－０１５００２（ＪＰ，Ａ）

【文献】中国特許出願公開第１１０７９３５１８（ＣＮ，Ａ）

【文献】米国特許第６０６１６３１（ＵＳ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｓ １９／００－１９／５５

Ｇ０１Ｃ ２１／２６－２１／３６

(57)【特許請求の範囲】

【請求項1】

移動体の姿勢を推定する姿勢推定装置であって、
測位衛星から送信され、複数のアンテナで受信した衛星信号を取得する衛星信号取得部と、
前記衛星信号取得部が取得した前記衛星信号を用いて前記アンテナ間の相対的な位置関係を算出する相対測位部と、
前記衛星信号取得部が取得した前記衛星信号を用いて前記移動体の三次元速度ベクトルを算出する速度ベクトル算出部と、
前記相対測位部が算出した前記位置関係を用いて前記移動体の姿勢を推定する第１姿勢推定部と、
前記速度ベクトル算出部が算出した前記三次元速度ベクトルを用いて前記移動体の姿勢を推定する第２姿勢推定部と、
前記第１姿勢推定部の推定結果と、前記第２姿勢推定部の推定結果とを統合する姿勢統合部と、
を備える、姿勢推定装置。

【請求項2】

前記第２姿勢推定部が推定した前記三次元速度ベクトルの大きさを用いて、前記第２姿勢推定部の推定結果の信頼性を判定する第２信頼性判定部をさらに備える、請求項１に記載の姿勢推定装置。

【請求項3】

前記姿勢統合部は、前記第２信頼性判定部が判断した信頼性に基づく重みを前記第２姿勢推定部の推定結果に与えて、前記第１姿勢推定部の推定結果と統合する、請求項２に記載の姿勢推定装置。

【請求項4】

前記第１姿勢推定部の推定結果又は入力情報を用いて、前記第１姿勢推定部の推定結果の信頼性を判定する第１信頼性判定部をさらに備える、請求項１～３のいずれか１項に記載の姿勢推定装置。

【請求項5】

前記移動体に搭載された慣性センサが測定した情報を取得する慣性センサ情報取得部をさらに備え、
前記姿勢統合部は、前記慣性センサ情報取得部が取得した情報を用いて、前記第１姿勢推定部の推定結果と、前記第２姿勢推定部の推定結果とを統合する、請求項１～４のいずれか１項に記載の姿勢推定装置。

【請求項6】

移動体の姿勢を推定する姿勢推定方法であって、
プロセッサが、
複数のアンテナが受信した、測位衛星から送信される衛星信号を用いて前記アンテナ間の相対的な位置関係を算出し、
取得した前記衛星信号を用いて前記移動体の三次元速度ベクトルを算出し、
算出した前記位置関係を用いて前記移動体の姿勢を推定し、
算出した前記三次元速度ベクトルを用いて前記移動体の姿勢を推定し、
前記位置関係を用いた推定結果と、前記三次元速度ベクトルを用いた推定結果とを統合する、
姿勢推定方法。

【請求項7】

移動体の姿勢を推定する姿勢推定プログラムであって、
コンピュータを、
測位衛星から送信され、複数のアンテナで受信した衛星信号を取得する衛星信号取得部と、
前記衛星信号取得部が取得した前記衛星信号を用いて前記アンテナ間の相対的な位置関係を算出する相対測位部と、
前記衛星信号取得部が取得した前記衛星信号を用いて前記移動体の三次元速度ベクトルを算出する速度ベクトル算出部と、
前記相対測位部が算出した前記位置関係を用いて前記移動体の姿勢を推定する第１姿勢推定部と、
前記速度ベクトル算出部が算出した前記三次元速度ベクトルを用いて前記移動体の姿勢を推定する第２姿勢推定部と、
前記第１姿勢推定部の推定結果と、前記第２姿勢推定部の推定結果とを統合する姿勢統合部と、
を備える、姿勢推定装置として機能させるための姿勢推定プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、姿勢推定装置、姿勢推定方法及び姿勢推定プログラムに関する。

【背景技術】

【0002】

移動体の姿勢を推定する技術が提案されている。例えば特許文献１には、移動体の姿勢情報を算出する姿勢情報算出装置であって、ＧＮＳＳアンテナの鉛直方向に対する姿勢を維持する保持機構と、移動体の座標系に固定され、移動体の姿勢変動を検出する慣性センサと、ＧＮＳＳアンテナで得た情報と、慣性センサが検出した情報と、に基づいて、移動体の姿勢情報を算出する演算部と、を備えることを特徴とする姿勢情報算出装置の技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１３－２５３９２８号公報

【発明の概要】

【発明が解決しようとする課題】

【0004】

上記特許文献１で開示された技術のように、ＧＰＳコンパスに基づく姿勢推定を行い、その推定結果と慣性センサの計測値とを統合し姿勢推定を行う技術がある。しかし、ＧＰＳコンパスは、衛星受信環境が良好ではない市街地などにおいては。姿勢推定結果を得ることが困難となる。そのため、慣性センサのドリフトなどにより姿勢推定の精度が劣化する恐れがある。

【0005】

本発明は、上記の点に鑑みてなされたものであり、衛星受信環境や移動速度の程度によらず、精度の良い姿勢推定を可能とする、姿勢推定装置、姿勢推定方法及び姿勢推定プログラムを提供することを目的とする。

【課題を解決するための手段】

【0006】

本発明の第１態様に係る姿勢推定装置は、移動体の姿勢を推定する姿勢推定装置であって、測位衛星から送信され、複数のアンテナで受信した衛星信号を取得する衛星信号取得部と、前記衛星信号取得部が取得した前記衛星信号を用いて前記アンテナ間の相対的な位置関係を算出する相対測位部と、前記衛星信号取得部が取得した前記衛星信号を用いて前記移動体の三次元速度ベクトルを算出する速度ベクトル算出部と、前記相対測位部が算出した前記位置関係を用いて前記移動体の姿勢を推定する第１姿勢推定部と、前記速度ベクトル算出部が算出した前記三次元速度ベクトルを用いて前記移動体の姿勢を推定する第２姿勢推定部と、前記第１姿勢推定部の推定結果と、前記第２姿勢推定部の推定結果とを統合する姿勢統合部と、を備える。

【0007】

本発明の第２態様に係る姿勢推定装置は、第１態様に係る姿勢推定装置であって、前記第２姿勢推定部が推定した前記三次元速度ベクトルの大きさを用いて、前記第２姿勢推定部の推定結果の信頼性を判定する第２信頼性判定部をさらに備える。

【0008】

本発明の第３態様に係る姿勢推定装置は、第２態様に係る姿勢推定装置であって、前記姿勢統合部は、前記第２信頼性判定部が判断した信頼性に基づく重みを前記第２姿勢推定部の推定結果に与えて、前記第１姿勢推定部の推定結果と統合する。

【0009】

本発明の第４態様に係る姿勢推定装置は、第１態様～第３態様のいずれかに係る姿勢推定装置であって、前記第１姿勢推定部の推定結果又は入力情報を用いて、前記第１姿勢推定部の推定結果の信頼性を判定する第１信頼性判定部をさらに備える。

【0010】

本発明の第５態様に係る姿勢推定装置は、第１態様～第４態様のいずれかに係る姿勢推定装置であって、前記移動体に搭載された慣性センサが測定した情報を取得する慣性センサ情報取得部をさらに備え、前記姿勢統合部は、前記慣性センサ情報取得部が取得した情報を用いて、前記第１姿勢推定部の推定結果と、前記第２姿勢推定部の推定結果とを統合する。

【0011】

本発明の第６態様に係る姿勢推定方法は、移動体の姿勢を推定する姿勢推定方法であって、プロセッサが、複数のアンテナが受信した、測位衛星から送信される衛星信号を用いて前記アンテナ間の相対的な位置関係を算出し、取得した前記衛星信号を用いて前記移動体の三次元速度ベクトルを算出し、算出した前記位置関係を用いて前記移動体の姿勢を推定し、算出した前記三次元速度ベクトルを用いて前記移動体の姿勢を推定し、前記位置関係を用いた推定結果と、前記三次元速度ベクトルを用いた推定結果とを統合する。

【0012】

本発明の第７態様に係る姿勢推定プログラムは、移動体の姿勢を推定する姿勢推定プログラムであって、コンピュータを、測位衛星から送信され、複数のアンテナで受信した衛星信号を取得する衛星信号取得部と、前記衛星信号取得部が取得した前記衛星信号を用いて前記アンテナ間の相対的な位置関係を算出する相対測位部と、前記衛星信号取得部が取得した前記衛星信号を用いて前記移動体の三次元速度ベクトルを算出する速度ベクトル算出部と、前記相対測位部が算出した前記位置関係を用いて前記移動体の姿勢を推定する第１姿勢推定部と、前記速度ベクトル算出部が算出した前記三次元速度ベクトルを用いて前記移動体の姿勢を推定する第２姿勢推定部と、前記第１姿勢推定部の推定結果と、前記第２姿勢推定部の推定結果とを統合する姿勢統合部と、を備える、姿勢推定装置として機能させる。

【発明の効果】

【0013】

本発明によれば、推定結果を統合することで、衛星受信環境や移動速度の程度によらず、精度の良い姿勢推定を可能とする、姿勢推定装置、姿勢推定方法及び姿勢推定プログラムを提供することができる。

【図面の簡単な説明】

【0014】

【図1】本実施形態に係る姿勢推定装置の概要を示す図である。

【図2】姿勢推定装置のハードウェア構成を示すブロック図である。

【図3】姿勢推定装置の機能構成の例を示すブロック図である。

【図4】姿勢推定装置による姿勢推定処理の流れを示すフローチャートである。

【図5】姿勢推定装置の機能構成の例を示すブロック図である。

【図6】姿勢推定装置による姿勢推定処理の流れを示すフローチャートである。

【図7】姿勢推定装置の機能構成の例を示すブロック図である。

【図8】姿勢推定装置による姿勢推定処理の流れを示すフローチャートである。

【発明を実施するための形態】

【0015】

以下、本開示の実施形態の一例を、図面を参照しつつ説明する。なお、各図面において同一または等価な構成要素および部分には同一の参照符号を付与している。また、図面の寸法比率は、説明の都合上誇張されており、実際の比率とは異なる場合がある。

【0016】

図１は、本実施形態に係る姿勢推定装置の概要を示す図である。姿勢推定装置１０は、移動体の一例である車両１の姿勢を推定する装置である。姿勢推定装置１０は、車両１の姿勢を推定するにあたって、複数のアンテナからなる信号受信装置２が受信した、測位衛星から送信される衛星信号を用いる。信号受信装置２が備えるアンテナの数は、２本でもよく、３本以上であってもよい。また、姿勢推定装置１０は、車両１の姿勢を推定するにあたって、慣性センサ３が取得した情報を用いて車両１の姿勢を推定してもよい。

【0017】

以下、本実施形態に係る姿勢推定装置１０の構成例を説明する。

【0018】

図２は、姿勢推定装置１０のハードウェア構成を示すブロック図である。

【0019】

図２に示すように、姿勢推定装置１０は、ＣＰＵ（Ｃｅｎｔｒａｌ Ｐｒｏｃｅｓｓｉｎｇ Ｕｎｉｔ）１１、ＲＯＭ（Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）１２、ＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）１３、ストレージ１４、入力部１５、表示部１６及び通信インタフェース（Ｉ／Ｆ）１７を有する。各構成は、バス１９を介して相互に通信可能に接続されている。

【0020】

ＣＰＵ１１は、中央演算処理ユニットであり、各種プログラムを実行したり、各部を制御したりする。すなわち、ＣＰＵ１１は、ＲＯＭ１２またはストレージ１４からプログラムを読み出し、ＲＡＭ１３を作業領域としてプログラムを実行する。ＣＰＵ１１は、ＲＯＭ１２またはストレージ１４に記録されているプログラムにしたがって、上記各構成の制御および各種の演算処理を行う。本実施形態では、ＲＯＭ１２またはストレージ１４には、車両１の姿勢を推定する姿勢推定プログラムが格納されている。

【0021】

ＲＯＭ１２は、各種プログラムおよび各種データを格納する。ＲＡＭ１３は、作業領域として一時的にプログラムまたはデータを記憶する。ストレージ１４は、ＨＤＤ（Ｈａｒｄ Ｄｉｓｋ Ｄｒｉｖｅ）、ＳＳＤ（Ｓｏｌｉｄ Ｓｔａｔｅ Ｄｒｉｖｅ）またはフラッシュメモリ等の記憶装置により構成され、オペレーティングシステムを含む各種プログラム、および各種データを格納する。

【0022】

入力部１５は、マウス等のポインティングデバイス、およびキーボードを含み、各種の入力を行うために使用される。

【0023】

表示部１６は、たとえば、液晶ディスプレイであり、各種の情報を表示する。表示部１６は、タッチパネル方式を採用して、入力部１５として機能しても良い。

【0024】

通信インタフェース１７は、他の機器と通信するためのインタフェースであり、たとえば、イーサネット（登録商標）、ＦＤＤＩ、Ｗｉ－Ｆｉ（登録商標）等の規格が用いられる。

【0025】

上記の姿勢推定プログラムを実行する際に、姿勢推定装置１０は、上記のハードウェア資源を用いて、各種の機能を実現する。

【0026】

次に、姿勢推定装置１０の機能構成について説明する。

【0027】

図３は、姿勢推定装置１０の機能構成の例を示すブロック図である。

【0028】

図３に示すように、姿勢推定装置１０は、機能構成として、衛星情報取得部１１０、相対測位部１２０、第１姿勢推定部１３０、速度ベクトル算出部１４０、第２姿勢推定部１５０および姿勢統合部１６０を有する。各機能構成は、ＣＰＵ１１がＲＯＭ１２またはストレージ１４に記憶された姿勢推定プログラムを読み出し、実行することにより実現される。

【0029】

衛星情報取得部１１０は、測位衛星から送信され、複数のアンテナからなる信号受信装置２が受信した衛星信号を取得する。

【0030】

相対測位部１２０は、衛星信号取得部１１０が取得した衛星信号を用いて、アンテナ間の相対的な位置関係を算出する。具体的には、相対測位部１２０は、衛星信号取得部１１０が取得した衛星信号の搬送波位相に基づいて、複数のアンテナ間の相対的な位置関係を推定する。

【0031】

一般に、２本のアンテナＡ、Ｂがある場合、２本のアンテナで受信した衛星信号の搬送波位相の二重差分をとることで以下の数式（１）が成り立つ。

【0032】

【数1】

【0033】

数式（１）において、ＤＤΦは搬送波位相の二重差分であり、ρはアンテナと測位衛星との間の距離であり、ｃは光速であり、ｆは衛星信号の周波数であり、Ｎは整数値バイアスである。また、数式（１）において、ρ^ｉ _Ａ、ρ^ｉ _Ｂはそれぞれ以下の通りである。

【0034】

【数2】

【0035】

Ｘ_ｓａｔは測位衛星の位置であり、Ｘ_ＡはアンテナＡの位置であり、ΔＸ_ＢはアンテナＡに対するアンテナＢの相対的な位置である。なお、添え字のｉ，ｊはそれぞれ主衛星、従衛星のインデックスであり、Ａ、ＢはアンテナＡ、Ｂをそれぞれ示す。

【0036】

アンテナＡの位置Ｘ_Ａは、疑似距離などによる一般的な衛星測位により得られるため既知と考えることができる。このため、上記の数式（１）において、未知のパラメータはアンテナＢの位置のみとなる。従って、相対測位部１２０は、上記数式（１）を衛星数分だけ連立することで未知のパラメータを解くことができる。

【0037】

なお、アンテナの数が３つ（アンテナＡ、Ｂ、Ｃ）の場合でも、同様にアンテナＡとアンテナＢとの間、アンテナＡとアンテナＣとの間の相対測位を行うことで、相対測位部１２０は、アンテナＡ、Ｂ、Ｃ間の相対的な位置関係を得ることができる。

【0038】

第１姿勢推定部１３０は、相対測位部１２０が算出したアンテナ間の相対的な位置関係を用いて、移動体１の姿勢を推定する。具体的には、第１姿勢推定部１３０は、アンテナが２つの場合には３軸姿勢（ヨー、ロール、ピッチ）のうち２つを算出し、アンテナが３つの場合には３軸姿勢の全てを算出する。

【0039】

速度ベクトル算出部１４０は、衛星信号取得部１１０が取得した衛星信号を用いて移動体１の三次元速度ベクトルを算出する。具体的には、速度ベクトル算出部１４０は、衛星信号の搬送波のドップラーシフトに基づき、移動体１の三次元速度ベクトルを推定する。以下の数式（２）に、衛星のドップラーシフトと、移動体１の三次元速度ベクトルＶとの関係式を示す。

【0040】

【数3】

【0041】

数式（２）において、ｉは衛星のインデックス（ｉ＝１，・・・，Ｎ）であり、Ｎは１時刻あたりに観測可能な衛星の数である。また数式（２）において、Ｇ_ｉは測位衛星と移動体１との間の視線方向単位ベクトル（無次元）であり、Ｖは移動体１の三次元速度ベクトル（単位［ｍ／ｓ］）であり、τはクロックドリフトと呼ばれる信号受信装置２の時計誤差変化率（無次元）であり、ｃは光速［ｍ／ｓ］である。また、数式（２）において、Ｖｓ_ｉ、Ｇ_ｉはそれぞれ以下の通りである。

【0042】

【数4】

【0043】

Ｘ^’ _ｉは測位衛星の位置であり、ｘは移動体１の位置である。Ｄ_ｉは測位衛星のドップラーシフトであり、ｆ１は測位衛星の搬送波周波数（Ｈｚ）であり、Ｇ_ｉは単位ベクトルであり、ｖ^’ _ｉは測位衛星の三次元速度ベクトルである。以上から、Ｇ_ｉは測位衛星の位置と移動体１の位置とを用いて求めることができる。

【0044】

数式（２）において、未知のパラメータは移動体１の三次元速度ベクトルＶ、及びクロックドリフトτの合計４つである。観測可能な衛星と同数だけ連立できるため，速度ベクトル算出部１４０は、最低４機の測位衛星があれば、これらの未知のパラメータを解くことができる。

【0045】

第２姿勢推定部１５０は、速度ベクトル算出部１４０が算出した移動体１の三次元速度ベクトルを用いて移動体１の姿勢を推定する。具体的には、第２姿勢推定部１５０は、速度ベクトル算出部１４０が算出した移動体１の三次元速度ベクトルの方位方向と勾配方向とから、それぞれ移動体１のヨー角とピッチ角とを算出する。

【0046】

姿勢統合部１６０は、第１姿勢推定部１３０による移動体１の姿勢推定結果と、第２姿勢推定部１５０による移動体１の姿勢推定結果とを統合する。具体的には、姿勢統合部１６０は、第１姿勢推定部１３０による姿勢推定結果と、第２姿勢推定部１５０による姿勢推定結果とのそれぞれに、所定の重みを掛けて統合する。所定の重みは、例えばそれぞれ１／２である。

【0047】

本実施形態に係る姿勢推定装置１０は、図３に示した構成を有することで、複数の情報に基づいた移動体１の姿勢を推定し、推定結果を統合する。本実施形態に係る姿勢推定装置１０は、推定結果を統合することで、衛星受信環境や移動速度の程度によらず、精度の良い姿勢推定を可能とする。

【0048】

次に、姿勢推定装置１０の作用について説明する。

【0049】

図４は、姿勢推定装置１０による姿勢推定処理の流れを示すフローチャートである。ＣＰＵ１１がＲＯＭ１２又はストレージ１４から姿勢推定プログラムを読み出して、ＲＡＭ１３に展開して実行することにより、姿勢推定処理が行なわれる。

【0050】

ＣＰＵ１１は、測位衛星から送信され、信号受信装置２が受信した衛星信号を取得する（ステップＳ１０１）。

【0051】

ステップＳ１０１に続いて、ＣＰＵ１１は、取得した衛星信号を用いて、信号受信装置２に備えられる複数のアンテナ間の相対的な位置関係を算出する（ステップＳ１０２）。アンテナ間の相対的な位置関係の算出方法は、相対測位部１２０の説明において示したものである。

【0052】

ステップＳ１０２に続いて、ＣＰＵ１１は、算出したアンテナ間の相対的な位置関係を用いて、移動体１の姿勢を推定する（ステップＳ１０３）。アンテナ間の相対的な位置関係を用いた移動体１の姿勢の推定方法は、第１姿勢推定部１３０の説明において示したものである。

【0053】

また、ＣＰＵ１１は、ステップＳ１０１に続いて、取得した衛星信号を用いて、移動体１の三次元速度ベクトルを算出する（ステップＳ１０４）。移動体１の三次元速度ベクトルの算出方法は、速度ベクトル算出部１４０の説明において示したものである。

【0054】

ステップＳ１０４に続いて、ＣＰＵ１１は、移動体１の三次元速度ベクトルを用いて、移動体１の姿勢を推定する（ステップＳ１０５）。三次元速度ベクトルを用いた移動体１の姿勢の推定方法は、第２姿勢推定部１５０の説明において示したものである。

【0055】

ステップＳ１０３及びステップＳ１０５で、移動体１の姿勢を推定すると、ＣＰＵ１１は、それぞれの推定結果を統合する（ステップＳ１１１）。移動体１の姿勢の推定結果の統合方法は、姿勢統合部１６０の説明において示したものである。

【0056】

本実施形態に係る姿勢推定装置１０は、図４に示した処理を実行することで、複数の情報に基づいた移動体１の姿勢を推定し、推定結果を統合する。本実施形態に係る姿勢推定装置１０は、推定結果を統合することで、衛星受信環境や移動速度の程度によらず、精度の良い姿勢推定を可能とする。

【0057】

上述の説明では、アンテナ間の相対的な位置関係を用いた姿勢の推定結果と、移動体１の三次元速度ベクトルを用いた姿勢の推定結果とを、それぞれ１／２の重みを掛けて統合していたが、本発明は係る例に限定されるものでは無い。例えば、姿勢推定装置１０は、三次元速度ベクトルの大きさに応じて重みを設定した上で、２つの推定結果を統合してもよい。

【0058】

図５は、姿勢推定装置１０の機能構成の例を示すブロック図である。

【0059】

図５に示すように、姿勢推定装置１０は、機能構成として、衛星情報取得部１１０、相対測位部１２０、第１姿勢推定部１３０、速度ベクトル算出部１４０、第２姿勢推定部１５０、第２信頼性判定部１５５および姿勢統合部１６０を有する。各機能構成は、ＣＰＵ１１がＲＯＭ１２またはストレージ１４に記憶された姿勢推定プログラムを読み出し、実行することにより実現される。

【0060】

図５に示した姿勢推定装置１０は、図３に示した姿勢推定装置１０から第２信頼性判定部１５５が追加されたものである。ここでは、追加された構成である第２信頼性判定部１５５について説明する。

【0061】

第２信頼性判定部１５５は、速度ベクトル算出部１４０が算出した三次元速度ベクトルの大きさから、第２姿勢推定部１５０が推定した移動体１の姿勢の信頼性を判定する。具体的には、第２信頼性判定部１５５は、以下の数式（３）のように、０から１の間で正規化されるように、信頼性Ｒを判定してもよい。Ｖは速度ベクトル算出部１４０が算出した三次元速度ベクトルの大きさである。
Ｒ＝１－１／１＋Ｖ （３）

【0062】

なお、第２信頼性判定部１５５は、信頼性を判定する際に、外部センサである車速パルスなどから取得した移動体１の移動速度の情報を用いてもよい。また第２信頼性判定部１５５は、測位衛星の数、仰角、ＳＮＲ（ＳＮ比）、ＤＯＰ（Ｄｉｌｕｔｉｏｎ Ｏｆ Ｐｒｅｃｉｓｉｏｎ；精度低下率）などの情報を用いても、移動体１の姿勢の信頼性を判定してもよい。

【0063】

姿勢統合部１６０は、第２信頼性判定部１５５が判定した信頼性Ｒを用いて、移動体１の三次元速度ベクトルを用いた姿勢の推定結果に重みを与えて、アンテナ間の相対的な位置関係を用いた姿勢の推定結果と統合する。例えば、姿勢統合部１６０は、第２信頼性判定部１５５が０から１の間で正規化されるように信頼性を判定した場合、その信頼性Ｒを重みとして、第２姿勢推定部１５０が推定した移動体１の姿勢の情報に乗算してもよい。また、姿勢統合部１６０は、（１－Ｒ）を重みとして第１姿勢推定部１３０が推定した移動体１の姿勢の情報に乗算してもよい。そして、姿勢統合部１６０は、重みを乗算した後の姿勢の推定結果を統合することで、移動体１の姿勢の情報を得てもよい。

【0064】

本実施形態に係る姿勢推定装置１０は、図５に示した構成を有することで、複数の情報に基づいた移動体１の姿勢を推定し、推定結果を統合する。また、本実施形態に係る姿勢推定装置１０は、図５に示した構成を有することで、三次元速度ベクトルを用いた移動体１の姿勢の推定結果の信頼性に基づいた、推定結果の統合を行う。本実施形態に係る姿勢推定装置１０は、推定結果を統合することで、衛星受信環境や移動速度の程度によらず、精度の良い姿勢推定を可能とする。

【0065】

図６は、姿勢推定装置１０による姿勢推定処理の流れを示すフローチャートである。ＣＰＵ１１がＲＯＭ１２又はストレージ１４から姿勢推定プログラムを読み出して、ＲＡＭ１３に展開して実行することにより、姿勢推定処理が行なわれる。

【0066】

図６に示したフローチャートは、図４に示したフローチャートに、ステップＳ１０７の処理が追加されたものである。

【0067】

ＣＰＵ１１は、ステップＳ１０５において、移動体１の三次元速度ベクトルを用いて、移動体１の姿勢を推定すると、三次元速度ベクトルを用いた移動体１の姿勢の推定結果の信頼性Ｒを判定する（ステップＳ１０７）。三次元速度ベクトルを用いた移動体１の姿勢の推定結果の信頼性の判定方法は、第２信頼性判定部１５５の説明において示したものである。

【0068】

ステップＳ１０３及びステップＳ１０５で、移動体１の姿勢を推定すると、ＣＰＵ１１は、それぞれの推定結果を統合する（ステップＳ１１１）。具体的には、ＣＰＵ１１はステップＳ１０７が判定した信頼性Ｒを用いて、移動体１の三次元速度ベクトルを用いた姿勢の推定結果に重みを与えて、ステップＳ１０３で求めた、アンテナ間の相対的な位置関係を用いた姿勢の推定結果と統合する。

【0069】

本実施形態に係る姿勢推定装置１０は、図６に示した処理を実行することで、複数の情報に基づいた移動体１の姿勢を推定し、推定結果を統合する。また、本実施形態に係る姿勢推定装置１０は、図６に示した処理を実行することで、三次元速度ベクトルを用いた移動体１の姿勢の推定結果の信頼性に基づいた、推定結果の統合を行う。本実施形態に係る姿勢推定装置１０は、推定結果を統合することで、衛星受信環境や移動速度の程度によらず、精度の良い姿勢推定を可能とする。

【0070】

姿勢推定装置１０は、アンテナ間の相対的な位置関係を用いた姿勢の推定結果についても信頼性を判定してもよい。

【0071】

図７は、姿勢推定装置１０の機能構成の例を示すブロック図である。

【0072】

図７に示すように、姿勢推定装置１０は、機能構成として、衛星情報取得部１１０、相対測位部１２０、第１姿勢推定部１３０、第１信頼性判定部１３５、速度ベクトル算出部１４０、第２姿勢推定部１５０、第２信頼性判定部１５５、姿勢統合部１６０および慣性センサ情報取得部１７０を有する。各機能構成は、ＣＰＵ１１がＲＯＭ１２またはストレージ１４に記憶された姿勢推定プログラムを読み出し、実行することにより実現される。

【0073】

図７に示した姿勢推定装置１０は、図５に示した姿勢推定装置１０から第１信頼性判定部１３５及び慣性センサ情報取得部１７０が追加されたものである。ここでは、追加された構成である第１信頼性判定部１３５及び慣性センサ情報取得部１７０について説明する。

【0074】

第１信頼性判定部１３５は、第１姿勢推定部１３０が姿勢の推定に用いた情報及び姿勢推定の結果を用いて、第１姿勢推定部１３０が推定した移動体１の姿勢の信頼性を判定する。具体的には、第１信頼性判定部１３５は、第１姿勢推定部１３０が姿勢推定出来た場合は１、姿勢推定出来なかった場合は０を出力してもよい。また、第１信頼性判定部１３５は、測位衛星の数、仰角、ＳＮＲ、ＤＯＰなどに基づいて信頼性を判定してもよい。また、第１信頼性判定部１３５は、相対測位部１２０による測位結果と、既知のアンテナ間の距離との差分の情報を用いて信頼性を判定してもよい。

【0075】

慣性センサ情報取得部１７０は、慣性センサ３から角速度及び加速度の情報を取得して、姿勢統合部１６０に送る。

【0076】

姿勢統合部１６０は、第１信頼性判定部１３５が判定した信頼性と、第２信頼性判定部１５５が判定した信頼性とを用いて、姿勢の推定結果にそれぞれ重みを与えて統合する。具体的には、姿勢統合部１６０は、以下の数式（４）を用いて姿勢の推定結果を統合する。

【0077】

【数5】

【0078】

数式（４）において、Ｐ_１、Ｐ_２、Ｐ_３、Ｐ_３’は、それぞれ第１姿勢推定部１３０、第２姿勢推定部１５０、現時刻の最終姿勢推定結果、前時刻の最終姿勢推定結果を示す。また、ｗ_１、ｗ_２はそれぞれ第１姿勢推定部１３０の姿勢推定結果の重み、第２姿勢推定部１５０の姿勢推定結果の重み、αは前時刻の結果からの更新重みである。また、ｗ_１、ｗ_２は、それぞれ第１信頼性判定部１３５、第２信頼性判定部１５５から取得することができる。また、αは、観測精度に応じて適当な係数を調整する。

【0079】

また姿勢統合部１６０は、慣性センサ情報取得部１７０からの角速度及び加速度の情報を用いる場合は、以下の数式（５）を用いて姿勢の推定結果を統合する。

【0080】

【数6】

【0081】

上記数式（５）において、Ｐ_０は慣性センサ３から算出した移動体１の姿勢であり、ｗ_０はその重みである。ｗ_０は慣性センサ３のスペックに応じて適切な固定値が設定されてもよいし、慣性センサ３の温度特性などに応じた可変量としてもよい。数式（５）のその他の変数は数式（４）と同一である。

【0082】

なお、上述の例では慣性センサ３を用いたが、本発明は係る例に限定されるものでは無い。慣性センサ３に代えて地磁気センサが用いられてもよい。

【0083】

本実施形態に係る姿勢推定装置１０は、図７に示した構成を有することで、複数の情報に基づいた移動体１の姿勢を推定し、推定結果を統合する。また、本実施形態に係る姿勢推定装置１０は、図７に示した構成を有することで、アンテナ間の相対的な位置関係を用いた移動体１の姿勢の推定結果の信頼性と、三次元速度ベクトルを用いた移動体１の姿勢の推定結果の信頼性とに基づいた、推定結果の統合を行う。本実施形態に係る姿勢推定装置１０は、信頼性に基づいて推定結果を統合することで、衛星受信環境や移動速度の程度によらず、精度の良い姿勢推定を可能とする。

【0084】

図８は、姿勢推定装置１０による姿勢推定処理の流れを示すフローチャートである。ＣＰＵ１１がＲＯＭ１２又はストレージ１４から姿勢推定プログラムを読み出して、ＲＡＭ１３に展開して実行することにより、姿勢推定処理が行なわれる。

【0085】

図８に示したフローチャートは、図６に示したフローチャートに、ステップＳ１０６の処理が追加され、さらにステップＳ１１１の処理がステップＳ１１２の処理に置き換わったものである。

【0086】

ＣＰＵ１１は、算出したアンテナ間の相対的な位置関係を用いて、移動体１の姿勢を推定すると、アンテナ間の相対的な位置関係を用いた移動体１の姿勢の推定結果の信頼性を判定する（ステップＳ１０６）。アンテナ間の相対的な位置関係を用いた移動体１の姿勢の推定結果の信頼性の判定方法は、第１信頼性判定部１３５の説明において示したものである。

【0087】

ステップＳ１０６及びステップＳ１０７で、移動体１の姿勢の推定結果の信頼性をそれぞれ判定すると、ＣＰＵ１１は、移動体１の姿勢の推定結果を統合する（ステップＳ１１２）。ＣＰＵ１１は、移動体１の姿勢の推定結果を統合する際に、慣性センサ３の情報を用いて統合する。移動体１の姿勢の推定結果の統合方法は、図７の姿勢統合部１６０の説明において示したものである。

【0088】

本実施形態に係る姿勢推定装置１０は、図８に示した処理を実行することで、複数の情報に基づいた移動体１の姿勢を推定し、推定結果を統合する。また、本実施形態に係る姿勢推定装置１０は、図８に示した処理を実行することで、アンテナ間の相対的な位置関係を用いた移動体１の姿勢の推定結果の信頼性と、三次元速度ベクトルを用いた移動体１の姿勢の推定結果の信頼性とに基づいた、推定結果の統合を行う。本実施形態に係る姿勢推定装置１０は、信頼性に基づいて推定結果を統合することで、衛星受信環境や移動速度の程度によらず、精度の良い姿勢推定を可能とする。

【0089】

なお、上記各実施形態でＣＰＵがソフトウェア（プログラム）を読み込んで実行した姿勢推定処理を、ＣＰＵ以外の各種のプロセッサが実行してもよい。この場合のプロセッサとしては、ＦＰＧＡ（Ｆｉｅｌｄ－Ｐｒｏｇｒａｍｍａｂｌｅ Ｇａｔｅ Ａｒｒａｙ）等の製造後に回路構成を変更可能なＰＬＤ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｄｅｖｉｃｅ）、及びＡＳＩＣ（Ａｐｐｌｉｃａｔｉｏｎ Ｓｐｅｃｉｆｉｃ Ｉｎｔｅｇｒａｔｅｄ Ｃｉｒｃｕｉｔ）等の特定の処理を実行させるために専用に設計された回路構成を有するプロセッサである専用電気回路等が例示される。また、姿勢推定処理を、これらの各種のプロセッサのうちの１つで実行してもよいし、同種又は異種の２つ以上のプロセッサの組み合わせ（例えば、複数のＦＰＧＡ、及びＣＰＵとＦＰＧＡとの組み合わせ等）で実行してもよい。また、これらの各種のプロセッサのハードウェア的な構造は、より具体的には、半導体素子等の回路素子を組み合わせた電気回路である。

【0090】

また、上記各実施形態では、姿勢推定処理のプログラムがＲＯＭまたはストレージに予め記憶（インストール）されている態様を説明したが、これに限定されない。プログラムは、ＣＤ－ＲＯＭ（Ｃｏｍｐａｃｔ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、ＤＶＤ－ＲＯＭ（Ｄｉｇｉｔａｌ Ｖｅｒｓａｔｉｌｅ Ｄｉｓｋ Ｒｅａｄ Ｏｎｌｙ Ｍｅｍｏｒｙ）、及びＵＳＢ（Ｕｎｉｖｅｒｓａｌ Ｓｅｒｉａｌ Ｂｕｓ）メモリ等の非一時的（ｎｏｎ－ｔｒａｎｓｉｔｏｒｙ）記録媒体に記録された形態で提供されてもよい。また、プログラムは、ネットワークを介して外部装置からダウンロードされる形態としてもよい。

【符号の説明】

【0091】

１ 移動体
２ 信号受信装置
３ 慣性センサ
１０ 姿勢推定装置

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】