JP2023132340A - 移動体の位置推定装置及び観測値採用方法 - Google Patents

Abstract

【課題】冗長な複数の観測値を用いて、安定した自己位置推定を行う。【解決手段】移動体に搭載された内界センサによる計測値に基づいて当該移動体の位置及び方位の第１推定値を取得する第１推定手段と、少なくとも移動体の位置についての冗長な観測値に基づいて、少なくとも移動体の位置の前記第１推定値を補正して第２推定値を取得する第２推定手段と、を備え、前記第２推定手段は、観測値採用決定手段を備え、前記観測値採用決定手段は、観測値を間引いて採用する観測値間引き手段と、観測値が外れ値か否かを判定する外れ値判定手段と、を含んでいる。【選択図】図４

Description

本発明は、自動運転のための自己位置推定（Localization）に関するものである。

自動運転を実現するためには、自己位置推定が必須である。自己位置推定には、幾つかの手法が知られており、代表的な手法として、(i)IMU(Inertial Measurement Unit)に代表される内界センサを用いたデッドレコニング(Dead Reckoning)、(ii)GNSS(Global Navigation Satellite System)を用いた測位、(iii)カメラやLiDAR（Light Detection and Ranging）に代表されるvision-basedセンサを用いたマップマッチング（SLAMを含む）を例示することができる。

上記(i)～(iii)に挙げた手法は、それぞれの手法に依存した課題を備えている。内界センサを用いたデッドレコニングでは、時間と共に誤差が蓄積されるという欠点がある。GNSSを用いたRKT測位は高精度な位置推定を可能とするが、例えば、トンネル内や地下、ビル群に囲まれた道を走行する場合には信頼度が落ちるという欠点がある。カメラやLiDARは光学的悪環境（濃霧、降雨、降雪、逆光等）に弱いという欠点がある。したがって、複数の位置推定手法を組み合わせること、すなわちセンサフュージョンによる自己位置推定が提案されている（特許文献１～４）。センサフュージョンにおいて複数の情報を統合するにあたり、典型的には、拡張カルマンフィルタ(EKF)が用いられる。

特許文献１において、自己位置推定部は、オドメトリ等のデッドレコニングにより運搬車両の自己位置の事前推定値を推定し、拡張カルマンフィルタを用いて、事前推定値に、第１の計測値や第２の計測値を反映させることで事前推定値を修正して事後推定値を推定する。第１の計測値や第２の計測値を事前推定値に反映させる度合いは、各計測値の品質（所定の指標によって評価される）に応じて設定される第１の計測値の誤差分散値、第２の計測値の誤差分散値によって調整される。

特許文献２には、拡張カルマンフィルタを用いた自己位置推定が開示されており、ＩＭＵとホイールオドメトリによるデッドレコニングで前時刻サンプルから現時刻サンプルでの自己位置を予測し、現時刻サンプルで取得された観測値を用いて予測値を補正することで自己位置を推定する。移動体の走行環境に応じて、拡張カルマンフィルタの共分散の値を動的に変更することが開示されている。

特許文献３では、観測データの信頼度を反映した観測誤差標準偏差の項を備えた観測方程式を用意し、カルマンフィルタによる演算処理において、冗長的に観測された横位置のうち、観測誤差標準偏差の小さいセンサの観測データのほうが演算に使われやすくし、観測誤差標準偏差の大きいセンサの観測データのほうが演算に使われにくくしている。なお、特許文献４に開示された磁気マーカはレーンマーカであり、車両の横変位を検出するためのもので、自己位置推定に用いられるものではない。

特許文献４では、複数のセンサにより得られる複数の観測量を用いて自己位置推定を行うにあたり、異常値を出力するセンサを識別し、異常値を出力すると識別されたセンサを除外して得た観測量で補正した状態量の予測値に基づいて車両の位置を推定することが開示されている。

自己位置推定手法として、さらに、(iv)磁気マーカシステムを用いた手法も提案されている（非特許文献１）。磁気マーカシステムは、走行経路の所定箇所に所定間隔で埋設した複数の磁気マーカと、車両に搭載した磁気センサアレイと、からなる。磁気マーカの構成や磁気センサアレイの構成の詳細については、例えば、特許文献５を参照することができる。磁気センサアレイによって磁気マーカを検出することで、車両の位置を推定する手法を磁気ポジショニングシステム(MPS)と称する場合もある。非特許文献１には、１本の磁気センサアレイで車両の位置とヨー角の推定を行う手法として、慣性計測装置(IMU) 等の別のセンサ情報による慣性航法と磁気マーカによる位置情報を統合（センサフュージョン）することで位置とヨー角を推定する手法が提案されている。磁気ポジショニングシステムを用いた自己位置推定手法を、センサフュージョンに加えることで、GNSSやマップマッチングを用いた自己位置推定の欠点を補うことができる。

上述の通り、幾つかの自己位置推定手法を紹介したが、自己位置推定手法によって得られる観測値の頻度ないし周期が異なる点に留意する必要がある。観測頻度について例示すると、GNSSに基づく観測値は100Hz、マップマッチングに基づく観測値は10Hz、磁気マーカシステムに基づく観測値は2m間隔であり、観測頻度にかなりの差があるため、センサフュージョンに際して、各観測値の観測誤差を増減させたとしても観測頻度の差による影響が出るおそれがある。

