JP5210396B2 - コンピュータを援用してセンサデータから物体の運動を計算する方法 - Google Patents

Description

本発明は、コンピュータを援用してセンサデータから物体の運動を計算する方法および相応する装置、ならびに相応するコンピュータプログラム製品に関する。

本発明は例えば車両、飛行機、ロボットなどのような移動物体の固有運動を求める技術分野に属する。このような物体の自律運動では、空間内での、例えば運動の２次元平面内での、物体の位置および向きを求めることが必要である。位置測定には、物体に取り付けられた測定センサ、例えばレーザスキャナが使用される。これらのセンサは物体の周囲環境を把握し、これによって複数の測定点を形成する。これらの測定点は、例えば周囲の物で反射したレーザスキャナのレーザ光線の伝搬時間から得られる。

レーザスキャナを位置測定に使用する場合、レーザスキャナはたいてい、地面に平行に光線を旋回させ、最も近い反射点までの距離を測定するように取り付けられる。このようにして、２次元測定点の系列が自律物体の座標として得られる。なお、この系列はスキャンとも呼ばれる。これらのデータから物体の固有運動を求めるために、先行技術では、早い時点で行われたスキャンを現スキャンと比較することが知られている。その際、これら両方のスキャンをできるだけ良好に相互に写像する変換が探される。

先行技術では、様々な時点における測定点の比較に基づいて物体の固有運動を求める標準的な方法が種々存在している。よく用いられている方法を概観するには、文献［１］を参照されたい。

公知の方法は基本的には２つのグループに分かれる。第１のグループの方法は複数の測定点から構造要素、とりわけ直線分を抽出し、これらの構造要素をできるだけ良好に相互に写像する変換を探す。第２のグループは検出された測定点の形態で生データを処理する。第１のグループは例えば室内のような構造化された環境の中で正確に、高い信頼性をもって動作する。これに対して、第２のグループは汎用的であるが、正確でなく、計算コストも高い。これら２つのグループの方法は、周囲環境が部分的にしか構造化されていない場合、例えば単独の壁しか抽出できない場合には、良く機能しない。この場合、第１のグループの方法は、壁に平行な、物体の変位は抽出せず、第２のグループの方法は、互いに比較される測定点の、偶然にしかも誤って割り当てられたペアに固執する傾向がある。

本発明の課題は、コンピュータを援用してセンサデータから物体の運動を計算する方法において、物体の固有運動を簡単に、高い信頼性をもって求めることができるようにすることである。

この課題は独立請求項１、請求項２６、または請求項２７に記載されている特徴によって解決される。本発明の展開形態は従属請求項に定められている。

本発明による方法では、物体に取り付けられたセンサのセンサデータが処理される。その際、センサデータは様々な時点に検出された物体周囲の測定点からなる測定点の集合を含んでおり、第１の時点に検出された第１のデータ集合と第２の時点に検出された第２のデータ集合との間の物体の運動が求められる。

本発明による方法では、第１のステップａ）において、第１の測定点集合から１つまたは複数の構造タイプの構造要素を抽出することによって、第１のデータ集合を得る。同様に、第２の測定点集合から構造要素を抽出することによって、第２のデータ集合を得る。第１および第２のデータ集合はそれぞれ抽出された構造要素と、構造要素に割り当てることのできない割り当て不能な測定点とを含んでいる。

ステップｂ）では、第２のデータ集合の同じ構造タイプの構造要素が第１のデータ集合の相応する構造要素に対応付けられる。対応付けが見つかるかぎり、その対応付けに基づいて、第２のデータ集合の構造要素を第１のデータ集合の構造要素に写像する変換シーケンス（場合によってはただ１つの変換だけ、対応付けが存在しない場合には、変位のない零変換を含む）が実行され、それにより第３のデータ集合が得られる。

次にステップｃ）において、第３のデータ集合の構造要素が第１のデータ集合の割り当て不能な測定点に対応付けられる。対応付けが見つかるかぎり、その対応付けに基づいて、第３のデータ集合の構造要素を第１のデータ集合の割り当て不能な測定点に写像する第２の変換シーケンス（場合によってはただ１つの変換だけ、対応付けが存在しない場合には、変位のない零変換を含む）が実行され、それにより第４のデータ集合が得られる。

最終的に、少なくとも第１および第２の変換シーケンスに基づいて、第１の時点と第２の時点の間の物体の運動が求められる。

