WO2023218526A1

WO2023218526A1 - 移動体測位装置

Info

Publication number: WO2023218526A1
Application number: PCT/JP2022/019805
Authority: WO
Inventors: 弘明北野; 翔太亀岡; 範純元岡; 雅也酒井; 岳也島
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2022-05-10
Filing date: 2022-05-10
Publication date: 2023-11-16
Anticipated expiration: 2024-11-10
Also published as: JP7262684B1; JPWO2023218526A1; DE112022007174T5; US20250327667A1

Abstract

本開示は、センサによる移動体の測位を高精度に行う移動体測位装置を提供することを目的とする。本開示に係る移動体測位装置（７）は、センサにより検出された移動体に関するセンサ値を取得するセンサ情報取得部と、センサ値を用いて移動体の横滑り角を推定する横滑り角推定部（１２）と、センサ値と横滑り角とを用いて移動体の慣性測位を行う慣性測位部（１４）と、を備え、横滑り角推定部（１２）は、移動体に関する複数の運動モデル（ｆ１，ｆ２）を移動体の状態量に基づき重みづけして統合することにより得られた混合モデル（ｆ）と、センサ値とに基づき、横滑り角を推定する。

Description

移動体測位装置

　本開示は、移動体の測位に関する。

　移動体の自動走行を行うためには、移動体が走行すべき領域を検出し、移動体が走行すべき経路である走行経路を生成し、生成した走行経路に従って走行するように移動体を制御する必要がある。

　従来、ＧＮＳＳ（Ｇｌｏｂａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓａｔｅｌｌｉｔｅ　Ｓｙｓｔｅｍ）による衛星測位結果と高精度に測量されている地図データとを用いて、道路を検出するシステムが考えられている。特に近年では、準天頂衛星およびネットワーク型ＲＴＫ（Ｒｅａｌ　Ｔｉｍｅ　Ｋｉｎｅｍａｔｉｃ）に代表される高精度に車両位置を測位するシステムも実用化されつつある。

　特許文献１では、速度センサにより検出された自車の速度と、角速度センサにより検出された前記自車の角速度と、測位装置により検出された前記自車の位置及び姿勢角とを取得する取得部と、前記取得部が取得した前記自車の速度と、前記自車の角速度と、予め求められた車体横滑り角とに基づいて、前記自車の位置である第２位置を推定する第２自己位置推定部と、前記取得部によって前記自車の位置が取得された場合、前記第２自己位置推定部により推定された前記第２位置と、前記取得部が取得した前記自車の位置との偏差とに基づいて、姿勢角補正値を算出する姿勢角補正量算出部と、前記取得部によって前記自車の位置が取得されなかった場合、前記取得部が取得した前記自車の速度、前記自車の角速度、及び前記取得部によって前回取得された前記自車の位置及び姿勢角と、前記姿勢角補正量算出部により算出された前記姿勢角補正値と、前記車体横滑り角とに基づいて前記自車の位置である第１位置を推定する第１自己位置推定部と、を含む自己位置推定装置が記載されている。

特開２０２０－１１２４９０号公報

　しかしながら特許文献１に記載の構成においては、移動体の横滑り角を考慮して位置および姿勢角を推定するものの、操舵角と車速に基づき予め定められたテーブルを参照するか、あるいは操舵角の変化しない定常円旋回を仮定して横滑り角を推定しており、時々刻々操舵角が変化する状況に対しては姿勢角の推定精度が低下するおそれがあった。

　本開示の技術は上記課題を解決するためのものであり、センサによる移動体の測位を高精度に行う移動体測位装置を提供することを目的とする。

　本開示の１つの移動体測位装置は、センサにより検出された移動体に関するセンサ値を取得するセンサ情報取得部と、センサ値を用いて移動体の横滑り角を推定する横滑り角推定部と、センサ値と横滑り角とを用いて移動体の慣性測位を行う慣性測位部と、を備え、横滑り角推定部は、移動体に関する複数の運動モデルを移動体の状態量に基づき重みづけして統合することにより得られた混合モデルと、センサ値とに基づき、横滑り角を推定する。

　本開示の移動体測位装置は、複数の運動モデルを統合して得られた混合モデルにより横滑り角を高精度に推定するため、移動体の測位を高精度に行うことができる。本開示の目的、特徴、態様、および利点は、以下の詳細な説明と添付図面とによって、より明白となる。

実施の形態１に係る移動体測位装置を搭載した車両の全体構成を示す図である。実施の形態１に係る移動体測位装置の構成を示すブロック図である。実施の形態１に係る移動体測位装置の処理を示すフローチャートである。第一運動モデルを示す模式図である。第二運動モデルを示す模式図である。重み関数の説明図である。第一運動モデル、第二運動モデル、および混合モデルの比較図である。実施の形態１の変形例に係る移動体測位装置の構成を示すブロック図である。実施の形態１の変形例に係る移動体測位装置の構成を示すブロック図である。実施の形態１の変形例に係る移動体測位装置の構成を示すブロック図である。移動体測位装置のハードウェア構成を示す図である。移動体測位装置のハードウェア構成を示す図である。

