JP2023525927A - 車両位置推定システム及び方法 - Google Patents

Abstract

車両位置推定システム及び方法は、第１のセンサから車両の第１の航法状態を示す第１のデータを取得することと、第２のセンサから車両付近の環境を示す第２のデータを取得することと、第１のセンサ及び第２のセンサに通信可能に接続されたプロセッサを使用して、第２のデータを環境の既知の地図に一致させることに基づいて第２の航法状態を発生させることと、プロセッサを使用して、第１の航法状態及び第２の航法状態に基づいて現在の航法状態を発生させることとを含む。

Description

分野
本開示は、一般に車両位置推定システム及び方法に関し、より詳細には、車両位置推定のための航法センサと組み合わせた測距センサを使用することに関する。

背景
ＧＮＳＳ受信機は、屋外の車両位置を決定するための最も一般的な解決策である。ＧＮＳＳ受信機は、異なるモード及び操作条件に基づいて数センチメートルから数メートルまで異なることがある精度を提供する。例えば異なるモード（これはローバＧＮＳＳ受信機若しくは仮想基準局ネットワークから特定距離内のローカル基準局を必要とする）又は高精度単独測位（ＰＰＰ）モードのいずれかで操作するＧＮＳＳ受信機は、センチメートル、デシメートル又はサブメートルレベルの精度を提供することができる。しかしＧＮＳＳ受信機は、空が見渡せて、同時に複数の人工衛星に直接視線が届かなければ、それらの精度を維持できない。その結果、ＧＮＳＳの性能は、高層ビルのある都市領域では、地下トンネルを通過している時、陸橋の下を進んでいる時、又は林冠若しくは他の障害物の下を運転している時に非常に低減する。

自動運転及びコネクテッドカーは、ＧＮＳＳのみの手法に固有の制限を克服するために、車両位置推定のためのマルチセンサ融合法を利用することがある。このようなマルチセンサ融合法は、ＧＮＳＳからのデータを慣性センサ、カメラ、及びＬｉＤＡＲデバイスなどの他のセンサシステムと組み合わせてもよい。この手法は、同様に（夜又は霧環境による）視界不良、雨（雨はＬｉＤＡＲ信号を分散させ、カメラレンズを覆う可能性がある）、及び雪（雪はカメラレンズ及びＬｉＤＡＲ送信機を遮る可能性がある）などの操作条件に基づいて性能が変わることに悩まされる。

車両位置推定に対する更なる手法は、車両を観察及び追跡するために、環境に埋め込まれた外部センサ・インフラストラクチャに依拠する。この手法は、閉鎖した若しくは制御された屋内又は屋外環境で広く行われている。しかし特化したインフラストラクチャを関心領域に設置して維持することが必要であるために、車両位置推定システムの費用及び複雑性が非常に増し、それらの適用は、必要な外部センサ・インフラストラクチャを有する特定領域に限定される。

公知の技法の欠点を克服し、追加の利点を提供するために、車両位置推定の更なる改良及び進歩を発展させることが依然として望ましい。

本章は、本開示に関連することがある、当技術分野の様々な態様を導くことを意図する。この検討は、本開示の特定の態様をより良く理解するために骨組みを提供する支援をすると考えられる。従って本章はこの観点で読むべきであり、必ずしも先行技術の許可として読むべきではないことを理解するべきである。

次に添付図面を参照して、例のみとして実施形態について記載する。

図面の簡単な説明
本明細書に開示されたようなＲＡＤＡＲに基づいた車両位置推定システム、及び車両に具備されたＲＡＤＡＲセンサによって発生されたＲＡＤＡＲデータに基づいた、対応するポイントマップで操作する車両の略図である。 図１に描かれた車両に具備された、ＲＡＤＡＲに基づいた車両位置推定システムの実施形態のブロック図である。 各ＲＡＤＡＲセンサの基準フレーム、車両基準フレーム、及び航法基準フレームに対するｘ－ｙ座標系のオーバレイを含む、図１に描かれた車両の略図である。 本明細書に開示されたような車両位置推定システム及び方法による、車両に１つ又は複数のＲＡＤＡＲセンサを提供する実施形態である。 本明細書に開示されたような車両位置推定システム及び方法による、車両に１つ又は複数のＲＡＤＡＲセンサを提供する実施形態である。 本明細書に開示されたような車両位置推定システム及び方法による、車両に１つ又は複数のＲＡＤＡＲセンサを提供する実施形態である。 本明細書に開示されたような車両位置推定システム及び方法による、車両に１つ又は複数のＲＡＤＡＲセンサを提供する実施形態である。 ＲＡＤＡＲセンサデータ、相対車両姿勢変換、及びタイムエポックを記憶するために使用する、ＦＩＦＯ構造実施形態の略図である。 複数の対象を含む既知の地図の実施形態である。 ４つの地図領域に更に分割した、図６Ａに例示された既知の地図である。 図６Ｂに描かれた南西角部の地図領域の例示である。 車両から見た際に見える及び見えない地図要素を含む、図６Ｃに描かれた地図領域の例示である。 各セルに対する確率分布関数を発生させるためにセルに更に細分化された、図６Ｄに描かれた地図領域を例示する。 異なる地図及び異なる地図領域への遷移を決定する際に使用するために、図６Ａ及び６Ｂに例示された既知の地図に対する状態遷移図の実施形態である。 図２に例示された車両位置推定システムに基づいた、ＲＡＤＡＲに基づいた車両位置推定方法のブロック図である。 図８に描かれたＲＡＤＡＲに基づいた、車両位置推定方法の更なるステップを例示するシステム及びブロック図である。 図１１に例示された試験軌跡を進む車両に使用された、高密度点群として表された地下駐車場用の既知の地図であり、車両は、本明細書の開示によるＲＡＤＡＲに基づいた車両位置推定システムの実施形態を具備する。 図１０に例示された高密度点群上の車両試験軌跡のオーバレイである。車両は、本明細書に開示されたようなＲＡＤＡＲに基づいた車両位置推定システムの実施形態を具備した試験軌跡を走行した。 図１１に例示された車両試験軌跡のコースの上に生成された未加工のＲＡＤＡＲ点群を例示する。 ノイズを除去するために処理された、図１２Ａに描かれたＲＡＤＡＲ点群を例示する。 図１２Ｂに印を付けたように、試験軌跡に離散点で一定時間（２５秒）に発生したＲＡＤＡＲデータに基づいた点群である。 図１２Ｂに印を付けたように、試験軌跡に離散点で一定時間（６０秒）に発生したＲＡＤＡＲデータに基づいた点群である。 第２の車両試験に使用された環境を例示する。環境は、屋内領域の地図、屋外領域の地図、及び屋内領域と屋外領域が相互接続するランプの２つの地図を含む、４つの地図として例示されている。本明細書に開示されたような車両位置推定システム及び方法を具備された車両は、図１５に更に例示されたような４つの領域を走行した。 図１４に例示された４つの地図上に重ねた車両軌跡を例示する。車両軌跡は、車両に提供された車両位置推定システム及び方法から生じた航法状態を反映する。

図面を通して、図に見える要素の例の１つのみ又は全てより少ない要素が、簡略にし、混乱を避けるだけのために、リード線及び基準特性によって示されていることがある。しかしこのような例では、対応する記載に従って、全ての他の例が同様に対応する記載に従って示されて包含されることが理解されよう。

詳細な説明
以下は、本明細書に開示されたような車両位置推定システム及び方法の例である。

一態様では、本明細書に開示された車両位置推定システムは、車両の第１の航法状態を示す第１のデータを発生させるように構成された第１のセンサと、車両付近の環境を示す第２のデータを発生させるように構成された第２のセンサと、第１のセンサ及び第２のセンサに通信可能に接続されたプロセッサであって、プロセッサは、環境の既知の地図に第２のデータを一致させることに基づいて第２の航法状態を発生し、第１の航法状態及び第２の航法状態に基づいて現在の航法状態を発生させるように構成された、プロセッサとを含む。

一実施形態では、プロセッサは、第２のデータに基づいて点群を発生させるように構成され、第２の航法状態は、点群と既知の地図を一致させることに基づく。一実施形態では、点群は、車両付近の対象を示す複数の反射点を含む。一実施形態では、プロセッサは、動的対象を示す反射点を除去するように構成される。一実施形態では、プロセッサは、第１のデータから引き出された相対車両姿勢変換を使用して、共通基準フレームに第２のデータを蓄積することに基づいて点群を発生させるように構成される。一実施形態では、共通基準フレームは、航法基準フレームである。

