特開 2024-127840 ＳＬＡＭ方法

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

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2024127840

(43)【公開日】2024-09-20

(54)【発明の名称】ＳＬＡＭ方法

(51)【国際特許分類】

   G05D 1/246 20240101AFI20240912BHJP

   G01C 21/28 20060101ALI20240912BHJP

【ＦＩ】

G05D1/246

G01C21/28

【審査請求】未請求

【請求項の数】14

【出願形態】ＯＬ

【外国語出願】

(21)【出願番号】P 2024033787

(22)【出願日】2024-03-06

(31)【優先権主張番号】10 2023 105 585.2

(32)【優先日】2023-03-07

(33)【優先権主張国・地域又は機関】DE

(71)【出願人】

【識別番号】506012213

【氏名又は名称】ディスペース ゲー・エム・ベー・ハー

【氏名又は名称原語表記】ｄＳＰＡＣＥ ＧｍｂＨ

【住所又は居所原語表記】Ｒａｔｈｅｎａｕｓｔｒ．２６，Ｄ－３３１０２ Ｐａｄｅｒｂｏｒｎ， Ｇｅｒｍａｎｙ

(74)【代理人】

【識別番号】100114890

【弁理士】

【氏名又は名称】アインゼル・フェリックス＝ラインハルト

(74)【代理人】

【識別番号】100098501

【弁理士】

【氏名又は名称】森田 拓

(74)【代理人】

【識別番号】100116403

【弁理士】

【氏名又は名称】前川 純一

(74)【代理人】

【識別番号】100134315

【弁理士】

【氏名又は名称】永島 秀郎

(74)【代理人】

【識別番号】100162880

【弁理士】

【氏名又は名称】上島 類

(72)【発明者】

【氏名】トビアス ビースター

(72)【発明者】

【氏名】ボリス ノイベルト

(72)【発明者】

【氏名】ファイス レートリングスヘーファー

【テーマコード（参考）】

2F129

5H301

【Ｆターム（参考）】

2F129AA03

2F129BB03

2F129BB19

2F129BB33

2F129BB47

2F129BB49

5H301AA03

5H301AA10

5H301BB14

5H301CC03

5H301CC06

5H301CC08

5H301CC10

5H301FF11

5H301GG08

5H301GG09

5H301GG16

5H301HH01

5H301HH02

5H301QQ06

(57)【要約】      （修正有）

【課題】提供される種々のリソースをフレキシブルに扱うことのできる、かつ／または要求度の高いデータセットにおいても信頼性の高い結果をもたらすＳＬＡＭアルゴリズムを提供する。
【解決手段】移動体システムの自己姿勢を決定し、因子グラフによって表現される最適化問題を用いて環境の３Ｄ楕円体ベースのサーフェルマップを作成する方法では、サーフェルデータとサーフェルマップとの比較により新しいサーフェルおよび予め既知であるサーフェルが識別され、予め既知であると識別されたサーフェルに対してはサーフェル因子（１４ａ，１４ａ’）が因子グラフ＝に追加され、かつ／または、新しいサーフェルであると識別されたサーフェル因子（１４ａ）に対してはサーフェルノード（１２ｂ）およびサーフェル因子（１４ａ’）が因子グラフに追加される。
【選択図】図３

【特許請求の範囲】

【請求項1】

移動体システムの自己姿勢を決定し、因子グラフ（１０）によって表現される最適化問題を用いて環境の３Ｄ楕円体ベースのサーフェルマップを作成するためのコンピュータにより実装される方法であって、前記方法は、
－前記移動体システムに取り付けられて前記移動体システムの環境を測定する少なくとも１つの環境センサにより生成された環境センサデータを受信するステップであって、前記環境センサデータは、それぞれ１つずつの特定の期間（２０）における点群（１８）として前記移動体システムの環境を表現しているステップと、
－受信された環境センサデータの点群（１８）をサーフェルデータへと変換することによりサーフェル（１６）を生成するステップと、
－サーフェルデータとサーフェルマップとの比較により、生成されたサーフェル（１６）において新しいサーフェル（１６）および予め既知であるサーフェル（１６）を識別するステップと、
－予め既知であると識別されたサーフェル（１６）に対してサーフェル因子（１４ａ，１４ａ’）を前記因子グラフ（１０）に追加するステップおよび／または新しいサーフェルであると識別されたサーフェル（１６）に対してサーフェルノード（１２ｂ）およびサーフェル因子（１４ａ）を前記因子グラフ（１０）に追加するステップと、
を含み、
前記因子グラフ（１０）のノード（１２ａ）は、前記移動体システムの特定の自己姿勢を表現しているかまたはサーフェルノード（１２ｂ）としてマップ座標内の特定のサーフェル（１６）を表現しており、
自己姿勢を表現するノード（１２ａ）とサーフェル（１６）を表現するノード（１２ｂ）との間のエッジは、サーフェル因子（１４ａ，１４ａ’）として、ノード（１２ａ）によって表現される自己姿勢と前記環境センサの特定の自己姿勢から識別可能なサーフェル（１６）との間の空間的制約を表現しており、かつ、前記特定の自己姿勢からの、前記環境センサのための前記サーフェル（１６）の表示可能性に関する確率分布を含み、
最適化問題を表現する前記因子グラフ（１０）は、第１の特定の期間（２０）での環境センサデータの受信後にインクリメント方式で作成され、前記予め既知であると識別されたサーフェルのサーフェル因子（１４ａ，１４ａ’）および／または前記新しいサーフェル（１６）のサーフェルノード（１２ｂ）およびサーフェル因子（１４ａ）が前記因子グラフ（１０）に追加されて、後の特定の期間（２０）での環境センサデータのためのステップが前記第１の特定の期間（２０）でのステップとして繰り返されるよりも前に、前記最適化問題がインクリメント方式で解決される、
方法。

【請求項2】

自己姿勢を表現する２つのノード（１２ａ）間のエッジは、各ノード（１２ａ）によって表現される自己姿勢間の空間的制約を表現しておりかつ２つの自己姿勢間の変換に関する確率分布を含む、自己運動因子（１４ｂ，１４ｂ’，１４ｂ’’）を含む、
請求項１記載の方法。

【請求項3】

前記自己運動因子（１４ｂ，１４ｂ’，１４ｂ’’）は、自己運動モデルのモデル仮定に基づいており、または、
前記方法は、自己運動センサデータ、例えば前記移動体システムの慣性センサデータ、ＧＰＳデータおよび／または推進システムデータ、例えば車輪回転数センサデータを受信するステップを含み、前記自己運動因子（１４ｂ，１４ｂ’，１４ｂ’’）は、自己運動モデルのモデル仮定と受信された自己運動センサデータとに基づく、
請求項２記載の方法。

【請求項4】

ａ）前記受信された環境センサデータの点群（１８）をサーフェルデータへと変換することによりサーフェル（１６）を生成するステップは、前記移動体システムの局所座標にサーフェル（１６）を生成することを含み、前記サーフェルデータとサーフェルマップとの比較により、生成されたサーフェル（１６）において新しいサーフェル（１６）および予め既知であるサーフェル（１６）を識別するステップは、前記移動体システムの局所座標に生成されたサーフェル（１６）をマップ座標内のサーフェル（１６）へと変換することを含み、または、
ｂ）前記受信された環境センサデータの点群（１８）をサーフェルデータへと変換することによりサーフェル（１６）を生成するステップは、前記移動体システムの局所座標からサーフェルマップのマップ座標へと点群を変換し、前記マップ座標へと変換された点群を変換することによってマップ座標にサーフェルを生成することを含む、
請求項１から３までのいずれか１項記載の方法。

【請求項5】

前記サーフェル（１６）を生成するステップは、受信された環境センサデータの点群（１８）をサーフェルデータへと再帰的に変換することによって行われ、
－前記点群（１８）の個々の点（２４）を予め定められたボクセル（２６）に割り当てるステップであって、各ボクセル（２６）は前記環境の体積割合を表しているステップと、
－すべてのボクセル（２６）に対して、それぞれ１つのボクセル（２６）に割り当てられた点（２４）を用いて主成分分析を行うことによって、第１のサーフェル（１６）を生成するステップと、
－すべてのボクセル（２６）をサブボクセル（２８）へと分割し、すべてのサブボクセル（２８）に対して、それぞれサブボクセル（２８）に割り当てられた点（２４）を用いて主成分分析を行うことによって、第２のサーフェル（１６）を新たに生成するステップと、
－生成された前記第１のサーフェル（１６）の品質値と生成された前記第２のサーフェル（１６）と比較するステップと、
－生成された前記第１のサーフェル（１６）の品質値の比較から前記第１のサーフェル（１６）に対する改善された品質値が得られるまで、または、分割によって生成されたサブボクセル（２８）が予め定められた最小数の点（２４）を下回るまで、先行の２つのステップを反復実行するステップと、
をさらに含む、
請求項１から４までのいずれか１項記載の方法。