自己位置推定では、信頼度が低い観測値（典型的には外れ値）を除去することが行われるが、外れ値や信頼度が低い観測値を一切使わないとすると、全く観測が使えない時間が継続した場合に、デッドレコニングの誤差の蓄積が大きくなって、観測が入ったときの偏差が大きくなりすぎて不安定になるおそれがあり、また、誤差の蓄積によって、常に観測値から閾値以上の距離となってしまい復帰できなくなってしまうおそれもある。

特開２０２０－３８４９８ 特開２０２０－９５３３９ 特許第４２２９１４１号 特開２０２１－１８１１２ 特許第６７６６５２７号

磁気マーカによる座標位置と慣性航法を用いた車両ヨー角推定石井 響弥、霜野 慧亮、須田 義大、安藤 孝幸、長尾 知彦、山本 道治、椋本 博学、瀬川 雅也、第30回交通・物流部門大会（TRANSLOG2021）、一般社団法人 日本機械学会

本発明は、冗長な複数の観測値を用いて、安定した（滑らかな）自己位置推定を行うことを目的とするものである。

本発明が採用した技術手段は、
移動体に搭載された内界センサによる計測値に基づいて当該移動体の位置及び方位の第１推定値を取得する第１推定手段と、
少なくとも移動体の位置についての冗長な観測値に基づいて、少なくとも移動体の位置の第１推定値を補正して第２推定値を取得する第２推定手段と、を備え、
前記第２推定手段は、第２推定値の推定に用いられる観測値を決定する観測値採用決定手段を備え、
前記観測値採用決定手段は、
観測値を間引いて採用する観測値間引き手段と、
観測値が外れ値か否かを判定する外れ値判定手段と、
を含んでいる、
移動体の位置推定装置、である。

１つの態様では、前記外れ値判定手段は、前記観測値間引き手段により採用された観測値に対して適用される。
１つの態様では、自動運転モードと手動運転モードが切替可能であり、
自動運転モードでは、
前記外れ値判定手段は、前記観測値間引き手段により採用された観測値に対して適用され、外れ値ではない観測値が、前記観測値採用決定手段によって採用され、
手動運転モードでは、
前記外れ値判定手段は無効化され、前記観測値間引き手段により採用された観測値が、前記観測値採用決定手段によって採用される。

１つの態様では、前記観測値間引き手段は、各観測値の採用確率を決定する採用確率決定手段を備えており、採用確率に基づいて観測値を間引くようになっている。
１つの態様では、採用確率決定手段は、複数（２つ以上）の採用確率を格納した採用確率テーブルから１つの採用確率を選択する。

１つの態様では、前記採用確率に基づく観測値の間引きは、採用確率を規定する閾値と、一様乱数を生成する疑似乱数生成器と、を用いて実行される。
１つの態様では、観測の信頼度評価手段を備えており、
前記採用確率決定手段は、信頼度評価に基づいて採用確率を決定する。
１つの態様では、信頼度評価手段は、複数の信頼度評価を格納した信頼度評価テーブルを備えており、各信頼度評価と採用確率テーブルの採用確率が紐づけられている。

１つの態様では、前記冗長な観測値は、少なくともGNSSに基づく第１観測値を含み、
前記観測値間引き手段は、前記第１観測値に適用される。
１つの態様では、前記冗長な観測値には、磁気マーカシステムに基づく第２観測値が含まれる。
１つの態様では、磁気マーカ設置区間においては、前記観測値間引き手段によって、前記第１観測値が全て不採用となる。
１つの態様では、前記冗長な観測値には、マップマッチングに基づく第３観測値が含まれる。
１つの態様では、前記第２推定手段は、移動体の位置に加えて、方位（方位角ないし姿勢）を推定する。

１つの態様では、前記第１推定手段及び前記第２推定手段は、拡張カルマンフィルタ（EKF）から構成される。
１つの態様では、前記移動体の位置推定装置は、拡張カルマンフィルタの観測誤差共分散を規定する観測誤差設定手段を備えている。
１つの態様では、観測誤差共分散は一定である。
１つの態様では、前記観測誤差設定手段は、信頼度評価に基づいて観測誤差共分散を設定する。
拡張カルマンフィルタ（EKF）に代えて、パーティクルフィルタ（PF）を用いてもよい。

本発明は、コンピュータを、上記の各手段として機能させるためのコンピュータプログラムとしても提供され得る。

本発明が採用した他の技術手段は、
冗長な複数の観測値を用いた移動体の位置推定における観測値の採用方法であって、
各観測値の採用確率に基づいて、各観測値を間引くステップと、
間引き後の観測値が外れ値か否かを判定するステップと、
間引き後の観測値が外れ値でない場合に、間引き後の観測値を採用して、自己位置推定に用いるものである。

１つの態様では、手動運転モードでは、前記外れ値判定ステップが無効化され、前記間引き後の観測値が採用される。
１つの態様では、前記採用確率を、観測値の信頼度に基づいて決定するステップを含む。
１つの態様では、前記冗長な観測値は、少なくともGNSSに基づく第１観測値を含み、
前記観測値を間引くステップは、前記第１観測値に適用される。