一実施形態では、第１の航法状態は、第１の車両位置及び第１の車両の向きを含む、第１の車両姿勢を含む。一実施形態では、相対車両姿勢変換は、前の車両姿勢から第１の車両姿勢への車両姿勢の変化である。

一実施形態では、プロセッサは、既知の地図に一致するように点群を変換することに基づいて、地図変換関数を発生させるように構成される。一実施形態では、第２の航法状態は、地図変換関数に基づく。一実施形態では、変換は、変換パラメータを含む反復変換である。一実施形態では、変換パラメータは、反復の最大数及び最小反復ステップを含む。一実施形態では、変換パラメータは地図解像度を含む。

一実施形態では、プロセッサは、点群と既知の地図との間の一致に基づいて一致尤度スコアを発生させるように構成される。一実施形態では、一致尤度スコアは、ベイズマッチング法すなわち点群登録方法に基づいて発生される。一実施形態では、点群登録方法は、反復最接近点方法又は正規分布変換方法である。

一実施形態では、既知の地図は、環境内の対象を表す複数の地図要素を含む。一実施形態では、複数の地図要素は、線分又は点の少なくとも１つを含む。一実施形態では、プロセッサは、第１の車両航法状態に基づいた既知の地図から見えない地図要素を除去するように構成される。一実施形態では、プロセッサは、新しい地図に隣接した境界領域内で第１の航法状態を検出することに基づいて新しい地図を獲得するように構成される。

一実施形態では、第１のセンサは、第１の基準フレーム内に第１のデータを発生させるように構成され、第２のセンサは、第２の基準フレーム内に第２のデータを発生させるように構成される。一実施形態では、プロセッサは、第１のデータ及び第２のデータを車両基準フレームに変換するように構成される。

一実施形態では、第２のセンサは測距センサであり、第２のデータは測距センサによって発生された測距データである。

一実施形態では、測距センサは、無線探知及び測距（ＲＡＤＡＲ）センサであり、測距データはＲＡＤＡＲセンサによって発生されたＲＡＤＡＲデータである。一実施形態では、ＲＡＤＡＲセンサは複数のＲＡＤＡＲセンサである。一実施形態では、ＲＡＤＡＲデータは、測距情報及び方位情報を含む。一実施形態では、ＲＡＤＡＲデータは相対速度情報を含む。

一実施形態では、測距センサは、光検出及び測距（ＬｉＤＡＲ）センサであり、測距データは、ＬＩＤＡＲセンサによって発生されたＬＩＤＡＲデータである。

一実施形態では、第１のセンサは、航法データを発生させるように構成された航法センサである。一実施形態では、航法センサは、ホイールティック・エンコーダ、慣性センサ、及びハンドルセンサを含む。一実施形態では、航法センサは、カメラを更に含む。一実施形態では、カメラは複数のカメラである。一実施形態では、航法センサは、ＧＮＳＳデバイスを更に含む。

一実施形態では、車両位置推定システムは、プロセッサに通信可能に接続されたメモリを更に含み、既知の地図はメモリに記憶される。一実施形態では、メモリは、データベース、リモートサーバ、又はクラウドサーバである。一実施形態では、メモリは、システムに対してローカル、システムに対してリモート、又はそれらの組み合わせである。

一態様では、車両位置推定のためのコンピュータ実装方法は、第１のセンサから車両の第１の航法状態を示す第１のデータを取得することと、第２のセンサから車両付近の環境を示す第２のデータを取得することと、第１のセンサ及び第２のセンサに通信可能に接続されたプロセッサを使用して、第２のデータを環境の既知の地図に一致させることに基づいて第２の航法状態を発生させることと、プロセッサを使用して、第１の航法状態及び第２の航法状態に基づいて現在の航法状態を発生させることとを含む。

一実施形態では、コンピュータ実装車両位置推定方法は、プロセッサを使用して、第２のデータを蓄積することに基づいて点群を発生させることを更に含み、第２の航法状態を発生させることは、点群を既知の地図に一致させることに基づく。

一実施形態では、コンピュータ実装車両位置推定方法は、プロセッサを使用して、第１の航法状態と前の第１の航法状態との間の航法状態の変化に基づいた相対車両姿勢変換を発生させることを更に含み、プロセッサは、第１の航法状態に関して第２のデータを蓄積するために、相対車両姿勢変換を使用することに基づいて点群を発生させる。

一実施形態では、既知の地図は複数の対象を含み、コンピュータ実装車両位置推定方法は、既知の地図を複数のセルに細分化するためにプロセッサを使用することと、対応するセルを占有する複数の対象の対応する部分集合を示す対応する確率分布関数を発生させるために、それぞれのセルを評価することとを更に含む。

一実施形態では、点群と既知の地図を一致させることは、複数のセルのそれぞれに対して対応する確率分布関数に対して点群を評価することに基づく。一実施形態では、点群を既知の地図に一致させることは、点群を繰り返し変換することと、既知の地図に一致する各反復点群の尤度を評価することとを更に含み、第２の航法状態は、既知の地図に一致する最高尤度を有する反復点群に基づく。一実施形態では、一致する尤度を評価することは、正規分布変換法を含む。

一実施形態では、現在の航法状態は、ベイズフィルタを使用して第１の航法状態と第２の航法状態を融合することに基づく。

一実施形態では、第１のセンサは航法センサであり、第２のセンサは無線探知及び測距（ＲＡＤＡＲ）センサである。一実施形態では、ＲＡＤＡＲセンサは複数のＲＡＤＡＲセンサである。

一態様は、コンピュータによって実行される時に、本明細書の開示による車両位置推定のためのコンピュータ実装方法を行う、コンピュータで実行可能な命令をその上に記憶するコンピュータ可読媒体である。

説明を単純にするために、本明細書に提供された記載及び例示は、二次元ｘ－ｙ直交座標系に関連し、概してローカルセンサ基準フレーム、車両基準フレーム、及び／又は航法基準フレームに関して表される。しかし本発明はそのように限定されず、本明細書に開示されたような車両位置推定システム及び方法は、本開示の教示から逸脱することなく、異なる自由度、異なる座標系、及び異なる基準フレームで実行されてもよいことが当業者には認識されよう。

本明細書に開示されたような車両位置推定システム及び方法は、継ぎ目のない屋内屋外の車両位置推定へのマルチセンサ法を含む。システム及び方法は、車両の航法状態を示すデータを発生させるために、車両上に第１のセンサを提供することと、車両付近の環境を示すデータを発生させるために、車両上に第２のセンサを提供することとを含む。第１のセンサ及び第２のセンサと通信可能に接続されたプロセッサは、第１及び第２の車両航法状態を発生させるように構成される。第１の車両航法状態は、第１のセンサデータに基づき、第２のセンサと無関係に引き出される。第２の車両航法状態は、第１の航法状態及び第１のセンサデータから引き出された変換関数を使用して、既知の地図と共通の基準フレームに第２のセンサデータを蓄積することに基づく。蓄積された第２のセンサデータは、第２の航法状態の推定を発生させる際に使用するために、環境の既知の地図に対して評価される。プロセッサは、次に第１及び第２の車両航法状態を評価することに基づいて、現在の車両状態を発生させる。本明細書に開示された車両位置推定システム及び方法は、ＧＮＳＳを必要とせず、周囲環境に埋め込まれた前から存在するセンサ・インフラストラクチャも必要としない。しかしそのような位置決めシステム及びセンサが利用できる場合、本明細書に開示されたシステム及び方法は、それにもかかわらずそのようなシステム及びセンサを使用してもよい。第１のセンサは、これに限定されないが、ホイールティック・エンコーダ、慣性測定ユニット、加速度計、ジャイロスコープ、１つ若しくは複数のカメラ、ハンドルセンサ、ＧＮＳＳ受信機、及びそれらの組み合わせを含む、航法センサを含んでもよい。第２のセンサは、これに限定されないが、無線探知及び測距（ＲＡＤＡＲ）センサ、光検出及び測距（ＬｉＤＡＲ）センサ、及びそれらの組み合わせを含む、飛行時間型センサを含む測距センサを含んでもよい。

