JP2020528994A - ポイントクラウドに基づく姿勢推定を用いた車両ナビゲーションシステム - Google Patents

Abstract

本開示の実施形態は、車両を位置決めするためのシステムおよび方法を提供する。このシステムは車両が軌道に沿って移動するときに、シーンに関するポイントクラウドフレームと、車両に装備されたセンサによって取り込まれた車両の初期姿勢データとを受信するように構成された通信インターフェースを含む。システムは、また、ポイントクラウドフレームおよび初期姿勢データを格納するように構成されたストレージを含む。システムは、初期姿勢データおよびポイントクラウドフレームに基づいて、ポイントクラウドフレームのそれぞれに関連付けられた車両の姿勢情報を推定するように構成されたプロセッサをさらに含む。プロセッサは、モデルに基づいて車両の推定姿勢情報を調整するようにも構成される。モデルは、複数のポイントクラウドフレーム間の空間的関係および時間的関係を含む。プロセッサは、調整された姿勢情報に基づいて車両を位置決めするようにさらに構成される。【選択図】図７

Description

技術分野
本開示は車両姿勢を推定するためのナビゲーション装置および方法に関し、より詳細には、ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ Ａｎｄ Ｒａｎｇｉｎｇ）およびナビゲーションセンサを使用して車両の姿勢を推定するためのナビゲーション装置および方法に関する。

背景
自律駆動技術は、正確な地図に大きく依存している。例えば、ナビゲーションマップの精度は、位置決め、雰囲気認識、意思決定、および制御などの自律走行車両の機能にとって重要である。高分解能マップは車両が走行するときに、車両上の様々なセンサ、検出器、および他のデバイスによって取得される画像および情報を集めることによって取得され得る。例えば、車両は、ＬｉＤＡＲレーダ、全地球測位システム（ＧＰＳ）受信機、１つ以上の慣性測定ユニット（ＩＭＵ）センサ、および１つ以上のカメラなどの複数の統合センサを備え、車両が運転している道路または周囲の物体の特徴を取り込むことができる。取り込まれたデータは、例えば、車線の中心線または境界線座標、建物、別の車両、標識、歩行者、または交通標識などの物体の座標および画像を含むことができる。

高分解能マップは、調査車両の対応する３次元（３−Ｄ）姿勢情報（例えば、位置および向き）に基づいて複数のポイントクラウドフレームを集約することによって取得することができるので、高分解能マップの精度は、ポイントクラウドデータを取り込む際の車両の位置決めの精度に大きく依存する。いくつかの既知のナビゲーションシステムは、ＧＰＳ、ＩＭＵ、および無線基地局を利用して、通常の状態で約１０ｃｍの精度で車両姿勢推定を提供する。しかし、複雑な都市環境、特にＧＰＳ衛星信号が乏しい環境（例えば、高層ビル、オーバーパスなどによってブロックされる）では、測位精度が大幅に低下する可能性がある。他の既知のナビゲーションシステムは、測位精度を改善するためにリアルタイムポイントクラウドを導入しようと試みるが、依然としてＧＰＳ衛星信号の不良による不正確さに悩まされている。測位精度の損失は、調査車両によって使用される既存のナビゲーションシステムによって排除することができない測位誤差の蓄積を引き起こし得る。その結果、調査車両が同じシーンを繰り返し通過する場合、得られる高分解能マップは、異なり得る（すなわち、同じシーンの不一致のポイントクラウドに起因する、いわゆる「グローバル不整合」）。

本開示の実施形態は、車両を位置するための改善されたナビゲーションシステムおよび方法によって、上記の問題に対処する。

本開示の実施形態は、車両を位置決めするためのナビゲーションシステムを提供する。前記システムは、前記車両が軌道に沿って移動するときに、前記車両に装備された複数のセンサによって取り込まれた前記車両のシーンおよび初期姿勢データに関する複数のポイントクラウドフレームを受信するように構成された通信インターフェースを含むことができる。前記システムは、前記複数のポイントクラウドフレームおよび前記初期姿勢データを格納するように構成されたストレージも含むことができる。前記システムは、前記初期姿勢データおよび前記複数のポイントクラウドフレームに基づいて、前記複数のポイントクラウドフレームのそれぞれに関連付けられた前記車両の姿勢情報を推定するように構成されたプロセッサをさらに含むことができる。前記プロセッサは、モデルに基づいて前記車両の推定された前記姿勢情報を調整するように構成されてもよい。前記モデルは、前記複数のポイントクラウドフレーム間の空間的関係および時間的関係を含む。前記プロセッサは、調整された前記姿勢情報に基づいて前記車両を位置決めするようにさらに構成されてもよい。

本開示の実施形態はまた、車両を位置決めする方法を提供する。前記方法は、前記車両が軌道に沿って移動するときに、前記車両に装備された複数のセンサによって取り込まれた前記車両のシーンおよび初期姿勢データに関する複数のポイントクラウドフレームを受信するステップを含むことができる。前記方法は、また、プロセッサによって、前記初期姿勢データおよび前記複数のポイントクラウドフレームに基づいて、前記複数のポイントクラウドフレームのそれぞれに関連付けられた前記車両の姿勢情報を推定するステップを含むことができる。前記方法は、前記プロセッサによって、モデルに基づいて前記車両の推定された前記姿勢情報を調整するステップ前記モデルは、前記複数のポイントクラウドフレーム間の空間的関係および時間的関係を含む。前記方法は、調整された前記姿勢情報に基づいて前記車両を位置決めするステップをさらに含むことができる。