本発明によれば、観測値間引き手段によって適宜観測値を間引くことで、冗長な複数の観測値を用いた安定した自己位置推定を可能とする。

本実施形態に係る位置推定装置の概要を示す図である。 本実施形態に係る位置推定装置を示すブロック線図である。 本実施形態に係る位置推定装置のハードウェア構成示す図である。 本実施形態に係る観測値採用決定手段を示す図である。 本実施形態に係る観測値間引き手段を示す図である。 本実施形態に係る自己位置推定を示すフローチャートである。 本実施形態に係る自己位置推定を示すフローチャートである。 本実施形態に係る位置推定装置（EKF）の全体構造を示す概略図である。

[Ａ] 移動体の位置推定装置の概要
図１に示すように、本実施形態に係る移動体の位置推定装置は、移動体に搭載された内界センサによる計測値に基づいて当該移動体の位置及び方位の第１推定値を取得する第１推定手段と、少なくとも移動体の位置についての冗長な観測値に基づいて、少なくとも移動体の位置の前記第１推定値を補正して第２推定値を取得する第２推定手段と、を備えている。

図２は、本実施形態に係る位置推定装置を示すブロック線図であり、冗長な観測値を用いて、IMUの計測値に基づくデッドレコニング（Dead Reckoning）により推定された位置の補正を行う。図２、図７に示すように、典型的な態様では、本実施形態に係る移動体の位置推定装置は、拡張カルマンフィルタ（EKF）を用いるものであり、第１推定値は、デッドレコニングによって計算された移動体の位置及び方位に基づいて取得された事前推定値であり、第２推定値は、第１観測値、第２観測値、第３観測値の１つあるいは複数の組み合わせに基づいて事前推定値を補正してなる事後推定値である。拡張カルマンフィルタ（EKF）を用いた具体的な処理については後述する。

本実施形態において、第１観測値は、GNSS（典型的には、RTK-GNSS）に基づいて取得された観測値であり、車体の位置情報、あるいは、車体の位置情報及び方位である。第２観測値は、磁気マーカシステムによって取得された観測値であり、車体の位置情報、あるいは、車体の位置情報及び方位である。第３観測値は、マップマッチングによって取得された観測値であり、車体の位置情報及び方位である。本実施形態では、GNSSに基づく第１観測値は100Hz、磁気マーカシステムに基づく第２観測値は2m間隔、マップマッチングに基づく第３観測値は10Hz、で取得される。

図１、図２では、第１観測値、第２観測値、第３観測値の３つの観測値を明示しているが、観測の数は限定されず、例えば、図７に示すように、４つ以上の観測値を用いてもよい。具体的には、マップマッチングのセンサとして、LiDARとカメラを用いることで、LiDARによる第３観測値、カメラによる第４観測値を取得するようにしてもよい。

[Ｂ] ハードウェア構成及び自己位置推定手法
[Ｂ－１]ハードウェア構成
図３を参照しつつ、本実施形態に係る位置推定装置のハードウェア構成について説明する。位置推定装置のハードウェアは車体に搭載されたセンサ群と、コンピュータと、からなる。センサ群には、内界センサと外界センサが含まれる。内界センサは、代表的にはIMUである。内界センサには、その他、車速センサ、操舵角センサ、オドメトリ等のセンサが含まれてもよい。外界センサとしては、GNSS受信機、磁気センサ、LiDAR、カメラが例示される。自己位置推定に用いられる外界センサの種類は、採用される自己位置推定手法に依存する。

コンピュータは、プロセッサとメモリを含み、メモリには、マップマッチングのためのマップ（例えば、LiDAR用マップ、カメラ用マップ）が格納されている。また、メモリには、プロセッサが自己位置推定を実行するための各種プログラムやその他の情報が格納されている。なお、自己位置推定のためのコンピュータの機能の一部をクラウドベースで提供するようにしてもよい。

[Ｂ－２] GNSS
GNSS受信機は、GNSS（典型的には、RTK-GNSS）による情報を取得し、当該情報を用いてコンピュータが第１観測値（位置及び方位、あるいは、位置）を算出する。本実施形態では、第１観測値は10ms周期で出力される。GNSSを用いた自己位置推定の具体的な構成は当業者に知られているので、本明細書における詳細な説明は省略する。

[Ｂ－２]磁気マーカシステム（磁気ポジショニングシステム：MPS）
磁気マーカシステムは、磁気マーカが発する微弱な磁気信号を、磁気センサが検知するようにしたシステムであり、検知した磁気マーカが持つ位置情報を用いて自己位置推定が可能である。磁気センサは、磁気マーカからの磁気信号を検出し、磁気マーカによって得られた情報を用いてコンピュータが第２観測値（位置及び方位、あるいは、位置）を算出する。本実施形態では、第２観測値は2m間隔で出力される。

磁気マーカを用いた自己位置推定には、磁気センサアレイが２本の場合、磁気センサアレイが１本の場合の２つの態様がある。磁気センサアレイが２本の場合には、２本の磁気センサアレイが車体の前後に離間（磁気マーカの間隔）して配置されており、ヨー角が幾何学的に取得できる。磁気ポジショニングシステムで移動体の位置及び方位角が得られる。