本明細書に開示されたシステム及び方法は、リアルタイムで第１及び第２のセンサデータを発生させる。プロセッサは、複数のタイムエポックを表す時間のウィンドウに対するセンサデータを蓄積することがある。時間のウィンドウは、スライディング・ウィンドウが重複しないスライディング・ウィンドウである実施形態、及びスライディング・ウィンドウが重複するスライディング・ウィンドウである実施形態を含む、固定幅のスライディング・ウィンドウであってよい。プロセッサは、第１のセンサデータから直接、又は間接に、例えば第１の航法状態を発生させるために第１のセンサデータを車両運動モデルに入力することにより、第１の車両航法状態を獲得してもよい。プロセッサは、２つのタイムエポックの間の車両航法状態の変化に基づいて、車両姿勢変換関数を更に引き出してもよい。プロセッサは、第２のセンサデータに適合して蓄積するために、車両姿勢変換関数を使用する。プロセッサは、蓄積された第２のセンサデータを既知の地図と共通の基準フレームに変換するために第１の航法状態を使用する。一実施形態では、既知の地図は、航法基準フレームで表される。

一例として、プロセッサは、第１の航法状態を発生させる際に使用するために、現在のタイムエポックに対する第１のセンサデータを受信してもよい。プロセッサは、第１の航法状態と前の航法状態との間の車両航法状態の変化に基づいて、車両姿勢変換関数を更に発生させる。プロセッサは、前の航法状態に蓄積された第２のセンサデータを第１の航法状態に変換するために、車両姿勢変換関数を使用する。このやり方で、本明細書に開示されたシステム及び方法は、車両姿勢の対応する変化を使用して、第２のセンサデータを繰り返し変換することにより、第２のセンサデータを共通の基準フレームに蓄積する。

蓄積する第２のセンサデータは、例示的に雲に似ていることがある。例えばＲＡＤＡＲデータは、所与の対象に対する複数の反射点を含むことがある。そのような反射点を継時的に蓄積することにより、反射点の雲をもたらすことがあり、それによってＲＡＤＡＲデータは、対象のぼやけた又は雲状の型に似ている。そのような点群は、反射点を記載し及び／又は例示する１つの方法に過ぎない。例えば生のＲＡＤＡＲデータは、反射点を線分、多角形、又は他の構造に変換するために、処理するステップを受けてもよい。従って点群は、データを点の群として表すことに限定されず、むしろ点群は、データが点、線分、多角形、又はその他として表される、継時的なデータの蓄積を包含する。

本明細書に開示されたシステム及び方法は、点群を環境の既知の地図と比較する。比較は、既知の地図に一致させるために点群を変換することに基づく。一実施形態では、比較は、既知の地図と一致する尤度を最大にするために、異なる車両姿勢を使用して点群を変換することに基づく。一実施形態では、一致することは、反復最接近点（ＩＣＰ）又は正規分布変換（ＮＤＴ）法を含む、点群登録方法に基づく。得られる車両姿勢変換は、第２の車両航法状態の見積を特徴付ける。第１及び第２の航法状態は、現在の車両航法状態を発生させるために評価される。一実施形態では、第１及び第２の航法状態は、ベイズ法に基づいて一緒に融合される。本明細書に更に開示されたような実施形態は、確実な第２の航法状態を見積もる際に使用するために、点群が既知の地図に一致する尤度に基づいて尤度スコアを発生させることを含む。

図１及び２は、本明細書に開示されたような、ＲＡＤＡＲに基づいた車両位置推定システム及び方法を例示する。図１は、複数の対象１４２を含む環境１４０で操作する車両１１０を例示する。対象は、本来静的又は動的であってもよい。静的対象は、これに限定されないが、柱、壁、柵、塀、木、及び駐車した車両を含んでもよい。動的対象は、これに限定されないが、人々、動物、及び動いている車両を含んでもよい。車両１１０は、プロセッサ１５０に通信可能に接続された第１のセンサ１２０を含む、車両位置推定システム１００を具備する。第１のセンサ１２０は、車両の航法状態を示すデータを発生させる。図２に描かれたように、第１のセンサ１２０は、慣性センサ１２２ａ、ホイールティック・エンコーダ１２２ｂ、ハンドルセンサ１２２ｃ、ＧＮＳＳデバイス１２２ｄ、及び１つ又は複数のカメラ１２２ｅを含む、複数の航法センサ１２２を含む。他の実施形態は、加速度計及びジャイロスコープを含む慣性測定ユニットなどの独立型航法センサ１２２を含む。航法センサ１２２は、ローカルセンサ基準フレーム、車両基準フレーム、又は航法基準フレームなどの基準フレームに関する車両航法データを発生させる。車両航法データは、これに限定されないが、絶対車両位置（例えば経度、緯度、及び高度）、相対車両位置、車両配向（例えばピッチ、ロール、ヨー）、車両速度、及び車両姿勢を含んでもよい。例えばＧＮＳＳデバイス１２２ｄは、地理基準フレームに絶対車両位置を示す航法データを発生してもよく、慣性センサ１２２ａは、車両位置及び配向の変化を示す航法データを発生してもよく、ハンドルセンサ１２２ｃは、車両の向きの変更を示す航法データを発生してもよい、その他などである。プロセッサ１５０は、ローカルメモリ１５２ａ又はリモートメモリ１５２ｂなどのメモリ１５２に記憶するための、航法センサ１２２からの航法データを受信する。リモートメモリ１５２ｂは、これに限定されないが、携帯機器、データベース、リモートサーバ、又はクラウドサーバの記憶装置を含む。プロセッサは、航法データから直接又は間接に車両の第１の航法状態を発生してもよい。車両航法状態は、これに限定されないが、車両姿勢（位置及び向き）を含む。

第１の航法状態を発生させる例として、ホイールティック・エンコーダ１２２ｂは、車両の変位及び向きを含む車両航法状態を示す航法データを発生させるように構成されてもよい。プロセッサ１２２ｂは、航法データから直接、又は航法データを位置若しくは相対位置にマッピングするための車両運動モデルに基づいて、第１の航法状態を発生してもよい。車両運動モデルは、デッドレコニング・システムの場合にあり得るように、前の航法状態からの相対変化に基づいて第１の航法状態を発生させることを含んでもよい。ホイールティック・エンコーダ１２２ｂは、車輪の回転データを捕獲し、プロセッサ１５０は、車輪の回転データを車輪の既知のパラメータと組み合わせて車両航法状態の相対変化を発生させることができる。例えばプロセッサ１５０は、以下の方程式（１）－（４）に従って前の航法状態から車両の変位及び向きの相対変化を引き出すことができる。車両の変位及び向きの相対変化は、ホイールティック・エンコーダ・データ及び既知の車輪パラメータから引き出し得るように、左後輪の変位の変化Δｄ_Ｌ及び右後輪の変位の変化Δｄ_Ｒに基づく。
Δｄ_Ｌ＝Ｎ_ＬＳ_Ｌ （１）
Δｄ_Ｒ＝Ｎ_ＲＳ_Ｒ （２）

Ｎ_Ｌ及びＮ_Ｒは、左及び右車輪のそれぞれに対する前の航法状態からの車輪パルスカウントの変化である。Ｓ_Ｌ及びＳ_Ｒは、左及び右車輪のそれぞれに対する単位（ｍ／パルス）で画定されたホイールスケールである。車輪の変位の変化Δｄ_Ｌ及びΔｄ_Ｒを使用して、プロセッサ１５０は、方程式（３）及び（４）に基づいて、前の航法状態に関して車両の距離の変化Δｄ及び向きの変化Δθを決定することができる。

式中ｂは車輪又は車輪軌道の間の距離であり、これは所与の車両に対して定数である。プロセッサ１５０は、こうして前の航法状態に関して車両の距離の変化Δｄ及び車両の向きの変化Δθに基づいて第１の航法状態を発生させるために、ホイールティック・エンコーダ１２２ｂによって発生された航法データを使用することができる。