本開示の実施形態は、１以上のプロセッサによって実行されると、前記１以上のプロセッサに動作を実行させる命令が格納された非一時的コンピュータ可読媒体をさらに提供する。前記動作は、車両が軌道に沿って移動するときに、前記車両に装備された複数のセンサによって取り込まれた前記車両のシーンおよび初期姿勢データに関する複数のポイントクラウドフレームを受信することを含むことができる。前記動作は、前記初期姿勢データおよび前記複数のポイントクラウドフレームに基づいて、前記複数のポイントクラウドフレームのそれぞれに関連付けられた前記車両の姿勢情報を推定することを含むことができる。前記動作は、モデルに基づいて前記車両の推定された前記姿勢情報を調整することをさらに含むことができる。前記モデルは、前記複数のポイントクラウドフレーム間の空間的関係および時間的関係を含む。前記動作は、調整された前記姿勢情報に基づいて前記車両を位置決めすることをさらに含むことができる。

当然ながら、上記の一般的記載と下記の詳細な記載単に例示的かつ説明的なものであり、主張されているように発明を制限するものではない。

図１は、本発明の実施形態による、センサを有する例示的な車両の概略図を示す。 図２は、本開示の実施形態による、ポイントクラウドに関連する姿勢情報を決定するための例示的なコントローラのブロック図を示す。 図３は、本開示の実施形態による、軌道補間の前後の例示的なポイントクラウドフレームを示す。 図４は、本開示の実施形態による、正規分布変換（ＮＤＴ）および無香カルマンフィルタ（ＵＫＦ）を使用して推定された姿勢情報に基づいてポイントクラウドフレームから集約された例示的なポイントクラウドを示す。 図５は、本開示の実施形態による例示的な姿勢グラフを示す。 図６は、本開示の実施形態による、最適化されていない姿勢情報および最適化された姿勢情報に関連する例示的なポイントクラウドを示す。 図７は、本開示の実施形態による、ポイントクラウドに関連する姿勢情報を提供する例示的な方法のフローチャートを示す。

詳細な説明
ここで、例示的な実施形態を詳細に参照し、その例を添付の図面に示す。図面において、同一の参照符号は、可能な限り同一または類似の構成要素に用いる。

図１は、本開示の実施形態による、複数のセンサ１４０、１５０、および１６０を有する例示的な車両１００の概略図を示す。いくつかの実施形態と一致して、車両１００は、高分解能地図または３次元都市モデリングを構築するためのデータを取得するように構成された調査車両であってもよい。車両１００は、電気車両、燃料電池車両、ハイブリッド車両、または従来の内燃機関車両であり得ることが企図される。車両１００は、本体１１０と、少なくとも１つの車輪１２０とを有することができる。本体１１０は、スポーツビークル、クーペ、セダン、ピックアップトラック、ステーションワゴン、スポーツユーティリティビークル（ｓｐｏｒｔｓ ｕｔｉｌｉｔｙ ｖｅｈｉｃｌｅ：ＳＵＶ）、ミニバン（ｍｉｎｉｖａｎ）、またはコンバージョンバン（ｃｏｎｖｅｒｓｉｏｎ ｖａｎ）などの任意の本体スタイルとすることができる。いくつかの実施形態では図１に示すように、車両１００は、一対の前輪および一対の後輪を含むことができる。しかしながら、車両１００は、車両１００が移動することを可能にするより少ない車輪または同等の構造を有し得ることが企図される。車両１００は、全輪駆動（ＡＷＤ）、前輪駆動（ＦＷＲ）、または後輪駆動（ＲＷＤ）であるように構成され得る。いくつかの実施形態では、車両１００は、車両を占有するオペレータによって、遠隔制御および／または自律的に操作されるように構成されてもよい。

図１に示すように、車両１００は、取付け機構１３０を介して本体１１０に取り付けられたセンサ１４０および１６０を備えることができる。取付構造１３０は、車両１００の本体１１０に取り付けられるか、もしくは取り付けられる電気機械装置であってもよい。いくつかの実施形態では、取付構造１３０は、ネジ、接着剤、または別の取り付け機構を使用してもよい。車両１００は、任意の適切な取り付け機構を使用して、本体１１０の内側または外側にセンサ１５０をさらに備えることができる。各々のセンサ１４０、１５０、または１６０は、車両１００に装備され得る方法は図１に示される実施形態によって限定されず、望ましい感知性能を達成するために、センサ１４０〜１６０および／または車両１００のタイプに応じて変更され得ることが企図される。

いくつかの実施形態では、センサ１４０〜１６０は、車両１００が軌道に沿って移動するときにデータを取り込むように構成されてもよい。本開示と一致して、センサ１４０は、周囲を走査してポイントクラウドを取得するように構成されたＬｉＤＡＲスキャナ／レーダとすることができる。ＬｉＤＡＲは、パルスレーザ光でターゲットを照射し、反射パルスをセンサで測定することによって、ターゲットまでの距離を測定する。次いで、レーザ戻り時間および波長の差を使用して、ターゲットのデジタル３Ｄ表現を作成することができる。ＬｉＤＡＲ走査に使用される光は、紫外線、可視光、または近赤外線であってもよい。狭いレーザビームは、非常に高い分解能で物理的特徴をマッピングすることができるので、ＬｉＤＡＲスキャナは、高分解能地図調査に特に適している。いくつかの実施形態では、ＬｉＤＡＲスキャナは、ポイントクラウドを取り込むことができる。車両１００が軌道に沿って移動するとき、センサ１４０は、複数の時点で一連のポイントクラウド（それぞれ、ある時点で取得されたポイントクラウドフレームとして知られている）を取得することができる。