本発明による方法は、適切な仕方で、構造情報を構造要素の形態でも、割り当て不能な測定点としても処理できることを特徴としている。したがって、本願発明の方法は冒頭で述べた第１のグループの方法と第２のグループの方法の双方の利点を併せ持つ。本願発明の方法では、まず抽出した構造要素が一致するかどうかが調べられ、続いてその結果得られた変換が割り当て不能な測定を構造要素と比較することによって精緻化される。このようにして、検出された測定点の２つの集合を比較することによって、対応付けの誤りが少ししかないまたは全くない形で、物体の固有運動を簡単に求めることのできる方法が得られる。

本発明による方法の好適な実施形態では、抽出される構造要素は少なくとも直線分タイプの構造要素である。有利には、本方法のステップｂ）において、第１の構造要素として直線分タイプの構造要素が抽出される。その際、構造要素の抽出には、例えば文献［１］に記載されている公知の方法を使用してよい。

本発明による方法の好適な実施形態では、抽出される構造要素は別の構造タイプも、とりわけ円弧のタイプも含む。これら別の構造タイプを使用することにより、構造要素間のより正確な対応付けが、したがってまたデータ集合相互間のより正確な写像が達成される。

本発明による方法の好適な実施形態では、ステップａ）において、まず連続する測定点を含む連結成分が求められる。なお、１つの連結成分に属する隣り合う測定点は最大の点間距離を超えないものとする。この場合、円弧タイプの構造要素は以下のようにして連結成分から特に簡単かつ効率的に抽出することができる。
まず、円を計算する。ただし、この円は、連結成分の各端部にある境界測定点と、連結成分の境界測定点の間の測定点、とりわけ境界測定点間の実質的に中点に当たる測定点とを通る。連結成分の各測定点の円までの最小距離が所定の閾値よりも小さければ、連結成分の測定点は計算された円に割り当てられ、そうでなければ、円からの距離が最大である連結成分の測定点において連結成分が２つの連結成分に分割される。

上記の方法によれば、短時間の簡単な幾何学的考察によって、構造要素として処理しうる適切な円弧が得られる。

本発明による方法の特に好適な実施形態では、ステップｂ）およびｃ）において、適切な統計的手法によってセンサの測定雑音も考慮される。これにより、物体の固有運動を求める際の正確さが高まる。好ましくは、この測定雑音は共分散行列および相応するマハラノビス距離の形態で取り込まれる。これらの量は当業者には周知である。ステップｂ）の第１の変換シーケンスは特に以下のように反復によって決定される。
ステップｂ．ｉ）において、変換された第２のデータ集合の構造要素と第１のデータ集合の構造要素をそれぞれ含んだ構造要素ペアの１つの集合について、構造要素ペアの構造要素を互いに写像する複数の変換のマハラノビス距離を計算する。ただし、前記マハラノビス距離はその時の共分散行列に依存している。その上で、マハラノビス距離を考慮して構造要素ペアが選ばれ、選ばれた構造要素ペアの構造要素を互いに写像する変換が行われる。これによって、変換された新しい第２のデータ集合が得られる。

ステップｂ．ｉｉ）では、構造要素ペアの集合から少なくとも選ばれた構造要素ペアが削除され、共分散が更新され、ステップｂ．ｉ）に戻る。

上記の反復プロセスでは、ステップｂ．ｉ）における変換の選択の際に、好ましくはマハラノビス距離の小さな変換および／または長い構造要素の間の変換および／または変換の集まりが選好される。

ステップｂ．ｉ）で行われる変換は、本発明の好適な実施形態では、とりわけカルマンフィルタに基づいて推定される。ステップｂ．ｉｉ）における共分散行列の更新はカルマンフィルタを使用しても同様に特に効率的に行われる。なお、その際、共分散行列を新たに推定するためにカルマンフィルタの更新式が使用される。

好ましくは、ステップｂ．ｉｉ）において、所定の閾値よりも大きなマハラノビス距離を有する変換によって互いに写像される構造要素から成る構造要素ペアも構造要素の集合から削除される。ただし、上記マハラノビス距離は更新された共分散行列に依存する。

本発明による方法の別の実施形態では、第１の変換シーケンスを求めるための反復の開始時には、変換された第２のデータ集合はステップａ）で得られた第２のデータ集合と一致しており、構造要素ペアの集合は、第１のデータ集合と第２のデータ集合の構造要素を含む可能なすべてのペアまたは事前に選ばれた構造要素ペアを含んでいる。ただし、事前に選ばれた構造要素ペアには、所定の閾値以下のマハラノビス距離を有する変換によって互いに写像される構造要素が含まれている。ただし、上記マハラノビス距離は初期共分散行列に依存する。

