JP6432825B2 - ３次元点群データの位置合わせ方法と装置及びその移動体システム - Google Patents

Description

本発明は、移動体により取得した３次元点群データの位置合わせ方法と装置及びその移動体システムに関する。

自律走行又は半自律走行を行う移動体にレーザ装置（例えば、レーザレンジファインダ：ＬＲＦ）を搭載し、前方の路面や障害物を検出して、周辺の地形を把握し、経路計画や速度制御などの車両制御を行うための地図情報を構築する手段が既に開示されている（例えば、特許文献１）。

上記地図情報として、構造格子あるいは非構造格子の地図上に、各点の高さ情報を与えたモデルをＤＥＭ（Ｄｉｇｉｔａｌ Ｅｌｅｖａｔｉｏｎ Ｍｏｄｅｌ：数値標高モデル）と呼ぶ。
レーザレンジファインダ（ＬＲＦ）の計測密度は、計測位置から離れるほど疎になり、例えば移動体から４０〜５０ｍ離れた位置では、計測間隔は２〜３ｍ以上となる。そのため、数値標高モデル（ＤＥＭ）のデータ密度を高めるためには移動体が移動しながら地形計測を繰り返し、複数のデータを統合する必要がある。同様に、広域の数値標高モデルを生成する場合にも、移動体が移動しながら地形計測を繰り返し、複数のデータを統合する必要がある。
複数の点群データを位置合わせする点群位置合わせ手段は、例えば特許文献２〜４に開示されている。

一方、移動体の位置と姿勢の情報（位置姿勢情報）は、加速度センサ、重力センサ、地磁気センサ、ジャイロ、ＧＰＳなど各種センサ類の計測値を統合することで推定できる。かかる推定方法は、一般にデッドレコニングと呼ばれる。

特開２０１１−１５０４７３号公報 特開２００７−３５０４２号公報 特開２０１２−６３８６６号公報 特開２００８−２６２３０７号公報

上記デッドレコニングによっても、移動体の位置情報又は姿勢情報には誤差が発生する。この誤差により、レーザレンジファインダで取得した複数の点群データを位置合わせする際に、位置ずれが生じる。この位置ずれに起因し、移動体から離れた位置の小さい障害物（例えば高さ２〜３０ｃｍ）が誤差に埋もれて早期に検出できず、直前になって検出されるため、高速走行（例えば時速３０〜１００ｋｍ）が困難となる問題が発生し得る。

また、レーザレンジファインダによる３次元点群データは、毎秒約２０フレーム（例えば５〜５０Ｈｚ）の頻度で計測される。従って前回と今回のフレーム（前フレームと現フレーム）の点群位置合わせは、１／２０秒以内（１／５０〜１／５秒以内）に完了する必要がある。
しかし、特許文献２〜４の点群位置合わせ手段では、複数の点群データから位置合わせの基準となるデータとして、地面、特徴部分、建物のコーナーなどを抽出する前処理が不可欠である。この前処理によって位置合わせに用いる点数を削減できるが、前処理に要する計算コストが大きい、あるいは点数の削減効果が薄いなどの理由により、高速処理が困難又は不可能であった。

本発明は上述した問題点を解決するために創案されたものである。すなわち本発明の目的は、移動体の位置情報又は姿勢情報に誤差が発生しても、移動体から離れた位置の小さい障害物（例えば高さ２〜３０ｃｍ）を検出でき、かつ短時間（例えば１／２０秒以内）に点群位置合わせを完了できる３次元点群データの位置合わせ方法と装置及びその移動体システムを提供することにある。これにより、移動体の位置情報又は姿勢情報を修正でき、かつ移動体の高速走行（例えば時速３０〜１００ｋｍ）が可能となる。

本発明によれば、移動体に搭載されたレーザレンジファインダで取得した３次元点群データの位置合わせ方法であって、
（Ａ）直前の３次元点群データのうち、直前の移動体の進行方向である第１方向に位置する鉛直平面上の直前データと、
現在の３次元点群データのうち、前記第１方向に位置する鉛直平面上の現在データと、を抽出するデータ抽出ステップと、
（Ｂ）直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する位置修正ステップと、
（Ｃ）修正した現在の３次元点群データを直前までの３次元点群データに加算して、移動体周辺の高さ情報を含む地図データを更新する地図更新ステップと、を有する、ことを特徴とする３次元点群データの位置合わせ方法が提供される。

前記位置修正ステップにおいて、直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する位置合わせ量を算出する。

前記位置修正ステップにおいて、前記第１方向に位置する鉛直平面上に、鉛直平面近傍の直前又は現在の３次元点群データの一部を投影する。

前記位置修正ステップにおいて、鉛直平面上の直前データ又は現在データから前記鉛直平面上の地表面に相当する形状パラメータを求める。

前記位置修正ステップにおいて、現在の３次元点群データの幾何変換を行う。

前記位置修正ステップにおいて、直前データと現在データの距離の差が最小となるように、移動体の現在の位置又は姿勢を修正する。