図１に示されるように、車両１００は、ＧＰＳ受信機および１以上のＩＭＵセンサなどの、車両１００の測位のための航法ユニットで使用されるセンサを含み得るセンサ１５０をさらに備え得る。ＧＰＳは、地理的位置および時間情報をＧＰＳ受信機に提供するグローバルナビゲーション衛星システムである。ＩＭＵは、加速度計およびジャイロスコープ、時には磁力計などの各種慣性センサを使用して、車両の特定の力、角速度、時には車両を取り巻く磁界を測定し、提供する電子デバイスである。ＧＰＳ受信機とＩＭＵセンサとを組み合わせることによって、センサ１５０は、各時点における車両１００の位置および方向（例えば、オイラー角）を含む、移動中の車両１００のリアルタイム姿勢データを提供することができる。

本開示と一致して、車両１００は、デジタル画像を取り込むように構成されたセンサ１６０をさらに備えることができる。いくつかの実施形態では、センサ１６０は、写真を撮影するか、またはそうでなければ画像データを収集するカメラを含むことができる。例えば、センサ１６０は、単眼カメラ、両眼カメラ、またはパノラマカメラを含むことができる。センサ１６０は、車両１００が軌道に沿って移動すると、複数の画像（それぞれ画像フレームとして知られている）を取得することができる。各画像フレームは、ある時点でセンサ１６０によって取得されてもよい。

本発明によれば、車両１００は、車両１００の本体１１０内にローカルコントローラ１７０を含んでもよいし、ポイントクラウドに基づく車両姿勢推定のためにリモートコントローラ（図１には示されていない）と通信してもよい。本開示と一致して、コントローラ１７０による姿勢推定は、精度の改善および誤差累積の低減を提供する。いくつかの実施形態では、ポイントクラウドの精度を改善するために、各ポイントクラウドフレームのサンプリング時点に基づく軌道補間は、複数のポイントクラウドフレームを集約する際に使用され得る。いくつかの実施形態では、リアルタイム姿勢情報のより良い推定精度のために、非線形関数自体ではなく、非線形関数の確率密度分布を推定し、非線形関数の高次成分を保存するために、無香カルマンフィルタ（Ｕｎｓｃｅｎｔｅｄ Ｋａｌｍａｎ Ｆｉｌｔｅｒ）（ＵＫＦ）などの方法を実施することができる。いくつかの実施形態では、閉ループテストが誤差累積を低減するために、各ポイントクラウドフレームに関連付けられた姿勢情報を最適化する際に、例えば、姿勢グラフ最適化において使用することができる。さらに、リアルタイムの車両姿勢情報を使用して高精度の測位を提供する一方で、ローカルコントローラ１７０および／またはリモートコントローラは、また、複数のポイントクラウドフレームをそれらの関連する姿勢情報に基づいて集約することによって、高精度のポイントクラウドデータを提供することができる。

例えば、図２は、本発明の実施形態による、ポイントクラウドに関連する姿勢を決定するための例示的なコントローラ２００の構成図を示す。本開示と一致して、コントローラ２００は、車両姿勢推定のために様々なタイプのデータを使用することができる。様々なタイプのデータは車両１００が軌道に沿って移動するときに、周囲のシーンに関して車両１００に装備されたセンサ１４０〜１６０によって取り込まれ得る。データは様々な時点における複数のポイントクラウドフレームからなるセンサ１４０（例えば、ＬｉＤＡＲスキャナ）によって取り込まれたポイントクラウド２０１を含むことができる。データは、また、センサ１５０（例えば、ＧＰＳ受信機および／または１つ以上のＩＭＵセンサ）によって取得された車両１００の初期姿勢データ２０３を含んでもよい。いくつかの実施形態では、ポイントクラウド２０１は、ＧＰＳ受信機およびＩＭＵセンサからの初期姿勢データ２０３に基づいて、ローカル座標系からのネイティブＬｉＤＡＲデータをグローバル座標系（例えば、経度／緯度座標）に変換することによって較正され得る。センサ１６０によって撮影されたデジタル画像および／または無線基地局（図示せず）からの測位信号などの、さらなるタイプのデータが、車両姿勢推定のためにコントローラ２００に提供され得ることが企図される。

いくつかの実施形態では、図２に示すように、コントローラ２００は、通信インターフェース２０２、プロセッサ２０４、メモリ２０６、およびストレージ２０８を含むことができる。いくつかの実施形態では、コントローラ２００は、集積回路（ＩＣ）チップ（特定用途向け集積回路（ＡＳＩＣ）またはフィールドプログラマブルゲートアレイ（ＦＰＧＡ）として実装される）、または専用機能を有する別個のデバイスなど、単一のデバイス内に異なるモジュールを有することができる。いくつかの実施形態では、コントローラ２００の１以上のコンポーネントは、車両１００の内部に配置されてもよく（例えば、図１のローカルコントローラ１７０）、あるいはモバイル装置、クラウド、または別の遠隔地にあってもよい。コントローラ２００のコンポーネントは、統合された装置内にあってもよく、または異なる場所に分散されてもよいが、ネットワーク（図示せず）を介して互いに通信する。例えば、プロセッサ２０４は、オンボード車両１００のプロセッサ、モバイル装置内のプロセッサ、クラウドプロセッサ、またはそれらの任意の組合せとすることができる。