【請求項6】

前記品質値は、生成されたすべてのサーフェル（１６）にわたる、次に示す値からの重み付け和の平均、すなわち、
長いほうの２つの主軸に沿ったサーフェル（１６）の広がりに基づく値、
最短の主軸に沿ったサーフェル（１６）の広がりに基づく値、
上記の２つの値の組み合わせに基づく値、および、
サーフェル（１６）の棒状性に基づく値、
からの重み付け和の平均を含む、
請求項５記載の方法。

【請求項7】

前記最適化問題を解決する際に可変であるダイナミクス値は、各サーフェル（１６）に割り当てられ、前記ダイナミクス値は、前記最適化問題の解決の開始時に前記サーフェル（１６）が動的でないことを特徴付けるものである、
請求項１から６までのいずれか１項記載の方法。

【請求項8】

前記最適化問題を解決する際に可変であるダイナミクス値は、各サーフェル（１６）に割り当てられ、
前記サーフェルデータとサーフェルマップとの比較により、生成されたサーフェル（１６）において新しいサーフェル（１６）および予め既知であるサーフェル（１６）を識別するステップにおいて、前記予め既知であるサーフェル（１６）と前記生成されたサーフェル（１６）との間のマッピング誤差が決定され、前記最適化問題を解決する際に前記マッピング誤差が前記ダイナミクス値で重み付けされる、
請求項１から７までのいずれか１項記載の方法。

【請求項9】

前記最適化問題の解決中のサーフェル（１６）のダイナミクス値の増大にペナルティが課される、
請求項７または８記載の方法。

【請求項10】

前記最適化問題を解決する際に可変であるダイナミクス値は、各サーフェル（１６）に割り当てられ、予め既知であると識別されたサーフェル（１６）は、相互の空間的距離および／または相互の類似した配向に基づいてクラスタへとまとめられ、
前記因子グラフ（１０）は、各クラスタに対して、前記最適化問題の解決中のクラスタ内のサーフェル（１６）のダイナミクス値の差にペナルティが課されるようにするクラスタ因子（１４ｄ）を有する、
請求項１から９までのいずれか１項記載の方法。

【請求項11】

受信された環境センサデータは、前記点群（１４）の各点（２４）に対して測定時点を有しており、
前記サーフェルデータとサーフェルマップとの比較により、生成されたサーフェル（１６）において新しいサーフェル（１６）および予め既知であるサーフェル（１６）を識別するステップは、前記移動体システムの推定速度を考慮して、サーフェルデータから歪み補正されたサーフェルを生成し、前記歪み補正されたサーフェルとサーフェルマップとを比較することを含む、
請求項１から１０までのいずれか１項記載の方法。

【請求項12】

請求項１から１１までのいずれか１項記載の方法を実行する手段を備えたデータ処理装置。

【請求項13】

コンピュータによりプログラムが実行される際に請求項１から１１までのいずれか１項記載の方法を前記コンピュータに実行させるための命令を含んでいる、コンピュータプログラム製品。

【請求項14】

請求項１３記載のコンピュータプログラム製品を記憶したコンピュータ可読データ担体。

【発明の詳細な説明】

【技術分野】

【0001】

本発明は、移動体システムの自己姿勢を決定し、環境の３Ｄ楕円体ベースのサーフェルマップを作成するためのコンピュータ実装方法に関する。

【0002】

本発明はまた、上述の方法を実行する手段を備えたデータ処理装置に関する。

【0003】

本発明はさらに、コンピュータによってプログラムが実行される際に上述した方法をコンピュータに実行させるための命令を含んでいる、コンピュータプログラム製品に関する。

【0004】

本発明はさらに、上記のコンピュータプログラム製品を記憶したコンピュータ可読データ担体に関する。

【背景技術】

【0005】

ＳＬＡＭ（英語：Simultaneous Localization and Mapping，独語：Simultane Positionsbestimmung und Kartierung、同時位置特定およびマッピング）とは、例えば車両のような移動体システムの環境センサのセンサデータを評価することによって環境のマップを作成し、同時に、マップ内での移動体システムの空間的位置（自己姿勢とも称される）を推定するためのアルゴリズム的な問題提起であると理解される。

【0006】

ＳＬＡＭ問題を解決するためのコンピュータ支援方法における課題は、正確な姿勢決定のために相応に詳細なマップが必要であり、詳細なマップを生成するために移動体システムの正確な姿勢が既知でなければならないことである。このことから明らかとなるのは、これら２つの要求、すなわちマップ作成と自己位置特定とが、相互に独立しては解決されえないということである。このように、ＳＬＡＭ問題は、マップおよび姿勢の双方が既知でなく同時に推定されるため、卵が先か鶏が先かの問題となる。したがって、数学的に表現すると、ＳＬＡＭ問題は数値最適化問題となる。

【0007】

従来技術において、ＳＬＡＭ問題の解を提供する幾つかのアルゴリズムが公知である。こうしたＳＬＡＭアルゴリズムは、通常、利用可能な複数のリソース、例えば移動体システムの既存のセンサデータ形式、例えばＬｉＤＡＲデータ、慣性センサデータ、ＧＰＳデータおよび／または推進システムデータなどのようなデータ形式へと裁断されている。ＳＬＡＭアルゴリズムは、とりわけ、自律型ロボットシステム、自動運転車両および／または無人航空機のような移動体システムにおいてナビゲーション支援のために使用されている。

【0008】

ＳＬＡＭアルゴリズムのさらなる適用は、アノテーションされたデータセットの作成において見出される。ここでは、手動および自動で作成されるアノテーション結果を改善するために、ＳＬＡＭアルゴリズムがアノテーションプロジェクトにおける準備ステップとして使用される。アノテーションされたデータセットは、例えば機械学習に基づく幾つかのアルゴリズムをトレーニングするための基本的な前提条件である。ナビゲーション支援のために移動体システムにおいて直接に使用され、通常は計算速度についても最適化されているＳＬＡＭアルゴリズムとは異なり、データセットのアノテーションの際に使用されるＳＬＡＭアルゴリズムの計算速度は副次的なものとなっていることが多い。また、ナビゲーション支援のために移動体システムにおいて直接に使用されるＳＬＡＭアルゴリズムは、移動体システムによって提供されるリソースに合わせて最適化されている。これに対して、データセットのアノテーションの際には、予め定められた特定のセンサコンフィグレーションを有する特定の移動体システムからのデータのみが処理されるわけではないので、このようなケースでは、ＳＬＡＭアルゴリズムに対して種々のリソースをフレキシブルに扱うことができると有利である。

【0009】

従来技術に存在しているＳＬＡＭアルゴリズムは、通常、比較的多数の容易に区別可能な特徴を有するデータセットを用いて、きわめて良好な結果を生成することができる。このようなデータセットは、通常は都市環境において低速での走行の際に記録され、この場合、低速であることから、環境センサの順次連続する個々の画像間で、移動体システム（多くの場合に車両である）が空間的に僅かしか移動していないという利点を有する。しかし、このようなＳＬＡＭアルゴリズムの結果は、とりわけ例えば自動車専用道路走行のような他の条件のもとで記録された要求度の高いデータセットでは大幅に低い信頼性しか有さない。多くの場合に自動車専用道路の側方に緑地帯またはガードレールが存在していることに起因する、順次連続する個々の画像間のセンサデータの類似性のために、相互の類似性に基づいて特徴をマッピングすることは困難である。しかも、車両が高速であることおよび（他の交通加入者のような）動的なオブジェクトの存在もさらなる課題となる。

【0010】