特に好ましい実施形態では、最初の共分散行列は走行距離測定の測定ノイズに基づいてまたは運動モデルから求められる。

本発明による方法の別の実施形態では、ステップｃ）の第２の変換シーケンスは以下のように反復によって求められる。

ステップｃ．ｉ）において、変換された第３のデータ集合の構造要素と第１のデータ集合の割り当て不能な測定点をそれぞれ含んだ構造要素-測定点ペアの１つの集合について、構造要素-測定点ペアの構造要素を構造要素-測定点ペアの割り当て不能な測定点に写像する複数の変換のマハラノビス距離を計算する。ただし、このマハラノビス距離は共分散行列に依存している。その上で、マハラノビス距離を考慮して構造要素-測定点ペアが選ばれ、構造要素を選ばれた構造要素-測定点ペアの測定点に写像する変換が行われる。これによって、変換された新しい第３のデータ集合が得られる。

ステップｃ．ｉｉ）では、構造要素-測定点ペアの集合から少なくとも選ばれた構造要素-測定点ペアが削除され、共分散が更新され、ステップｃ．ｉ）に戻る。

好ましくは、上記の反復プロセスでは、ステップｃ．ｉ）における変換の選択の際に、マハラノビス距離の小さな変換および／または類似した変換の集まりが選好される ステップｃ．ｉ）で行われる変換は、本発明の好適な実施形態では、とりわけカルマンフィルタに基づいて推定される。共分散行列の更新もカルマンフィルタを用いて行ってよい。

上記反復プロセスの好適な実施形態では、ステップｃ．ｉｉ）において、所定の閾値よりも大きなマハラノビス距離を有する変換によって割り当て不能な測定点に写像される構造要素を含む構造要素-測定点ペアも、構造要素-測定点ペアの集合から削除される。ただし、上記マハラノビス距離は更新された共分散行列に依存する。

本発明による方法の別の実施形態では、第２の変換シーケンスを求めるための反復の開始時には、変換された第３のデータ集合はステップｂ）で得られた第３のデータ集合と一致しており、構造要素-測定点ペアの集合は、第３のデータ集合と第１のデータ集合の割り当て不能な測定点を含む可能なすべてのペアまたは事前に選ばれた構造要素-測定点ペアを含んでいる。なお、事前に選ばれた構造要素-測定点ペアには、所定の閾値以下のマハラノビス距離を有する変換によって構造要素が割り当て不能な測定点に写像されるような構造要素-測定点ペアが含まれている。ただし、上記マハラノビス距離は現在の共分散行列に依存する。

本発明による方法は好適な実施形態ではさらにステップｃ）の実行後にまだ残っている割り当て不能な測定点の対応付けも探す。これにより、データ集合の相互の写像をさらに正確に求めることが可能になる。残っている割り当て不能な測定点は上で定義した４つのデータ集合に含まれており、これらの測定点は、適切な対応付けが見つかる限り、第１のデータ集合の割り当て不能な測定点と対応付けられる。次に、上記の対応付けに基づいて第４のデータ集合の割り当て不能な測定点を第１のデータ集合の割り当て不能な測定点に写像する第３の変換シーケンス（場合によってはただ１つの変換のみを含んでいてもよいし、対応付けが存在しなければ、変位なしの零変換を含んでいてもよい）が実行され、これにより第５のデータ集合が得られる。そして、第１の時点と第２の時点の間の物体の運動が第１、第２および第３の変換シーケンスに基づいて求められる。

本発明による方法の上記の実施形態では、第３の変換シーケンスを求めるために、とりわけ以下のステップを有する反復が行われる。
ステップｉ）では、変換された第５のデータ集合の割り当て不能な測定点と第１のデータ集合の割り当て不能な測定点とを含む測定点ペアからなる１つの集合について、測定点ペアの割り当て不能な測定点を互いに写像する変換の、共分散行列に依存するマハラノビス距離を計算し、このマハラノビス距離を考慮して、測定点ペアを選び、選ばれた測定点ペアの測定点を互いに写像する変換を実行する。これにより、変換された新しい第５のデータ集合が得られる。
ステップｉｉ）では、上記の測定点ペアの集合から少なくとも上記の選ばれた測定点ペアを削除し、共分散行列を更新し、ステップｉ）に戻る。上記ステップｉ）において変換を選択する際、好ましくは最小のマハラノビス距離を有する変換および／または同様の変換の集まりが選好される。

上で第５のデータ集合を求める際、再び統計的手法を用いてセンサの測定雑音を考慮することが好ましい。本発明の有利な実施形態の１つでは、ステップｉ）で実行される変換はとりわけカルマンフィルタに基づいて推定される。共分散行列の更新もカルマンフィルタを用いて行うことが好ましい。