　＜Ａ．実施の形態１＞
　＜Ａ－１．構成＞
　図１は実施の形態１に係る移動体測位装置７を搭載した車両１の全体構成を示す図である。車両１は移動体の例である。図１に示されるように車両１は、ハンドル２、操舵アクチュエータ３、アンテナ５、駆動装置６、移動体測位装置７、移動体センサ８および車両制御装置９を備える。また、図１には図示されていないが、車両１は車両１を制動させるためのブレーキを備える。

　操舵アクチュエータ３は、前輪の２つのタイヤを操作するハンドル２に取付けられている。操舵アクチュエータ３は、例えば、ＥＰＳ（Ｅｌｅｃｔｒｉｃ　Ｐｏｗｅｒ　Ｓｔｅｅｒｉｎｇ：電動パワーステアリング）用モータおよびＥＣＵ（Ｅｌｅｃｔｒｏｎｉｃ　Ｃｏｎｔｒｏｌ　Ｕｎｉｔ）を備える。操舵アクチュエータ３は、車両制御装置９からの操舵指令に従って動作することで、ハンドル２および前輪の回転を制御することができる。操舵アクチュエータ３は、車両制御装置９から入力された操舵指令値に従って、車両１が道路に沿って走行するように操舵制御を行う。

　駆動装置６は、車両１の前輪軸に備えられ、車両１を駆動する。駆動装置６は、例えば駆動用モータ、ＥＣＵおよびブレーキにより構成される。駆動装置６は、車両制御装置９から入力された速度または加速度の指令値に従って動作することで、車両１の制動および駆動を行うことができる。駆動装置６は、車両制御装置９から入力された速度または加速度の指令値に従って、車両１が交通状況に応じた速度となるように車両１の制動および駆動を制御する。

　アンテナ５は、衛星４からの衛星信号を受信し、受信した衛星信号を移動体測位装置７へ送信する。衛星４は、例えば、複数のＧＰＳ（Ｇｌｏｂａｌ　Ｐｏｓｉｔｉｏｎｉｎｇ　Ｓｙｓｔｅｍ）衛星により構成される。但し、衛星４はＧＰＳ衛星に限定されない。ＧＬＯＮＡＳＳ（Ｇｌｏｂａｌ　Ｎａｖｉｇａｔｉｏｎ　Ｓａｔｅｌｌｉｔｅ　Ｓｙｓｔｅｍ）等の他の測位衛星を衛星４として用いることもできる。

　移動体センサ８は、操舵角または車速などの車両１の状態量を検出し、検出した状態量を移動体測位装置７へ送信する。

　移動体測位装置７は、アンテナ５で受信した衛星信号と、移動体センサ８で検出した車両１の状態量とに基づき、車両１の位置を測定し、測定した車両１の位置を車両制御装置９に送信する。

　車両制御装置９は、移動体測位装置７が測定した車両１の位置、移動体センサ８が検出した車両１の状態量、および図示しないカメラまたはミリ波レーダー等の認知結果に基づき、操舵アクチュエータ３および駆動装置６へ指令値を出力する。

　次に、実施の形態１に係る移動体測位装置７の構成について、図２を参照して説明する。図２は、実施の形態１の移動体測位装置７の構成を示すブロック図である。

　図２に示すように移動体測位装置７は、衛星測位結果受信部１０、慣性センサ１１、横滑り角推定部１２、センサ補正部１３、慣性測位部１４、およびフィルタ部１５を備えている。この他、移動体測位装置７は図２に示されないセンサ情報取得部を備えている。また、移動体測位装置７には、アンテナ５および移動体センサ８が接続されている。

　衛星測位結果受信部１０は、アンテナ５からの出力である衛星信号を緯度、経度または方位等のデータに変換し、衛星測位結果としてフィルタ部１５に出力する。衛星測位結果は、例えばＧＰＧＧＡフォーマット等の移動体測位装置７内で使用可能な形式で表される。

　慣性センサ１１は、移動体測位装置７に搭載された角速度センサであって、車両１の旋回に伴い発生する角速度（ヨーレート）を出力する。本実施の形態では慣性センサ１１を移動体測位装置７に搭載されたものとしている。しかし、慣性センサ１１は移動体測位装置７外の車両１に搭載され、検出結果を移動体測位装置７へ入力する構成であってもよい。これにより、移動体測位装置７のコストを削減することが可能となる。