つまり、換言すれば、従来技術において存在するＳＬＡＭアルゴリズムは、要求度の高いデータセットにおいて、かつ／または例えば個々の画像間で識別可能かつ補償可能な特徴の数が限定されているデータセットにおいて、かつ／または移動体システムが高速で記録したデータセットにおいて、かつ／または動的なオブジェクトを含むデータセットにおいて、弱点を示す。

【発明の概要】

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

【0011】

これらのことに基づいて、本発明の課題は、提供される種々のリソースをフレキシブルに扱うことのできる、かつ／または要求度の高いデータセットにおいても信頼性の高い結果をもたらすＳＬＡＭアルゴリズムを提供することである。

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

【0012】

上記の課題は、各独立請求項の対象によって解決される。好ましい発展形態は各従属請求項に示されている。

【0013】

つまり、本発明によれば、移動体システムの自己姿勢を決定し、因子グラフによって表現される最適化問題を用いて環境の３Ｄ楕円体ベースのサーフェルマップを作成するためのコンピュータ実装方法であって、
－移動体システムに取り付けられて移動体システムの環境を測定する少なくとも１つの環境センサにより生成された環境センサデータを受信するステップであって、ここで、環境センサデータは、それぞれ１つずつの特定の期間における点群として移動体システムの環境を表現している、ステップと、
－受信された環境センサデータの点群をサーフェルデータへと変換することにより、サーフェルを生成するステップと、
－サーフェルデータとサーフェルマップとの比較により、生成されたサーフェルにおいて新しいサーフェルおよび予め既知であるサーフェルを識別するステップと、
－予め既知であると識別されたサーフェルに対してサーフェル因子を因子グラフに追加するステップおよび／または新しいサーフェルであると識別されたサーフェルに対してサーフェルノードおよびサーフェル因子を因子グラフに追加するステップと、
を含み、
因子グラフのノードは、移動体システムの特定の自己姿勢を表現しているかまたはサーフェルノードとしてマップ座標内の特定のサーフェルを表現しており、
自己姿勢を表現するノードとサーフェルを表現するノードとの間のエッジが、サーフェル因子として、ノードによって表現される自己姿勢と環境センサの特定の自己姿勢から識別可能なサーフェルとの間の空間的制約を表現しており、かつ特定の自己姿勢からの、環境センサのための当該サーフェルの表示可能性に関する確率分布を含み、
最適化問題を表現する因子グラフは、第１の特定の期間での環境センサデータの受信後にインクリメント方式で作成され、予め既知であると識別されたサーフェルのサーフェル因子および／または新しいサーフェルのサーフェルノードおよびサーフェル因子が因子グラフに追加されて、後の特定の期間での環境センサデータのためのステップが第１の特定の期間でのステップとして繰り返されるよりも前に、最適化問題がインクリメント方式で解決される、
方法が提供される。

【0014】

換言すれば、本発明による方法は、最適化問題を因子グラフとしてモデル化するＳＬＡＭアルゴリズムである。因子グラフは、確率論的グラフィックモデルの一形態である。当該方法により、問題につき、ある種の抽象化を行うことが可能となり、最適化問題のためのコンピュータ実装される解決方法の発見がより簡単となる。因子グラフのノードは、最適化問題の未知の変数（本明細書では、特定の期間における移動体システムの自己姿勢とマップ座標におけるサーフェルの姿勢とを指す）を表現しており、ここで、因子グラフの因子は、測定結果および／または予備知識に基づく当該変数に対する確率論的制約を表している。このような因子グラフによって表現される最適化問題を解決する際には、まず、未知の変数を表す値の特定のセットが初期値として使用され、この値が最適化問題をどの程度良好に解決するかが算定される。値を変化させることによって、因子グラフにより表現される問題を最良に解決する変数の値が見出される。

【0015】

本明細書における移動体システムとは、環境内（例えば空中、水上もしくは水中、または陸上もしくは地中）を移動することのできるあらゆるシステムであると理解されたい。移動体システムは、例えば、車両、航空機、ロボット、鉄道車両または船舶として構成されていてよい。好ましくは、移動体システムは車両であり、すなわち陸上を移動する移動体システムである。さらに好ましくは、移動体システムはレールに拘束されない車両であり、特に好ましくは自動車である。

【0016】

ここでは、姿勢とは、オブジェクトの位置および向き（すなわち配向）であると理解される。したがって、自己姿勢とは、移動体システムの位置および向きである。したがって、サーフェルマップにおけるサーフェルの姿勢とは、マップ座標におけるサーフェルの位置および向きを意味するものと理解されたい。

【0017】

好ましくは、環境センサデータはＬｉＤＡＲデータであり、環境センサはＬｉＤＡＲセンサである。ＬｉＤＡＲとは、距離の算定にレーザービームを使用する３次元レーザー走査の形態である。

【0018】

方法の特定の期間とは、環境センサが点群を生成した期間である。当該特定の期間はフレームとも称される。環境センサとしてのＬｉＤＡＲセンサに関して、当該期間の長さは、ＬｉＤＡＲセンサが１回転に対してどの程度の速度で必要とされるかを定めるものである。６００ｒｐｍの場合、当該期間は例えば０．１秒の長さである。当該期間中、環境からの複数の点を有する点群が生成される。例えば、６４個の走査チャネルを有するＬｉＤＡＲセンサは、１秒あたり１３３万個の点を出力することができる。よって、６００ｒｐｍで回転することにより、この実施例では、ＬｉＤＡＲセンサは、フレームあたり０．１３３万個の測定点で１秒間に１０個のフレームを更新する。

【0019】

本発明の一態様は、マップが３Ｄ楕円体に基づいていることである。３Ｄ楕円体は、楕円体状サーフェルとも称される（サーフェルとは、表面要素を表すsurface elementなる英語概念の略である）ので、これはサーフェルマップである。つまり、サーフェルマップでは、オブジェクトは、１つ以上の３Ｄ楕円体によって表現される。好ましくは、サーフェルは点群のそれぞれ１つずつの近似である。さらに好ましくは、サーフェルは、サーフェルの中心、サーフェルの回転（特にサーフェルの局所的な２つの主軸に沿った配向）、３つの主軸に沿った広がり、３つの主軸に沿った点の測定時点分布および／または測定時点分布の近似、に関する１つもしくは複数の情報と、サーフェルの棒状性に関する値および／またはサーフェルの円盤状性に関する値と、を含む。

【0020】

さらに好ましくは、測定時点分布の近似は、点群の点の測定時点から形成された少なくとも４つの、好ましくは６つの測定時点を含む。特に好ましくは、これは、長いほうの２つの主軸に沿ったサーフェルの最大広がりを規定する４つの点の測定時点であり、さらに好ましくは３つの主軸に沿ったサーフェルの最大広がりを規定する６つの点の測定時点である。

【0021】

好ましくは、棒状サーフェルは、正確に１つの主軸に沿った大きな広がりと他の２つの主軸に沿った小さな広がりとを有するサーフェルである。円盤状サーフェルは、好ましくは、正確に１つの主軸に沿った小さな広がりと他の２つの主軸に沿った大きな広がりとを有するサーフェルである。よって、棒状性および／または円盤状性の値は、好ましくは、３つの主軸に沿ったサーフェルの広がりから形成される。

【0022】

本発明のさらなる態様は、因子グラフのインクリメント方式での作成およびこの因子グラフによって表現される最適化問題のインクリメント方式での解決である。換言すれば、マップ座標における移動体システムの自己姿勢の解の発見は、それ自体のみが個々の特定の期間ごとに実行されるわけではなく、マップ座標内のサーフェルが最適化可能な変数としてインクリメント方式で因子グラフへと組み込まれるようにして行われる。特に、最適化問題は、予め既知であると識別されたサーフェルのサーフェル因子および／または新しいサーフェルであると識別されたサーフェルのサーフェルノードおよびサーフェル因子が因子グラフへと追加され、後の期間の環境センサデータを用いて各ステップが繰り返されるよりも前に、インクリメント方式で解決される。さらに好ましくは、最適化問題は、サーフェルデータとサーフェルマップとの比較が行われるよりも前に、インクリメント方式で解決される。このことにより、一方の、サーフェルマップの測定精度に起因する内在的な非定常性と、他方の、環境センサデータの記録の際の移動体システムの自己運動に基づく環境センサデータの点群の歪みと、を扱うことができる。例えば、自動車専用道路の走行において発生するような移動体システムの高い速度は、点群の点が移動体システムの自己運動によって歪むため、特定の期間において点群の点が移動体システムの環境の正確な表現を形成しないことに結びつく。既に一度観察されたサーフェルについてのサーフェル因子を因子グラフに追加することにより、当該サーフェルの姿勢、すなわち当該サーフェルの位置および配向が最適化される。つまり、換言すれば、方法において環境の表現が共に最適化されることで、センサ側で形成される環境表現があまり正確でないという欠点が補償される。