通信インターフェース２０２は、通信ケーブル、無線ローカルエリアネットワーク（ＷＬＡＮ）、広域ネットワーク（ＷＡＮ）、電波などの無線ネットワーク、全国的なセルラーネットワーク、および／またはローカル無線ネットワーク（例えば、Ｂｌｕｅｔｏｏｔｈ（登録商標）またはＷｉＦｉ）、または他の通信方法を介して、センサ１４０〜１６０などの成分との間でデータを送受信することができる。いくつかの実施形態では、通信インターフェース２０２は、ＩＳＤＮ（ｉｎｔｅｇｒａｔｅｄ ｓｅｒｖｉｃｅｓ ｄｉｇｉｔａｌ ｎｅｔｗｏｒｋ）カード、ケーブルモデム、衛星モデム、またはデータ通信接続を提供するモデムとすることができる。別の例として、通信インターフェース２０２は、互換性のあるＬＡＮへのデータ通信接続を提供するローカルエリアネットワーク（ＬＡＮ：ｌｏｃａｌ ａｒｅａ ｎｅｔｗｏｒｋ）カードであり得る。無線リンクは、通信インターフェース２０２によっても実現できる。そのような実装形態では、通信インターフェース２０２は、ネットワークを介して様々なタイプの情報を表すデジタルデータストリームを搬送する電気信号、電磁信号、または光信号を送受信することができる。

いくつかの実施形態と一致して、通信インターフェース２０２は、ポイントクラウド２０１および初期姿勢データ２０３を含む、センサ１４０および１５０によって取り込まれたデータを受信し、受信したデータを、記憶のためにメモリ２０６およびストレージ２０８に、または処理のためにプロセッサ２０４に提供することができる。通信インターフェース２０２は、また、プロセッサ２０４によって生成された最適化された姿勢情報および関連付けられたポイントクラウドを受信し、ネットワークを介して車両１００内の任意のローカルコンポーネントまたは任意のリモートデバイスに姿勢情報および関連付けられたポイントクラウドを提供することができる。

プロセッサ２０４は、任意の適切なタイプの汎用または専用マイクロプロセッサ、デジタル信号プロセッサ、またはマイクロコントローラを含み得る。プロセッサ２０４は、車両姿勢の推定に専用の別個のプロセッサモジュールとして構成されてもよい。あるいは、プロセッサ２０４は、車両姿勢推定とは関係のない他の機能を実行するための共有プロセッサモジュールとして構成されてもよい。

図２に示すように、プロセッサ２０４は、ポイントクラウド補間ユニット２１０、姿勢情報推定ユニット２１２、姿勢情報最適化ユニット２１４、ポイントクラウド集約ユニット２１６等のような多数のモジュールを含むことができる。これらのモジュール（および任意の対応するサブモジュールまたはサブユニット）は他のコンポーネントと共に使用するために、またはプログラムの一部を実行するために設計されたプロセッサ２０４のハードウェアユニット（例えば、集積回路の一部）であり得る。プログラムは、コンピュータ可読媒体に格納することができ、プロセッサ２０４によって実行されると、１つまたは複数の機能を実行することができる。図２は、ユニット２１０〜２１６の全てを１つのプロセッサ２０４内に示しているが、これらのユニットは互いに近くまたは遠隔に配置された複数プロセッサに分散されてもよいと考えられる。

ポイントクラウド補間ユニット２１０は、初期姿勢データ２０３に基づく軌跡補間を使用して、ポイントクラウド２０１内の複数のポイントクラウドフレームを処理するように構成され得る。いくつかの実施形態では、ポイントクラウドフレームは、複数のシャッタタイムスタンプ（サンプリング時点）に関連付けられてもよく、したがって、それぞれがシャッタタイムスタンプに関連付けられた複数のセグメントを含んでもよい。ポイントクラウド補間ユニット２１０は、軌跡補間を使用してポイントクラウドフレームの各セグメントに対して姿勢変換を実行して、より高い精度で処理されたポイントクラウドフレームを生成するように構成され得る。例えば、ポイントクラウドフレームの各セグメントにおける追加のデータ点は、車両１００の初期姿勢データ２０３によって示される車両１００の軌道に基づく線形補間を使用して構築および挿入されてもよい。例えば、図３は、本発明の実施形態による、軌道補間の前後の例示的なポイントクラウドフレームを示す。３１０は、ギザギザの転位を伴う軌道補間の前のポイントクラウドフレームおよびその拡大領域を示す。対照的に、３２０は、ポイントクラウド補間ユニット２１０によって処理された軌道補間後のポイントクラウドフレームおよびその拡大された領域を示す。３２０におけるポイントクラウドフレームの滑らかさは、軌跡補間を使用するデータ点の加算により改善される。

図２に戻って参照すると、姿勢情報推定ユニット２１２は、最初の姿勢データ２０３およびポイントクラウドフレームに基づいて、ポイントクラウドフレームのそれぞれに関連付けられた車両１００の姿勢情報を推定するように構成され得る。推定された姿勢情報は、ポイントクラウドフレーム（つまり、ポイントクラウド姿勢情報）に基づく推定と、初期姿勢データに基づく推定とをマージすることによって取得され得る。いくつかの実施形態では、姿勢情報推定ユニット２１２は、最初に、ポイントクラウドフレームに基づいて、車両１００のポイントクラウド姿勢情報を推定することができる。例えば、姿勢情報推定ユニット２１２は、各ポイントクラウドフレームにおけるポイントの３Ｄ座標とそれらの関連する属性（例えば、反射レーザ強度）とに基づいて、隣接するポイントクラウドフレーム間の姿勢変化を計算することができる。一例では、４次元（４−Ｄ）正規分布変換（ＮＤＴ）を使用して、Ｘ、Ｙ、およびＺ座標ならびに各点の反射層強度に基づいて姿勢変化を計算することができる。４−Ｄ ＮＤＴは、単一のポイントクラウドフレームから再構成された３−Ｄ点の離散セットを、３−Ｄ空間において定義された区分的連続かつ微分可能な確率密度に伝達する。確率密度は、容易に計算できる１組の正規分布から構成することができる。確率密度分布は、対応するポイントクラウドフレームのポイントクラウド姿勢情報を表すために使用され得る。