　横滑り角推定部１２は、移動体センサ８から入力された車速および操舵角と、慣性センサ１１から入力されたヨーレートとに基づいて、複数の運動モデルの重み付けにより横滑り角を推定し、フィルタ部１５へ横滑り角推定結果を出力する。センサ補正部１３は、移動体センサ８および慣性センサ１１からのセンサ値を取得し、センサ値に含まれるスケールファクタまたはバイアスなどのセンサ誤差を補正して慣性測位部１４およびフィルタ部１５に出力する。すなわち、センサ補正部１３は、移動体センサ８および慣性センサ１１からのセンサ値を取得するセンサ情報取得部としても機能する。

　慣性測位部１４は、センサ補正部１３で補正されたセンサ値を用いて、車両１の測位結果である位置、姿勢および速度などの慣性測位演算を行い、慣性測位結果をフィルタ部１５に出力する。

　フィルタ部１５は、慣性測位部１４から入力された慣性測位結果を用いて、慣性測位結果および移動体センサ８が出力するセンサ値の誤差推定を行う。

　＜Ａ－２．動作＞
　次に図３に示すフローチャートを用いて実施の形態１の移動体測位装置７の処理フローについて説明する。

　移動体測位装置７が動作を開始すると、ステップＳ１０１においてセンサ補正部１３が慣性センサ１１から慣性センサ値を、移動体センサ８から移動体センサ値をそれぞれ取得する。ここで、慣性センサ値は、車両１の現在の角速度および加速度を含む。角速度はヨーレート、ピッチレートおよびロールレートを含み、加速度は、前後加速度、横加速度および上下加速度を含む。また、移動体センサ値は、車両１の現在の操舵角および車速を含む。

　次に、ステップＳ１０２においてセンサ補正部１３は、フィルタ部１５から前回周期において演算されたセンサ補正値を取得する。電源投入直後など何らかの理由により前回のセンサ補正値が存在しない場合、センサ補正部１３は事前に設定された初期値（０または出荷時の値）を用いる。

　次に、ステップＳ１０３においてセンサ補正部１３は、ステップＳ１０２で取得したセンサ補正値を用いて、ステップＳ１０１で取得した慣性センサ値および移動体センサ値の誤差を補正し、フィルタ部１５へ出力する。

　次に、ステップＳ１０４において横滑り角推定部１２は、移動体センサ８から入力された車両１の操舵角および車速、ならびに慣性センサ１１から入力された車両１のヨーレートに基づき、車両１の横滑り角を演算し、慣性測位部１４へ出力する。

　ここで、横滑り角推定部１２における横滑り角の推定方法について説明する。横滑り角推定部１２は、複数の車両運動モデルと重み関数を用いて車両１の状態を演算する。本実施の形態では、横滑り角推定部１２が２つの車両運動モデルと１つの重み関数を用いて車両１の状態を演算することについて説明する。しかし、横滑り角推定部１２は車両１の状態演算にあたり３つ以上の車両運動モデルを用いても良いし、複数の重み関数を用いてもよい。

　図４は第一運動モデルを模式的に表した図である。第一運動モデルは車両の横方向と回転の運動方程式を用いた動力学モデルである。このモデルによれば、タイヤが発生する力に応じた車両運動を計算できるため、特に旋回時に横加速度が生じるような高車速時の車両運動を精度よく表せる。まず、第一運動モデルｆ_１における車両状態量ｘと入力ｕを下記のように設定する。

　式（１）において、Ｘ，Ｙ，θは慣性座標系での車両１の重心位置と方位角を表し、Ｖは車速、γはヨーレート、βは横滑り角、δは前輪舵角、ａ_ｘは前後加速度を表す。式（２）においてωは前輪舵角速度、ｊ_ｘは前後ジャークを表す。

　式（１）と式（２）の変数を使って第一運動モデルｆ_１は以下の式で表される。

　ここで、Ｍは車両質量、Ｖは車速、Ｉは車両のヨー慣性モーメント、ｌ_ｆは車両重心と前車軸との距離、ｌ_ｒは車両重心と後車軸までの距離である。Ｙ_ｆ，Ｙ_ｒは前後輪のコーナリングフォースであり、前後輪のコーナリングスティフネスＫ_ｆ，Ｋ_ｒを用いて以下の式で表される。

　式（４）および式（５）を用いて式（３）を整理すると、動的運動モデルｆは次式のようになる。

　図５は第二運動モデルを模式的に表した図である。第二運動モデルは車両の幾何学的な関係から導出される幾何学モデルである。このモデルは、第一運動モデルとは異なりタイヤが発生する力を考慮しておらず、タイヤの方向に沿って車両が旋回するような低車速時の車両運動を精度よく表せる。第二運動モデルの車両状態量ｘと入力ｕは第一運動モデルと同じに設定する。式（１）と式（２）の変数を使って第二運動モデルｆ_２は以下の式で表される。