【0023】

本発明の方法は特に、例えば高速時に記録された要求度の高いデータセット、動的なオブジェクトを含んでいるデータセットおよび／または識別可能かつ補償可能な特徴を僅かしか含まないデータセットを評価することに適している。また、本発明による方法は、アノテーションプロジェクトにおいて使用されることに特に適している。なぜなら、当該方法は、受信された環境センサデータのみをリソースとして利用し、移動体システムの他のリソース、例えば慣性センサデータ、ＧＰＳデータおよび／または推進システムデータ、例えば車輪回転数センサデータにアクセスできない場合にも結果を提供することができるからである。当該方法の構造は、さらなるリソースのフレキシブルな統合、ひいてはフレキシブルな拡張を可能にし、このことはアノテーションプロジェクトにとって特に有利である。

【0024】

本発明の好ましい発展形態によれば、方法の高いフレキシビリティに関連して、自己姿勢を表現する２つのノード間のエッジが、各ノードによって表現される自己姿勢間の空間的制約を表現しておりかつ２つの自己姿勢間の変換に関する確率分布を含む、自己運動因子を含むように構成される。

【0025】

特に、別のリソースが存在しない場合、好ましくは、自己運動因子は、自己運動モデルのモデル仮定に基づく。自己運動モデルは、好ましくは、移動体システムの姿勢誤差を、その時点での姿勢と速度に伴って伝播した最後の姿勢との間の差として計算する。好ましくは、速度誤差が、連続する２つの速度間の単純な差として計算される。つまり、方法は、好ましくは、直接に連続する２つの期間における移動体システムの速度差は小さいという基本的仮定を基礎としている。

【0026】

これに対して、さらなるリソースが存在する場合、好ましくは、方法が、自己運動センサデータ、例えば移動体システムの慣性センサデータ、ＧＰＳデータおよび／または推進システムデータ、例えば車輪回転数センサデータを受信するステップを含み、自己運動因子が、自己運動モデルのモデル仮定と自己運動センサデータとに基づくように構成される。

【0027】

ＧＰＳデータがリソースとして存在する場合、好ましくは、自己運動因子は、少なくとも部分的に、移動体システムの推定された自己姿勢と移動体システムのＧＰＳデータから算定された自己姿勢との間の差に基づくように構成される。慣性センサデータおよび／または推進システムデータ、車輪回転数センサデータの場合、好ましくは、自己運動因子は、少なくとも部分的に、直接に連続して記録された２つの慣性センサデータ間の差および／または推進システムデータ間の差に基づくように構成される。

【0028】

好ましくは、サーフェルデータは、環境センサデータから出発して、まず移動体システムの局所座標に生成され、これに対してサーフェルマップは当該局所座標には存在しないので、方法は、好ましくは、移動体システムの局所座標からサーフェルマップのマップ座標へとサーフェルデータを変換するステップを含む。さらに、本発明の好ましい発展形態によれば、受信された環境センサデータの点群をサーフェルデータへと変換することによりサーフェルを生成するステップが、移動体システムの局所座標におけるサーフェルを生成することを含み、サーフェルデータとサーフェルマップとの比較により、生成されたサーフェルにおいて新しいサーフェルおよび予め既知であるサーフェルを識別するステップが、移動体システムの局所座標に生成されたサーフェルをマップ座標内のサーフェルへと変換することを含む。

【0029】

換言すれば、代替形態では、好ましくは、まず受信された環境センサデータの点群から出発して、移動体システムの局所座標にサーフェルを生成することによって、サーフェルデータが生成される。好ましくは、続いて、当該サーフェルデータがマップ座標へと変換される。好ましくは、変換の際に、環境センサデータの測定の特定の期間における移動体システムの推定自己姿勢が考慮され、ここで、当該期間における移動体システムの推定自己姿勢は、因子グラフによって表現される最適化問題のインクリメント方式の解決によって既知となったものである。

【0030】

代替的に、好ましくは、方法は、移動体システムの局所座標からサーフェルマップのマップ座標へと点群を変換するステップを含むように構成される。このことに関連して、さらに好ましくは、受信された環境センサデータの点群をサーフェルデータへと変換することによりサーフェルを生成するステップが、マップ座標へと変換された受信された環境センサデータの点群をサーフェルデータへと変換することによってマップ座標にサーフェルを生成することを含むように構成される。

【0031】

上述したように、最適化問題はインクリメント方式で解決される。特に好ましくは最適化問題がサーフェルまたは点群を移動体システムの局所座標からマップ座標へと変換するよりも前に解決されるので、移動体システムの自己姿勢のその時点での推定が常に座標変換に使用される。

【0032】

移動体システムの自己姿勢が仮説によって完全に正確に推定される場合には、予め既知であると識別されたサーフェルは、生成されたサーフェルにおいてサーフェルマップとの比較の際に、サーフェルマップ内に存在するサーフェルへと完全に正確にマッピングされなければならない。ただし、自己姿勢もサーフェルマップもそれぞれ推定量であるので、予め既知であると識別されたサーフェルは、通常、サーフェルマップ内に存在するサーフェルへと完全に正確にはマッピングされない。サーフェルマップ内のサーフェルと変換されたサーフェルまたは変換された点群から生成されたサーフェルとの間のこのような誤差は、以下ではマッピング誤差と称する。好ましくは、円盤状のサーフェルでのマッピング誤差は、サーフェルマップのサーフェルと変換されたサーフェルまたは変換された点群から生成されたサーフェルとの間の対称の点－面距離を含み、また棒状のサーフェルでは、サーフェルマップのサーフェルと変換されたサーフェルまたは変換された点群から生成されたサーフェルとの間の対称の点－直線距離を含む。

【0033】

さらに、マッピング誤差は、好ましくは円盤状のサーフェルおよび棒状のサーフェルのいずれにおいても、サーフェルマップのサーフェルと変換されたサーフェルまたは変換された点群から生成されたサーフェルとの間の回転誤差を含み、ここで、当該回転誤差は、２つのサーフェルの法線ベクトル間の角度によって与えられる。

【0034】

対称の点－面距離とは、本明細書では、以下に示す２つの距離ｄ１およびｄ２の平均であって、ここで、第１のサーフェルはサーフェルマップのサーフェルを記述し、第２のサーフェルは変換されたサーフェルまたは変換された点群から生成されたサーフェルを記述すると理解され、すなわち、ｄ１は、第１のサーフェルの中心から、第２のサーフェルの長いほうの２つの主軸によって定められる平面までの最短距離であり、ｄ２は、第２のサーフェルの中心から、第１のサーフェルの長いほうの２つの主軸によって定められる平面までの最短距離である。

【0035】

対称の点－直線距離とは、本明細書では、以下に示す２つの距離ｄ３およびｄ４の平均であって、ここで、第１のサーフェルはサーフェルマップのサーフェルを記述し、第２のサーフェルは変換されたサーフェルまたは変換された点群から生成されたサーフェルを記述すると理解され、すなわち、ｄ３は、第１のサーフェルの中心から、第２のサーフェルの最長の主軸によって定められる直線までの最短距離であり、ｄ４は、第２のサーフェルの中心から、第１のサーフェルの最長の主軸によって定められる直線までの最短距離である。

【0036】

よって、換言すれば、方法の次のステップにおいて、予め既知であると識別されたサーフェルに追加されるサーフェル因子は、サーフェルマップのサーフェルと変換されたサーフェルまたは変換された点群から生成されたサーフェルとの間のここでのマッピング誤差をさらなる最適化において考慮するために使用される。よって、好ましくは、予め既知であると識別されたサーフェルの因子グラフにサーフェル因子を追加するステップは、サーフェルマップのサーフェルと変換されたサーフェルまたは変換された点群から生成されたサーフェルとの間のマッピング誤差を考慮して、予め既知であると識別されたサーフェルの因子グラフにサーフェル因子を追加することを含む。