図３に例示されたように、データは、ローカルＲＡＤＡＲセンサ基準フレームＯ_ｒ１、Ｏ_ｒ２、Ｏ_ｒ３、及びＯ_ｒ４、車両基準フレームＯ_ｖ、並びに航法基準フレームＯ_ｎなどの異なる基準フレームで表されてもよい。その結果として、更なるステップは、第１及び第２のセンサデータを航法基準フレームＯ_ｎなどの異なる基準フレームに変換する必要があることがある。例えば車両の向きθは、車両基準フレームＯ_ｖのｙ軸と航法基準フレームＯ_ｎのｙ軸との間の角度として表されてもよい。従来車両基準フレームＯ_ｖが航法基準フレームＯ_ｎと一致する時に、車両の向きθは、ゼロ度である。向きの正の変化Δθは、従来時計回りの車両の回転を示す。他の慣習も可能である。記載された慣習を使用して、第１の航法状態は、方程式（５）～（７）に従って航法基準Ｏ_ｎで表されてもよい。
θ_ｋ＝θ_ｋ－１＋Δθ_ｋ－１ （５）
ｘ_ｋ＝ｘ_ｋ－１＋Δｄ_ｋ－１ｓｉｎ（θ_ｋ－１） （６）
ｙ_ｋ＝ｙ_ｋ－１＋Δｄ_ｋ－１ｃｏｓ（θ_ｋ－１） （７）

式中ｘ_ｋ及びｙ_ｋは、航法基準Ｏ_ｎの水平車両位置であり、θ_ｋは対応する車両の向きである。車両位置（ｘ，ｙ）及び向きθは、対応する車両航法状態に対する車両姿勢を集合的に表す。方程式（５）～（７）は、こうして前のタイムエポックｋ－１からの前の車両姿勢に基づいて現在の車両姿勢を発生させる。

車両１１０は、プロセッサ１５０に通信可能に接続された第２のセンサ１３０を更に具備する。第２のセンサ１３０は、車両付近の環境を示すデータを発生させる。図２及び３に具体的に例示されたように、車両１１０は、車両１１０上の４つのそれぞれの場所１１１、１１２、１１３、及び１１４に提供された、第１、第２、第３、及び第４のＲＡＤＡＲセンサ１３１、１３２、１３３、及び１３４のそれぞれを含む、第２のセンサ１３０を具備する。車両１１０上にＲＡＤＡＲセンサを提供することについての更なる実施形態は、図４Ａ～４Ｄに例示されており、それぞれは単一のＲＡＤＡＲセンサの実施形態、２つのＲＡＤＡＲセンサの実施形態、異なる２つのＲＡＤＡＲセンサの実施形態、及び３つのＲＡＤＡＲセンサの実施形態を描く。センサが車両付近の環境を走査するために車両に具備された手法は、本明細書に開示された実施形態にそれほど限定されないことが当業者には認識されよう。例えば第２のセンサ１３０は、５つ以上のＲＡＤＡＲセンサを含んでもよく、又は車両上の異なる場所にＲＡＤＡＲセンサを置いてもよい。

各ＲＡＤＡＲセンサ１３１、１３２、１３３、及び１３４は、それぞれのローカルセンサ基準フレームＯ_ｒ１、Ｏ_ｒ２、Ｏ_ｒ３、及びＯ_ｒ４に関してＲＡＤＡＲデータを発生させる際に使用するためにＲＡＤＡＲ信号を送受信する。本明細書に開示されたシステム及び方法は、対応するローカルセンサ基準フレームＯ_ｒ１、Ｏ_ｒ２、Ｏ_ｒ３、及びＯ_ｒ４からのＲＡＤＡＲデータを車両基準フレームＯ_ｖに、続いて既知の地図と比較するために航法基準フレームＯ_ｎに変換することを更に含む。変換は、ＲＡＤＡＲデータを異なる基準フレームに変換する際に使用するために、ローカルセンサ基準フレームＯ_ｒ１、Ｏ_ｒ２、Ｏ_ｒ３、及びＯ_ｒ４の構成から引き出された較正データを組み込んでもよい。較正データは、例えば車両１１０の操作前及び／又は車両１１０の操作中に獲得されてもよい。

ＲＡＤＡＲセンサ１３１、１３２、１３３、及び１３４は信号を送信し、対象１４２に反射した、送信された信号の反射を受信する。反射した信号は、ＲＡＤＡＲデータを発生させるように処理される。ＲＡＤＡＲデータは、これに限定されないが、対象の範囲、方向、角度、及び／又は相対速度などの測距データを含んでもよい。そのようにＲＡＤＡＲセンサ１３１、１３２、１３３、及び１３４は、車両１１０付近の環境１４０を示す対応するＲＡＤＡＲデータを発生させるために、車両１１０付近の対象を検出して特徴付けることができる。プロセッサ１５０は、メモリ１５２に記憶するために、及び他の車両位置推定ステップで使用するために、ＲＡＤＡＲデータを受信する。例えばプロセッサ１５０は、第２の車両航法状態を発生させる際に使用するために、点群１６０を発生させるためにＲＡＤＡＲデータを変換するように航法データを使用する。第１及び第２の車両航法状態は、現在の車両航法状態を発生させるために使用される。一実施形態では、プロセッサ１５０は、ＲＡＤＡＲデータをまず対応するローカルセンサ基準フレームＯ_ｒ１、Ｏ_ｒ２、Ｏ_ｒ３、及びＯ_ｒ４から航法センサ１２２と共通の基準フレームに変換する。

点群１６０を発生させる前に、プロセッサ１５０は、ＲＡＤＡＲデータをリファインし、フィルタリングし、又は別法により前処理してもよい。例えばプロセッサ１５０は、ノイズを除去し、外れ値データを除去し、ダウンサンプリングし、及び／又は有効な対象を同定するために、ＲＡＤＡＲデータを評価してもよい。有効な対象は静的対象のみを含んでもよい。それに応じて、本明細書に開示された実施形態は、点群１６０を発生させる前にＲＡＤＡＲデータを前処理するためにプロセッサ１５０を構成することを含む。前処理の利点には、これに限定されないが、ＲＡＤＡＲデータを記憶するために必要なメモリのサイズを低減すること、及びＲＡＤＡＲデータの品質を向上することが含まれ、それに対応して車両位置推定の見積を向上させることがある。

一実施形態では、プロセッサ１５０は、図６Ａに例示された既知の地図１８０などの既知の地図と比較するために、航法基準フレームＯ_ｎに点群１６０を発生させるように構成される。点群１６０は、車両１１０付近の環境１４０を示す、継時的に蓄積された複数の対象の反射点１６２を含む。対象の反射点１６２は、ＲＡＤＡＲデータに基づき、ＲＡＤＡＲデータは、ＲＡＤＡＲセンサ１３１、１３２、１３３、及び１３４によって検出された対象１４２上のデータを含む。プロセッサ１５０は、既知の地図１８０と同じ基準フレームに複数の対象の反射点１６２を一般的に表すように、ＲＡＤＡＲデータを航法基準フレームＯ_ｎに蓄積して変換するために航法データを使用することに基づいて点群１６０を発生させる。この手法では、複数のタイムエポックにわたって取得したＲＡＤＡＲデータは、車両航法状態の対応する変化に従って変換されて蓄積される。

一実施形態では、プロセッサ１５０は、図５の例示的実施形態に描かれたように、キューなどの先入先出（ＦＩＦＯ）データ構造１７０を使用して、データをメモリ１５２に仕切るように構成される。データ構造１７０は、データのリアルタイムのスライディング・ウィンドウとして機能することがある。データ構造１７０は、データのＮ個の基準フレーム又はユニットを保持するために、固定したウィンドウサイズ又は長さＮを有してもよい。データ構造１７０は、第１の基準フレームｉ＝０から最後の基準フレームｉ＝Ｎ－１まで連続して索引を付けられてもよく、各基準フレームはタイムエポックｔに対応する。実例として、連続したタイムエポック間の差Δｔは、センサの走査速度に反比例することがある。こうしてセンサの走査速度２０Ｈｚは、長さ０．０５秒のタイムエポックに対応することがある。それに応じて、データ構造のサイズＮ＝４０は、２秒のウィンドウを提供するはずである。データ構造のサイズＮ＝２０は、１秒のウィンドウを提供するはずである。以下同様である。その結果として、点群１６０は、特定の長さ又はウィンドウの時間にわたって獲得された対象の反射点１６２を表す。こうしてウィンドウサイズＮを選択することは、総合的な作業である。ＲＡＤＡＲセンサ１３１、１３２、１３３、及び１３４は、対象の反射点１６２の十分な密度を有するＲＡＤＡＲデータを発生させるために数回の走査が必要であることがある一方で、リアルタイムの処理及びハードウェアの制約は、データ構造１７０のサイズを限定することがある。