前記位置修正ステップにおいて、粒子フィルタを用いる。

前記位置修正ステップにおいて、状態推定方法を適用する。

前記位置修正ステップにおいて、路面推定処理を含む。

また、本発明によれば、移動体に搭載されたレーザレンジファインダで取得した３次元点群データの位置合わせ装置であって、
移動体周辺の高さ情報を含む地図データを記憶する地図メモリと、
直前の３次元点群データのうち、直前の移動体の進行方向である第１方向と、該第１方向に位置する鉛直平面上の直前データとを記憶する直前メモリと、
現在の３次元点群データを記憶する現在メモリと、
データ処理装置と、を備え、該データ処理装置は、
（Ａ）現在の３次元点群データのうち、前記第１方向に位置する鉛直平面上の現在データを抽出し、
（Ｂ）直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正し、
（Ｃ）修正した現在の３次元点群データを直前までの地図メモリに加算する、ことを特徴とする３次元点群データの位置合わせ装置が提供される。

前記データ処理装置は、前記（Ｃ）の後、
前記直前メモリの前記第１方向を、現在の移動体の進行方向に置換し、
前記直前メモリの直前データを、現在の移動体の進行方向に位置する鉛直平面上のデータに置換する。

さらに、本発明によれば、移動体と、上述した３次元点群データの位置合わせ装置とを搭載する、ことを特徴とする移動体システムが提供される。

上記本発明の方法と装置及びその移動体システムによれば、データ抽出ステップにおいて、直前データと現在データを抽出する。直前データは、直前の移動体の進行方向である第１方向に位置する鉛直平面上のデータであり、直前の３次元点群データの一部である。また、現在データは、前記第１方向に位置する鉛直平面上のデータであり、現在の３次元点群データの一部である。
直前データと現在データは取得時がわずか（例えば１／２０秒以内）に異なるが、同一の第１方向（直前の移動体の進行方向）のデータなので、実質的にほとんど一致する。しかし、移動体の位置又は姿勢に誤差がある場合、その誤差により、直前データと現在データとの間に距離の差が生じる。

本発明では、位置修正ステップにおいて、直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する。この修正により、移動体の位置又は姿勢の誤差の影響を排除することができる。
従って、本発明によれば、移動体の位置情報又は姿勢情報に誤差が発生しても、誤差の影響を排除した地図データを作成することができ、移動体から離れた位置の小さい障害物（例えば高さ２〜３０ｃｍ）を検出できる。

また、直前データと現在データは、レーザレンジファインダで取得した３次元点群データのうち、直前の移動体の進行方向である第１方向のデータであり、それぞれ抽出が容易であり、かつその点数は少ない（例えば、１０〜１００点）。
従って、追加の処理なしに短時間（例えば１／２０秒以内）に点群位置合わせを完了でき、これにより、移動体の位置情報又は姿勢情報を修正でき、かつ移動体の高速走行（例えば時速３０〜１００ｋｍ）が可能となる。

言い換えれば、本発明によれば、少ない計算コストで、デッドレコニングによる自己位置姿勢を補正して数値標高モデルの精度を向上することにより、進行方向に存在する背の低い（回避すべき）障害物を発見しやすくなる。

本発明の位置合わせ装置を備えた移動体の構成図である。 本発明の位置合わせ装置の全体構成図である。 本発明による位置合わせ方法の全体フロー図である。 周囲環境計測ステップの説明図である。 データ抽出ステップの説明図である。 平面投影ステップの説明図である。 法線推定ステップの説明図である。 位置合わせ量算出ステップの説明図である。 幾何変換ステップの説明図である。

以下、本発明の好ましい実施形態を添付図面に基づいて詳細に説明する。なお、各図において共通する部分には同一の符号を付し、重複した説明を省略する。

図１は、本発明の位置合わせ装置１０を備えた移動体１の構成図である。この図において、（Ａ）は側面図、（Ｂ）は平面図である。また図中の○印は、レーザレンジファインダ２による計測点を示している。

移動体１は車両であり、レーザレンジファインダ２を備える。
レーザレンジファインダ２（ＬＲＦ）は、移動体１の前方あるいは上方に設置されている。レーザレンジファインダ２の設置高さは地面から３０ｃｍ以上、例えば１．５〜４ｍの範囲である。
レーザレンジファインダ２は、レーザ光３の入射光の投光から反射光の受光までの時間差から距離を検出する。
レーザレンジファインダ２は、この例では、上下に等ピッチ（例えば０．１〜２度）のチルト角で配列されたレーザ光３を水平旋回軸Ｚを中心に水平に（水平面内で）旋回させ、１回転で移動体１の周囲の３次元点群データ４（図２参照）を取得する。回転方向の計測ピッチは例えば０．１〜２度である。
水平旋回軸Ｚの向きは移動体１の姿勢に追随する（移動体１に対して固定されている）。すなわち、移動体１の姿勢が水平であるとき、水平旋回軸Ｚの向きは鉛直になる。換言すれば、水平旋回軸Ｚは、移動体１が水平に位置するときの鉛直軸である。