【0037】

さらに好ましくは、サーフェルデータとサーフェルマップとの比較により、生成されたサーフェルにおいて新しいサーフェルおよび予め既知であるサーフェルを識別するステップは、サーフェルが移動体システムの局所座標からマップ座標へと変換されるよりも前に、移動体システムの推定自己姿勢を精緻化するステップを含む。好ましくは、推定自己姿勢の精緻化は、変更されたサーフェルに基づいて行われ、ここで、当該変更されたサーフェルは、移動体システムの局所座標内に存在する点群からGeneralized Iterative Closest Pointアルゴリズムを用いて生成される。

【0038】

生成されたサーフェルにおいて予め既知であるサーフェルを識別するために、好ましくは、平衡化された探索木を用いて、特にｋｄ木を用いて、マップ座標内に存在するサーフェルデータの提案が生成され、その後、この提案がサーフェルマップと比較される。有利には、このような手段によっても予め既知であると識別されなかったサーフェルデータのサーフェルが、新しいサーフェルであると識別される。

【0039】

同様に、サーフェルデータとサーフェルマップとの比較により、生成されたサーフェルにおいて新しいサーフェルおよび予め既知であるサーフェルを識別するステップに関連して、好ましくは、予め既知であると識別されたサーフェルの集合から予め定められた部分集合が引き続き使用されるように構成される。多数の点を含む点群では、予め既知であるサーフェルにつき多くの冗長識別が行われうる。計算能力を改善するために、好ましくは部分集合のみが引き続き使用される。部分集合の算定は、好ましくは、予め既知であると識別されたサーフェルを、それぞれの姿勢に基づいて低解像度のグリッドへとソートすることによって行われる。こうしたグリッドから、好ましくはGreedyアルゴリズムを用いて、予め定められた数の予め既知であると識別されたサーフェルが選択され、ここから、引き続いて使用される部分集合が形成される。

【0040】

点群からのサーフェルの生成に関連して、好ましくは、これは、主成分分析に基づく再帰的方法によって行われるように構成される。このようにすれば、最大の棒状性または円盤状性を同時に有する可能な限り大きいサーフェルを点群から生成するという目的を容易に達成することができる。こうしたサーフェルを生成すると、環境を高いダイナミックレンジで詳細にマッピングすることができる。例えば、このようにすることで、ＬｉＤＡＲによって生成されたリングパターン間で個々のリング間に大きな距離が存在するＬｉＤＡＲデータに基づいて道路を良好に表示でき、同時にガードレールおよび／または緑地帯についての高い詳細性が得られる。

【0041】

主成分分析とは、主軸変換または特異値分解としても知られる数学的方法である。これにより、共分散行列の固有ベクトルを利用することで大規模なデータセットを構造化することができる。換言すれば、複数の統計変数を少数の可能な限り有意な線形結合（いわゆる主成分）によって近似することにより、データセットを見分けることができる。

【0042】

このことに関連して、本発明のさらなる好ましい発展形態によれば、サーフェルを生成するステップが、受信された環境センサデータの点群をサーフェルデータへと再帰的に変換することによって行われ、さらに、
－点群の個々の点を予め定められたボクセルに割り当てるステップであって、各ボクセルが環境の体積割合を表している、ステップと、
－すべてのボクセルに対して、それぞれ１つのボクセルに割り当てられた点を用いて主成分分析を行うことによって、第１のサーフェルを生成するステップと、
－すべてのボクセルをサブボクセルへと分割し、すべてのサブボクセルに対して、それぞれサブボクセルに割り当てられた点を用いて主成分分析を行うことによって、第２のサーフェルを新たに生成するステップと、
－生成された第１のサーフェルの品質値と生成された第２のサーフェルと比較するステップと、
－生成された第１のサーフェルの品質値の比較から第１のサーフェルに対する改善された品質値が得られるまで、または分割によって生成されたサブボクセルが予め定められた最小数の点を下回るまで、先行の２つのステップを反復実行するステップと、
を含む、ように構成される。

【0043】

つまり、第１のステップでは、複数の点がボクセルへとグループ化される。ボクセル（英語：volume element）は、環境の体積割合に相当する。好ましくは、１つのボクセルは、約４ｍ^３の体積割合に相当する。好ましくは、ボクセルは、サーフェルのサイズの上方限界を決定している。なぜなら、好ましくは、ボクセルよりも大きいサーフェルを生成することはできないからである。方法は再帰的に行われるので、「すべてのボクセルをサブボクセルへと分割する」ステップにおいても、２つの先行のステップが反復実行されると、先行のサブボクセルが、第２のサーフェルが生成されるボクセルの位置に生じる。したがって、さらなる「品質値を比較する」ステップにおいて、新たに生成された第２のサーフェルの品質値も、先行の過程で生成された第２のサーフェルの品質値と比較され、これにより、先行の第２のサーフェルが第１のサーフェルの位置に生じる。

【0044】

サーフェルの品質値に関して、本発明のさらなる好ましい発展形態によれば、品質値は、生成されたすべてのサーフェルにわたる、次に示す値からの重み付け和の平均、すなわち、３つの主軸に沿ったサーフェルの広がりに基づく値、好ましくは長いほうの２つの主軸に沿ったサーフェルの広がりに基づく値、最短の主軸に沿ったサーフェルの広がりに基づく値、上記の２つの値の組み合わせに基づく値、特に２つの値の比に基づく値、および、サーフェルの棒状性に基づく値、からの重み付け和の平均を含むように構成される。こうした品質値によって、最適なサーフェルを容易に見出すことができる。

【0045】

動的なオブジェクトが発生するデータセットを用いて、特に信頼性の高い結果を得るために、本発明の好ましい発展形態によれば、最適化問題を解決する際に可変であるダイナミクス値が各サーフェルに割り当てられ、当該ダイナミクス値は、最適化問題の解決の開始時にサーフェルが動的でないことを特徴付けるものであるように構成される。好ましくは、ダイナミクス値は０から１（または０％～１００％）の値範囲からの値であり、ダイナミクス値０は完全な静的なサーフェルとしてのサーフェルを表し、ダイナミクス値１は完全な動的なサーフェルとしてのサーフェルを表す。静的なサーフェルでは時間に関してその姿勢が変化しないと仮定されるが、これに対して動的なサーフェルでは時間に関してその姿勢の変化が起こると仮定される。

【0046】

好ましくは、初期化値はそれぞれ０であり、すなわち、サーフェルが完全に静的であることを前提としている。好ましくは、ダイナミクス値は可変であり、最適化問題の一部である。これにより、より良好な結果が得られる場合、最適化中にサーフェルのダイナミクス値を変化させることができる。

【0047】

このことに関連して、本発明のさらなる好ましい発展形態によれば、最適化問題を解決する際に可変であるダイナミクス値が各サーフェルに割り当てられ、サーフェルデータとサーフェルマップとの比較により、生成されたサーフェルにおいて新しいサーフェルおよび予め既知であるサーフェルを識別するステップにおいて、予め既知であるサーフェルと生成されたサーフェルとの間のマッピング誤差が決定され、最適化問題を解決する際にマッピング誤差がダイナミクス値で重み付けされるように構成されている。換言すれば、最適化問題の解決にあたり、これまで説明してきたマッピング誤差は、サーフェルが動的なものであればあるほど重み付けの度合が小さくなる。特に、これは、マッピング誤差に１とサーフェルのダイナミクス値との間の差が乗算されることによって行われる。

【0048】

さらに、このことに関連して、さらなる好ましい発展形態によれば、最適化問題の解決中のサーフェルのダイナミクス値の増大にペナルティが課されるように構成される。これにより、最適化アルゴリズムが、すべてのサーフェルがダイナミクス値１を有する解へと収束し、したがって、すべてのサーフェルのマッピング誤差をもはや考慮する必要がなくなることが防止される。すなわち、換言すれば、このようにして最適化アルゴリズムが妥協点を見出すことができる。なぜなら、より動的なサーフェルでは、より静的なサーフェルにおけるよりもより大きなマッピング誤差が小さく重み付けされるからである。

【0049】