一実施形態では、ＲＡＤＡＲデータ１７４は、車両航法データ１７５を更なるステップで使用するために車両基準フレームＯ_ｖで表される。プロセッサ１５０は、メモリ１５２の記憶装置に複数のＲＡＤＡＲデータ基準フレームＦ_ｋ－Ｆ_{ｋ－Ｎ＋１}を含む第１のＦＩＦＯデータ構造１７１にＲＡＤＡＲデータ１７４を仕切る。各ＲＡＤＡＲデータ基準フレームＦ_ｋ－Ｆ_{ｋ－Ｎ＋１}は、対応するタイムエポックｔ_ｋ－ｔ_{ｋ－Ｎ＋１}に対して発生されたＲＡＤＡＲデータ１７４を表す。各ＲＡＤＡＲデータ基準フレームＦ_ｋ－Ｆ_{ｋ－Ｎ＋１}は、対象の反射点１６２などの可変の複数ｍ個の対象の反射点ｐを含む。例えばＲＡＤＡＲデータ基準フレームＦ_ｋは、現在のタイムエポックｔ_ｋの間に取得された複数ｍ（ｋ）の対象の反射点ｐ、すなわちＦ_ｋ＝｛ｐ_０，…，ｐ_ｍ－１｝を含む。同様にＲＡＤＡＲデータ基準フレームＦ_ｋ－１は、現在のタイムエポックｔ_ｋの前のタイムエポックｔ_ｋ－１の間に取得された複数ｍ（ｋ－１）の対象の反射点ｐ、すなわちＦ_ｋ－１＝｛ｐ_０，…，ｐ_ｍ－１｝を含む。各タイムエポックｔに対する対象の反射点ｐの数ｍは、対応するタイムエポックの間にＲＡＤＡＲセンサ１３１、１３２、１３３、及び１３４によって発生されたＲＡＤＡＲデータ１７４に依存して異なる。

航法データ１７５は、ＲＡＤＡＲデータを航法基準フレームＯ_ｎに変換する際に使用するために車両航法状態を示すデータを含む。相対車両姿勢変換は、航法状態の間の変化を評価することによって引き出されてもよい。例えば第１のタイムエポックに対する航法データ１７５は、第１の車両姿勢を含み、第２のタイムエポックに対する航法データ１７５は、第２の車両姿勢を含む。相対車両姿勢変換は、第１の車両姿勢から第２の車両姿勢までの車両姿勢の変化に基づく。それに応じて、プロセッサは、第２のタイムエポック及び第２の車両姿勢で収集されたデータを蓄積するために、第１のタイムエポック及び第１の車両姿勢で収集されたＲＡＤＡＲデータ１７４を変換する際に使用するために、タイムエポックｔの間の相対車両姿勢変換Ｔを発生させるために航法データ１７５を評価する。プロセッサ１５０は、次のタイムエポックにＲＡＤＡＲデータを連続して蓄積するために、車両姿勢変換を再帰的に引き出してもよい。この手法では、プロセッサ１５０は、メモリ１５２の記憶装置に複数の相対車両姿勢変換

～

を含む、第２のＦＩＦＯデータ構造１７２に航法データ１７５を仕切る。各車両姿勢変換

～

は、２つの連続したタイムエポックｔの間の相対姿勢変換を表す。例えば車両姿勢変換

は、タイムエポックｔ_ｋとｔ_ｋ－１との間の相対車両姿勢変換を表す。プロセッサ１５０は、現在の車両姿勢Ｐ_ｋに関して航法基準フレームＯ_ｎにＲＡＤＡＲデータ１７４を蓄積するために対応する車両姿勢変換

～

を使用して、各ＲＡＤＡＲデータ基準フレームＦ_ｋ－Ｆ_{ｋ－Ｎ＋１}を再帰的に変換してもよく、それによって以下の疑似コードにより点群１６０を発生させる。
入力：車両姿勢Ｐ_Ｋ
Ｎ個のレーダ点群基準フレームの組｛Ｆ_ｋ，…，Ｆ_{ｋ－Ｎ＋１}｝
Ｎ個の相対変換逆数の組

出力：現在の車両姿勢ＰＣ_ｋにおけるレーダ点群ＰＣ_Ｋ

２ ｆｏｒ ｉ←０ ｔｏ Ｎ－１ ｄｏ
３ エポックｋ－ｉにおける姿勢を評価する：

４ ｆｏｒ ａｌｌ ｐ∈Ｆ_ｋ－１ ｄｏ
５ 点ｐをその対応する姿勢Ｐ_ｋ－ｉに変換する：ｐ^ｋ－ｉ←Ｐ_ｋ－ｉ×ｐ
６ 点ｐ^ｋ－ｉを点群ＰＣ_ｋに加える：ＰＣ_ｋ←ＰＣ_ｋＵｐ^ｋ－ｉ
７ ｅｎｄ ｆｏｒ
８ エポックｋからエポックｋ－ｉ－１への変換を評価する：

９ ｅｎｄ ｆｏｒ

プロセッサ１５０は、対応する新しいＲＡＤＡＲデータ基準フレームを蓄積する際に使用し、新しい車両姿勢Ｐ_ｋに関して点群を表すために、新しい相対車両姿勢変換を反復して発生させる。

プロセッサ１５０は、更なる車両位置推定ステップで使用するために、点群１６０を周囲環境１４０の既知の地図１８０と比較する。本明細書に開示されたシステム及び方法は、ローカルメモリ１５２ａ又はリモートメモリ１５２ｂなどのメモリ１５２内に既知の地図１８０を予め取得して記憶してもよい。一実施形態では、既知の地図１８０はリアルタイムで取得される。一実施形態では、メモリ１５２に通信可能に接続された通信デバイス１５４は、携帯機器、データベース、リモートサーバ、又はクラウドサーバなどのリモートメモリ１５２ｂから既知の地図１８０を獲得する。プロセッサ１５０は、点群と容易に比較可能なフォーマットに既知の地図１８０を表すために、既知の地図１８０をフォーマットする必要があることがある。比較可能なフォーマットは、これに限定されないが、シェープファイルフォーマットを含む地理空間ベクトル地図又は点群を含んでもよい。

図６Ａ及び６Ｂの例示された実施形態に描かれたように、既知の地図１８０は、航法基準フレームに提供された二次元ｘ－ｙ座標系に表され、更にその上に重ねた車両１１０を含む。既知の地図１８０は複数の対象１４２を含み、４つのそれぞれの地図領域１８０ａ、１８０ｂ、１８０ｃ、及び１８０ｄに分割され、それぞれは既知の地図１８０内の異なる領域を表す。図６Ｃに具体的に例示されたように、地図領域１８０ａは、正方形の柱１４２ａ、１４２ｂ、１４２ｃ、及び１４２ｄ、２側面の障壁１４２ｅ、並びに柵１４２ｆを描く、複数の対象１４２を含み、各対象は複数の地図要素１４４、つまり線分１４６として表されている。地図要素は、これに限定されないが、線分、点、多角形、点群、及び地図に表された対象の一部若しくは全てを表す、又は別法により作成する他の構造を含んでもよい。

車両１１０は、プロセッサ１５０を含み、更に既知の地図１８０を評価するように構成された、本明細書に開示されたような車両位置推定システムを具備する。例えばプロセッサ１５０は、車両１１０がどの地図領域を占有するかを査定するために車両航法状態を評価してもよい。図６Ａ及び６Ｂに例示されたように、車両１１０は既知の地図１８０の地図領域１８０ａを占有する。プロセッサは、車両１１０に見える地図要素１４４ａの部分集合を発生させるために、車両航法状態も評価し得る。図６Ｃは地図領域１８０ａを例示し、図６Ｄは、車両航法状態に基づいて見える地図要素１４４ａ及び見えない地図要素１４４ｂの組を例示する。このような前処理ステップは、好都合なことに次の車両位置推定ステップにおけるコンピュータによる計算の複雑性を低減し、更に車両航法状態の見積を向上させることがある。更に点群１６０を見える地図要素１４４ａの組に一致させることは、既知の地図１８０に一致させるために点群１６０を変換しやすく向上させることがある。例えばＲＡＤＡＲセンサ１３１、１３２、１３３、１３４は、それらの視野内にない対象又は対象の一部に対してＲＡＤＡＲデータを発生させるそれらの機能が限定されることがある。その結果として、点群１６０は、所与の航法状態でＲＡＤＡＲセンサに見えない対象及び／又は対象の一部を本質的に除外することがある。点群１６０は、こうして見える地図要素１４４ａの組とより容易に一致させることがあり、見える地図要素１４４ａの組は、同様に同じ航法状態から車両に見えない対象及び／又は対象の一部を除外する。