本発明の別の実施形態では、センサデータは物体にレーザスキャナをあてて得られたレーザスキャンの形態で処理される。本発明による方法の１つの好ましい適用分野はロボットおよび／またはクレーンおよび／または車両の運動の分析、とりわけこのような物体の自律誘導のための運動分析である。

上記の方法の他に、本発明はさらにコンピュータを援用して物体に取り付けられたセンサのセンサデータから物体の運動を計算する装置も包摂している。ここで、センサデータは、様々な時点において検出された、物体周囲の測定点からなる集合を含んでいる。上記の装置は、第１の時点に検出された第１の測定点集合と第２の時点に検出された第２の測定点集合との間における物体の運動を求める手段を有している。この手段は、装置の動作中に上で説明した本発明による方法を実行するように構成されている。

本発明はさらに、コンピュータ上でプログラムを走らせたときに本発明による上記の方法を実行する、機械可読媒体に記憶されたプログラムコードを含んだコンピュータプログラム製品にも関する。

以下では、本発明の実施例を添付の図面に基づいて詳しく説明する。

本発明による方法の実施形態による連結成分の抽出を説明した概略図である。 本発明による方法の実施形態による円の抽出を説明した概略図である。 測定点の構造要素を本発明による方法の１つの実施形態に従って対応付ける本発明による方法の流れを描写した概略図である。 測定点の構造要素を本発明による方法の１つの実施形態に従って対応付ける本発明による方法の流れを描写した概略図である。 測定点の構造要素を本発明による方法の１つの実施形態に従って対応付ける本発明による方法の流れを描写した概略図である。 測定点の構造要素を本発明による方法の１つの実施形態に従って対応付ける本発明による方法の流れを描写した概略図である。 測定点の構造要素を本発明による方法の１つの実施形態に従って対応付ける本発明による方法の流れを描写した概略図である。

ここに説明する本発明による方法の実施形態では、センサデータはレーザスキャナによっていわゆるレーザスキャンとして捉えられた２次元の点の形態で考察される。スキャン内の個々の点は、スキャンの最中に動くレーザ光線がレーザスキャナの取り付けられた物体の周囲にある物体において反射することにより生じる。以下に説明する方法の目的は、様々な時点に求めたレーザスキャンを互いに変換し、それによってこれらの時点の間における物体の運動を求めることにより、物体の固有運動を求めることである。

そもそも異なる２つのレーザスキャンを互いに比較できるようにするため、ここに記載する本発明による方法の実施形態では、各スキャンの個々の測定点から直線分と円の形態で構造要素を求める。これらの構造要素を抽出するために、まず連結成分を点の列として求め、続いて連結成分から対応する構造要素を決定する。

本発明の方法は、２つのスキャンを互いに変換する際に、抽出された構造要素だけでなく、割り当て不能な測定点も、すなわち構造要素を割り当てることのできない測定点も処理することを特徴としている。したがって、構造要素と測定点とを含む複合的なスキャンが処理される。本願明細書で説明する実施形態におけるこのような複合スキャン内の要素は点Ｐ、直線分Ｌおよび円Ｃの集合によって特徴付けられている。したがって、要素ｅ_iを有するスキャンＳは次のように表される。

既に述べたように、直線分と円を抽出するためには、まずスキャン内の測定点の連結部分を求めなければならない。図１には、壁Ｗに沿って配置された測定点を基に、スキャンを連結部分に分割するアルゴリズムが示されている。測定点の連結領域は最大の点間距離によって次のように特徴付けられる。すなわち、測定点は測定点間の距離が最大の点間距離以下である場合にのみ連結成分に属する。図１では、壁Ｗ上の２つの点１と２の間の最大距離がａｍａｘで表されている。軸ｘおよびｙを有するセンサシステムＳは点１ないし２を隣り合う光線Ｂ１とＢ２を介して捕捉する。壁Ｗ上で隣り合う測定点の間の距離ａは、正弦定理により、左側の光線Ｂ１または右側の光線Ｂ２と図１において破線で示されている仮想的な真ん中の光線Ｂ３との間の角度距離δ／２と、この真ん中の光線と壁Ｗとの間の角度γと、センサシステムＳによって測定された、センサシステムの原点から点１までの距離（ｄ₁で表す）とセンサシステムＳの原点から点２までの距離（ｄ₂で表す）の２つの距離とから得られる。
特に