ダイナミクス値の変化に関して、好ましくは、ボクセルマップを用いて、ボクセルマップに存在するボクセル内にあるサーフェルのダイナミクス値が、移動平均値、特に指数関数的に平滑化された平均を用いて所定の時間にわたって維持され、ボクセルの移動平均値が予め定められた限界値を下回った場合に、サーフェルのダイナミクス値、好ましくはボクセル内の円盤状のサーフェルのダイナミクス値が静的な方向へと変化され、かつ／またはボクセルの移動平均値が予め定められた限界値を上回った場合に、ボクセル内の円盤状のサーフェルのダイナミクス値が動的な方向へと変化される。

【0050】

ダイナミクス値を変化させる方式は、以前に動的なオブジェクトであった領域が突発的に静的なオブジェクトを含む可能性は高くないというアイデアに基づいている。このため、サーフェルマップのうち動的なサーフェルが存在していた位置にある新しいサーフェルも、同様に動的である可能性が高い。すなわち、好ましくは、まず、サーフェルがボクセルマップ内のボクセルに割り当てられる。ボクセルマップ内の、ここでの体積割合においてこのときまでサーフェルが観察されていなかったためにこのときまでボクセルが記録されていなかった場所にあるボクセルにサーフェルが割り当てられる場合、ボクセルマップのこの場所に新しいボクセルが追加され、この新しいボクセルに１つのサーフェルのダイナミクス値が割り当てられる。これに対して、当該体積割合においてこれまで既に他のサーフェルが観察されているためにボクセルマップ内に既に存在しているボクセルにサーフェルが割り当てられる場合には、当該既存のボクセルに割り当てられているダイナミクス値が好ましくは指数関数的に平滑化された平均によって更新される。続いて、好ましくは、当該ボクセルの更新されたダイナミクス値が限界値と比較され、限界値を下回っている場合には、すべてのサーフェルに対するダイナミクス値、特に好ましくはボクセル内のすべての円盤状のサーフェルに対するダイナミクス値が低減され、限界値を上回っている場合には、すべてのサーフェルに対するダイナミクス値、好ましくはボクセル内のすべての円盤状のサーフェルに対するダイナミクス値が増大される。

【0051】

さらに、本発明の好ましい発展形態によるダイナミクス値に関連して、最適化問題を解決する際に可変であるダイナミクス値が各サーフェルに割り当てられ、予め既知であると識別されたサーフェルが相互の空間的距離および／または相互の類似した配向に基づいてクラスタへとまとめられ、また因子グラフが、それぞれのクラスタに対して、最適化問題の解決中のクラスタ内のサーフェルのダイナミクス値の差にペナルティが課されるようにするクラスタ因子を有するように構成される。

【0052】

換言すれば、最適化アルゴリズムのクラスタ因子により、クラスタに属するサーフェルがきわめて類似したダイナミクス値を有する解が優先される。その背景にある基本的なアイデアは、サーフェルがオブジェクトに属しており、オブジェクトがユニットとして運動するため、例えば他の交通加入者と同等の大きさのダイナミクス値を有していなければならないことにある。

【0053】

好ましくは、これに関連して、予め既知であると識別されたサーフェルをクラスタへとまとめることは、ｋ近傍アルゴリズム（k-Nearest-Neighbor-Algorithm）を用いて、特定のサーフェルの近傍にある予め定められた数の近傍サーフェルを識別することと、識別された近傍サーフェルから、この近傍サーフェルの姿勢と特定のサーフェルとの差を予め定められた限界値と比較することによって、サブグループを選択することとを含んでいる。特に、近傍サーフェルから特定のサーフェルまでの距離が、予め定められた第１の限界値と比較され、近傍サーフェルの法線ベクトルと特定のサーフェルの法線ベクトルとの間の角度が、予め定められた第２の限界値と比較される。２つの比較において近傍サーフェルの姿勢が特定のサーフェルの姿勢ときわめて類似していることが示された場合には、当該近傍サーフェルが同じオブジェクトに属するサーフェルであることが基礎とされる。すなわち、換言すれば、ここでのサブグループにおけるサーフェルは、同じオブジェクトに属するサーフェルを近似する。サブグループの１つのサーフェルが既にクラスタに割り当てられており、他のサーフェルがどのクラスタにも割り当てられていない場合、当該サブグループのさらなるサーフェルが同じクラスタに割り当てられる。サブグループのいずれのサーフェルも全くクラスタに割り当てられていない場合には、新たなクラスタが生成される。サブグループの１つのサーフェルが第１のクラスタに割り当てられ、当該サブグループの別のサーフェルが既に別のクラスタに割り当てられている場合には、新たなクラスタは生成されず、また他のクラスタへのサーフェルの再割り当ても行われない。換言すれば、第３のケースでは、既存のサーフェルのクラスタへの割り当てが維持される。

【0054】

本発明のさらなる好ましい発展形態によれば、さらに、受信された環境センサデータが点群の各点に対して測定時点を有しており、サーフェルデータとサーフェルマップとの比較により、生成されたサーフェルにおいて新しいサーフェルおよび予め既知であるサーフェルを識別するステップが、移動体システムの推定速度を考慮して、サーフェルデータから歪み補正されたサーフェルを生成し、当該歪み補正されたサーフェルデータとサーフェルマップとを比較することを含む。点群の各点が測定時点を有することの結果として、サーフェルは、好ましくは３つの主軸に沿った点の測定時点分布および／または測定時点分布の近似を含んでいる。よって、これらのデータが好ましくはサーフェルの歪み補正のために使用される。

【0055】

多くの環境センサは、１つもしくは複数のレーザーが自身を回転させることによって距離を測定するものであるので、移動体システムの移動によりいわゆるローリングシャッタ効果が生じる。この効果は、レーザーの回転の第１のデータ点の測定時点における環境センサの場所が、レーザーの回転の最後のデータ点における環境センサの場所と同じでないことから生じる。つまり、１回転の特定の期間内に記録された点群のすべての点を、環境センサが移動しなかった場合の個々の画像と同様に観察すると、これらの点は歪んでいるように見える。

【0056】

すなわち、この効果を考慮するために、好ましくは、サーフェルデータとサーフェルマップとの比較において、移動体システムの推定速度を考慮してサーフェルの新しい中心とサーフェルの新たな回転とを算定することにより、歪み補正されたサーフェルの生成が行われる。次いで、当該歪み補正されたサーフェルがさらにマッピング誤差を算定するために利用される。

【0057】

当該手法では、特に有利には、例えば移動体システムの慣性センサデータ、ＧＰＳデータおよび／または推進システムデータ、例えば車輪回転数センサデータなどの自己運動センサデータが存在せず、環境センサデータのみが受信される。このような場合、最適化の開始時の移動体システムの推定速度はきわめて不確実である。サーフェルの歪み補正がない場合、強いローリングシャッタ効果が信頼性の低い推定速度と組み合わされて最適化においてドリフトをもたらしうるものとなり、このため最適化全体の結果が損なわれる可能性がある。

【0058】

本発明のさらなる好ましい発展形態によれば、因子グラフが移動体システム上の１つもしくは複数の環境センサの取り付け高さを表現する少なくとも１つのノードを有し、当該方法が、
－サーフェルマップにおいて道路を表現するサーフェルを識別するステップと、
－道路を表現するサーフェルとして識別されたサーフェルに対して、環境センサ取り付け因子を因子グラフに追加するステップと、
を含む。

【0059】

複数の環境センサの場合、さらに好ましくは、取り付け高さを表現するノードは、複数の環境センサの相互の配向を考慮するように構成されている。

【0060】

特に、移動体システムが自動車として構成されている適用事例では、環境の測定中、移動体システムにおける環境センサの取り付け高さおよび基本配向または道路に対する環境センサ相互の基本配向は変化しないと仮定することができる。当該仮定は、好ましくは、取り付け高さが最適化可能なさらなる変数として因子グラフにおいてノードとして表現され、サーフェルマップにおいて道路を表現するサーフェルが識別されるという点において実現される。道路を表現するサーフェルは、好ましくは、高い円盤状性と、移動体システムの推定自己姿勢に対するその配向および／または移動体システムの推定自己姿勢の下方に中心を有するその姿勢と、に基づいて識別される。

【0061】