上述したレーザレンジファインダ２は、１秒間に約２０回（５〜５０回）旋回し、毎秒約２０フレーム（例えば５〜５０フレーム）の３次元点群データ４を取得する。
各３次元点群データ４は、例えばレーザ光３が５０であり、回転方向の計測ピッチが１度の場合、５０×３６０＝１８０００点の点群データを含む。
従って、本発明の位置合わせ装置１０は、この例では、２万点程度（１８０００点）の点群データを扱い、１／２０秒以内（１／５０〜１／５秒以内）に位置合わせを完了する必要がある。
なお、レーザレンジファインダ２は、上述したものに限定されず、回転することで点群データを取得するレーザレンジファインダであればよい。また本発明はレーザレンジファインダに限定されず、レーダ、あるいは距離画像センサを用いてもよい。

図１において、説明の都合上、ｎ点（例えば５０点）のレーザ光３を移動体１に近いほうから、１，２，３，・・・，ｋ，・・・ｎ−１，ｎと番号を付ける。番号ｎのレーザ光３は、移動体１から例えば３０〜１００ｍの走行路面、番号１のレーザ光３は、移動体１から１〜１０ｍの走行路面を照射するように設定されている。
ｎ＝５０かつチルト方向の角度ピッチが１度の場合、番号１，２の水平間隔は例えば数ｃｍ程度であるが、番号ｎ−１，ｎの水平間隔は数ｍにも及ぶため、遠方の小さな障害物を早期に発見することは困難である。
そのため、上述したように、数値標高モデル（ＤＥＭ）のデータ密度を高めるためには移動体１が移動しながら地形計測を繰り返し、複数のデータを統合する必要がある。

図２は、本発明の位置合わせ装置１０の全体構成図である。
この図において、本発明の位置合わせ装置１０は、地図メモリ１２、直前メモリ１４、現在メモリ１６、及びデータ処理装置１８を備える。

地図メモリ１２は、移動体１の周辺の高さ情報を含む地図データ５を記憶する。地図データ５は、上述した数値標高モデル（ＤＥＭ）に相当し、後述する地図更新ステップＳ４により、移動体周辺の高さ情報が繰り返し蓄積され、データ密度を高めるようになっている。なお、例えば不要になったときに、地図データ５の一部又は全部を消去することもできる。

直前メモリ１４は、直前の３次元点群データ４Ａのうち、直前の移動体１の進行方向である第１方向Ｆ（図５参照）と、第１方向Ｆに位置する鉛直平面上の直前データ６とを記憶する。「第１方向Ｆに位置する鉛直平面」とは、第１方向Ｆと水平旋回軸Ｚの張る平面を意味する。
直前データ６は、レーザレンジファインダ２により計測された進行方向である第１方向Ｆに位置する鉛直平面の直前のデータであり、上述の例では５０点のデータ数を有する。以下、この鉛直平面を「直前基準平面Ｐ１」と呼ぶ。
直前メモリ１４は、直前の３次元点群データ４Ａのすべては記憶せず、少なくとも第１方向Ｆと、第１方向Ｆに位置する鉛直平面の直前のデータとを記憶すればよい。従って、直前メモリ１４の記憶容量を小さく設定することができる。

現在メモリ１６は、現在の３次元点群データ４Ｂを記憶する。現在メモリ１６は、現在の３次元点群データ４Ｂのみを記憶する。また、後述する地図更新ステップＳ４の後に、現在の３次元点群データ４Ｂの一部のみを直前メモリ１４に置換した後、現在の３次元点群データ４Ｂは消去される。従って、現在メモリ１６は、１フレームの３次元点群データ４Ｂのみを記憶できればよく、その記憶容量を小さく設定することができる。

データ処理装置１８は、例えばコンピュータ（ＰＣ）であり、以下の演算を実施する。
（Ａ）初めに、データ処理装置１８は、現在メモリ１６から、現在の３次元点群データ４Ｂのうち、上述した第１方向Ｆ（図５参照）に位置する鉛直平面上の現在データ７を抽出する。現在データ７は、直前データ６と同一の第１方向Ｆ（方位）に位置する鉛直平面上の現在のデータであり、上述の例では５０点のデータ数を有する。以下、この鉛直平面を「現在基準平面Ｐ２」と呼ぶ。
なお、第１方向Ｆは、「直前の移動体１の進行方向」であり、「現在の移動体１の進行方向」ではない。現在の移動体１の進行方向は、第１方向Ｆと一致しても相違してもよい。
また、レーザレンジファインダのレーザ配向、あるいは計測誤差によって、第１方向Ｆ（方位）に位置する鉛直平面、すなわち現在基準平面Ｐ２上にデータが整列しない場合が有り得る。そのため、現在基準平面Ｐ２の近傍に位置し、同時に計測したとみなせるｎ点（例えば５０点）のデータを現在基準平面Ｐ２上に投影して現在データ７として用いることが好ましい。この近傍は、第１方向に対して３０度以内の範囲であることが好ましい。