　ここで、τはヨーレートγと横滑り角βの時定数であり、ヨーレートγと横滑り角βに対して別々の値が用いられてもよい。また、γ_ｋｍ，β_ｋｍは幾何学的な関係を用いた二輪モデルで計算できるヨーレートと横滑り角であり、それぞれ以下の式で表される。

　式（８）、（９）を用いて式（７）を整理すると、動的運動モデルｆ_２は次式のようになる。

　式（６）で表される第一運動モデルｆ_１と式（１０）で表される第二運動モデルｆ_２は、同じ車両状態量ｘと入力ｕを持つが、ヨーレートγと横滑り角βに関する微分方程式が異なる。なお、第一運動モデルと第二運動モデルは、同じ車両状態量ｘと入力ｕを持ち、少なくとも１つの車両状態量について異なる微分方程式を持つ運動モデルであれば、上記のモデルに限らない。

　例えば、第二運動モデルは、定常円旋回時における車両の横方向と回転の運動方程式を用いた動力学モデルであってもよい。このモデルは、第一運動モデルとは異なり過渡的な運動を表すことができないが、低車速時の車両運動を精度よく表せる。このモデルを使うと第二運動モデルは、以下の式で表される。

　ここで、γ_ｓｓｔ，β_ｓｓｔは定常円旋回でのヨーレートおよび横滑り角であり、それぞれ以下の式で表される。

　ここでＡはスタビリティファクタと呼ばれ、以下の式で表される。

　本実施の形態では、第一運動モデルには高車速時に精度がよいモデル、第二運動モデルには低車速時に精度がよいモデルを用いている。このように、特性が異なるモデルを第一運動モデルと第二運動モデルに設定することで、後述する重み関数を用いた重みづけにより、第一運動モデルと第二運動モデルの両方のメリットが得られる。

　重み関数は第一運動モデルと第二運動モデルを重みづけする関数であり、０から１の間の値をとるように設定される。本実施の形態における第一運動モデルは車速による除算を含むため、０ｋｍ／ｈ付近において値が発散し、０ｋｍ／ｈ付近の精度が悪い。一方で第二運動モデルは操舵角によってヨーレートγおよび横滑り角βが発生するモデルとなっており、車両に発生する力を考慮していない。そのため、第二運動モデルは旋回時に遠心力が生じるような高い車速での精度が悪い。そこで、重み関数を速度の関数とし、高速では第一運動モデル、低速では第二運動モデルの重みが大きくなるよう、以下の式で表す。

　Ｖ_ｓは重み関数のパラメータであり、Ｖ＝Ｖ_ｓの時に第一運動モデルと第二運動モデルの重みが同じとなる。この速度による重み関数の例を図６に示す。第一運動モデルと第二運動モデルとこの重み関数を用いて新たな車両運動モデルｆを構築する。

　これを整理すると以下の式になる。

　この車両運動モデルｆは、第一運動モデルｆ_１および第二運動モデルｆ_２と同じ車両状態量ｘおよび入力ｕを持つ微分方程式により構築される。以降この車両運動モデルｆを混合モデルと呼ぶ。このように車両運動モデルを構築することで、図７に示すように、車速が高いときには第一運動モデルｆ_１により車両状態量が演算され、車速が低いときには第二運動モデルｆ_２により車両状態が演算されるため、どの車速においても高精度に車両状態量を演算できる。さらに第一運動モデルｆ_１と第二運動モデルｆ_２が重み関数αにより連続的に遷移するため、滑らかに車両状態量を演算できる。さらに車両状態量ｘ、制御入力ｕの数は第一運動モデルｆ_１、第二運動モデルｆ_２と変わらないため、車両状態量を増やした高精度な運動モデルに比べて計算量の増加を抑制できる。すなわち、横滑り角推定部１２は、車両１の速度に基づいて複数の運動モデルを重みづけして混合モデルを作成する。

　本実施の形態では、重み関数αを分子および分母にそれぞれ車速Ｖの２次項を持つ関数とした。しかし、重み関数αは０から１の間の値をとる限りにおいて、多項式関数または指数関数など、どのような関数であってもよい。また、重み関数αは車速Ｖを変数とする関数としたが、第一運動モデルと第二運動モデルに応じて異なる変数の関数であってもよい。また、計算する車両状態量毎に異なる重み関数αが用いられてもよい。例えば、ヨーレートγの重み関数の切換の速度をＶ_ｓ１、横滑り角βの重み関数の切換の速度をＶ_ｓ２とすれば、混合モデルｆは以下のように表される。

　このように混合モデルｆにより得られた横滑り角βの変化量を積分することにより、現在の横滑り角βを推定することができる。