いくつかの実施形態では、姿勢情報推定ユニット２１２は、ポイントクラウド姿勢情報および初期姿勢データ２０３に基づいて、各ポイントクラウドフレームに関連付けられた姿勢情報を推定することができる。例えば、ポイントクラウド姿勢情報と初期姿勢データ２０３は、ＵＫＦ方法を用いてマージすることができる。ＵＫＦ方法は、無香変換（ＵＴ）として知られる決定論的サンプリング技方法を使用して、事後平均の周りのサンプル点の最小セット（シグマ点と呼ばれる）を選ぶ。次いで、シグマ点は、非線形関数を通して伝搬され、それから、新しい事後平均および共分散推定値が形成される。結果として得られるＵＫＦフィルタは、真の平均および共分散をより正確に推定することができる。ＵＫＦは、任意の非線形性について、事後平均および共分散を３次（テイラー級数展開）に正確に決定することができる。対照的に、姿勢推定のためにいくつかの既存のナビゲーションシステムによって使用されている拡張カルマンフィルタ（ＥＫＦ）は、ＵＫＦのそれと同じ順序で計算複雑性で１次精度しか達成しない。

姿勢推定に４−Ｄ ＮＤＴおよびＵＫＦを適用することにより、姿勢情報推定ユニット２１２は、処理速度を大きく犠牲にすることなく、非線形関数の確率密度分布を使用し、非線形関数の高次成分を保存して、より高い推定精度を達成することができる。いくつかの実施形態では、ポイントクラウドフレームに関連付けられた推定姿勢情報は、ポイントクラウドフレーム集約およびセグメンテーションのために使用され得る。例えば、図４は、本発明の実施形態による、ＮＴＤおよびＵＫＦを用いて推定された姿勢情報に基づいてポイントクラウドフレームから集約された例示的なポイントクラウドを示す。４１０は、個々のポイントクラウドフレームを示し、４２０は、ＮＴＤおよびＵＫＦを使用して推定された姿勢情報に基づいて隣接するポイントクラウドフレームと集約されたポイントクラウドフレームを示す。

図２に戻って参照すると、姿勢情報最適化ユニット２１４は、モデルに基づいて各々のポイントクラウドフレームに関連付けられた車両１００の推定された姿勢情報を調整するように構成され得る。モデルは、ポイントクラウドフレーム間の空間的関係および時間的関係を含むことができる。モデルは、閉ループ試験を適用して、累積された位置決め誤差を低減し、最終的に、各ポイントクラウドフレームに関連する最適化された姿勢情報を提供して、累積された位置決め誤差を排除することができる。いくつかの実施形態では、姿勢情報最適化ユニット２１４は、最初に、空間関係および時間関係ならびにしきい値のセットに基づいて、ポイントクラウドフレームを複数のバッチに分割することができる。例えば、閾値のセットは、最小時間隔ｔ、最小位置変化ｘ、および最小方位変化ｙを含むことができる。ポイントクラウドフレームの分割は、空間閾値ｘおよびｙならびに時間閾値ｔに対するそれらの空間変化および時間変化に基づくことができる。一例では、ｔ未満の時間隔、ｘ未満の位置変化、およびｙ未満の方向変化を有する任意のポイントクラウドフレームは同じバッチ内にある。最適化のために隣接するポイントクラウドフレームを同じバッチに配置することによって、計算の複雑さを低減することができる。

いくつかの実施形態では、姿勢情報最適化ユニット２１４によって使用されるモデルは、各バッチにおけるポイントクラウドフレームの姿勢をそれぞれ表す複数のノードを含む姿勢グラフとすることができる。一例では、ポイントクラウドフレームは、バッチ内の最も早い時点を有するフレームとすることができる。姿勢情報最適化ユニット２１４は、姿勢グラフのコスト関数の値を低減するように、姿勢グラフの各ノードの姿勢情報を調整してもよい。姿勢グラフのコスト関数値が最小化されると、姿勢グラフの各ノードの対応する姿勢情報は、その最適化された値に達する。

図５は、本開示の実施形態による例示的な姿勢グラフ５００を示す。姿勢グラフ５００の各ノードは、ポイントクラウドフレームのバッチを表し、バッチ内の第１のポイントクラウドフレームの姿勢情報（例えば、最も早い時点を有するもの）に関連付けられる。姿勢グラフ５００は、３つのタイプのエッジを含む：（１）空間的に隣接しているが、時間的に隣接していない２つのバッチは閉ループを形成する。２つの閉ループバッチ間の推定ポイントクラウド姿勢情報（例えば、４−Ｄ ＮＤＴを使用する）の変化は、２つのノード間の２バッチエッジ（破線矢印として示される）として表される。例えば、バッチ０およびバッチ２は閉ループを形成し、２バッチエッジによって接続され、バッチ１およびバッチｉ−１も同様である。（２）各バッチについて、推定された姿勢（例えば、ＵＫＦを使用して取得された前の姿勢）は、ノードの１つのバッチのエッジ（円の矢印として示される）として表される。例えば、図５の各々のバッチは、それ自体の１バッチエッジを有する。（３）姿勢グラフ５００内の２つの隣接するバッチは、初期姿勢データ２０３（例えば、ＵＫＦを使用して取得された相対的な前の姿勢）に基づく推定された姿勢情報の変化を表す別の２バッチエッジ（実線の矢印として示される）によって接続される。いくつかの実施形態では、総相対的事前姿勢は、姿勢グラフの各ノードの姿勢情報を最適化するためのコスト関数として使用することができる。