直前基準平面Ｐ１と現在基準平面Ｐ２は、「直前の移動体１の進行方向である第１方向Ｆに位置する鉛直平面」である点で共通する。すなわち、両平面とも、直前の移動体１の第１方向Ｆ（方位）を基準としている。そのため上述した水平旋回軸Ｚが互いに平行である限り、移動体１の位置と向きが異なっても、直前基準平面Ｐ１と現在基準平面Ｐ２は互いに平行である。
従って、直前データ６と現在データ７は取得時がわずか（例えば１／２０秒程度）に異なるが、同一の第１方向Ｆ（方位）の３次元点群データ４Ａ、４Ｂの一部なので、実質的にはほとんど一致する。しかし、移動体１の位置又は姿勢に誤差がある場合、その誤差により、直前データ６と現在データ７との間に距離の差が生じる。

（Ｂ）次に、データ処理装置１８は、直前データ６と現在データ７の距離の差が最小となるように、現在の３次元点群データ４Ｂの位置を修正する。
（Ｃ）次いで、データ処理装置１８は、修正した現在の３次元点群データ４Ｃを直前までの地図メモリ１２に加算する。ここで「加算」とは、情報を付け加える意味である。

またデータ処理装置１８は、（Ｃ）の後、直前データ６の第１方向Ｆを現在の移動体１の進行方向に置換する。さらに、直前メモリ１４の直前データ６を現在の移動体１の進行方向に位置する鉛直平面上のデータに置換する。これらの進行方向とデータは、現在の３次元点群データ４Ｂの一部であり、追加の処理なしに置換することができる。
この場合、現在の移動体１の進行方向（方位）は、直前の移動体１の進行方向（方位）とは通常相違する。

図３は、本発明による位置合わせ方法の全体フロー図である。この図において、本発明の位置合わせ方法は、Ｓ１〜Ｓ４の各ステップ（工程）からなる。
また、図４〜図９は、本発明による位置合わせ方法の説明図である。
以下、図３〜図９を用いて、本発明の方法を説明する。

ステップＳ１は、周囲環境計測ステップであり、移動体１の現在の位置姿勢情報と、現在の３次元点群データ４Ｂを取得する。

移動体１の現在の位置姿勢情報は、移動体１に搭載された加速度センサ、重力センサ、地磁気センサ、ジャイロ、ＧＰＳなど各種センサ類の計測値を統合して推定する。従って、移動体１の進行方向（図１参照）は、位置姿勢情報として常に得られている。しかしこの位置姿勢情報（移動体１の位置又は姿勢の情報）には誤差が含まれる。

図４は、ステップＳ１（周囲環境計測ステップ）の説明図であり、（Ａ）は側面図、（Ｂ）は平面図である。また、図中において、Ｇは、移動体１の進行方向真正面に位置する障害物である。

現在の３次元点群データ４Ｂは、例えば上述した２万点程度（１８０００点）の点群データである。３次元点群データ４の各点データは、位置データと、移動体１の進行方向真正面に対する水平旋回軸Ｚまわりの角度データと、を含んでいる。

ステップＳ２は、データ抽出ステップであり、直前データ６と現在データ７を抽出する。図３において、ステップＳ２は、直前データ６の抽出ステップＳ２１と、現在データ７の抽出ステップＳ２２とからなる。抽出ステップＳ２１、Ｓ２２は順序が逆であってもよい。

図５は、ステップＳ２（データ抽出ステップ）の説明図である。
図５（Ａ）は、図４（Ｂ）と同様の平面図である。この図において、移動体１は、実線は現在、破線は直前のものを示している。また、図５（Ｂ）は、移動体１の姿勢変化を（１）（２）（３）の順で示している。

直前データ６は、直前の３次元点群データ４Ａのうち、直前メモリ１４に記憶された直前の移動体１の進行方向である第１方向Ｆと、この第１方向Ｆに位置する直前基準平面Ｐ１上の５０点のデータである。直前データ６は、直前メモリ１４から追加の処理なしに抽出することができる。この直前データ６は、図５（Ａ）の直前基準平面Ｐ１（破線の矢印）上のデータに相当する。
なお図５（Ａ）には、直前データ６は示されていない。

現在データ７は、現在の３次元点群データ４Ｂのうち、直前データ６と同一の第１方向Ｆに位置する５０点のデータである。このデータは、ステップＳ１で取得した３次元点群データ４の角度データと、移動体１の現在の位置姿勢情報の進行方向から、抽出することができる。この現在データ７は、図５（Ａ）の現在基準平面Ｐ２（実線の矢印）上のデータ（●印）に相当する。