　これにより、停車時から高速時まで連続的に横滑り角βが得られ、精度良く車両１の測位を行うことが可能となる。

　図３に戻る。ステップＳ１０４の後、ステップＳ１０５において慣性測位部１４は、センサ補正部１３から入力されたセンサ補正値を用いて、車両１の位置および姿勢の変化量を演算して更新し、慣性測位結果としてフィルタ部１５へ出力する。

　次に、フィルタ部１５は、ステップＳ１０６からステップＳ１０８において、慣性測定結果と衛星測位結果受信部１０から出力される衛星測位結果とを用いて、慣性測位結果、慣性センサ出力および移動体センサ出力の誤差を推定し、車両制御装置９へと出力する。

　具体的には、ステップＳ１０６において、フィルタ部１５は今回の演算周期において衛星測位結果が更新されているか否かを判定する。

　一般的に、衛星測位の演算周期は慣性センサ１１および移動体センサ８の測定周期に対して同等程度または遅い。そのため、衛星測位結果が更新されていない場合、フィルタ部１５は慣性センサ１１および移動体センサ８に対してセンサ補正量を更新せず、前回推定されたセンサ補正量と、慣性センサ値、移動体センサ値および横滑り角推定結果とに基づいて測位演算結果を出力する（ステップＳ１０７）。

　フィルタ部１５の演算時に衛星測位結果が更新されている場合、フィルタ部１５は衛星測位結果、慣性センサ値および移動体センサ値を用いてセンサ補正量を演算する（ステップＳ１０８）。

　具体的には、フィルタ部１５は拡張カルマンフィルタにより構成されたフィルタを用い、ルースカップリングと呼ばれる衛星測位結果と慣性測位結果の誤差に対して、慣性センサ１１および移動体センサ８のセンサ補正量を決定するフィルタを用いる。

　図４および図５に示したように、車両の進行方向ψは車両の方位角θと横滑り角βを用いて以下の式で表される。

　衛星測位による方位角はアンテナの移動量または移動速度により決定されるため、車両の方位角θは式（１９）を用いて下式で表される。

　このようにして得られた横滑り角βを考慮した車両１の方位角θを用いて、慣性測位部１４は例えば次のように状態変数を定義して慣性測位結果を計算する。

　式（２１）におけるｙ_ｄは、慣性測位に関する状態変数をまとめた慣性測位に関する状態ベクトルを表している。また、λ_ｄは慣性測位演算で得られた緯度、φ_ｄは慣性測位演算で得られた経度、ｈ_ｄは慣性測位演算で得られた楕円体高、θ_ｄは式（２０）で得られた横滑り角βを考慮した方位角を表す。

　式（２１）におけるｙ_ｄを時間微分したベクトルを

とし、入力

をベクトルｕとして表す。

はそれぞれ、センサ補正部１３により誤差を補正された車速Ｖおよびヨーレートγを表す。この

に対し１ステップ前のｙ_ｄと入力ｕを代入し、時々刻々積分を行うことによって慣性測位結果を得ることができる。

　上記のようにして求められた横滑り角βを考慮した慣性測位結果を用いて、フィルタ部１５はカルマンフィルタにより状態量およびセンサの誤差を推定する。

　具体的にはセンサモデルを下記として、推定を行う。

　式（２２）は、車速の真値Ｖ_ｔに対して車速のスケールファクタＳ_ｖが掛ったモデルである。式（２３）は、ヨーレートの真値γ_ｔに対してヨーレートセンサのバイアスｂ_γが重畳し、ヨーレートのスケールファクタｓ_γが掛ったモデルである。

　この例では、フィルタ部１５は、ｓ_Ｖ、ｓ_γおよびｂ_γのそれぞれの推定値ｓ_ｖｅ、ｓ_γｅおよびｂ_γｅをセンサ誤差として推定する。センサ補正部１３は、フィルタ部１５が推定したセンサ誤差の推定値を用いて、以下の式によって移動体センサ８および慣性センサ１１のセンサ値を補正する。

　フィルタ部１５は、以下の式に示すように状態ベクトルを定義する。

　車速のスケールファクタｓ_Ｖおよびヨーレートのスケールファクタｓ_γが微小であるとすると、式（２４）および式（２５）により、車速の真値Ｖ_ｔおよびヨーレートの真値γ_ｔは、それぞれ以下の式で近似できる。

　車速のスケールファクタｓ_Ｖ、ヨーレートのスケールファクタｓ_γおよびヨーレートセンサのバイアスｂ_γの動的なモデルを、以下の式で表す。現在の状態から次の状態を予測する１次マルコフ過程で駆動するものとする。

　式（２９）から式（３１）において、

はｓ_Ｖの時間微分、

はｓ_γの時間微分、