図６は、本開示の実施形態による、最適化されていない姿勢情報および最適化された姿勢情報に関連する例示的なポイントクラウドを示す。６１０は、最適化されていない姿勢情報に関連するポイントクラウドを示し、６２０は、最適化された姿勢情報に関連する同じポイントクラウドを示す。図５に示すように、姿勢グラフを用いて姿勢の最適化を行う。

図２に戻って参照すると、ポイントクラウド集約ユニット２１６は、調整された姿勢情報をそれぞれのポイントクラウドフレームに登録し、それらの調整された姿勢情報に基づいてポイントクラウドフレームを集約するように構成され得る。すなわち、高精度測位のために車両１００の姿勢を推定することに加えて、コントローラ２００は、高精度のポイントクラウドデータを提供するために、関連する姿勢情報に基づいてポイントクラウドフレームを集約することもできる。したがって、各ポイントクラウドフレームは、車両１００の位置および向き、ならびにポイントクラウドフレームが取り込まれる時点などの最適化された姿勢情報に登録することができる。いくつかの実施形態では、空間において隣接するポイントクラウドフレームは、それらの最適化された姿勢情報に基づいて集約され得る。

図２を再び参照すると、メモリ２０６およびストレージ２０８は、プロセッサ２０４が動作する必要がある任意のタイプの情報を記憶するために提供される任意の適切なタイプの大容量記憶装置を含むことができる。メモリ２０６およびストレージ２０８は、ＲＯＭ、フラッシュメモリ、ダイナミックＲＡＭ、およびスタティックＲＡＭを含むがこれらに限定されない、揮発性または不揮発性、磁気、半導体、テープ、光、リムーバブル、非リムーバブル、または他のタイプのストレージ装置または有形（すなわち、非一時的）コンピュータ可読媒体とすることができる。メモリ２０６および／またはストレージ２０８は、本明細書で開示される車両姿勢推定機能を実行するためにプロセッサ２０４によって実行され得る１つまたは複数のコンピュータプログラムを記憶するように構成され得る。例えば、メモリ２０６および／またはストレージ２０８は、車両１００が軌道に沿って移動するときに様々なタイプのデータを取り込むようにセンサ１４０〜１６０を制御するためにプロセッサ２０４によって実行され得るプログラム（複数可）を記憶し、取り込まれたデータを処理して車両１００の姿勢情報を推定するように構成され得る。

メモリ２０６および／またはストレージ２０８は、プロセッサ２０４によって使用される情報およびデータを記憶するようにさらに構成され得る。例えば、メモリ２０６および／またはストレージ２０８は、センサ１４０〜１６０によって取り込まれた様々なタイプのデータおよび推定された姿勢情報を格納するように構成され得る。様々なタイプのデータは永久的に格納されてもよいし、定期的に除去されてもよいし、データの各フレームが処理された直後に無視されてもよい。

図７は、本開示の実施形態による、ポイントクラウドに関連付けられた推定姿勢情報に基づいて車両１００を位置決めするための例示的な方法７００のフローチャートを示す。例えば、方法７００は、とりわけ、コントローラ２００およびセンサ１４０および１５０を含む車両１００の車両姿勢推定システムによって実施されてもよい。しかし、方法７００は、その例示的な実施形態に限定されない。方法７００は、以下に説明するステップＳ７０２〜Ｓ７１８を含むことができる。ステップのいくつかは、本明細書で提供される開示を実行するために任意選択であり得ることを理解されたい。さらに、ステップのいくつかは、同時に、または図７に示す順序とは別の順序で実行することができる。

ステップＳ７０２において、車両１００が軌道に沿って移動すると、車両１００のシーンおよび初期姿勢データに関する複数のポイントクラウドフレームは、車両１００に関連付けられたセンサ１４０および１５０によって取り込まれ得る。例えば、車両１００に装備されたＬｉＤＡＲスキャナは、シーンの３Ｄ情報を表すポイントクラウドフレームを取り込むことができる。車両１００に装備されたＧＰＳ受信機および１つまたは複数のＩＭＵセンサは、時間、位置、および向きを含む車両１００の初期姿勢データを取得することができる。いくつかの実施形態では、データを取り込む前に、センサ１４０および１５０を較正して、相対姿勢変換を得ることができる。

ステップＳ７０４において、ポイントクラウドフレームは、プロセッサ２０４によって、初期姿勢データに基づく軌跡補間を用いて処理される。初期姿勢データは、車両１００の軌道を示すために使用することができる。区分的定数補間、線形補間、スプライン補間などの各種補間手法を使用して、軌跡およびサンプリング時点に基づいてポイントクラウドフレームの範囲内で新しいデータ点を構築することができる。新しいデータポイントは、ポイントクラウドの滑らかさを改善するためにポイントクラウドフレームを補足することができる。

ステップＳ７０６において、ポイントクラウド姿勢情報は、ポイントクラウドフレームに基づいて、プロセッサ２０４によって推定され得る。ポイントクラウド姿勢情報推定は、ポイントクラウドフレーム内の各点の３Ｄ座標および関連する属性（例えば、反射レーザ強度）に基づいて実行され得る。空間において隣接する２つ以上のポイントクラウドフレーム間のポイントクラウド姿勢情報の変化も推定することができる。いくつかの実施形態では、４−Ｄ ＮＤＴは、ポイントクラウド姿勢情報を推定するために使用されてもよい。４−Ｄ ＮＤＴは、非線形関数自体ではなく、非線形関数の確率密度分布に基づいてポイントクラウド姿勢情報を推定することができる。