図５（Ｂ）において、（１）が直前の姿勢（２）が現在の姿勢の場合、（２）の実線の矢印は現在の進行方向、破線の矢印は直前の進行方向である。
同様に、（２）が直前の姿勢（３）が現在の姿勢の場合、（３）の実線の矢印は現在の進行方向、破線の矢印は直前の進行方向である。

図３において、ステップＳ３は位置修正ステップであり、直前データ６と現在データ７の距離の差が最小となるように、現在の３次元点群データ４Ｂの位置を修正する。
この際、直前データ６と現在データ７の距離の差が最小となる位置合わせ量に従い、現在の３次元点群データ４Ｂに幾何変換を実施する。なお、本発明における幾何変換とは、点群全体に対して均一な回転ないし並進移動を行う、いわゆる剛体変換を意図している。

図３において、ステップＳ３（位置修正ステップ）は、Ｓ３１〜Ｓ３６の各ステップ（工程）からなる。

ステップＳ３１は、平面投影ステップである。図６は、平面投影ステップの説明図である。
ステップＳ３１において、図６に示すように、第１方向Ｆに位置する鉛直平面である直前基準平面Ｐ１上に、この鉛直平面近傍の直前又は現在の３次元点群データ４Ａ、４Ｂの一部を投影する。
直前データ６と現在データ７は、レーザレンジファインダ２による前回と今回のフレーム（前フレームと現フレーム）のデータであるため、例えば１／２０秒以内（１／５０〜１／５秒以内）の時間差がある。そのため、レーザレンジファインダのレーザ配向、わずかな計測時間のずれ、あるいは計測誤差により、直前データ６と現在データ７のデータが同一方向に整列していない場合がある。この場合、直前データ６と現在データ７を直前基準平面Ｐ１上に投影して同一平面上で扱うことが好ましい。

なおステップＳ３において、点群の位置合わせを行う際、位置合わせ不良の原因となる点群を事前に除去しておくことで、位置合わせ計算を安定化できる。

図３において、ステップＳ３２は、直前データ６の法線推定ステップである。図７は、法線推定ステップの説明図である。
各点における法線は、例えば前後の点との３点による接続角度をもとに推定することができる。各点における法線を推定することは、言い換えれば地表面の形状を各点近傍において平面近似することに他ならない。
また法線推定に限らず、各点を結ぶ折れ線、あるいは折れ線を近似する曲線を用いて、地表面形状を推定してもかまわない。
なお、地表面形状の推定は、現在データ７に適用してもよい。
すなわちこのステップでは、鉛直平面上の直前データ又は現在データから鉛直平面上の地表面に相当する形状パラメータを求める。「形状パラメータ」には、曲線、折れ線、法線などが含まれる。

図３において、ステップＳ３３は、近傍点探索ステップである。直前データ６の各点について、現在データ７の各点のうちもっとも距離が近いと推定される点を探索し、対応付けを行う。上述した推定した地表面形状に対し、現在データ７の各点を投影し、近傍点探索を行うことが好ましい。なお、直前データ６と現在データ７の親子関係は入れ替えてもよい。

図３において、ステップＳ３４は、位置合わせ量算出ステップである。図８は、位置合わせ量算出ステップの説明図である。
この図に示すように、ステップＳ３４において、直前データ６と現在データ７の差が最小となるように、現在の３次元点群データ４Ｂの位置を修正する位置合わせ量を算出する。
ここで「直前データ６と現在データ７の差が最小となる」とは、「直前データ６と現在データ７の差、例えば図８では直前データ６をもとに推定あるいは近似した地表面に対する現在データ７の垂直距離Ｌの二乗和が最小となる」ことを意味する。
位置合わせ量は、現在の３次元点群データ４Ｂを幾何変換するための、位置と姿勢の変換量である。

図３において、ステップＳ３５は、幾何変換ステップである。図９は、幾何変換ステップの説明図である。
図９において、（Ａ）は変換前、（Ｂ）は変換後を示している。また各図において、破線は直前データ６、実線は現在データ７である。
図９（Ａ）は、前方の地形が同じであるが、移動体１が上向きにチルトしたため、現在データ７と直前データ６の距離の差が大きい状態を示している。この場合、図９（Ｂ）に示すように、ステップＳ３４で求めた位置合わせ量に基づき、現在データ７を直前データ６の位置まで幾何変換することで、直前データ６との誤差を最小化することができる。
なお、この幾何変換は、現在データ７のみではなく、現在の３次元点群データ４Ｂの全体を幾何変換する。

図３において、ステップＳ３６は、自己位置姿勢情報の修正ステップである。
ステップＳ３６において、直前データ６と現在データ７の距離の差が最小となるように、移動体１の自己位置姿勢情報（現在の位置情報と姿勢情報）を修正することで、計測誤差の影響を低減し、より妥当な自己位置姿勢情報を得ることができる。
このステップＳ３６により、現在の移動体１が有する位置姿勢情報（移動体１の位置又は姿勢の情報）の誤差が修正される。また、このステップを常に繰り返すことから、直前の移動体１の位置姿勢情報の誤差が低減され、同様に数値標高モデル（ＤＥＭ）である地図データ５は、移動体１の位置姿勢情報の誤差に起因する進行方向の路面上におけるずれが低減されたものとなる。