さらなる好ましい発展形態によれば、方法は、予め定められた数の最適化の後、サーフェルデータとサーフェルマップとの比較により、生成されたサーフェルにおいて新しいサーフェルおよび予め既知であるサーフェルを識別するステップにおいて予め既知であるサーフェルの識別に使用されなかったサーフェルをサーフェルマップから除去するステップを含む。すなわち、換言すれば、一度しか観察されていないサーフェルが、所定の時間の後、サーフェルマップから再び除去される。サーフェルマップからのサーフェルの除去により、サーフェルデータとサーフェルマップとの比較により生成されたサーフェルにおいて新しいサーフェルおよび予め既知であるサーフェルを識別するステップにおいて、必要とされる計算能力が小さくなる。

【0062】

さらなる好ましい発展形態によれば、方法は、当該サーフェルが予め定められた期間にわたって予め既知であるサーフェルの識別に使用されなかった場合、かつ／または当該サーフェルが予め定められた数のサーフェル因子を因子グラフに寄与させた場合、かつ／または当該サーフェルが予め定められた数の反復を超えて既に最適化問題の一部である場合に、サーフェルのさらなる最適化を除外するステップを含む。すなわち、換言すれば、好ましくは、数値最適化における出力上の理由から、サーフェルを最適化から除外することができる。さらなる最適化からのサーフェルの除外は、好ましくは、サーフェル因子を因子グラフ内のサーフェル凍結因子によって置き換えることにより、実現される。

【0063】

上記の課題はさらに、上述した方法を実行する手段を備えたデータ処理装置によって解決される。

【0064】

本発明はさらに、コンピュータによってプログラムが実行される際に当該コンピュータに上述した方法を実行させるための命令を含むコンピュータプログラム製品に関する。

【0065】

さらに、本発明によれば、上記のコンピュータプログラム製品を記憶したコンピュータ可読データ担体が提供される。

【0066】

データ処理装置、コンピュータプログラム製品およびコンピュータ可読データ担体の技術的な利点は、移動体システムの自己姿勢を決定し、環境の３Ｄ楕円体ベースのサーフェルマップを作成するための方法の説明ならびに以下に記載する実施例によって、当業者に明らかとなる。

【図面の簡単な説明】

【0067】

【図1】本発明の好ましい一実施例による、移動体システムの自己姿勢を決定し、環境の３Ｄ楕円体ベースのサーフェルマップを作成するためのコンピュータ実装方法（ＳＬＡＭ方法）によって作成された例示的な因子グラフを概略的に示す図である。

【図2】本発明の別の好ましい一実施例によるＳＬＡＭ方法のフローを概略的に示す図である。

【図3】図２のＳＬＡＭ方法によって作成された別の例示的な因子グラフを概略的に示す図である。

【図4】図２においてフローボックス内にまとめられたステップの詳細なフローを概略的に示す図である。

【図5】本発明の好ましい一実施例による、点群からサーフェルを算定する再帰的な方法を概略的に示す図である。

【図6】本発明の好ましい一実施例による、サーフェルに割り当てられたダイナミクス値を維持するフローを概略的に示す図である。

【図7】本発明の好ましい一実施例による、個々のサーフェルをクラスタへとまとめるフローの概略図である。

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

【0068】

図１には、好ましい一実施例による、移動体システムの自己姿勢を決定し、環境の３Ｄ楕円体ベースのサーフェルマップを作成するためのコンピュータ実装方法（ＳＬＡＭ方法）により作成された例示的な因子グラフ１０が概略的に示されている。因子グラフ１０は、確率論的グラフィックモデルの一形態である。因子グラフ１０内のノード１２は最適化問題の未知の変数を表現しており、一方、因子グラフ１０の因子１４は測定結果および／または予備知識に基づくこれらの変数に対する確率論的制約を表している。

【0069】

図１の因子グラフ１０は、ここでの実施例では、移動体システムの特定の自己姿勢を表現するノード１２ａと、マップ座標内の特定のサーフェル１６を表現するサーフェルノード１２ｂと、を有する。また、自己姿勢を表現するノード１２ａとサーフェル１６を表現するノード１２ｂとの間のエッジが、いわゆるサーフェル因子１４ａを有する。サーフェル因子１４ａは、ノード１２ａによって表現される自己姿勢と、環境センサに対する当該特定の自己姿勢から識別可能なサーフェル１６と、の間の空間的制約を表現しており、当該特定の自己姿勢からの、環境センサのための当該サーフェル１６の表示可能性に関する確率分布を含む。さらに、自己姿勢を表現する２つのノード１２ａ間のエッジは、各ノード１２ａによって表現される自己姿勢間の空間的制約を表現しておりかつ２つの自己姿勢間の変換に関する確率分布を含む、自己運動因子１４ｂを有している。

【0070】

図２にはＳＬＡＭ方法の好ましい一実施例のフローが概略的に示されている。以下では、方法の各ステップを説明するためのさらなる例示的な因子グラフ１０が図３に示されているので、図２および図３を参照して説明する。

【0071】

方法の準備ステップＳ１００，Ｓ２００，Ｓ３００において、移動体システムの自己運動モデル、センサモデルおよびマップモデルが初期化された後、次いで、因子グラフ１０がインクリメント方式で作成される。センサモデルの初期化Ｓ２００は、既存のリソースを考慮して行われる。ここでの実施例では、移動体システムに取り付けられて移動体システムの環境を測定した環境センサからの環境センサデータに加えて、別の自己運動センサデータ、すなわち慣性センサデータおよびＧＰＳデータも存在する。環境センサデータは、ここでの実施例では、ＬｉＤＡＲセンサデータである。

【0072】

因子グラフ１０をインクリメント方式で作成するために、因子グラフ１０は最初に初期化されなければならず、このことはステップＳ４００において行われる。このために、まず、ノード１２の種類が定義される。ここでの実施例では、４種類のノードが使用される。図１と同様に、移動体システムの特定の自己姿勢を表現するノード１２ａが初期化され、マップ座標において特定のサーフェル１６を表現するサーフェルノード１２ｂが初期化される。また、本発明では、それぞれのサーフェル１６に可変のダイナミクス値が割り当てられ、当該可変のダイナミクス値がサーフェルダイナミクスノード１２ｃとして初期化される。さらに、移動体システムでの環境センサの取り付け高さを表現するノード１２ｄが初期化される。図３では、ノード１２は、丸みを帯びた角部を有する直方体状のボックスとして示されている。

【0073】

その後、移動体システムの環境を特定の期間２０における点群１８として表現する環境センサデータをまず受信することによって、因子グラフ１０の構造とこの因子グラフ１０によって表される最適化問題のインクリメント方式の解とが見出される。その後、相応に移動体システムの特定の自己姿勢を表現するノード１２ａを因子グラフ１０に追加する各特定の期間２０に対して、２つの自己姿勢を表現するノード１２ａ間の自己運動因子１４ｂ，１４ｂ’，１４ｂ’’が追加される（ステップＳ５００およびＳ６００）。因子１４は、図３では菱形として図示されている。

【0074】

ここでの実施例では、さらなるリソースが供給されるため、自己運動因子１４ｂは、一方では自己運動モデルのモデル仮定に基づいており、同時に、受信された自己運動センサデータにも基づいている。ゆえに、図３には、移動システムの自己姿勢を表現するノード１２ａ間にそれぞれ３つずつの菱形、すなわち、自己運動モデルからの因子１４ｂ、受信された慣性測定ユニットデータからの因子１４ｂ’および受信されたＧＰＳデータからの因子１４ｂ’’が示されている。

【0075】

その後（ステップＳ７００において）、移動体システムの自己姿勢についての最初の推定を得るために、これまでに作成された因子グラフ１０によって表現される最適化問題が解かれる。以下の各ステップはマップモデルによって実行されるが、図２ではこれらのステップがフローボックス２２としてまとめられており、図４にその詳細が示されている。しかし、ここでは、フローボックス２２内のステップは、良好な理解のために短く扱う。移動体システムの自己姿勢についての最初の推定が得られた後、受信された環境センサデータからサーフェルデータが生成され、このようにして生成されたサーフェル１６において、サーフェルデータとサーフェルマップとの比較により、新しいサーフェル１６および予め既知であるサーフェル１６が識別される。新しいサーフェル１６に対しては、サーフェルノード１２ｂおよびサーフェル因子１４ａが因子グラフ１０に追加される。予め既知であると識別されたサーフェル１６に対しては、サーフェル因子１４ａが因子グラフ１０に追加される。当該過程は、受信された環境センサデータのすべての特定の期間２０が処理されるまで、特定の期間２０ごとに反復される。これに続いて、ステップＳ９００において、最適化問題の最終的な解決が行われ、ここから、受信された環境センサデータの各特定の期間２０における移動体システムの、最終最適化されたサーフェルマップと最終最適化された自己姿勢とが得られる。