はｂ_γの時間微分である。また、τ_ｓＶは車速スケールファクタのモデルパラメータ値、Ｗ_ｓＶは車速スケールファクタの時間遷移に関する雑音、τ_ｓγはヨーレートスケールファクタのモデルパラメータ値、Ｗ_ｓγはヨーレートスケールファクタの時間遷移に関する雑音、τ_ｂγはヨーレートバイアスのモデルパラメータ値、ｗ_ｂγはヨーレートバイアスの時間遷移に関する雑音である。

　式（２９）から式（３１）をまとめると、状態方程式は以下の式（３２）で表される。

　式（３２）において、

は状態ベクトルｘを時間微分したベクトルを表す。また、ｕは入力ベクトル

を表す。

　次に衛星測位結果受信部１０から得られるカルマンフィルタの観測値に関して説明する。衛星測位結果受信部１０からはアンテナ５の緯度、経度および高度などの座標情報が出力される。以下ではＧＮＳＳセンサの観測値を（λ_ｍ，φ_ｍ，ｈ_ｍ，ψ_ｍ）とする。一方、慣性測位結果によってもこれらの座標情報が得られるが、慣性測位結果は車両の航法中心の座標であるため、車両航法中心からアンテナ５の位置までのオフセット量を用いて、ＧＮＳＳセンサの観測値が予測される。すなわち、車両の航法座標系で表現された車両航法中心からアンテナ５までのオフセット量を（Δｘ，Δｙ，Δｚ）とすると、予測されたＧＮＳＳセンサの観測値（λ_ｐ，φ_ｐ，ｈ_ｐ，ψ_ｐ）は、慣性測位値ｙ_ｄとオフセット量ｖ（Δｘ，Δｙ，Δｚ）とにより、座標変換関数ｃ（ｙ_ｄ，ｖ）を用いて以下の式（３３）のように求められる。

　このように、式（３２）の状態方程式および式（３３）の観測方程式を用いて、拡張カルマンフィルタを適用して状態推定を行い、式（２６）の状態ベクトルの推定値が与えられ、位置誤差およびセンサ誤差を求めることが可能となる。

　上記のように横滑り角を考慮した方位角を慣性測位部１４で用いることで、慣性測位結果の精度が向上し、フィルタ部１５における状態量およびセンサ誤差推定の精度も向上する。

　上記で説明したように、実施の形態１に係る移動体測位装置７は、移動体である車両１に搭載されたセンサである移動体センサ８および慣性センサ１１により検出された車両１の操舵角、車速、角速度を含むセンサ値を取得するセンサ情報取得部と、センサ値を用いて車両１の慣性測位を行う慣性測位部１４と、センサ値を用いて車両１の横滑り角を推定する横滑り角推定部１２と、を備える。そして、横滑り角推定部１２は、車両１の少なくとも１つの状態量について異なる微分方程式を有する複数の運動モデルｆ１，ｆ２を車両１の状態量に基づき重みづけして統合することにより得られた混合モデルｆと、センサ値とに基づき、横滑り角を推定する。従って、移動体測位装置７によれば、車両１が定常状態でない場合にも精度よく車両１の横滑り角を推定することが可能となり、車両１の位置および姿勢を精度よく推定することができる。これにより、例えば移動体測位装置７が自動運転などのアプリケーションに用いられる場合には、車両１の制御の安全性および乗り心地が向上する。

　また、移動体測位装置７は、慣性測位の結果および横滑り角を用いて、センサ値に含まれるセンサ誤差を推定するフィルタ部１５と、センサ値に含まれるセンサ誤差を補正するセンサ補正部１３と、を備え、慣性測位部１４はセンサ補正部１３で補正されたセンサ値を用いて慣性測位を行ってもよい。

　＜Ａ－３．変形例＞
　図２に示される構成では、移動体センサ８および慣性センサ１１のセンサ値が横滑り角推定部１２に入力された。しかし、図８に示されるように、センサ補正部１３により補正されたセンサ値が横滑り角推定部１２に入力されてもよい。すなわち、横滑り角推定部１２は、センサ補正部１３で補正されたセンサ値に基づき横滑り角を推定してもよい。これにより、横滑り角推定部１２の入力に含まれる誤差が小さくなり、横滑り角の推定精度が向上する。

　図２に示される構成では、横滑り角推定部１２による横滑り角の推定結果がフィルタ部１５へ出力された。しかし、図９に示されるように、横滑り角推定部１２による横滑り角の推定結果は慣性測位部１４に出力されてもよい。すなわち、慣性測位部１４は、横滑り角を用いて、横滑り角を考慮した慣性測位を行ってもよい。