真ん中の光線Ｂ３と壁Ｗの間の角度γをパラメータとして選ぶことによって、隣り合う点のペアの各々について、壁上で隣り合う点の間の距離の上限値ａｍａｘが得られる。

つぎに、以下の方法に基づいて、連結成分の抽出が距離ａｍａｘを用いて行われる。
まず、スキャンの第１の測定点を最初の現連結成分に分類する。つぎに、連結成分の最後の点と次の点との間の距離が最大距離ａｍａｘ以下ならば、この現連結成分を次の点まで拡張する。しかし、最後の点から次の点までの距離が最大距離ａｍａｘよりも大きければ、次の点は新しい現連結成分に割り当てられる。割り当てられていない測定点はスキャン内に残り、捨てられはしない。最小の個数よりも少ない点を含む連結成分は再び削除され、割り当て不能な測定点としてスキャン内に戻される。

すべての連結成分を求めた後、最後に、直線分および円の形態で幾何学的特徴を個々の連結成分から抽出する。その際、各連結成分を一方では直線分で近似し、他方では円で近似することが試みられる。さらに、例えば直角の角のような他の要素もスキャンから抽出してよい。スキャンからの直線分の抽出は当業者には十分に知られており、例えば文献［１］に記載されている。ここで説明する実施形態では、まずよく行われている方法によって直線分をスキャンから抽出する。つぎに、円の再帰的抽出を行う。この抽出については、以下に図２を参照して説明する。

図２の上部には、測定点Ｐ１，Ｐ２，．．．，Ｐ１２からなる連結成分が示されている。この点の集合から円または円弧を抽出するために、各連結成分を円に分解する。その際、考察している連結成分が短すぎる場合、すなわち、点の個数が閾値に満たない場合には、この連結成分の測定点は再びスキャンに戻される。図２の連結成分から円を抽出するために、まず境界点Ｐ１およびＰ１２ならびに連結成分の中央の点Ｐ８を考察する。これら３つの点からは、これら３つの点すべてを通る周を有する円が一意に決まる。これは境界点Ｐ１，Ｐ１２を中央の点Ｐ８とそれぞれを直線で結び、各直線の垂直二等分線を求めることによって行われる。ここで、垂直二等分線の交点が円の中心点である。

これによってまず円周Ｃ上に点Ｐ１，Ｐ８およびＰ１２が載った円が得られる。この円については、半径ｒと中心ｃ＝（ｃ_x，ｃ_y）が知られている。次のステップでは、円の連結成分の各点の間の距離の平方が計算される。すなわち、下記の距離が計算される。

ここで、ｐ_xおよびｐ_yは連結成分の各点のｘおよびｙ座標を表している。ここで連結成分の点の間のすべての距離が所定の閾値よりも低くければ、その線分は円弧に分類される。

図２に示されている実施形態では、個々の距離ｄ（ｐ，ｃ，ｒ）は点と円周とを結ぶ垂直線として示されている。この例では、点Ｐ６と円周の間の距離は所定の閾値を超えているが、このことは点Ｐ６が黒く塗りつぶされていることによって示されている。したがって、この連結成分は円に分類される。それゆえ、次のステップでは、この連結成分が最大距離の点において、すなわち、図２の点Ｐ６において分割される。これは図２の下側に示されており、今や２つの連結成分が存在する。第１の連結成分は点Ｐ１〜Ｐ６から成っており、第２の連結成分は点Ｐ６〜Ｐ１２から成っている。各連結成分についても、上記と同様のことが行われる。すなわち、第１の連結成分の境界点Ｐ１およびＰ６と中央点Ｐ３とに基づいて相応の円が求められ、第２の連結成分の境界点Ｐ６およびＰ１２と中央点Ｐ９とに基づいて円が求められる。図２の例では、２つの連結成分に関して、円周の個々の点の距離は所定の閾値よりも小さいので、２つの線分は円に分類される。

上記のアルゴリズムは過度のセグメント化の傾向がある、すなわち、１つの円弧の代わりに、ほぼ同じ中心と半径を持つ２つの互いにぶつかり合う円弧が見つけ出される傾向がある。この２つの円弧は後で再びまとめてもよい。続いて、直線分の抽出の場合と同じように、円の中心と半径が連結成分に相応して、例えば以下の最小値を求めることにより、正確に推定される。

ここで、Ｚは連結成分の点の集合を表している。この最小値を見つけるためには、ニュートン法、準ニュートン法または共役勾配法のような先行技術において周知の最適化法を使用してよい。