磁気センサアレイが１本の場合は、磁気ポジショニングシステム単独で方位を観測することができないため、位置推定装置によって取得された方位の前回値を使って、磁気マーカとの偏差から位置を算出し、この位置のみの観測値をセンサフュージョンに用いる。磁気ポジショニングシステムにおいて、１本の磁気センサを用いた位置の取得方法については、非特許文献１を参照することができる。

[Ｂ－３]マップマッチング
マップマッチングは、道路を走行して作成した地図と、運転中にセンサから得られた特徴をマッチングして自己位置を推定する手法である。図示の態様では、LiDARによって取得された情報を用いて、コンピュータがLiDAR用マップとのマッチングを行って第３観測値（位置及び方位）を算出する。本実施形態では、第３観測値は100ms周期で出力される。また、カメラによって取得された情報を用いて、コンピュータがカメラ用マップとのマッチングを行って第４観測値（位置及び方位）を算出してもよい。マップマッチングは、LiDARやカメラを用いたSLAM(Simultaneous Localization and Mapping)でもよい。なお、マップマッチングのためのセンサは、LiDAR及びカメラに限定されるものではなく、例えば、LiDAR、カメラに加えて、あるいは、いずれか１つ以上に代えて、ミリ波レーダを用いてもよい。マップマッチングの具体的な構成は当業者に知られているので、本明細書における詳細な説明は省略する。

[Ｃ] 観測値採用決定手段
図４に示すように、本実施形態に係る位置推定装置は、第２推定値の推定に用いられる観測値を決定する観測値採用決定手段を備えている。観測値採用決定手段は、観測値を間引いて採用する観測値間引き手段と、観測値が外れ値か否かを判定する外れ値判定部と、を含んでいる。外れ値判定手段は、観測値間引き手段により採用された観測値に対して適用される。本実施形態に係る移動体は自動運転モードと手動運転モードが切替可能となっている。観測値の間引き処理は、自動運転モード、手動運転モードにかかわらず、実行される。手動運転モードにおいて、外れ値判定手段は無効化される。

[Ｃ－１]観測値間引き手段
観測値間引き手段は、各観測値の信頼度を評価する信頼度評価部と、信頼度評価部によって評価された信頼度にしたがって観測の採用を決定する採用確率決定部と、を備えている。第１観測値、第２観測値、第３観測値の信頼度を評価して、事後推定値を算出する際の観測値として採用するか否かの確率を決める。本実施形態では、２段階の信頼度のHigh/Lowに応じて２段階の採用確率C1/C2を切り替えて、確率に基づいて観測値の間引きを行う。

観測値間引き手段を採用したことで、以下のような場合に対処することが可能となる。本実施形態では、GNSSに基づく第１観測値が100Hzで観測されるので、センサフュージョンにおいて第１観測値の影響が過度に大きくなるおそれがある（第１観測値の観測誤差を大きくしたとしても）。特に第２観測値は2m間隔（磁気マーカの間隔）で観測されるため、第１観測値の影響を抑制するためには、第１観測値を確率により間引くことが有用である。第１観測値の観測誤差を大きくすることで、第１観測体の影響を抑えることが可能であるが、第１観測値（GNSS）の観測誤差を非常に大きくした場合には、観測が第１観測値（GNSS）のみである場合に、観測に基づく補正量が適切な補正量よりも小さくなってしまうという不具合がある。第１観測値を適切に間引くことで、磁気マーカ設置区間では意図的に第１観測値（GNSS）の補正量を下げる一方、磁気マーカ非設置区間では第１観測値（GNSS）の補正量を適切なレベルに調整することを可能とする。

また、GNSSしか使えない環境では、仮に第１観測値の信頼度が低くても、デッドレコニングの誤差の蓄積を抑えるため5%程度に間引いて使うことを可能とする。すなわち、観測値の信頼度が低くても、全て棄却するのではなく、一部を採用することに対応可能である。また、場所についての事前知識(トンネル、磁気マーカ設置区間等)に基づいて確率を増減させてもよい。採用確率を0 or 1とすれば、第２推定値の推定において観測値を使う/使わないと同じ動きとなる。すなわち、採用確率を1とすることで、実質的に間引き処理を実行しないのと同じ処理となる。

図５を参照しつつ、本実施形態に係る観測値間引き手段について詳細に説明する。信頼度評価部は、第１観測値の信頼度を評価する第１信頼度評価テーブルと、第２観測値の信頼度を評価する第２信頼度評価テーブルと、第３観測値の信頼度を評価する第３信頼度評価テーブルと、を備えている。第１信頼度評価テーブルを参照することで第１観測値の信頼度が決定され、第２信頼度評価テーブルを参照することで第２観測値の信頼度が決定され、第３信頼度評価テーブルを参照することで第３観測値の信頼度が決定される。本実施形態では、信頼度はHighとLowの２段階であるが、３段階以上の信頼度を用いてもよい。あるいは、指標に応じて信頼度スコアを算出するようにしてもよい。