ステップＳ３４では、所望の最適化手法、例えば最小二乗法を適用できる。あるいは、粒子フィルタ（パーティクルフィルタ、モンテカルロ法などとも呼ばれる）を用いて解探索を行うことができる。
また、ステップＳ３６で自己位置姿勢を修正する際は、各センサの出力値および上述した最適化手法により得られた推定量を統合するため、カルマンフィルタなどの状態推定方法を用いることができる。

さらに、直前データ６ないし現在データ７に対して路面推定処理を行い、ステップＳ３の位置合わせ量算出への寄与度が小さい点をあらかじめ除去しておくことが好ましい。
例えば最小二乗法やＲＡＮＳＡＣ（ＲＡＮｄｏｍ ＳＡｍｐｌｅ Ｃｏｎｓｅｎｓｕｓ）などのランダムサンプリング手法を用いて、路面推定すなわち路面を構成する点群の抽出を行い、推定後の点群を用いて点群位置合わせ方法を適用することができる。
上述した路面推定処理により障害物Ｇを除去した状態の地図同士を位置合わせすることができるため、障害物Ｇに起因するミスマッチを低減できる。

図３において、ステップＳ４は、地図データ５の地図更新ステップであり、修正した現在の３次元点群データ４Ｃを直前までの３次元点群データ４に加算して、移動体周辺の高さ情報を含む地図データ５を更新する。
すなわち、ステップＳ３で現在の３次元点群データ４Ｂを幾何変換した３次元点群データ４Ｃを、直前までの３次元点群データ４に重ね合わせることで、ずれの少ない地図データ５を得ることができる。

なお、上述した第１方向Ｆは、直前の移動体１の進行方向に限定されず、その他の方向であってもよい。また、直前データ６と現在データ７は、上述したそれぞれ直前基準平面Ｐ１と現在基準平面Ｐ２上のデータに限定されず、その他の任意の部分データであってもよい。以下、この場合の直前データ６と現在データ７をそれぞれ単に、「第１の部分点群」、「第２の部分点群」と呼ぶ。

この場合、上述したステップＳ２では、位置合わせ対象となる前後の点群データについて、前フレームの点群から前フレームの進行方向中央付近すなわち第１方向Ｆに沿った第１の部分点群を抽出し、後フレームの点群から同一の第１方向Ｆに沿った方向の第２の部分点群を抽出する。

＜進行方向の部分地形抽出、前フレームの進行方向に沿った部分地形抽出＞
位置合わせ対象となる前後の点群について、前フレームの点群から前フレームの進行方向中央付近すなわち第１方向Ｆに沿った第１の部分点群を抽出し、後フレームの点群から第１方向Ｆに沿った方向の第２の部分点群を抽出する。以下、第１の部分点群と第２の部分点群に点群位置合わせ方法を適用し、双方のずれを減少する位置合わせ量を得る。

本発明の目的においては、少なくとも重力方向の併進ずれ、およびピッチ角方向の回転ずれ量を得る必要がある。また、一般的な車道においては進行方向に障害物Ｇが少なく、点群位置合わせのみでは進行方向のずれ量が推測できない場合があるため、進行方向の移動量はＧＰＳによる位置情報、あるいは加速度センサや車速パルスなどを統合して得られる移動量などを用いることができる。点群位置合わせ方法として、例えばＩＣＰ法（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ法）が利用できる。ＩＣＰ法は、対応点同士の距離を繰り返し計算により最小化し、点群の位置合わせを行う解析手段である。

＜平面投影、近傍点探索＞
上述した第１の部分点群あるいは第２の部分点群を得る際、レーザレンジファインダ２の仕様によって同一方向に点群が整列していない場合がある。また移動体１の姿勢変化や計測誤差などの要因により、第１の部分点群と第２の部分点群の向きは厳密には一致しない。そこで重力方向軸と上述した第１方向Ｆが張る平面に点群を投影することで、部分点群同士の向きを揃え、３次元点群の位置合わせを２次元的に行うことができる。
第１の部分点群と第２の部分点群の間には上述した平面に対し垂直方向のオフセットが生じてしまう場合があるが、上述した投影により同一平面上で点群位置合わせを行い、上述したオフセットの影響は少ないものと見なす。

ＩＣＰ法のような近傍点探索を必要とする点群位置合わせ方法において、近傍点探索による計算コストの増大が課題となる場合があるが、上述した投影により点群が第１方向Ｆに沿って配置されるため、近傍点探索が容易になる。点群の位置合わせを行う際、位置合わせ不良の原因となる点群を事前に除去しておくことで、位置合わせ計算を安定化できる。例えば路面推定により路面のみを含む点群同士で位置合わせを行うのが好適である。