　具体的には、ＧＮＳＳセンサの観測値（λ_ｍ，φ_ｍ，ｈ_ｍ，ψ_ｍ）を、横滑り角βを考慮した値、すなわち（λ_ｍ，φ_ｍ，ｈ_ｍ，ψ_ｍ－β）とすることで、慣性測位部１４は、横滑り角βを考慮して慣性測位演算を行う。これにより、慣性測位による方位角の推定精度が向上する。

　また、図１０に示されるように、慣性測位部１４が横滑り角推定部１２を備えていてもよい。つまり、横滑り角推定部１２において横滑り角の推定に用いられるモデルを、慣性測位部１４における慣性測位の計算に用いてもよい。すなわち、移動体測位装置７は、車両１に搭載されたセンサである移動体センサ８および慣性センサ１１により検出された車両１の操舵角、車速、角速度を含むセンサ値を取得するセンサ情報取得部と、センサ値に含まれるセンサ誤差を補正するセンサ補正部１３と、センサ補正部１３で補正されたセンサ値を用いて車両１の慣性測位を行う慣性測位部１４と、慣性測位の結果および移動体の横滑り角を用いて、センサ値に含まれるセンサ誤差を推定するフィルタ部１５と、を備えてもよい。また、慣性測位部１４は、車両１の少なくとも１つの状態量について異なる微分方程式を有する複数の運動モデルｆ１，ｆ２を車両１の状態量に基づき重みづけして統合することにより得られた混合モデルｆと、センサ値とに基づき、横滑り角を推定してもよい。

　具体的には、カルマンフィルタへの入力

に前輪舵角δを追加し、さらにカルマンフィルタで用いる状態方程式

における車両１の方位角θのモデルを、ヨーレートの積分だけではなく式（１８）に示すように、より詳細なモデルとしてもよい。このように慣性測位部１４において複数の運動モデルを重みづけして推定することにより、慣性測位で用いる運動モデルが高精度となり、推定精度が向上する。

　なお、本実施の形態では、ＧＮＳＳセンサにより測位演算された緯度、経度、高度、および方位などをＧＮＳＳによる観測値としている。しかし、ＧＮＳＳセンサによっては、擬似距離やドップラー、搬送派位相などの生データを出力することができるため、これらを観測値として利用するタイトカップリングを用いてもよい。この場合は、状態変数にＧＮＳＳセンサの受信機時刻のドリフトなどを追加し、受信機時刻についての誤差推定も実施する必要がある。しかし、可視衛星数が例えば1基のみなど少数の場合でも、ＧＮＳＳによる観測値を生成することが可能である。これにより可視衛星数が少ない場合においても、精度の高い測位が可能となる。

　また、本実施の形態ではアンテナ５を一つとしていたが、アンテナを複数個備える構成として、各アンテナの位置、速度を用いて方位を計算する構成としてもよい。すなわち、フィルタ部１５は、車両１に搭載された複数のアンテナ５が衛星４から受信した衛星信号により、車両１の進行方向を推定し、慣性測位の結果、横滑り角、および車両１の進行方向に基づき、センサ誤差を推定する。これにより、フィルタ部１５は、各アンテナ５の相互位置から車両の移動または回転を計算することが可能となり、より精度よく方位を推定することが可能となる。

　＜Ａ－４．ハードウェア構成＞
　上述した移動体測位装置７における、衛星測位結果受信部１０、横滑り角推定部１２、センサ補正部１３、慣性測位部１４およびフィルタ部１５は、図１１に示す処理回路８１により実現される。すなわち、処理回路８１は、衛星測位結果受信部１０、横滑り角推定部１２、センサ補正部１３、慣性測位部１４およびフィルタ部１５（以下、「横滑り角推定部１２等」と称する）を備える。処理回路８１には、専用のハードウェアが適用されても良いし、メモリに格納されるプログラムを実行するプロセッサが適用されても良い。プロセッサは、例えば中央処理装置、処理装置、演算装置、マイクロプロセッサ、マイクロコンピュータ、ＤＳＰ（Digital Signal Processor）等である。

　処理回路８１が専用のハードウェアである場合、処理回路８１は、例えば、単一回路、複合回路、プログラム化したプロセッサ、並列プログラム化したプロセッサ、ＡＳＩＣ（Application Specific Integrated Circuit）、ＦＰＧＡ（Field-Programmable Gate Array）、またはこれらを組み合わせたものが該当する。横滑り角推定部１２等の各部の機能それぞれは、複数の処理回路８１で実現されてもよいし、各部の機能をまとめて一つの処理回路で実現されてもよい。