スキャン内の点が抽出された直線分にも抽出された円弧にも現れる場合、直線分と円弧との間のオーバラップ領域における対立は個々の線分の長さとオーバラップ領域内での各偏差の和とに基づいて解消される。その際、（点の個数によって測って）長い方の線分が選好される。

複合スキャン、すなわち、抽出された直線分、円および割り当て不能な測定点を有するスキャンを得た後、最後に構造要素と２つのスキャンの測定点を互いに写像するためにスキャンマッチングアルゴリズムが実行される。このスキャンマッチングアルゴリズムについては、まず図３〜７に基づいて一般的な説明をし、後でアルゴリズムと使用される数学的な手法の詳細な説明を行う。

図３には、十字形の点によって示された、基準スキャンとも呼ばれる第１のスキャンが示されている。なお、いくつかの点には参照記号Ｍ１〜Ｍ５が付されている。さらに、円形の点によって示された第２のスキャンも描かれており、いくつかの点には参照記号Ｍ１’〜Ｍ５’が付されている。目標は、第２のスキャンの点を基準スキャンに写像する変換を求めることである。スキャンの相互の写像はセンサの測定雑音を考慮して行われる。これにより、個々のスキャンの間の点と構造要素との相応する対応付けが誤って解釈される確率が低くなる。

ここに説明する実施形態では、２つのスキャンを互いに写像する変換は変換シーケンスの個々の変換を求めることによって段階的に求められる。その際、測定雑音は変換シーケンスの個々の変換の運動共分散行列の形で考慮される。変換は、ｘ成分およびｙ成分として変換による並進を含み、別の成分として角度βの形で変換の回転を含むベクトルによって記述される。このような変換に対して共分散行列が求まる。変換の成分は確率変数として考察され、共分散行列の各要素が相応する要素の共分散を表す。ここで、共分散は第１の確率変数とその期待値の間の差と第２の確率変数とその期待値の間の差との積からなる期待値として定義されている。ここに説明する実施形態では、共分散に基づくセンサの測定雑音は公知のマハラノビス距離に基づいて考慮される。マハラノビス距離については、後でより詳細に説明する。

図３〜７には、センサ系の位置が２つの軸Ａ１およびＡ２の原点として描かれている。現在の運動共分散ないしマハラノビス距離は円または楕円によって示されている。これにより、測定雑音に基づく運動の確率が表される。楕円の場合には、運動の確率は半短軸の方向の方が半長軸の方向よりも低い。円の場合には、運動の確率はすべて方向において同じである。

方法の開始時にまず、例えば走行距離データから、つまり、スキャナが取り付けられた物体の車輪の運動から求められる初期共分散行列が設定される。図３の例では、初期共分散行列に関していずれの変換方向も同確率であるから、運動共分散は円Ｃとして表されている。つぎに、図３のスキャンにおいて、上で述べた構造要素の抽出が行われる。その際、各スキャンについて、直線分が抽出される。なお、この直線分は基準スキャンにおいてはスキャン点Ｍ１およびＭ２ならびにその間のすべての点を含んでおり、第２のスキャンにおいては点Ｍ１’およびＭ２’ならびにその間のすべての点を含んでいる。

続いて、もしあるならば、直線分の他に円も抽出される。図３の例では、円は見つからず、基準スキャンについては、どの構造要素にも割り当て不能な３つの測定点Ｍ３，Ｍ４およびＭ５があるだけであり、第２のスキャンについても、Ｍ３’，Ｍ４’およびＭ５’があるだけである。この抽出の後、両方のスキャンの抽出された直線分が互いに対応付けられる。その際、直線分相互の変換の運動共分散とマハラノビス距離が考慮される。図３の例では、スキャンごとにちょうど１つの直線分が見つかる。また、これらの直線分は、これら直線分を互いに写像する変換が運動共分散と良く協調するように、つまり、相応する変換が小さなマハラノビス距離を有するように、互いに対して位置している。これに応じて、第２のスキャンはこれらの直線分を互いに写像するために、図４に示されているように動かされる。変換後には、共分散行列の更新が行われる。その際、このために先行技術から公知のカルマンフィルタが用いられる。

図４から分かるように、今やすべての運動が同じ確率というわけではない。このことは相応する楕円Ｅによって示されている。特に、軸Ａ１の方向の運動は軸Ａ２の方向の運動よりも確率が低い、つまり、軸Ａ１の方向において、軸Ａ２の方向におけるよりも大きなマハラノビス距離が得られる。図４にはさらに、抽出され互いに写像された直線分Ｌと、基準スキャンの割り当て不能な測定点Ｍ３〜Ｍ５および第２のスキャンのＭ３’〜Ｍ５’が示されている。