ステップＳ７０８において、各ポイントクラウドフレームに関連する姿勢情報は、ポイントクラウド姿勢情報および初期姿勢データに基づいて、プロセッサ２０４によって推定され得る。いくつかの実施形態では、ポイントクラウド姿勢情報は、各ポイントクラウドフレームに関連付けられた姿勢情報を推定する際に非線形関数の高次成分を保存するために、ＵＫＦを使用して初期姿勢データとマージされ得る。推定された姿勢情報は、後の最適化のための初期値として使用することができる。

ステップＳ７１０において、ポイントクラウドフレームは、プロセッサ２０４によって、複数のバッチに分割され得る。ポイントクラウドフレームの分割は、ポイントクラウドフレーム間の空間的および時間的関係、例えば空間的および時間的変化に基づいて行うことができる。一例では、時間閾値未満の時間隔、位置閾値未満の位置変化、および方向閾値未満の方向変化に対応する任意のポイントクラウドフレームが同じバッチに割り当てられる。結果として、各バッチは、空間および時間において隣接する１つまたは複数のポイントクラウドフレームを含むことができる。いくつかの実施形態では、最も早い時点を有する各バッチ内のポイントクラウドフレームのうちの１つを、バッチの第１のポイントクラウドフレームと見なすことができ、第１のポイントクラウドフレームの姿勢情報を、バッチに関連付けられた姿勢情報として使用することができる。

ステップＳ７１２において、各ポイントクラウドフレームに関連付けられた、推定された姿勢情報は、モデルに基づいて、プロセッサ２０４によって調整され得る。モデルは、ポイントクラウドフレーム間の空間的関係および時間的関係を含むことができる。いくつかの実施形態では、モデルは、複数のバッチの各々におけるポイントクラウドフレームの姿勢をそれぞれ表す複数のノードを含む姿勢グラフによって表すことができる。姿勢グラフは、推定されたポイントクラウド姿勢情報の変化を表す閉ループバッチを接続する２バッチエッジ、推定された姿勢情報の変化を表す隣接するバッチを接続する２バッチエッジ、および各ノードの推定された姿勢を表す１バッチエッジなど、ノードを接続する様々なタイプのエッジも含むことができる。いくつかの実施形態では、総相対的事前姿勢（隣接するバッチを接続する２つのバッチ縁部によって表される）を、姿勢グラフの各ノードの姿勢情報を最適化するためのコスト関数として使用することができる。推定された姿勢情報は、値が最小化されるまで、コスト関数の値を低減するように調整され得る。

ステップＳ７１４において、調整された姿勢情報は、プロセッサ２０４によって、各ポイントクラウドフレームに登録される。ステップＳ７１６において、ポイントクラウドフレームは、各ポイントクラウドフレームに関連付けられた調整された姿勢情報に基づいて、プロセッサ２０４によって集約され得る。例えば、関連する姿勢情報によって示されるように空間において隣接するポイントクラウドフレームを集約することができる。ステップＳ７１８において、調整された姿勢情報に基づいて、プロセッサ２０４によって車両１００を位置決めすることができる。いくつかの実施形態では、調整された姿勢情報は、その最適化された値を有することができ、車両１００は高精度で位置決めすることができる。

本開示の別の態様は、実行されると、上述のような方法を１以上のプロセッサに実行させる命令を記憶する非一時的コンピュータ可読媒体に関する。コンピュータ可読媒体は、揮発性または不揮発性、磁気、半導体、テープ、光、リムーバブル、非リムーバブル、または他のタイプのコンピュータ可読媒体またはコンピュータ可読記憶装置を含むことができる。例えば、コンピュータ可読媒体は、開示されるように、コンピュータ命令が格納されたストレージ装置またはメモリモジュールであってもよい。いくつかの実施形態では、コンピュータ可読媒体は、コンピュータ命令が格納されたディスクまたはフラッシュドライブであってもよい。

開示されたシステムおよび関連する方法に対して様々な修正および変形を行うことができることは、当業者には明らかであろう。他の実施形態は開示されたシステムおよび関連する方法の明細書および実施を考慮することから、当業者には明らかであろう。

明細書および実施例は例示としてのみ考慮され、真の範囲は以下の特許請求の範囲およびそれらの均等物によって示されることが意図される。

Claims (20)

1. 車両を位置決めするためのナビゲーションシステムであって、
   前記車両が軌道に沿って移動するときに、前記車両に装備された複数のセンサによって取り込まれた前記車両のシーンおよび初期姿勢データに関する複数のポイントクラウドフレームを受信するように構成された通信インターフェースと、
   前記複数のポイントクラウドフレームおよび前記初期姿勢データを格納するように構成されたストレージと、
   プロセッサと、を備え、
   前記プロセッサは、
   前記初期姿勢データおよび前記複数のポイントクラウドフレームに基づいて、前記複数のポイントクラウドフレームのそれぞれに関連付けられた前記車両の姿勢情報を推定し、
   前記複数のポイントクラウドフレーム間の空間的関係および時間的関係を含むモデルに基づいて前記車両の推定された前記姿勢情報を調整し、
   調整された前記姿勢情報に基づいて車両を位置決めする
   ように構成される、
   システム。
2. 前記プロセッサは、
   前記車両の調整された前記姿勢情報を前記複数のポイントクラウドフレームのそれぞれに登録し、
   調整された前記姿勢情報に基づいて前記複数のポイントクラウドフレームを集約する
   ようにさらに構成される、請求項１に記載のシステム。