車両１１０が動くにつれて、プロセッサ１５０は、新しい車両航法状態に基づいて見える地図要素１４４ａの組を再発生し若しくは更新することがある。プロセッサは、新しい地図が評価に必要かどうか、又は異なる地図領域が評価に必要かどうかを決定するために、車両航法状態も評価することがある。例えばプロセッサは、車両１１０が新しい地図に動くことを見越して、車両が既知の地図１８０の縁部又は境界領域にあるかどうかを決定するために、車両航法状態を評価することができる。同様に、プロセッサは、車両１１０が新しい地図領域若しくは新しい地図に車両が動くことを見越して、車両が地図領域の縁部又は境界領域にあるかどうかを決定するために、車両航法状態を評価することができる。地図又は異なる地図領域の間を車両が遷移すると、プロセッサ１５０は、点群１６０と比較するために使用する地図要素の組を更新する。一実施形態では、プロセッサ１５０は、車両が現在の地図と新しい地図が重なる遷移領域に留まっている間、新しい地図要素及び現在の地図要素に対して点群１６０を評価する。一実施形態では、プロセッサ１５０は、車両が現在の地図領域と新しい地図領域が重なる遷移領域に留まっている間、新しい地図要素及び現在の地図に対して点群１６０を評価する。新しい地図及び現在の地図が異なる基準フレームに提供されることがあるので、本明細書に開示されたような実施形態は、車両航法状態及び点群を新しい地図と共通の基準フレームに変換することを含む。

新しい地図は、同様にプロセッサ１５０に通信可能に接続されたローカルメモリ１５２ａに予め読み込まれ若しくは記憶されてもよく、又は携帯機器、データベース、リモートサーバ、若しくはクラウドサーバなどのリモートメモリ１５２ｂから回収されてもよい。一実施形態では、地図間及び地図領域間の関係は、状態遷移グラフなどのグラフ構造に符号化されてもよい。図７は、図６Ａ及び６Ｂに例示された既知の地図１８０に対する状態遷移グラフ１８５の例示的実施形態である。各頂点１８０ａ、１８０ｂ、１８０ｃ、及び１８０ｄは、既知の地図１８０に対応する地図領域を表す。各指向性縁部１８１、１８２、及び１８３は、地図領域の間の可能な遷移を表す。頂点１９０は、異なる既知の地図１９０を表し、指向性縁部１８４は、地図１９０と既知の地図１８０の地図領域１８０ｄとの間の可能な遷移を表す。それに応じて、車両が地図領域１８０ａ内で操作する時に、プロセッサ１５０は、車両１１０が地図領域１８０ａ内に留まっているかどうか、車両が地図領域１８０ａと地図領域１８０ｂとの間の遷移領域にあるかどうか、又は車両が地図領域１８０ａと地図領域１８０ｃとの間の遷移領域にあるかどうかを決定するために車両航法状態を評価することができる。同様に車両１１０が地図領域１８０ｃ内で操作する時に、プロセッサ１５０は、車両１１０が地図領域１８０ｃ内に留まっているかどうか、又は車両が地図領域１８０ｃと地図領域１８０ａとの間の遷移領域にあるかどうかを決定するために車両航法状態を評価することができる。以下同様である。

本明細書に開示されたシステム及び方法は、第２の車両航法状態を発生させるために、点群１６０を既知の地図１８０と比較する。比較は、既知の地図１８０に一致するように点群１６０を変換することに基づく。一実施形態では、比較は、点群１６０が既知の地図１８０と一致する尤度を最大にするために、点群１６０を反復して変換することに基づき、変換パラメータは、反復の最大数及び最小の変換の反復を含む。得られる点群の変換は、第２の航法状態を特徴付ける。点群は最初に第１の航法状態に基づいて表されるので、点群を変換することは、本来第１の航法状態の変換を反映する。一実施形態では、プロセッサは、点群１６０を既知の地図１８０と一致することから得られる変換で、第１の航法状態を変換することに基づいて第２の航法状態を発生させるように構成される。一実施形態では、プロセッサ１５０は、それぞれの反復点群の変換に対する尤度スコアを生成し、尤度スコアは、反復点群１６０が既知の地図１８０に一致する尤度を反映する。一実施形態では、更に本明細書に開示されたように、尤度スコアは、反復最接近点（ＩＣＰ）法又は正規分布変換（ＮＤＴ）を含む、点群登録法に基づく。一実施形態では、既知の地図１８０は地図領域である。

一実施形態では、プロセッサ１５０は、ベイズ法を使用して尤度スコアを評価するように構成される。一実施形態では、プロセッサは、点群登録方法を使用して尤度スコアを評価するように構成される。一実施形態では、点群登録方法は、反復最接近点法（ＩＣＰ）又は正規分布変換（ＮＤＴ）法である。例えば正規分布変換は、図６Ｅに更に例示されたように、既知の地図を複数のセル１４８に分割することを含む。各セル１４８に対する確率分布関数１４９は、各セル１４８内の対応する対象１４２を見出す確率を特徴付ける。この手法では、プロセッサは、一致の尤度を評価するために、特定のセルに対応する点群内の点をそのセルの確率分布関数で評価する。一実施形態では、複数のセルは等しい大きさの正方形である。

一実施形態では、確率分布関数は、多変量正規分布として表されてもよく、点群内の個々の点ｐが分布に属し、ひいてはセル内の対応する障害点に一致する尤度ｐ（ｘ_ｉ）は、以下の方程式（８）に従って決定されてもよい。

式中μ及びΣは、それぞれセルの内側の障害点確立密度関数の平均値及び偏差である。更に、ｘ_ｉは点ｐの座標であり、例えばｘ_ｉ＝（ｘ_ｉ，ｙ_ｉ）であり、セルの平均値μ及び偏差Σは、方程式９及び１０に従って評価されてもよい。

式中ｙ_ｊはセル内の障害点ｊの位置であり、ｍはセルの内側の障害点の数である。ＮＤＴアルゴリズムの目的は、各点の尤度関数фを最大にする手法で点群内の点を変換するために、車両の姿勢Ｐ_ｖを見出すことである。

式中ｎは点群内の点の数であり、Ｔ（Ｐ_ｖ，ｘ_ｉ）は、姿勢Ｐ_ｖを使用して点ｘ_ｉを変換するための関数である。

図８及び９は、図２に例示された車両位置推定システム１００などの、本明細書に開示されたような車両位置推定システムによりＲＡＤＡＲに基づいた車両位置推定方法２００を例示する。方法は、以下のステップ、すなわち２１０航法データを取得することと、２２０航法データに基づいて第１の車両航法状態を発生させることと、２３０ＲＡＤＡＲデータを取得することと、２４０第１の航法状態を使用して、ＲＡＤＡＲデータを蓄積することに基づいて点群を発生させることと、２５０既知の地図を前処理することと、２６０点群と既知の地図を比較することに基づいて第２の車両航法状態を発生させることと、２７０第１及び第２の車両航法状態を融合することに基づいて現在の車両航法状態を発生させることを含む。

方法ステップ２１０の実施形態は、航法センサ１２２などの１つ又は複数の航法センサから車両航法状態を示す航法データを取得することを含む。方法ステップ２２０は、ステップ２１０で獲得した航法データに基づいて、航法基準フレームに第１の車両航法状態を発生させることを含む。第１の車両航法状態は、航法データから直接獲得されることがあり、又は前の航法状態に関する変化を決定するために、デッドレコニングの方法ステップ２２２などの追加の処理ステップを必要とすることがある。方法ステップ２２４は、第１の航法状態と前の車両航法状態との間の車両姿勢の相対変化に基づいて、相対車両姿勢変換を引き出すことを含む。相対車両姿勢変換を対応するＲＡＤＡＲデータに適用することにより、点群を発生させるためのＲＡＤＡＲデータを適合させて蓄積する。一実施形態では、方法ステップ２２６は、ＦＩＦＯ構造において方法ステップ２２４で発生した相対車両姿勢変換を記憶することを含む。