次のステップでは、割り当て不能な測定点Ｍ３’〜Ｍ５’の直線分Ｌへの対応付けの探索が行われる。マハラノビス距離が非常に大きいため、軸Ａ１の方向におけるさらなる運動の確率は非常に低いので、割り当て不能な測定点と直線分Ｌとの間の対応付けはアルゴリズムによって受理されない。それゆえ、アルゴリズムの次のステップでは、各スキャンの割り当て不能な測定点どうしの対応付けが行われる。図４に示されているように、変換の最も有利な方向は軸Ａ２に沿った方向である。測定点間の対応付けを求めるために、再び点のペアの間の可能な変換が考察され、これらの変換のマハラノビス距離に基づいて点Ｍ３とＭ３’が互いに対応付けられ、第１の変換によって互いに写像される。なお、この変換は図５では直線Ｇ１によって表されている。変換の実行後には再び共分散行列が更新される。

最後に、点対点の対応付けがさらなる点のペアについて繰り返される。その際、次のペアとして、測定点Ｍ４とＭ４’が図６の直線Ｇ２によって示されているように互いに対応付けられる。相応する変換を実行すると、より小さな共分散行列が得られる。このことは図５の楕円Ｅに比べて図６の楕円Ｅが相当して小さくなっていることから分かる。最後に、測定点Ｍ５’とＭ５を互いに対応付けることによって相応の変換が行われる。したがって、図７から分かるように、点のペアの個々の変換がすべて実行された後には、両方のスキャンを非常に良く相互に写像する変換が全体で１つ得られる。そのとき、共分散行列は十分に小さいので、スキャンマッチは十分な確実性をもって正しいと見なしうる。

以前に行われた直線分を直線分に写像し、測定点を相互に写像するすべての変換を全部合わせた変換から、最終的に物体の固有運動が求まる。

以下に、２つの複合スキャンを写像するスキャンマッチングアルゴリズムの個々のステップを詳細に説明する。

点ないし構造要素ｅ₁〜ｅ_nから成るスキャンＳ＝｛ｅ₁，．．，ｅ_n｝、例えば以前に記録した基準スキャンＳ^rは固定されているものと見なし、もう一方の、特に現在の、スキャンＳ^a ₀は可動であると見なす。これらの要素は点、直線または円弧としてよい。同時に、スキャンマッチングアルゴリズムを呼び出した後、初期共分散Σ₀に可動スキャンの位置Ｔ_k＝（ｘ₀ ｙ₀ β₀）^T＝（０ ０ ０）^Tの確率分布が渡される。

次のステップでは、まず複合スキャン内で直線分が相互に比較される。一方のスキャンに直線分が存在していなければ、直線分と点の比較へと進むか、複合スキャン内に円弧が含まれているならば、円弧の比較へと進む。

直線分のペア（Ｌ^a _i，Ｌ^r _j），Ｌ^a _i∈Ｓ^a _k，Ｌ^r _j∈Ｓ^rごとに直線分と直線分を比較するために、変換の集合｛Ｔ_i,j｜Ｔ_i,j（Ｌ^a _i）とＬ^r _jは共線｝が決定される。明らかにこれは、可動の直線分を基準スキャン内の基準直線分に平行な方向に向かせ、基準直線分上に移す変換である。変換の空間において、この集合自体が再び直線を形成する。各変換Ｔ＝（ｘ ｙ β）^Tにはそのときの運動共分散Σに依存するマハラノビス距離

が割り当てられる。
このマハラノビス距離は１つの直線分ペアについてパラメータとして直線分に沿った変位を含んでいる。

今度は、直線分の間の可能な変換から１つの変換が選ばれ、そうすることで、割り当てられた直線分ペアが選ばれる。このために以下の基準が用いられる。
・長い直線分の間の変換は短い直線分の間の変換よりも重要である。
・ノルムの小さい変換が良い。
・変換のクラスタが良い。
これらの基準に基づいて種々のストラテジーが導出される。本発明の１つの実施形態では、ノルムの十分に小さな、最も長い直線分の間の変換が選ばれる。これにより、誤った対応付けの危険性が最小化される。

次のステップでは、変換Ｔ_k+1によってスキャンが並進され、回転されることによって、下記の新しいスキャンが得られる。
Ｓ^a _k+1＝Ｔ_k+1（Ｓ^a _k）
ここで説明する実施形態では、変換は推定によって求められる。推定された変換は、請求項に従って実施される変換ないし変換シーケンスの特定の実施形態である。以下に、推定法の例を説明する。この例では、現スキャンの直線分Ｌ^a _iが基準スキャンの直線分Ｌ^r _ｊ上へと並進および回転させられる。直線分の端点の位置は非常に不確実であるから、変換の推定に用いるべきでない。それゆえ、直線上に載っている直線分は、この直線に従って、長さとこの直線への垂線の向きとによって、いわゆるヘッセの標準形