採用確率決定部は、第１観測値の採用確率を決定する第１採用確率選択テーブルと、第２観測値の採用確率を決定する第２採用確率選択テーブルと、第３観測値の採用確率を決定する第３採用確率選択テーブルと、を備えている。各信頼度にしたがって、第１採用確率選択テーブルを参照することで第１観測値の採用確率が決定され、第２採用確率選択テーブルを参照することで第２観測値の採用確率が決定され、第３採用確率選択テーブルを参照することで第３観測値の採用確率が決定される。

本実施形態において、採用確率は0～1の間の数値で規定される。採用確率が1であれば、間引きは行われず、後段の外れ値判定手段を経ることで、当初の観測値がそのまま採用される。採用確率が0であれば、その観測値は一切採用されない。

採用確率が0～1の間であるときは、一様乱数pを一つ生成し、一様乱数p＜採用確率Cの場合に採用し、p≧Cの場合には採用しない。本実施形態では、採用確率を用いた観測値の間引きは、疑似乱数生成器によって一様乱数を生成し、一様乱数が閾値以下である否かによって確率的に判定する。例えば、信頼度評価により、採用確率C＝5%で間引くこととなった場合は、0～1の一様乱数pを１つ生成し、その乱数の値がp＜C(＝0.05)であるかどうかで間引きを通過するかどうかの判定を行うことで、100Hzから5Hz程度に間引くことができる。なお、観測を複数回ごとに１回採用するという処理であってもよい。

本実施形態では、各観測値のそれぞれについて信頼度のHigh/Lowの判定をし、信頼度のHigh/Lowに応じて、信頼度が高いときの採用確率C₁、信頼度が低いときの採用確率C₂の２値を切り替えている。採用確率C₁、C₂は予めテーブルとして格納されている。なお、C₁とC₂の２値の切り替えではなく、所定の条件を設けて、条件によって、確率（間引く量）を可変としてもよい。また、High/Lowの２段階で切り替えるのではなく、複数の指標の組み合わせによって採用確率を算出するようにしてもよい。例えば、第１観測値の信頼度を評価するための複数の指標の各指標に基づく採用スコアを足し合わせて採用確率を算出するようにしてもよい。

[Ｃ－２] 観測値間引き手段の実施例
本実施形態に係る観測値間引き手段の実施例について説明する。以下に述べる信頼度評価テーブル及び採用確率選択テーブルは例示であって、本発明を限定するものではない点に留意されたい。表１に、GNSSによって取得される第１観測値の信頼度を評価するための第１信頼度評価テーブルを示す。

指標１は、GGA_FixQualityであり、GPS Quality Indicator（図示しない複数の指標からなる）においてRTK Fixedが観測された場合にのみHighとなる。指標２は、GST_error_ellipse_semimajor（誤差楕円長軸）であり、所定の閾値より大きい場合にHigh、所定の閾値より小さい場合にLowとなる。指標３は、GST_error_ellipse_semiminor（誤差楕円短軸）であり、所定の閾値より大きい場合にHigh、所定の閾値より小さい場合にLowとなる。指標４は、IMU_AlignmentStatusであり、図示しない複数の指標においてAlignedが観測された場合にのみHighとなる。指標５は、使用可能な衛星数であり、４個未満の場合にLowとなる。指標６は、GGA_HDOP（水平精度低下率）であり、所定の閾値未満の場合にLowとなる。指標７は、GGA_LastUpdateであり、所定の閾値未満の場合にLowとなる。指標８は、磁気マーカ埋設区間に基づく指標であり、磁気マーカ設置区間では、Lowとなる。本実施例では、AND(1,2,3,4の上側)で信頼度がLowからHighに移り、OR(1,2,3,4,5,6,7,8の下側)で信頼度がHighからLowに移るようになっている。表１に示す指標１～８は例示であって、当業者において、他の指標や条件を採用し得ることが理解される。

表２に、磁気マーカシステムによって取得される第２観測値の信頼度を評価するための第２信頼度評価テーブルを示す。

本実施例では、磁気マーカ埋設区間においてのみ第２観測値の信頼度はHighであり、実質的な信頼度評価は行っていない。

表３に、マップマッチングによって取得される第３観測値の信頼度を評価するための第３信頼度評価テーブルを示す。

指標１は、点群マッチングスコアが所定の閾値よりも大きい時にHighとなり、所定の閾値よりも小さい時にLowとなる。AND(指標１の上側)で信頼度がLowからHighに移り、OR(指標１の下側)で信頼度がHighからLowに移るようになっている。指標２は、任意の指標であり、磁気マーカ区間では第３観測値を採用しないようにすることも可能となっている。

本実施例では、信頼度のHigh/Lowに従って採用確率をC1、C2から選択するようにしている。本実施例に係る第１採用確率選択テーブルを表４に示す。表４に示す採用確率は例示である。

本実施例では、GNSSの第１観測値は、信頼度の高いときの採用確率は0.2、信頼度の低いときの採用確率は0となる。GNSSの入力周波数100Hzが高いので、信頼度が高い場合もC1_GNSS=0.2として20%に間引き、およそ20Hzに落としている。GNSSしか採用できる観測が存在せず、かつGNSSの信頼度がほぼ常に低い環境のような場合には、C2_GNSS=0.05として信頼度が低くても全く使わないのではなく多少使うようにしている。磁気マーカ設置区間においてはGNSSの信頼度を下げ、C2_GNSS=0.0としてGNSSを全て間引いて採用しない。