ＲＡＤＡＲデータを取得する方法ステップ２３０の実施形態は、車両上に提供される１つ又は複数のＲＡＤＡＲセンサ１３１、１３２、１３５、その他から、ＲＡＤＡＲデータを取得することを含む、車両付近の環境を示す。実施形態は、方法ステップ２３２、２３４、及び／又は２３６を通して取得したＲＡＤＡＲデータを処理することを含む。方法ステップ２３２は、ＲＡＤＡＲデータを車両基準フレームに変換することを含む。方法ステップ２３４は、静的対象を同定し、動的対象に関連するＲＡＤＡＲデータを処分し、削除し、又は除去するために、ＲＡＤＡＲデータを評価することを含む。方法ステップ２３６は、ＲＡＤＡＲデータをＦＩＦＯデータ構造内に記憶することを含む。

方法ステップ２４０は、方法ステップ２２４で引き出された相対車両姿勢変換を使用して、ＲＡＤＡＲデータを蓄積すること、更に方法ステップ２２０で引き出された第１の航法状態を使用して、蓄積されたＲＡＤＡＲデータを航法基準フレームに変換することに基づいて点群を発生させる。

方法ステップ２５０は、概して既知の地図を前処理することに関し、ローカルメモリ１５２ａ又はリモートメモリ１５２ｂなどのメモリ１５２から既知の地図を回収する方法ステップ２５２を含む。実施形態は、方法ステップ２２０で引き出された第１の航法状態に基づいて既知の地図から見えない地図要素を除去するために、方法ステップ２５４を通して既知の地図を処理することを更に含んでもよい。方法ステップ２５６は、既知の地図を複数のセルに細分化することと、各セル内の障害点の対応する確率密度関数（ＰＤＦ）を発生させることとを含む。一実施形態では、各セルは、等しい幅の正方形である。

方法ステップ２６０は、方法ステップ２４０によって発生された点群を方法ステップ２５０によって出力された既知の地図と比較することを含む。比較は、既知の地図に一致させるために点群を変換することに基づく。得られる点群変換は、第２の航法状態を特徴付ける。点群は最初に第１の航法状態で表されるので、第２の航法状態は、第１の航法状態の変換を本質的に反映する。一実施形態では、方法ステップ２６０は、既知の地図に一致させるために点群を繰り返し変換することを含み、尤度スコアは、反復点群が既知の地図に一致する尤度を評価するために、各反復に対して発生される。一実施形態では、方法ステップ２６０は、点群と既知の地図が一致する尤度を最大にすることを含み、変換パラメータは、反復の最大数及び最小変換反復を含む。

方法ステップ２７０は、方法ステップ２２０によって出力された第１の航法状態及び方法ステップ２６０によって出力された第２の車両航法状態に基づいて、現在の車両航法状態を発生させることを含む。更なる実施形態は、第１と第２の車両航法状態を融合するためにベイズフィルタを使用することを含む。

図１０～１３は、本明細書の本開示による車両位置推定システムを具備した、地下駐車場で操作する車両の第１の作業例に関する。図１０は、二次元点群地図の形の地下駐車場の既知の地図を例示し、図１１は、図１０に例示された既知の地図に重ねた車両試験軌跡を含む。車両試験軌跡は、約３分間にわたって約３００メートル広がった。車両試験軌跡は開始点Ａで始まり、次いで概ね地図の西から北東角に向かう。車両は、北東角で複数点の回転を完了し、次いで南に動いた後に概ね西に向かい、終了点Ｂに向かう。

車両は、本明細書に開示されたシステム及び方法に従って車両位置を推定する際に使用するために、４つのＲＡＤＡＲセンサ及びデッドレコニング・システムを具備した。具体的には４つのＲＡＤＡＲセンサは、図３に同様に例示されたように、車両の４つの異なる角に装着された。デッドレコニング・システムは、ホイールエンコーダを備えた。図１２Ａは、車両試験軌跡の全体にわたって発生された未加工のＲＡＤＡＲ点群を例示する。図１２Ｂは、ノイズを除去するために処理された、図１２Ａに描かれたＲＡＤＡＲ点群を例示する。図１３Ａ及び１３Ｂは、図１２Ｂに１３Ａ及び１３Ｂとそれぞれの印を付けた車両軌跡に２５秒及び６０秒の２つの離散したタイムエポックに対するＲＡＤＡＲ点群を例示する。

図１４及び１５は、本明細書に開示されたような車両位置推定システム及び方法を具備した、多層式屋内屋外環境における車両操作の第２の作業例に関する。具体的には図１４は、屋内領域、屋外領域、及び屋内と屋外の領域を相互接続する２つのランプに対する４つの相互接続した地図を例示する。図１５は、車両が地図を通して進んで、実装された位置推定システム及び方法から引き出された、得られる車両軌跡を例示する。軌跡は、屋内の地図上の点Ｘで始まり、完了するループを含む。ループは、車両が屋外領域に遷移するために出口ランプに向かって概ね東に進んで継続する。ループは、屋外領域に続き、車両は入口ランプに向かって概ね西に進み、屋内領域に戻って第１のループを完了する。車両は運転を続け、２回目のループを完了し、Ｙと印を付けた地点に隣接して車両を駐車することを達成する。車両軌跡は、具体的には屋内環境と屋外環境との間に継ぎ目がない遷移を含む、異なる地図と高度との間の継ぎ目がない車両航法状態を解決するために、車両位置推定システム及び方法の機能を例示する。

前の記述では説明のために、多くの詳細が実施形態の完全な理解を提供するために記載されている。しかしこれらの特定の詳細は必要ないことが当業者には明らかになろう。他の例では、周知の電気構造及び回路は、理解を曖昧にしないためにブロック図の形で示されている。例えば特定の詳細は、本明細書に記載された実施形態が、ソフトウェアルーチン、ハードウェア回路、ファームウェア、又はそれらの組み合わせとして実装されているように提供されていない。特許請求の範囲は、本明細書に示された具体的な実施形態によって限定されるべきではなく、全体として本明細書と一致する手法に解釈されるべきである。

本開示の実施形態は、機械可読媒体（コンピュータ可読媒体、プロセッサ可読媒体、又はその中に埋め込まれたコンピュータ可読プログラムコードを有するコンピュータ使用可能媒体とも呼ばれる）に記憶されたコンピュータプログラム製品として表すことができる。機械可読媒体は、ディスケット、コンパクトディスク読み取り専用メモリ（CD-ROM）、記憶装置（揮発性若しくは不揮発性）、又は類似記憶機構を含む、磁気、光学、或いは電子記憶媒体を含む、あらゆる適切な有形の非一時的媒体であることが可能である。機械可読媒体は、命令、符号系列、構成情報、又は他のデータの様々な組を含有することができ、これらにより、実行した時にプロセッサは本開示の実施形態による方法でステップを行う。記載された実装形態を実行するために必要な他の命令及び操作も、機械可読媒体に記憶させることができることが当業者には認識されよう。機械可読媒体に記憶された命令は、プロセッサ又は他の適切な処理デバイスによって実行することができ、記載された課題を行うために回路とインターフェースをとることができる。

上記の実施形態は、例に過ぎないことを意図する。本明細書に添付された特許請求の範囲によってのみ定義される範囲から逸脱することなく、当業者によって特定の実施形態に代替、修正及び変更を行うことができる。

Claims (46)

1. 車両のための位置推定システムであって、
   前記車両の第１の航法状態を示す第１のデータを発生させるように構成された第１のセンサと、
   前記車両付近の環境を示す第２のデータを発生させるように構成された第２のセンサと、
   前記第１のセンサ及び前記第２のセンサに通信可能に接続されたプロセッサであって、
   前記環境の既知の地図に前記第２のデータを一致させることに基づいて第２の航法状態を発生し、
   前記第１の航法状態及び前記第２の航法状態に基づいて現在の航法状態を発生させるように構成された、プロセッサを含む、システム。