によって表される。文献［２］には、直線分に属するスキャン点からこの表現を導く方法が記載されている。この表現はカルマンメカニズムの意味での「測定」と解釈される。

文献［２］ではさらに、このような直線分の共分散がスキャン点の共分散から求められる。これはもっと後で基準直線分の共分散Σ_rと可動直線分の共分散Σ_aを求める際に使用される。

カルマンフィルタの意味で推定されるべき状態は現スキャンと基準スキャンとの間の変換である。それゆえ、

が予測の役割を占め、

が「測定」として用いられる。予測の共分散は直線分抽出の共分散Σ_aと変換のこれまでの共分散とから構成される。

物体の変換Ｔ_{(x y β)}によって予測

は

に移される。
これのヤコビアンは

である。予測の共分散は運動共分散Σ_G＝Ｇ・Σ_k・Ｇ^Tから得られる。測定の確実性のために、これに共分散Σ_aが加えられる。それゆえ、カルマンフィルタのゲインはＫ_k＝Σ_k・Ｇ^T・（Ｇ・Σ_k・Ｇ^T＋Σ_a＋Σ_r）^-1と計算される。変換はこのランドマークの対応付けから

として計算される。

運動共分散Σ_k+1も同様にカルマンフィルタの更新式に従って下記のように新たに推定される。
Σ_k+1＝（Ｉｄ−Ｋ_k・Ｇ）・Σ_k
いま求めたこの変換Σ_k+1により直前の現スキャンが変換され、新しい直前のスキャンが結果として得られる。すなわち、
Ｓ^a _k+1＝Ｔ_k+1（Ｓ^a _k）
まだ考慮されていない直線ペアが存在するならば、ｋ←ｋ＋１とし、上記ステップに戻り、マハラノビス距離に基づいて直線分ペアを選ぶ。その際、いま考慮した直線分ペアはそれ以上考察されない。

直線分の比較を行った後、複合スキャン内で円と円が比較され、最後には測定点と直線分が比較される。円と円との比較は直線分と直線分との比較と同様に行われるので、その方法をもう一度説明することはしない。測定点を直線分と比較する際、測定点を直線分上に移す可能な変換は２次元多様体を形成する。実際的な考慮から、つねに適切な予選を行う。ここに説明する実施形態では、この予選は確率ノルムに基づいてその時点の有利な共分散によって決定される、すなわち、

に基づいて決定される。新しい共分散は以前のケースと同様に直線分の共分散、測定点の共分散、および測定点と直線分から成るペアを選ばれた変換に写像する関数から得られる。点と直線分のペアは再び逐次的に処理される。その際、ありそうにないペアは、つまりマハラノビス距離が閾値を超えているペアは捨てられる。

最後に、また測定点ペアが処理される。それも直線分ペアないし点と直線分のペアと同様に処理される。

最終的に、複合スキャン相互の写像を記述する変換シーケンスが得られ、この変換シーケンスから物体の固有運動が求められる。

いま説明した本発明による方法の実施形態は数々の利点を有している。対応付け誤りの危険性に従って順に構造要素ないし点の考えられうる対応付けを使用することによって、割り当て誤りの不確実性が低下する。スキャンマッチの際にカルマンフィルタを使用することにより、測定の不確実性が統計的に適切に考慮される。カルマンフィルタの代わりに、他の不確実性の記述、例えば粒子フィルタのような無作為標本に基づいた記述を使用してもよい。必要ならば、運動の不確実性を構造要素ないし点の対応付けに重み付けすることによって考慮するように構成された方法を使用してもよい。

参考文献
[l] Jens-Steffen Gutmann: ,,Robuste Navigation autonomer mobiler Systeme", Dissertation, 2000, Albrecht-Ludwig-Universitaet Freiburg
[2] Michael Bosse: "ATLAS: A Framework for Large Scale Auto-mated Mapping and Localization", Disseration MASSACHUSETTS INSTITUTE OF TECHNOLOGY, Seiten 57 bis 62, Feb 2004

Claims (28)

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. コンピュータ上で実行したときに請求項１から２６のいずれか１項記載の方法を実行する、機械可読媒体に記憶されたプログラムコードを含むコンピュータプログラム。

Images