本実施例に第２採用確率選択テーブル、第３採用確率選択テーブルをそれぞれ表５、表６に示す。表５、表６に示す採用確率は例示である。

磁気マーカシステムによる第２観測値、マップマッチングによる第３観測値については、信頼度の高いときの採用確率は１、信頼度の低いときの採用確率は０となる。本実施形態では、第２観測値の取得に用いられる磁気マーカは2m間隔で設置され（30km/h）で走行中の場合は4Hz）、第３観測値の周期は10Hzであり、観測頻度がおよそ揃っていると考えられる。本実施例では、実質的に間引き処理が実行されるのは、GNSSに基づく第１観測値（観測値で最も頻度が高い）のみである。

[Ｃ－３]外れ値判定手段
本実施形態に係る観測値採用決定手段は、観測値を間引いて採用する観測値間引き手段に加えて、観測値が外れ値か否かを判定する外れ値判定部を備えている。外れ値判定手段は、観測値間引き手段により採用された観測値に対して適用される。外れ値の処理は自動運転モードにおいて有効であり、手動運転モードにおいて無効である。自動運転中は安定を優先するため、外れ値を排除する必要がある。手動走行中は外れ値が正しい可能性もあるため、確率による間引きを通過させて用いることを考える。

本実施形態に係る位置推定装置は、車両が自動運転中であるか否かを判定する自動運転判定部を備えており、自動運転判定部によって自動運転中であると判定された場合に、観測値間引き手段を通過した観測値に対して外れ値判定手段が実行される。観測値が正常値であると判定された場合には、当該観測値は第２推定手段に入力され、観測値が外れ値であると判定された場合には、当該観測値は第２推定手段の計算に採用されない。

１つの態様では、外れ値判定手段は、観測値間引き手段を通過した観測値と事前予測値との距離と閾値とを比較して、観測値が外れ値であるか否かを判定する。１つの態様では、位置と方位が得られている場合には、位置と方位について、それぞれ距離を用いて、独立に外れ値か否かが判定され、位置と方位の両方が外れ値でなければ当該観測値を採用し、いずれかが外れ値の場合には当該観測値を採用しないようにする。外れ値判定手段の具体的な構成は、限定されず、例えば、マハラノビス距離を用いて、位置と方位の両方について外れ値判定を行ってもよい。

自動運転判定部によって手動運転モードであると判定された場合に、観測値間引き手段を通過した観測値は、外れ値判定手段が実行されることなく、第２推定手段に入力される。すなわち、手動運転モードでは、外れ値判定手段は無効化される。手動運転モードでは、外れ値の判定を行わずに、間引き処理後の観測値が事後推定値の算出において採用される。手動運転時に外れ値判定を行わないことで、以下のような事態に対応することが可能となる。

例えば、位置と方位について、それぞれ距離を用いて外れ値を判定する場合に、トンネルを手動運転モードで回送しているとき、GNSSが精度悪化で採用されず、マップマッチングのためのマップが存在しておらず、磁気マーカも埋設されておらず、観測が全くない状態が続いてEKFの座標が真の座標から著しく離れてしまい、その後トンネルを抜けてGNSSが精度良好に復帰したとしても、EKFの事前推定値とGNSSの観測値とが離れすぎていて、外れ値として判定されてしまって以後二度と観測が採用されなくなってしまう、というような事態を回避することが可能となる。一方、自動運転区間においては常に少なくとも１つの観測値が有効であるような状況が設定されているため（例えば、トンネル内を磁気マーカ設置区間とする）、そのようなEKFが誤差の蓄積で真値から乖離して常に外れ値の判定になってしまうような事態が生じないようになっている。

[Ｄ] 自己位置推定の流れ
図６Ａ、図６Ｂを参照しつつ、本実施形態に係る自己位置推定の手順について説明する。図６Ａに示すように、内界センサからの入力信号、各自己位置推定手段からの観測信号、自動走行状態を取得する。車体の位置及び方位についての前回値と入力信号から、車体の位置及び方位についての事前推定値を算出する。

観測値が取得されると、各観測値について信頼度の評価が実行される。各観測値の信頼度の評価は、各観測値について用意された信頼度評価テーブルに基づいて実行され、信頼度がHigh、Lowのいずれかで決定される。各観測値の信頼度に応じて、各観測値の採用確率がC₁、C₂から選択される。採用確率の決定は、各観測値について用意された採用確率選択テーブルに基づいて実行され、各観測値の評価High、Lowに対応して採用確率C₁、C₂が決定される。

図６Ｂに示すように、選択された採用確率を規定する閾値と、一様乱数とを対比して、採用確率にしたがって観測値を間引く。確率に基づく間引き処理は、0～1の一様乱数pを生成する疑似乱数生成器を用いて実行され、乱数pが採用確率Cよりも小さい場合に、その観測値は間引き処理を通過して採用される。間引き処理を通過して採用された観測値が次のステップに進み、間引かれた観測値は棄却され、事後推定値の算出には採用されない。