【0076】

図４には、図２のフローボックス２２にまとめられているステップの詳細なフローが概略的に示されており、ここでの各ステップはマップモデルによって実行される。まず、ステップＳ８０５において、サーフェルデータとサーフェルマップとの比較における予め既知であるサーフェル１６の識別にこのときまで使用されなかったサーフェル１６が、サーフェルマップから除去される。また、当該サーフェルが予め定められた時間にわたって予め既知であるサーフェルの識別に使用されなかった場合、かつ／または当該サーフェルが予め定められた数のサーフェル因子を因子グラフ１０に寄与させた場合、かつ／または当該サーフェルが予め定められた数の反復を超えて最適化問題の一部である場合、ステップＳ８１０において、所定のサーフェル１６がさらなる最適化から除外される。最適化からのサーフェルの除外は、サーフェル因子１４ａを因子グラフ１０におけるサーフェル凍結因子１４ａ’によって置き換えることにより行われる。

【0077】

その後、ステップＳ８１５において、このときまでに最適化によって見出されていた解に基づいて、サーフェルマップ内のすべてのサーフェル１６が更新される。図２に関して既に言及したように、続いて、ステップＳ８２０で受信された環境センサデータが処理される。まず、ステップＳ８２５において、このために、点群１８の自己運動補正が、移動体システムのその時点での推定速度（すなわち、直接に連続する２つの期間の間での自己姿勢の差を期間の長さで除算したもの）を考慮して行われる。これに応じて、ステップＳ８３０では、受信された環境センサデータの点群１８をサーフェルデータへと変換することによって、サーフェル１６が作成される。

【0078】

当該サーフェルデータは、ステップＳ８３５においてサーフェルマップと比較され、これにより、生成されたサーフェル１６において予め既知であるサーフェルおよび新しいサーフェル１６が識別される。図４に示されている実施例では、３つのケースが区別されている。新しいサーフェルであると識別されたサーフェル１６の場合、このサーフェルを表現する新しいサーフェルノード１２ｂおよびサーフェル因子１４ａが因子グラフ１０に追加される（Ｓ８４０）。これに対して、予め既知であるサーフェル１６の場合、サーフェル因子１４ａが因子グラフ１０に追加される（Ｓ８４５およびＳ８５０）。このケースではさらに、予め既知であるサーフェル１６がさらなる最適化から除外されるサーフェル１６であるか否かが判別される。予め既知であるサーフェル１６がさらなる最適化から除外されるサーフェルであった場合、追加されるサーフェル因子１４ａは、図３に破線で示されているサーフェル凍結因子１４ａ’となる（ステップＳ８４０）。

【0079】

続いて、ステップＳ８６０において、道路を表現するサーフェル１６がサーフェルマップにおいて識別され、道路を表現していると識別されたサーフェルに対する因子グラフ１０に、環境センサ取り付け因子１４ｃが追加される。道路を表現するサーフェルが最適化から除外されるサーフェルである場合、凍結された環境センサ取り付け因子１４ｃ’が追加される。

【0080】

その後、以下で図６に関してさらに詳細に説明するように、ステップＳ８６５において、サーフェル１６に割り当てられたダイナミクス値が維持され、ステップＳ８７０において、同様に図７に関連してさらに詳細に説明するように、これらのダイナミクス値が、予め既知であると識別されたサーフェル１６として、相互の空間的距離および／または相互の類似した配向に基づいて、クラスタへとまとめられる。

【0081】

図５を参照しながら、最初に、好ましい一実施例による再帰的な方法を用いて、受信された環境センサデータの点群１８からサーフェル１６がどのように生成されるかを説明する。このために、点群１８の個々の点２４は予め定められたボクセル２６に割り当てられ、ここで、各ボクセル２６は環境の体積割合を表している。図５には２次元の例が示されているので、ボクセル２６が正方形によって表現されている。主成分分析を用いて、すべてのボクセル２６に対して、それぞれ１つずつのボクセル２６に割り当てられた点を有する第１のサーフェル１６が生成される。サーフェル２６は、図５では、点群１８の点２４の楕円形状の外縁によって図示されている。その後、すべてのボクセル２６がサブボクセル２８へと分割され、再び主成分分析によって第２のサーフェル１６が生成される。生成された第１のサーフェル１６の品質値の比較が第１のサーフェル１６に対するより良好な品質値を有している場合、または分割によって生成されたサブボクセル２８が予め定められた最小数の点２４を下回っている場合、生成された第１のサーフェル１６の品質値と生成された第２のサーフェル１６とを比較し、２つの先行のステップの実行を反復することによって（ただし、新たな実行の際にはそれぞれ先行の第２のサーフェル１６が第１のサーフェル１６に対応するものとされ、先行のサブボクセル２８が新しいボクセル２６に対応するものとされる）、最適なサーフェル２６が算定される。

【0082】

図６には、別の好ましい実施例による、各サーフェル１６に割り当てられたダイナミクス値をどのように維持するかについてのフローが概略的に示されている。まずは、サーフェル１６がボクセルマップ内のボクセル２６に割り当てられる。ボクセルマップのうちこのときまでにボクセル２６が記録されていない場所にあるボクセル２６にサーフェル１６が割り当てられた場合（図６のケースＡを参照）、ボクセルマップ内のこの場所に新しいボクセル２６が追加され、当該新しいボクセル２６にサーフェル１６のダイナミクス値が割り当てられる。これに対して、ボクセルマップ内に既に存在しているボクセル２６にサーフェル１６が割り当てられる場合（ケースＢを参照）、既存の当該ボクセル２６に割り当てられたダイナミクス値が更新される。当該更新は、ここでの実施例では、指数関数的に平滑化された平均によって行われる。続いて、ボクセル２６の更新されたダイナミクス値が限界値と比較され、ケースＣではボクセル２６内のすべての円盤状のサーフェルに対して限界値が下方超過された場合にダイナミクス値が低減され、ケースＤではボクセル２６内のすべての円盤状のサーフェルに対する限界値が上方超過された場合にダイナミクス値が増大される。

【0083】

図７には、さらなる好ましい実施例による、予め既知であると識別されたサーフェル１６を相互の空間的距離および／または相互の類似した配向に基づいてクラスタへとまとめるフローが概略的に示されている。図３においても見て取れるように、各クラスタについての因子グラフ１０は、最適化問題の解決中のクラスタ内のサーフェル１６のダイナミクス値の差にペナルティが課されるようにするクラスタ因子１４ｄを有する。

【0084】

図７に関連して、まずステップＳ１０００において、予め定められた数の、特定のサーフェル１６に最も近い近傍サーフェルが識別される。ケースＥおよびケースＦにおいて、近傍サーフェルと特定のサーフェル１６とから成るペアに対して、姿勢の差が予め定められた限界値よりも小さく、当該ペアの２つのサーフェル１６のうちの一方が既に１つのクラスタに割り当てられている場合、当該ペアの他方のサーフェル１６も同様に当該クラスタに割り当てられる。これに対して、ケースＧにおいてペアのサーフェル１６のいずれもクラスタに割り当てられていない場合、新しいクラスタが生成される。最後に、各クラスタに対して因子グラフ１０にクラスタ因子１４ｄが追加される。

【符号の説明】

【0085】

１０ 因子グラフ
１２ ノード
１２ａ 自己姿勢を表現するノード
１２ｂ サーフェルノード
１２ｃ サーフェルダイナミクスノード
１２ｄ 移動体システム上の環境センサの取り付け高さを表現するノード
１４ 因子
１４ａ サーフェル因子
１４ａ’ サーフェル凍結因子
１４ｂ 自己運動因子
１４ｂ’ 慣性測定ユニットデータのための因子
１４ｂ’’ ＧＰＳデータのための因子
１４ｃ 環境センサ取り付け因子
１４ｃ’ 凍結された環境センサ取り付け因子
１４ｄ クラスタ因子
１６ サーフェル
１８ 点群
２０ 期間、フレーム
２２ フローボックス
２４ 点
２６ ボクセル
２８ サブボクセル

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【外国語明細書】