3. 前記車両の前記姿勢情報を推定するために、前記プロセッサは、前記初期姿勢データに基づく軌道補間を使用して前記複数のポイントクラウドフレームを処理するようにさらに構成される、請求項１に記載のシステム。
4. 前記車両の姿勢を推定するために、前記プロセッサは、
   前記複数のポイントクラウドフレームに基づいて前記車両のポイントクラウド姿勢情報を推定し、
   前記ポイントクラウド姿勢情報および前記初期姿勢データに基づいて前記姿勢情報を推定する
   ようにさらに構成される、請求項１に記載の装置。
5. 前記ポイントクラウド姿勢情報は、４次元（４−Ｄ）正規分布変換（ＮＤＴ）を使用して推定され、前記姿勢情報は、無香カルマンフィルタ（ＵＫＦ）を使用して推定される、請求項４に記載のシステム。
6. 前記車両の推定された前記姿勢情報を調整するために、前記プロセッサは、前記空間的関係および前記時間的関係ならびに閾値のセットに基づいて、前記複数のポイントクラウドフレームを複数のバッチに分割するようにさらに構成される、請求項１に記載のシステム。
7. 前記モデルは、前記複数のバッチの各々における前記ポイントクラウドフレームの姿勢をそれぞれ表す複数のノードを含む姿勢グラフによって表される、請求項６に記載のシステム。
8. 車両を位置決めする方法であって、
   前記車両が軌道に沿って移動するときに、前記車両に装備された複数のセンサによって取り込まれた前記車両のシーンおよび初期姿勢データに関する複数のポイントクラウドフレームを受信するステップと、
   プロセッサによって、前記初期姿勢データおよび前記複数のポイントクラウドフレームに基づいて、前記複数のポイントクラウドフレームのそれぞれに関連付けられた前記車両の姿勢情報を推定するステップと、
   前記プロセッサによって、前記複数のポイントクラウドフレーム間の空間的関係および時間的関係を含むモデルに基づいて前記車両の推定された前記姿勢情報を調整するステップと、
   調整された前記姿勢情報に基づいて車両を位置決めするステップと、
   を備える方法。
9. 前記車両の調整された前記姿勢情報を前記複数のポイントクラウドフレームのそれぞれに登録するステップと、
   調整された前記姿勢情報に基づいて前記複数のポイントクラウドフレームを集約するステップと、
   を備える請求項８に記載の方法。
10. 前記車両の前記姿勢情報を推定するステップは、前記初期姿勢データに基づく軌道補間を使用して前記複数のポイントクラウドフレームを処理するステップを含む、請求項８に記載の方法。
11. 前記車両の前記姿勢情報を推定するステップは、
    前記複数のポイントクラウドフレームに基づいて前記車両のポイントクラウド姿勢情報を推定するステップと、
    前記ポイントクラウド姿勢情報および前記初期姿勢データに基づいて前記姿勢情報を推定するステップと、
    を備える請求項８に記載の方法。
12. 前記ポイントクラウド姿勢情報は、４−Ｄ ＮＤＴを使用して推定され、前記姿勢情報は、ＵＫＦを使用して推定される、請求項１１に記載の方法。
13. 前記車両の推定された前記姿勢情報を調整するステップは、前記空間的関係および前記時間的関係ならびに閾値のセットに基づいて、前記複数のポイントクラウドフレームを複数のバッチに分割するステップを備える、請求項８に記載の方法。
14. 前記モデルは、前記複数のバッチの各々における前記ポイントクラウドフレームの姿勢をそれぞれ表す複数のノードを含む姿勢グラフによって表される、請求項１３に記載の方法。
15. １以上のプロセッサによって実行されると、前記１以上のプロセッサに、
    車両が軌道に沿って移動するときに、前記車両に装備された複数のセンサによって取り込まれた前記車両のシーンおよび初期姿勢データに関する複数のポイントクラウドフレームを受信することと、
    前記初期姿勢データおよび前記複数のポイントクラウドフレームに基づいて、前記複数のポイントクラウドフレームのそれぞれに関連付けられた前記車両の姿勢情報を推定することと、
    前記複数のポイントクラウドフレーム間の空間的関係および時間的関係を含むモデルに基づいて前記車両の推定された前記姿勢情報を調整することと、
    調整された前記姿勢情報に基づいて車両を位置決めすることと、
    を含む動作を行わせる命令が格納された非一時的コンピュータ可読媒体。
16. 前記動作は、
    前記車両の調整された前記姿勢情報を前記複数のポイントクラウドフレームのそれぞれに登録することと、
    調整された前記姿勢情報に基づいて前記複数のポイントクラウドフレームを集約することと、
    をさらに含む、請求項１５に記載のコンピュータ可読媒体。
17. 前記車両の前記姿勢情報を推定することは、前記初期姿勢データに基づく軌道補間を使用して前記複数のポイントクラウドフレームを処理することを含む、請求項１５に記載のコンピュータ可読媒体。
18. 前記車両の前記姿勢情報を推定することは、
    前記複数のポイントクラウドフレームに基づいて前記車両のポイントクラウド姿勢情報を推定することと、
    前記ポイントクラウド姿勢情報および前記初期姿勢データに基づいて前記姿勢情報を推定することと、
    を含む、請求項１５に記載のコンピュータ可読媒体。
19. 前記車両の推定された前記姿勢情報を調整することは、前記空間的関係および前記時間的関係ならびに閾値のセットに基づいて、前記複数のポイントクラウドフレームを複数のバッチに分割することを含む、請求項１５に記載のコンピュータ可読媒体。
20. 前記モデルは、前記複数のバッチの各々における前記ポイントクラウドフレームの姿勢をそれぞれ表す複数のノードを含む姿勢グラフによって表される、請求項１９に記載のコンピュータ可読媒体。