次に自動運転モードであるか否かが判定される。自動運転モードである場合には、間引き処理を通過した観測値について、外れ値判定処理が実行される。外れ値判定処理は、例えば、事前推定値と観測値との差が所定の閾値以下であるか否かによって判定される。その差が閾値以下であった場合には次のステップに進み、閾値よりも大きい場合には、その観測値は棄却され、事後推定値の算出には採用されない。手動運転モードである場合には、外れ値判定処理がスキップ（無効化）されて、次のステップに進む。

次いで、車両が走行中であるか否かが判定される。走行中である場合には、間引き処理、及び、外れ値処理（自動運転モードの場合のみ）を通過した観測値は、観測値として採用され、事後推定値の計算に用いられる。車両が走行中でない場合（停車時）には、観測値は事後推定値の算出には採用されない。すなわち、自動運転モードであるか手動運転モードであるかを問わず、停止している際には座標更新は行われない。

図６Ａに示すように、各観測値の信頼度に応じて、観測誤差σ_１、σ_２を選択するようにしてもよい。観測誤差σ_１、σ_２は、観測誤差選択テーブルとして用意される。観測誤差は、カルマンフィルタで用いられる観測誤差共分散として規定され、図６Ｂに示すように、事後推定値の計算時に用いられる。ある観測値の信頼度が低い場合に、その観測値の観測誤差共分散の値を大きく設定しておくことで、当該観測値のセンサフュージョンにおける重みを小さくすることができる。

[Ｅ] 拡張カルマンフィルタ(EKF)を用いたセンサフュージョン
図２、図７に示すように、本実施形態に係る位置推定装置は拡張カルマンフィルタ(EKF)から構成され、GNSSによって取得された第１観測値、磁気マーカシステムによって取得された第２観測値、マップマッチング（LiDAR）によって取得された第３観測値、マップマッチング（カメラ）によって取得された第４観測値が、全体として１つの拡張カルマンフィルタ（EKF）に入力される。デッドレコニングで前時刻サンプルから現時刻サンプルでの自己位置を予測して事前推定値を取得し、現時刻サンプルで取得された冗長な観測値（第１観測値～第４観測値）を用いて事前推定値を補正することで自己位置の事後推定値を取得する。

以下に、拡張カルマンフィルタ（EKF）を用いた自己位置推定について、位置及び方位角の冗長な観測値である、GNSSを用いて取得される第１観測値（添え字GNSS）、磁気マーカシステムを用いて取得される第２観測値（添え字MPS）、マップマッチングを用いて取得される第３観測値（添え字LiDAR）を用いる場合に基づいて説明する。拡張カルマンフィルタ（EKF）のアルゴリズムは以下の通りである。

カルマンフィルタは、状態を制御入力により更新する予測ステップ（時間更新）と、観測により予測を補正する補正ステップ（観測更新）と、からなる。予測ステップで事前推定値x^∧- _k、事前誤差共分散P^- _kを算出し、補正ステップで、カルマンゲインG_kを算出して、観測値用いて事前推定値x^∧- _kを補正して事後推定値x^∧ _kを出力し、誤差共分散を更新して事後誤差共分散P_kを取得する。以下に、上記アルゴリズムに登場する各要素について説明する。

状態ベクトルxを

とする。ｘ：緯度経度から変換したＸ座標、ｙ：緯度経度から変換したＹ座標、θ：方位角、である。

制御入力ベクトルを

とする。Ｖ：速度、δ：操舵角、γ：ヨーレートである。

状態方程式は

となり、入力雑音w_kが含まれている。座標x, yの更新は、速度Vで進んだ距離V_kT_s（T_s：サンプリング周期）と車両が実際に進む方向である航路角ψとによって定まる。方位角θに横滑り角βを加えることで航路角ψが得られる。ここで、横滑り角（二輪モデル）は、

であり、l_w＝l_f＋l_rはホイールベース（l_fは重心位置から前軸までの距離を、l_r は重心位置から後軸までの距離を表す）であり、スタビリティファクタK_sf、横滑り係数k_βはそれぞれ、

となる。また、k_δは、

である。

観測方程式は、

であり、観測雑音v_kが含まれている。

観測行列H_ｋは、

となる。
第１観測値、第２観測値、第３観測値が採用されるかどうかをf_GNSS(=0,1)等で表す。

入力雑音w_k、観測雑音v_kは、それぞれ、正規分布に従うものとし、入力雑音の共分散行列Ｑ、観測雑音の共分散行列Ｒは、それぞれ、

である。
上記の例では、観測誤差の共分散行列Ｒは固定値を用いているが、観測値の信頼度に応じて可変としてもよい。例えば、信頼度のHigh/Lowに基づいて、採用確率のC1/C2の切り替えに加えて、観測誤差共分散σ_１、σ_２を切り替えるようにしてもよい。また、磁気マーカ設置区間においては、第１観測値、第３観測値の観測誤差を大きくし、磁気マーカ非設置区間においては、観測誤差を小さくするようにしてもよい。

カルマンフィルタの出力である事後推定値 x^∧ _kは、以下式