　処理回路８１がプロセッサである場合、横滑り角推定部１２等の機能は、ソフトウェア等（ソフトウェア、ファームウェアまたはソフトウェアとファームウェア）との組み合わせにより実現される。ソフトウェア等はプログラムとして記述され、メモリに格納される。図１２に示すように、処理回路８１に適用されるプロセッサ８２は、メモリ８３に記憶されたプログラムを読み出して実行することにより、各部の機能を実現する。すなわち、移動体測位装置７は、処理回路８１により実行されるときに、移動体測位装置７の各部の機能が結果的に実行されることになるプログラムを格納するためのメモリ８３を備える。換言すれば、このプログラムは、横滑り角推定部１２等の手順または方法をコンピュータに実行させるものであるともいえる。ここで、メモリ８３は、例えば、ＲＡＭ（Random Access Memory）、ＲＯＭ（Read Only Memory）、フラッシュメモリ、ＥＰＲＯＭ（Erasable Programmable Read Only Memory）、ＥＥＰＲＯＭ（Electrically Erasable Programmable Read Only Memory）などの、不揮発性または揮発性の半導体メモリ、ＨＤＤ（Hard Disk Drive）、磁気ディスク、フレキシブルディスク、光ディスク、コンパクトディスク、ミニディスク、ＤＶＤ（Digital Versatile Disk）及びそのドライブ装置等、または、今後使用されるあらゆる記憶媒体であってもよい。

　以上、横滑り角推定部１２等の各機能が、ハードウェア及びソフトウェア等のいずれか一方で実現される構成について説明した。しかしこれに限ったものではなく、横滑り角推定部１２等の一部を専用のハードウェアで実現し、別の一部をソフトウェア等で実現する構成であってもよい。

　なお、各実施の形態を自由に組み合わせたり、各実施の形態を適宜、変形、省略したりすることが可能である。上記の説明は、すべての態様において、例示である。例示されていない無数の変形例が想定され得るものと解される。

　また、移動体として車両１が例示されたが、移動体は車両に限定されない。移動体として、例えば各種点検用ロボットおよびパーソナルモビリティなどがある。

　１　車両、２　ハンドル、３　操舵アクチュエータ、４　衛星、５　アンテナ、６　駆動装置、７　移動体測位装置、８　移動体センサ、９　車両制御装置、１０　衛星測位結果受信部、１１　慣性センサ、１２　横滑り角推定部、１３　センサ補正部、１４　慣性測位部、１５　フィルタ部。

Claims

　センサにより検出された移動体に関するセンサ値を取得するセンサ情報取得部と、
　前記センサ値を用いて前記移動体の横滑り角を推定する横滑り角推定部と、
　前記センサ値と前記横滑り角とを用いて前記移動体の慣性測位を行う慣性測位部と、を備え、
　前記横滑り角推定部は、前記移動体に関する複数の運動モデルを前記移動体の状態量に基づき重みづけして統合することにより得られた混合モデルと、前記センサ値とに基づき、前記横滑り角を推定する、
移動体測位装置。
　センサにより検出された移動体に関するセンサ値を取得するセンサ情報取得部と、
　前記センサ値に含まれるセンサ誤差を補正するセンサ補正部と、
　前記センサ補正部で補正された前記センサ値を用いて前記移動体の慣性測位を行う慣性測位部と、
　前記慣性測位の結果および前記移動体の横滑り角を用いて、前記センサ値に含まれるセンサ誤差を推定するフィルタ部と、を備え、
　前記慣性測位部は、前記移動体に関する複数の運動モデルを前記移動体の状態量に基づき重みづけして統合することにより得られた混合モデルと、前記センサ値とに基づき、前記横滑り角を推定し、前記横滑り角を用いて前記慣性測位を行う、
移動体測位装置。
　前記慣性測位の結果および前記横滑り角を用いて、前記センサ値に含まれるセンサ誤差を推定するフィルタ部と、
　前記センサ値に含まれるセンサ誤差を補正するセンサ補正部と、を備え、
　前記慣性測位部は前記センサ補正部で補正された前記センサ値を用いて前記慣性測位を行う、
請求項１に記載の移動体測位装置。
　前記横滑り角推定部は、前記センサ補正部で補正された前記センサ値を用いて前記横滑り角を推定する、
請求項３に記載の移動体測位装置。
　前記横滑り角推定部は、前記移動体の速度に基づいて前記複数の運動モデルを重みづけして前記混合モデルを作成する、
請求項１，３，４のいずれか１項に記載の移動体測位装置。
　前記フィルタ部は、前記移動体に搭載された複数のアンテナが衛星から受信した衛星信号により、前記移動体の進行方向を推定し、前記慣性測位の結果、前記横滑り角、および前記移動体の進行方向に基づき、前記センサ誤差を推定する、
請求項２から請求項４のいずれか１項に記載の移動体測位装置。
　前記慣性測位部は、前記移動体の速度に基づいて前記複数の運動モデルを重みづけして前記混合モデルを作成する、
請求項２に記載の移動体測位装置。