2. 前記プロセッサは、前記第２のデータに基づいて点群を発生させるように構成され、前記第２の航法状態は、前記点群と前記既知の地図を一致させることに基づく、請求項１に記載のシステム。
3. 前記点群は、前記車両付近の対象を示す複数の反射点を含む、請求項２に記載のシステム。
4. 前記プロセッサは、動的対象を示す反射点を除去するように構成される、請求項３に記載のシステム。
5. 前記プロセッサは、前記第１のデータから引き出された相対車両姿勢変換を使用して、共通基準フレームに前記第２のデータを蓄積することに基づいて前記点群を発生させるように構成される、請求項２～４のいずれか一項に記載のシステム。
6. 前記共通基準フレームは、前記航法基準フレームである、請求項５に記載のシステム。
7. 前記第１の航法状態は、第１の車両位置及び第１の車両の向きを含む、第１の車両姿勢を含む、請求項５又は６に記載のシステム。
8. 前記相対車両姿勢変換は、前の車両姿勢から前記第１の車両姿勢への車両姿勢の変化である、請求項７に記載のシステム。
9. 前記プロセッサは、前記既知の地図に一致するように前記点群を変換することに基づいて、地図変換関数を発生させるように構成される、請求項１～８のいずれか一項に記載のシステム。
10. 前記第２の航法状態は、前記地図変換関数に基づく、請求項９に記載のシステム。
11. 前記変換は、変換パラメータを含む反復変換である、請求項９又は１０に記載のシステム。
12. 前記変換パラメータは、反復の最大数及び最小反復ステップを含む、請求項１１に記載のシステム。
13. 前記変換パラメータは地図解像度を含む、請求項１１又は１２に記載のシステム。
14. 前記プロセッサは、前記点群と前記既知の地図との間の一致に基づいて一致尤度スコアを発生させるように構成される、請求項９～１３のいずれか一項に記載のシステム。
15. 前記一致尤度スコアは、ベイズマッチング法又は点群登録方法に基づいて発生される、請求項１４に記載のシステム。
16. 前記点群登録方法は、反復最接近点方法又は正規分布変換方法である、請求項１４に記載のシステム。
17. 前記既知の地図は、前記環境内の対象を表す複数の地図要素を含む、請求項１～１６のいずれか一項に記載のシステム。
18. 前記複数の地図要素は、線分、点、又は多角形の少なくとも１つを含む、請求項１７に記載のシステム。
19. 前記プロセッサは、前記第１の車両航法状態に基づいた前記既知の地図から見えない地図要素を除去するように構成される、請求項１７又は１８に記載のシステム。
20. 前記プロセッサは、新しい地図に隣接した境界領域内で前記第１の航法状態を検出することに基づいて、前記新しい地図を獲得するように構成される、請求項１７～１９のいずれか一項に記載のシステム。
21. 前記第１のセンサは、第１の基準フレーム内に前記第１のデータを発生させるように構成され、前記第２のセンサは、第２の基準フレーム内に前記第２のデータを発生させるように構成される、請求項１～２０のいずれか一項に記載のシステム。
22. 前記プロセッサは、前記第１のデータ及び前記第２のデータを車両基準フレームに変換するように構成される、請求項２１に記載のシステム。
23. 前記第２のセンサは測距センサであり、前記第２のデータは前記測距センサによって発生された測距データである、請求項１～２２のいずれか一項に記載のシステム。
24. 前記測距センサは、無線探知及び測距（ＲＡＤＡＲ）センサであり、前記測距データは前記ＲＡＤＡＲセンサによって発生されたＲＡＤＡＲデータである、請求項２３に記載のシステム。
25. 前記ＲＡＤＡＲセンサは複数のＲＡＤＡＲセンサである、請求項２４に記載のシステム。
26. 前記ＲＡＤＡＲデータは、測距情報及び方位情報を含む、請求項２４又は２５に記載のシステム。
27. 前記ＲＡＤＡＲデータは相対速度情報を含む、請求項２４～２６のいずれか一項に記載のシステム。
28. 前記測距センサは、光検出及び測距（ＬｉＤＡＲ）センサであり、前記測距データは、前記ＬＩＤＡＲセンサによって発生されたＬＩＤＡＲデータである、請求項２３に記載のシステム。
29. 前記第１のセンサは、航法データを発生させるように構成された航法センサである、請求項１～２８のいずれか一項に記載のシステム。
30. 前記航法センサは、ホイールティック・エンコーダ、慣性センサ、及びハンドルセンサを含む、請求項２９に記載のシステム。
31. 前記航法センサは、カメラを更に含む、請求項３０に記載のシステム。
32. 前記カメラは、複数のカメラである、請求項３１に記載のシステム。
33. 前記航法センサは、ＧＮＳＳデバイスを更に含む、請求項３０～３２のいずれか一項に記載のシステム。
34. 前記プロセッサに通信可能に接続されたメモリを更に含み、前記既知の地図は前記メモリに記憶される、請求項１～３３のいずれか一項に記載のシステム。
35. 前記メモリは、前記システムに対してローカル、前記システムに対してリモート、又はそれらの組み合わせである、請求項３４に記載のシステム。
36. 車両位置推定のためのコンピュータ実装方法であって、
    第１のセンサから車両の第１の航法状態を示す第１のデータを取得することと、
    第２のセンサから前記車両付近の環境を示す第２のデータを取得することと、
    前記第１のセンサ及び前記第２のセンサに通信可能に接続されたプロセッサを使用して、前記第２のデータを前記環境の既知の地図に一致させることに基づいて第２の航法状態を発生させることと、
    前記プロセッサを使用して、前記第１の航法状態及び前記第２の航法状態に基づいて現在の航法状態を発生させることを含む、コンピュータ実装方法。
37. 前記プロセッサを使用して、前記第２のデータを蓄積することに基づいて点群を発生させることを更に含み、前記第２の航法状態を発生させることは、前記点群を前記既知の地図に一致させることに基づく、請求項３６に記載のコンピュータ実装方法。
38. 前記プロセッサを使用して、前記第１の航法状態と前の第１の航法状態との間の航法状態の変化に基づいた相対車両姿勢変換を発生させることを更に含み、
    前記プロセッサは、前記第１の航法状態に関して前記第２のデータを蓄積するために、前記相対車両姿勢変換を使用することに基づいて前記点群を発生させる、請求項３７に記載のコンピュータ実装方法。
39. 前記既知の地図は複数の対象を含み、
    前記既知の地図を複数のセルに細分化するために前記プロセッサを使用することと、対応するセルを占有する前記複数の対象の対応する部分集合を示す対応する確率分布関数を発生させるために、それぞれの前記セルを評価することとを更に含む、請求項３７又は３８に記載のコンピュータ実装方法。
40. 前記点群と前記既知の地図を一致させることは、前記複数のセルのそれぞれに対して前記対応する確率分布関数に対して前記点群を評価することに基づく、請求項３９に記載のコンピュータ実装方法。
41. 前記点群を前記既知の地図に一致させることは、
    前記点群を繰り返し変換することと、前記既知の地図に一致する各反復点群の尤度を評価することとを更に含み、前記第２の航法状態は、前記既知の地図に一致する最高尤度を有する前記反復点群に基づく、請求項３７～４０のいずれか一項に記載のコンピュータ実装方法。
42. 前記一致する尤度を評価することは、正規分布変換法を含む、請求項４１に記載のコンピュータ実装方法。
43. 前記現在の航法状態は、ベイズフィルタを使用して前記第１の航法状態と前記第２の航法状態を融合することに基づく、請求項３６～４２のいずれか一項に記載のコンピュータ実装方法。
44. 前記第１のセンサは航法センサであり、前記第２のセンサは無線探知及び測距（ＲＡＤＡＲ）センサである、請求項３６～４３のいずれか一項に記載のコンピュータ実装方法。
45. 前記ＲＡＤＡＲセンサは、複数のＲＡＤＡＲセンサである、請求項４４に記載のコンピュータ実装方法。
46. コンピュータによって実行される時に、請求項３６～４５のいずれか一項に記載の方法を実行するコンピュータ実行可能命令をその上に記憶する、コンピュータ可読媒体。

Images