にしたがって、事前推定誤差共分散行列P^- _k、カルマンゲイン G_kを算出して、事前推定値x^∧- _k、カルマンゲインG_k、観測行列H_kを用いて取得される。また、事後推定誤差共分散行列 P_kを更新する。

なお、Ｆ、Ｂは次式で与えられるヤコビアンである。

第１観測値、第２観測値、第３観測値が、位置及び方位角[x,y,θ]を出力するものとして説明したが、位置[x, y]のみを出力する場合には、所定の行列式の要素を適宜減らせばよいことが当業者に理解される。また、３つの観測値に基づいて説明したが、４つ以上の観測値が用いられる場合には、所定の行列式の要素を適宜増やせばよいことが当業者に理解される。

Claims (19)

1. 移動体に搭載された内界センサによる計測値に基づいて当該移動体の位置及び方位の第１推定値を取得する第１推定手段と、
   少なくとも移動体の位置についての冗長な観測値に基づいて、少なくとも移動体の位置の第１推定値を補正して第２推定値を取得する第２推定手段と、を備え、
   前記第２推定手段は、第２推定値の推定に用いられる観測値を決定する観測値採用決定手段を備え、
   前記観測値採用決定手段は、
   観測値を間引いて採用する観測値間引き手段と、
   観測値が外れ値か否かを判定する外れ値判定手段と、
   を含んでいる、
   移動体の位置推定装置。
2. 前記外れ値判定手段は、前記観測値間引き手段により採用された観測値に対して適用される、
   請求項１に記載の移動体の位置推定装置。
3. 自動運転モードと手動運転モードが切替可能であり、
   自動運転モードでは、
   前記外れ値判定手段は、前記観測値間引き手段により採用された観測値に対して適用され、外れ値ではない観測値が、前記観測値採用決定手段によって採用され、
   手動運転モードでは、
   前記外れ値判定手段は無効化され、前記観測値間引き手段により採用された観測値が、前記観測値採用決定手段によって採用される、
   請求項２に記載の移動体の位置推定装置。
4. 前記観測値間引き手段は、各観測値の採用確率を決定する採用確率決定手段を備えており、採用確率に基づいて観測値を間引くようになっている、
   請求項１～３いずれか１項に記載の移動体の位置推定装置。
5. 前記採用確率に基づく観測値の間引きは、採用確率を規定する閾値と、一様乱数を生成する疑似乱数生成器と、を用いて実行される、
   請求項４に記載の移動体の位置推定装置。
6. 各観測の信頼度を評価する信頼度評価手段を備えており、
   前記採用確率決定手段は、信頼度に基づいて採用確率を決定する、
   請求項４、５いずれか１項に記載の移動体の位置推定装置。
7. 前記冗長な観測値は、少なくともGNSSに基づく第１観測値を含み、
   前記観測値間引き手段は、前記第１観測値に適用される、
   請求項１～６いずれか１項に記載の移動体の位置推定装置。
8. 前記冗長な観測値には、磁気マーカシステムに基づく第２観測値が含まれる、
   請求項７に記載の移動体の位置推定装置。
9. 磁気マーカ設置区間においては、前記観測値間引き手段によって、前記第１観測値が全て不採用となる、
   請求項８に記載の移動体の位置推定装置。
10. 前記冗長な観測値には、マップマッチングに基づく第３観測値が含まれる
    請求項７～９いずれか１項に記載の移動体の位置推定装置。
11. 前記第２推定手段は、移動体の位置に加えて、方位を推定する、
    請求項１～１０いずれか１項に記載の移動体の位置推定装置。
12. 前記第１推定手段及び前記第２推定手段は、拡張カルマンフィルタ（EKF）から構成される、
    請求項１～１１いずれか１項に記載の移動体の位置推定装置。
13. 前記拡張カルマンフィルタ（EKF）で用いる観測誤差共分散を規定する観測誤差設定手段を備えている、
    請求項１２に記載の移動体の位置推定装置。
14. 前記観測誤差設定手段は、信頼度に基づいて観測誤差を設定する、
    請求項１３（請求項６の従属項に限る）に記載の移動体の位置推定装置。
15. コンピュータを、請求項１～１４いずれか１項に記載の各手段として機能させるためのコンピュータプログラム。
16. 冗長な複数の観測値を用いた移動体の位置推定における観測値の採用方法であって、
    各観測値の採用確率に基づいて、各観測値を間引くステップと、
    間引き後の観測値が外れ値か否かを判定するステップと、
    間引き後の観測値が外れ値でない場合に、間引き後の観測値を採用して、自己位置推定に用いる、
    観測値の採用方法。
17. 手動運転モードでは、前記外れ値判定ステップが無効化され、前記間引き後の観測値が採用される、
    請求項１６に記載の方法。
18. 前記採用確率を、観測値の信頼度に基づいて決定するステップを含む、
    請求項１６、１７いずれか１項に記載の方法。
19. 前記冗長な観測値は、少なくともGNSSに基づく第１観測値を含み、
    前記観測値を間引くステップは、前記第１観測値に適用される、
    請求項１６～１８いずれか１項に記載の方法。
    
    

Images