＜前フレームの法線推定、位置合わせ量算出＞
ＩＣＰ法などの点群位置合わせ方法は、対応点同士の結びつきが強く、局所解に陥りやすい。そのため、レーザレンジファインダ２のような疎な点群同士の位置合わせには向いていない。本発明では、上述した局所解への解析手段として、一方あるいは双方の点群を折れ線や曲線で補間する、あるいは一方の点群の法線を推定し上述した法線による疑似平面を仮定することで位置合わせ量を得る。

例えば、点群対疑似平面の垂直距離をもとに位置合わせを行うよう拡張されたＩＣＰ法は「Ｐｏｉｎｔ‐ｔｏ‐Ｐｌａｎｅ型ＩＣＰ法」などと呼ばれ、本発明に適用可能な解析手段の１つである。ＩＣＰ法を適用する際、初期姿勢の決定方法が課題となる場合がある。例えば、パーティクルフィルタを用いて初期値探索を行うことができる。パーティクルフィルタは粒子フィルタ、あるいはモンテカルロフィルタなどと呼ばれることもあるが、本発明に適用可能な解析手段の別称にすぎない。また、本発明はパーティクルフィルタ以外の初期値探索の可能性を拒絶するものではなく、他の初期値探索方法を適用することも可能である。

＜地形情報の幾何変換、自己位置姿勢情報の修正＞
上述した位置合わせ量に従い、各部分点群の親である３次元点群に幾何変換、例えば回転変換および並進変換を行う。上述した幾何変換により、ずれの少ない合成地形を得ることができる。このとき、上述した位置合わせ量に応じて移動体１の自己位置姿勢情報を修正することで、計測誤差の影響を低減し、より妥当な自己位置姿勢情報を得ることができる。センサ情報や自己位置姿勢情報を統合する際、カルマンフィルタなどの状態推定方法を用いることができる。

＜前フレームの部分地形の上書き＞
複数の点群を連続して位置合わせしていく際、不要になった点群を極力早期に削除ないし削減したい場合がある。例えば部分点群を得る際、上述した第１の部分点群および第２の部分点群に加え、後フレームの進行方向すなわち第２の方向に沿った第３の部分点群さえ確保できれば、上述した位置合わせを連続して行うことができる。また、第１の部分点群を毎回更新せず、特定の時間保持し続けてもよい。

＜計算時間が低減できることに関する具体的説明＞
レーザレンジファインダ２で取得した２組以上の点群の位置ずれ量を求め位置合わせを行う場合、計測点数の増大に伴い計算時間も増大することが課題とされてきた。
上述した課題に対し、従来方法では例えば点間の起伏などをもとに点群の特徴部分を抽出し位置合わせを行う方法が提案されている。しかし、特徴部分の抽出のための前処理コストが生じ、抽出方法によっては十分に点数を削減することができない、あるいは適切に特徴部分を抽出することができないなどの課題があった。
本発明は位置合わせに必要な点を取得する際に全点群のうち所望の方位に分布する点群を選択するだけなので、追加の処理が不要である。また、位置合わせのために抽出する点数も高々レーザレンジファインダ２のレイヤ数（進行方向のレーザ照射点数）程度なので、位置合わせ計算のコストを容易に低減できる。

本発明の移動体システムは、移動体が３次元点群データの位置合わせ装置を搭載する。

また、移動体とは別の場所に設置され移動体と通信する通信装置を備え、この通信装置が３次元点群データの位置合わせ装置の一部を搭載してもよい。

上述した本発明の方法と装置及びその移動体システムによれば、データ抽出ステップＳ２において、直前データ６と現在データ７を抽出する。直前データ６は、直前の移動体１の進行方向である第１方向Ｆに位置する鉛直平面上のデータであり、直前の３次元点群データ４Ａの一部である。また、現在データ７は、第１方向Ｆに位置する鉛直平面上のデータであり、現在の３次元点群データ４Ｂの一部である。
直前データ６と現在データ７は取得時がわずか（例えば１／２０秒程度）に異なるが、同一の第１方向Ｆ（直前の移動体１の進行方向）のデータなので、実質的にほとんど一致する。しかし、移動体１の位置又は姿勢に誤差がある場合、その誤差により、直前データ６と現在データ７との間に距離の差が生じる。

本発明では、位置修正ステップＳ３において、直前データ６と現在データ７の距離の差が最小となるように、現在の３次元点群データ４Ｂの位置を修正する。この修正により、移動体１の位置又は姿勢の誤差の影響を排除することができる。
従って、本発明によれば、移動体１の位置情報又は姿勢情報に誤差が発生しても、誤差の影響を排除した地図データ５を作成することができ、移動体１から離れた位置の小さい障害物Ｇ（例えば高さ２〜３０ｃｍ）を検出できる。

また、直前データ６と現在データ７は、レーザレンジファインダ２で取得した３次元点群データ４Ａ、４Ｂのうち、直前の移動体１の進行方向である第１方向Ｆのデータであり、それぞれ３次元点群データ４Ａ、４Ｂから追加の処理なしに抽出でき、かつその点数は少ない（例えば５０点）。
従って、追加の処理なしに短時間（例えば１／２０秒以内）に点群位置合わせを完了でき、これにより、移動体１の位置情報又は姿勢情報を修正でき、かつ移動体１の高速走行（例えば時速３０〜１００ｋｍ）が可能となる。

言い換えれば、本発明によれば、少ない計算コストで、デッドレコニングによる自己位置姿勢を補正し数値標高モデルの精度を向上することにより、進行方向に存在する背の低い（回避すべき）障害物Ｇを発見しやすくなる。

なお本発明は上述した実施の形態に限定されず、本発明の要旨を逸脱しない範囲で種々変更を加え得ることは勿論である。

Ｆ 第１方向、Ｇ 障害物、Ｌ 垂直距離、
Ｐ１ 直前基準平面、Ｐ２ 現在基準平面、Ｚ 水平旋回軸、
１ 移動体、２ レーザレンジファインダ（ＬＲＦ）、
３ レーザ光、４ ３次元点群データ、４Ａ 直前の３次元点群データ、
４Ｂ 現在の３次元点群データ、４Ｃ 修正した現在の３次元点群データ、
５ 地図データ、６ 直前データ、７ 現在データ、１０ 位置合わせ装置、
１２ 地図メモリ、１４ 直前メモリ、１６ 現在メモリ、１８ データ処理装置

Claims (12)

1. 移動体に搭載されたレーザレンジファインダで取得した３次元点群データの位置合わせ方法であって、
   （Ａ）直前の３次元点群データのうち、直前の移動体の進行方向である第１方向に位置する鉛直平面上の直前データと、
   現在の３次元点群データのうち、前記第１方向に位置する鉛直平面上の現在データと、を抽出するデータ抽出ステップと、
   （Ｂ）直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する位置修正ステップと、
   （Ｃ）修正した現在の３次元点群データを直前までの３次元点群データに加算して、移動体周辺の高さ情報を含む地図データを更新する地図更新ステップと、を有する、ことを特徴とする３次元点群データの位置合わせ方法。
2. 前記位置修正ステップにおいて、直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正する位置合わせ量を算出する、ことを特徴とする請求項１に記載の３次元点群データの位置合わせ方法。
3. 前記位置修正ステップにおいて、前記第１方向に位置する鉛直平面上に、鉛直平面近傍の直前又は現在の３次元点群データの一部を投影する、ことを特徴とする請求項１に記載の３次元点群データの位置合わせ方法。
4. 前記位置修正ステップにおいて、鉛直平面上の直前データ又は現在データから前記鉛直平面上の地表面に相当する形状パラメータを求める、ことを特徴とする請求項１に記載の３次元点群データの位置合わせ方法。
5. 前記位置修正ステップにおいて、現在の３次元点群データの幾何変換を行う、ことを特徴とする請求項１に記載の３次元点群データの位置合わせ方法。
6. 前記位置修正ステップにおいて、直前データと現在データの距離の差が最小となるように、移動体の現在の位置又は姿勢を修正する、ことを特徴とする請求項１に記載の３次元点群データの位置合わせ方法。
7. 前記位置修正ステップにおいて、粒子フィルタを用いる、ことを特徴とする請求項１に記載の３次元点群データの位置合わせ方法。
8. 前記位置修正ステップにおいて、状態推定方法を適用する、ことを特徴とする請求項１に記載の３次元点群データの位置合わせ方法。
9. 前記位置修正ステップにおいて、路面推定処理を含む、ことを特徴とする請求項１に記載の３次元点群データの位置合わせ方法。
10. 移動体に搭載されたレーザレンジファインダで取得した３次元点群データの位置合わせ装置であって、
    移動体周辺の高さ情報を含む地図データを記憶する地図メモリと、
    直前の３次元点群データのうち、直前の移動体の進行方向である第１方向と、該第１方向に位置する鉛直平面上の直前データとを記憶する直前メモリと、
    現在の３次元点群データを記憶する現在メモリと、
    データ処理装置と、を備え、該データ処理装置は、
    （Ａ）現在の３次元点群データのうち、前記第１方向に位置する鉛直平面上の現在データを抽出し、
    （Ｂ）直前データと現在データの距離の差が最小となるように、現在の３次元点群データの位置を修正し、
    （Ｃ）修正した現在の３次元点群データを直前までの地図メモリに加算する、ことを特徴とする３次元点群データの位置合わせ装置。
11. 前記データ処理装置は、前記（Ｃ）の後、
    前記直前メモリの前記第１方向を、現在の移動体の進行方向に置換し、
    前記直前メモリの直前データを、現在の移動体の進行方向に位置する鉛直平面上のデータに置換する、ことを特徴とする請求項１０に記載の３次元点群データの位置合わせ装置。
12. 移動体と、請求項１０又は１１に記載の３次元点群データの位置合わせ装置とを搭載する、ことを特徴とする移動体システム。
    

Images