JP6272460B2 - 三次元地図生成システム - Google Patents

Description

本発明は、三次元地図生成システムに関し、特に、三次元点群データによる三次元地図の生成に有効な技術に関する。

近年、工場における効率的な改修やレイアウト確認あるいは美術館や商業施設などにおけるインテリアの変更などの様々な場面において、三次元地図が利用されている。この三次元地図の生成は、例えば三次元レーザスキャナなどを用いて計測される。

三次元レーザスキャナから照射されたレーザパルスを測定対象物に照射し、その反射光のデータを該三次元レーザスキャナが取得する。取得したデータは、三次元座標の点群データであり、該点群データによって、被測定対象の表面形状を形成する。

なお、この種の三次元地図における生成技術としては、例えば複数の計測箇所から被測定対象物の表面形状を測定し、測定箇所毎に取得した三次元点群データを合成して合成点群データを生成し、三次元点群データを取得する際のターゲット配置を不要とする技術が知られている（例えば特許文献１参照）。

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

三次元レーザスキャナなどを用いて三次元地図を生成する場合において、死角あるいは影などのない三次元座標の点群データを取得するには、計測箇所を複数にして測定し、それらの計測箇所の測定にて得られたデータを合成する必要がある。

そのため、計測箇所毎に三次元レーザスキャナなどの測定機器を設置する作業が必要となる。さらには、測定機器を設置後に、計測をしなければならないので、計測時間が長くなってしまう。このように、作業効率が低下してしまい、三次元地図の生成を容易に行うことができないという問題がある。

本発明の目的は、三次元地図の生成を短時間で容易に行うことのできる技術を提供することにある。

本発明の前記ならびにその他の目的と新規な特徴については、本明細書の記述および添付図面から明らかになるであろう。

本願において開示される発明のうち、代表的なものの概要を簡単に説明すれば、次のとおりである。

代表的な三次元地図生成システムは、以下に示す特徴を有するものである。

三次元地図生成システムは、第１の計測センサ、第２の計測センサ、および地図生成部を有する。第１の計測センサは、床面に対して水平方向の距離データを収集する。第２の計測センサは、床面に対して俯角または仰角が生じるように取り付けられており、床面に対して上下方向の距離データを収集する。

地図生成部は、第１の計測センサおよび第２の計測センサを移動させて取得した距離データから、三次元地図を生成する。この地図生成部は、第１の計測センサが収集した水平方向の距離データに基づいて、第１の計測センサの位置および角度を同定する。そして、同定した位置および角度に対して、第２の計測センサが収集した上下方向の距離データを対応付けして三次元地図を生成する。

（１）三次元地図の生成における作業効率を向上させることができる。

（２）三次元地図の生成コストを削減することができる。

本実施の形態における三次元地図生成システムにおける構成の一例を示す説明図である。 図１の三次元地図生成システムが有する処理装置の構成の一例を示す説明図である。 図２の処理装置が有する記憶装置に格納されるソフトウェア構成の一例を示す説明図である。 図１の三次元地図生成システムに備えられる計測センサにおける構成の一例を示す説明図である。 測定用台車に搭載された三次元地図生成システムによる計測の一例を示した説明図である。 計測センサによる測定用台車の位置姿勢の推定処理の一例を示す説明図である。 図１の三次元地図生成システムによる三次元地図の生成の一例を示す説明図である。 図２の三次元地図生成システムによる距離データの測定から三次元地図の生成までの処理の一例を示すフローチャートである。 計測センサによる側壁面の計測例を示す説明図である。 計測センサにおける計測時刻のずれの影響を示す説明図である。 図２の処理装置によるキャリブレーション処理の一例を示すフローチャートである。

以下の実施の形態においては便宜上その必要があるときは、複数のセクションまたは実施の形態に分割して説明するが、特に明示した場合を除き、それらはお互いに無関係なものではなく、一方は他方の一部または全部の変形例、詳細、補足説明等の関係にある。

また、以下の実施の形態において、要素の数等（個数、数値、量、範囲等を含む）に言及する場合、特に明示した場合および原理的に明らかに特定の数に限定される場合等を除き、その特定の数に限定されるものではなく、特定の数以上でも以下でもよい。

さらに、以下の実施の形態において、その構成要素（要素ステップ等も含む）は、特に明示した場合および原理的に明らかに必須であると考えられる場合等を除き、必ずしも必須のものではないことは言うまでもない。

同様に、以下の実施の形態において、構成要素等の形状、位置関係等に言及するときは特に明示した場合および原理的に明らかにそうではないと考えられる場合等を除き、実質的にその形状等に近似または類似するもの等を含むものとする。このことは、上記数値および範囲についても同様である。

また、実施の形態を説明するための全図において、同一の部材には原則として同一の符号を付し、その繰り返しの説明は省略する。なお、図面をわかりやすくするために平面図であってもハッチングを付す場合がある。
以下、実施の形態を詳細に説明する。
〈三次元地図生成システムの構成例〉

図１は、本実施の形態における三次元地図生成システム１０における構成の一例を示す説明図である。

三次元地図生成システム１０は、図１に示すように、計測センサ１１，１２、処理装置１３、入力部１４、表示部１５、およびバッテリ１６を有する。これら計測センサ１１，１２、処理装置１３、入力部１４、表示部１５、およびバッテリ１６は、測定用台車２０に搭載されている。

測定用台車２０は、作業者などが手押しで走行させる台車であり、例えば４つの車輪２１が設けられている。２つの車輪２１は、測定用台車２０の進行方向である前方にそれぞれ設けられており、残りの２つの車輪２１は、測定用台車２０の後方にそれぞれ取り付けられている。

測定用台車２０の前方に設けられている２つの車輪２１は、回転可能となるように自在キャスタがそれぞれ設けられている。測定用台車２０の後方に取り付けられている２つの車輪２１は、固定された構成となっている。

なお、測定用台車２０に取りけられている４つの車輪２１は、すべて自在キャスタが設けられた構成であってもよいし、あるいは測定用台車２０の後方に取り付けられている２つの車輪２１のみに自在キャスタが設けられた構成であってもよい。

さらに、測定用台車２０は、手押しによる走行に限定されるものではなく、例えば、電動走行が可能な構成としてもよい。この場合、測定用台車２０には、例えばモータおよびモータを駆動する駆動回路などが備えられる。そして、モータの動力によって測定台車の車輪を駆動させる。

また、電動走行可能である場合、測定用台車２０は、誘導用の磁気テープ、あるいは白線などのガイドテープなどの誘導設備によって走行する誘導型の自動走行車または誘導設備を用いることなく無軌道搬送を行う自動走行車などであってもよい。

第１の計測センサである計測センサ１１および第２の計測センサである計測センサ１２は、例えばレーザ測域センサなどからなる。このレーザ測域センサは、２次元平面をスキャンするセンサであり、レーザ測域センサの内部に設けられたリフレクタを回転させることによって周囲にレーザパルスを照射し、レーザパルスが当たった点までの距離を計測する。

計測センサ１１，１２は、例えば計測可能距離が３０ｍ、計測範囲が水平方向に±１３５°、角度分解能が０．２５°であり、合計１０８１点の距離を計測することができる。以下では、この１０８１点のデータの組を距離データという。

レーザパルスを照射した角度と距離が計測データとして得られているため、この距離データを直交座標系に変換すれば、台車周辺にある物体の二次元的な形状を得ることができる。この距離データの直交座標系による表現を幾何形状データと呼ぶ。

計測センサ１１は、照射されるレーザパルスが測定用台車２０が走行する走行路面２５、すなわち床面と水平となるように取り付けられており、該レーザパルスが側壁面２６を照射する。計測センサ１１の取り付け位置は、走行路面２５に対して垂直方向に変更可能である。

すなわち、計測センサ１１は、周囲環境にある物体に合わせて、路面までの高さが変更自在となっている。この計測センサ１１は、測定用台車２０の位置および姿勢を推定するために用いる。

計測センサ１２は、レーザパルスが走行路面２５を照射するような角度、すなわち俯角を付けて取りけられている。その角度は、俯角０°よりも大きい角度である。ここで、俯角９０°は、レーザパルスの照射角度が走行路面２５に対して垂直となる角度であり、俯角０°は、レーザパルスの照射角度が走行路面に対して水平となる角度であるものとする。

具体的には、計測センサ１２の取り付け角度が、望ましくは俯角５０°程度〜俯角７０°程度、特に望ましくは、俯角６０°程度である。

この計測センサ１２は、走行路面２５に対して上下方向、すなわち走行路面２５から天井面２７にかけての上下方向にレーザパルスを照射し、測定用台車２０の周囲にある物体の三次元的な形状を計測するために用いる。また、計測センサ１２の仰角に取り付けられた傾きによって、壁面を密に計測し、また、２つのセンサの距離データの時刻的な対応を求めることができる。本システムの本質は、この計測センサ１２の傾きにある。

計測センサ１１は、前述したように水平に取り付けられている。そのため、測定用台車２０を少し移動させて計測を行うと、距離データに同一の物体を計測したデータが多く含まれる。

そこで、計測センサ１１によって計測した２つの距離データのマッチング処理を行うことで、計測センサ１２の相対位置姿勢を検出することができる。このようにして、計測センサ１１の測定データから、測定用台車２０の位置と姿勢、すなわち測定用台車２０の位置姿勢を算出する。

計測センサ１１によって検出した計測センサ１１の位置姿勢、言い換えれば測定用台車２０の位置姿勢と、計測センサ１２の相対位置姿勢と、２種類の距離データの計測開始時刻とがわかれば、計測センサ１２が計測した物体の形状である、三次元地図データを算出することができる。

よって、三次元地図生成システム１０では、測定用台車２０を移動させながら計測を行っているので、通路をそのまま移動させることができる。測定用台車２０によって物体のまわりを移動すれば、対象の物体の形状を余すことなく計測することができる。

図１では、計測センサ１２は、レーザパルスが走行路面２５を照射するように取り付けられた例を示しているが、図１の点線にて示すように、該計測センサ１２をレーザパルスが走行路面と対向する面側、すなわち天井面２７側を照射する方向である仰角に取りけるようにしてもよい。

その角度は、仰角０°よりも大きい角度である。ここで、仰角０°は、レーザパルスの照射角度が走行路面２５に対して水平となる角度であり、仰角９０°は、レーザパルスの照射角度が走行路面２５に対して垂直となる角度である。

具体的には、計測センサ１２の取り付け角度は、望ましくは仰角５０°程度〜仰角７０°程度、特に望ましくは、仰角６０°程度である。

計測センサ１２を俯角に取り付けた場合、例えば俯角５０°よりも小さな角度では、計測できる範囲が小さくなってしまい、例えば俯角７０°程度よりも大きな角度では、レーザパルスの反射強度が弱くなり、効率のよい測定ができない恐れが生じる。

同様に、計測センサ１２を仰角に取り付けた場合、例えば仰角７０°程度よりも大きな角度では、計測できる範囲が小さくなってしまい、例えば仰角５０°よりも小さな角度では、レーザパルスの反射強度が弱くなり、効率のよい測定ができない恐れが生じてしまう。

また、計測センサ１２は、回転自在に取りけられ、レーザパルスが走行路面２５または天井面２７のいずれかに照射されるように回転させる回転機構を介して、測定用台車２０に取りけるようにしてもよい。このように、計測センサ１２においても、取り付け位置や傾きなどを測定する周囲の状況に応じて変更することが可能である。

さらに、三次元地図生成システム１０は、レーザパルスが走行路面２５を照射する計測センサと、天井面２７を照射する計測センサとの２つ計測センサを有する構成としてもよい。また、計測センサ１２は、前述したレーザ測域センサ以外であってもよく、例えば、ステレオカメラやデプスカメラなどの物体の形状情報を取得可能であり、かつ計測時間が記録できるものであればよい。

入力部１４は、例えばキーボードやマウスなどであり、処理装置１３を操作するデータなどを入力する。表示部１５は、液晶ディスプレイなどであり、測定結果などを表示する。バッテリ１６は、計測センサ１１，１２、処理装置１３、入力部１４、および表示部１５に電源を供給する。
〈処理装置の構成例〉

図２は、図１の三次元地図生成システム１０が有する処理装置１３の構成の一例を示す説明図である。

地図生成部となる処理装置１３は、例えばパーソナルコンピュータなどからなり、計測センサ１１，１２による測定データを処理する。処理装置１３は、図示するように、プロセッサ３０、メモリ３１、および記憶装置３２を有する。

プロセッサ３０は、処理装置１３における制御を司る。メモリ３１は、例えばＲＡＭ（Ｒａｎｄｏｍ Ａｃｃｅｓｓ Ｍｅｍｏｒｙ）などの揮発性メモリからなり、記憶装置３２に格納されているソフトウェアやデータ、あるいはプロセッサ３０による演算処理結果などが一時的に格納される。記憶装置３２は、例えばハードディスクドライブなどからなり、各種のソフトウェアやデータなどが格納されている。

また、プロセッサ３０は、センサ制御部３３、二次元地図生成・位置姿勢推定部３４、および三次元地図生成部３５を有する。センサ制御部３３は、計測センサ１１，１２の動作を制御する。そして、センサ制御部３３は、計測センサ１１，１２からそれぞれ取得した距離データに対応付けするための番号を付与し、番号を付与したその距離データをログデータとして、記憶装置３２の後述する第１のログデータ格納部３６と第２のログデータ格納部３７にそれぞれ格納する。

位置姿勢推定部である二次元地図生成・位置姿勢推定部３４は、計測センサ１１による距離データに基づいて、測定用台車２０の位置姿勢を推定し、位置姿勢推定データを生成する。位置姿勢推定データは、記憶装置３２の後述する地図データ・位置姿勢データ格納部３８に格納する。

また、二次元地図生成・位置姿勢推定部３４は、計測センサ１１による距離データに基づいて、測定用台車２０の周囲にある物体の二次元地図を生成する。生成した二次元地図も同様に、地図データ・位置姿勢データ格納部３８に格納する。

三次元地図生成部３５は、二次元地図生成・位置姿勢推定部３４が求めた測定用台車２０の位置姿勢から、計測センサ１２の距離データが計測した物体の座標（ｘ、ｙ、ｚ）の算出を行い、その算出結果を記憶装置３２の後述する三次元地図格納部３９に格納する。

また、三次元地図生成部３５は、キャリブレーション処理を実行する。計測センサ１１および計測センサ１２によるデータの計測開始時刻の大まかな対応はわかっているが、後述するように測定開始時刻は同期していないためにずれが生じている。キャリブレーション処理は、厳密に時刻のずれを補正するためである。

記憶装置３２は、第１のログデータ格納部３６、第２のログデータ格納部３７、地図データ・位置姿勢データ格納部３８、および三次元地図格納部３９を有している。第１のログデータ格納部３６および第２のログデータ格納部３７は、前述したセンサ制御部３３から出力される距離データをそれぞれ格納する。

地図データ・位置姿勢データ格納部３８は、二次元地図生成・位置姿勢推定部３４が生成した二次元地図および推定した測定用台車２０の位置姿勢推定データを格納する。三次元地図格納部３９は、前述したように三次元地図生成部３５による算出結果を格納する。
〈ソフトウェアの構成例〉

図３は、図２の処理装置１３が有する記憶装置３２に格納されるソフトウェア構成の一例を示す説明図である。

記憶装置３２には、前述した第１のログデータ格納部３６、第２のログデータ格納部３７、地図データ・位置姿勢データ格納部３８、および三次元地図格納部３９だけではなく、三次元地図を生成するための様々なソフトウェアが格納されている。

この場合、記憶装置３２は、図３に示すように、ＯＳ（Ｏｐｅｒａｔｉｎｇ Ｓｙｓｔｅｍ）４０、初期化プログラム４１、レーザ距離センサ制御プログラム４２、二次元地図生成・位置姿勢推定プログラム４３、三次元座標変換プログラム４４、およびキャリブレーションプログラム４５が格納されている。

ＯＳ４０は、入力部１４からの入力や表示部１５への出力などの入出力機能や記憶装置３２の管理などの処理装置１３全体を管理するソフトウェアである。初期化プログラム４１は、処理装置１３に電源が投入された際に、ＯＳ４０を起動させるプログラムである。

レーザ距離センサ制御プログラム４２は、計測センサ１１，１２（図１）の動作を制御するプログラムである。二次元地図生成・位置姿勢推定プログラム４３は、二次元地図生成・位置姿勢推定部３４が、計測センサ１１による計測結果に基づいて二次元地図データを生成する際、および測定用台車２０の位置姿勢を推定する際に用いられるプログラムである。

三次元座標変換プログラム４４は、三次元地図生成部３５が三次元地図を生成する際に用いるプログラムである。キャリブレーションプログラム４５は、計測センサ１１がデータ取得した際の時間のずれと計測センサ１２がデータ取得した際の時間のずれとをキャリブレーションするプログラムである。
〈計測センサの構成例〉

図４は、図１の三次元地図生成システム１０に備えられる計測センサ１１における構成の一例を示す説明図である。なお、図４では、計測センサ１１の構成例を示しているが、計測センサ１２における構成も同じである。

計測センサ１１は、発光部５０、リフレクタ５１、リフレクタ５２、受光部５３、回転部５４、エンコーダ５５、および第１の時計部である内部時計５６を有する構成からなる。

計測センサ１１は、図４の上方から下方にかけて、発光部５０、リフレクタ５１、リフレクタ５２、受光部５３が位置している。

発光部５０と受光部５３との間には、回転可能に取り付けられた回転部５４が設けられている。回転部５４の上部には、リフレクタ５１が取り付けられており、回転部５４の下部には、リフレクタ５２が取り付けられている。

発光部５０は、レーザパルスを照射する。リフレクタ５１は、発光部５０が照射したレーザパルスを反射させ、該レーザパルスを水平方向に照射させる。リフレクタ５２は、物体から反射したレーザパルスの反射光を、受光部５３が受信できるように反射させる。これらリフレクタ５１，５２は、ミラーなどの反射板からなる。受光部５３は、反射光を受光する。

発光部５０が照射したレーザパルスは、リフレクタ５１によって反射され、計測センサ１１の外部に水平照射される。レーザパルスは、照射された物体によって反射され、その反射光がリフレクタ５２によって反射し、受光部５３によって受光される。発光部５０が照射したレーザパルスを受光部５３が受光するまでの時間を計測することによって、物体までの距離が算出される。

回動動作を行っている回転部５４に取りけられたリフレクタ５１，５２が回転することによって水平方向に、例えば２７０°の範囲にてレーザパルスが照射される。また、発光部５０は、例えば０．２５°程度の刻みによってレーザパルスを照射する。受光部５３におけるスキャン１回当たりの時間は、例えば２５ｍｓ程度である。

回転部５４における回転角速度は、該回転部５４の移動方向や移動量、角度などを検出するエンコーダ５５によって制御され、その角速度は、例えば２０Ｈｚ程度〜４０Ｈｚ程度である。また、内部時計５６は、発光部５０によるレーザパルスが照射されたタイミングの時刻、すなわち測定開始時刻を計測する。

この計測開始時刻は、計測センサ１１が２７０°の範囲にてレーザパルスを照射する場合、基準となる最初のパルス、例えば０°の時にレーザパルスが照射される毎に計測される時間である。

このように、計測センサ１１からは、移動体周囲にある物体との距離を計測した距離データおよび測定開始時刻がそれぞれ出力される。なお、計測センサ１１の内部時計５６と計測センサ１２に設けられた第２の時計部となる内部時計５６とは、同期していない。

計測センサ１１，１２の内部時計５６を同期させていない場合、これら計測センサ１１，１２における距離データの測定開始時刻のずれが生じてしまうことになるが、この時間のずれにつては、上述したキャリブレーション処理によって補正を行う。

これによって、計測センサ１１，１２の内部時計５６を同期させハードウェアを不要とすることができ、三次元地図生成システム１０の構成を簡単化し、低コストを実現することができる。
〈三次元地図生成システムの計測例〉

図５は、測定用台車２０に搭載された三次元地図生成システム１０による計測の一例を示した説明図である。

この図５では、測定用台車２０に搭載されている計測センサ１１が、周囲にある物体を計測している様子を示したものである。計測センサ１１によって２次元的に水平方向にレーザパルスＬＢが照射され、物体までの距離を計測する。

この測定によって得られるデータは、例えば図５の右側に示すようになり、レーザパルスＬＢが照射されて測距されたポイントが点となって示される。この点によって、周囲にある物体の形状が表される。計測は、測定用台車２０を移動させながら、連続的に距離データを収集する。
〈姿勢位置の推定処理〉

図６は、計測センサ１１による測定用台車２０の位置姿勢の推定処理の一例を示す説明図である。

この推定処理は、二次元地図生成・位置姿勢推定部３４が二次元地図生成・位置姿勢推定プログラム４３に基づいて、実行する処理である。また、測定用台車２０の位置姿勢とは、二次元地図上において位置（ｘ，ｙ）および方向θの３つのパラメータで表現される状態を意味する。方向θは、例えばＸ軸を基準として何度の方向を向いているかなどと定義する。

図２に示す記憶装置３２の第１のログデータ格納部３６に格納されている距離データは、ログデータとして、例えば距離｛ｄ１ｉ｝＿ｋ、方向｛ω１ｉ｝＿ｋ、および時刻ｔ１＿ｋ；ｉ＝１，２，…，１０８１，ｋ＝１，２，．．，Ｎが保存されている。ｋは距離データの通し番号である。この距離データは、時刻ｔ＿ｋにおける移動体周辺にある物体の形状を表している。

ここで、時刻ｔ＿ｋにおける計測センサ１１が計測したデータを３６＿ｋとし、時刻ｔ＿ｋ＋１における距離データを３６＿ｋ＋１とする。距離データ３６＿ｋと３６＿ｋ＋１は、測定用台車２０が移動しながら連続的に計測を行ったデータであり、移動体の移動量は、ある程度限定されているものとする。

距離データ３６＿ｋと距離データ３６＿ｋ＋１は、近い位置姿勢で計測を行っているため、物体６０のほぼ同じ部分を計測している。よって、２つの距離データの形状には、一致する部分が多くあるため、マッチング処理を行えば、物体６０の形状を介して、お互いの相対的な位置関係を知ることができる。

幾何形状データのマッチングの方法としては、例えば、占有格子地図を用いる方法がある。占有格子地図とは、空間を格子状の領域に分割し、レーザによって計測できる物体の有無を各領域で表現する方法である。例えば物体がある場合を１、ない場合を０などと表現する方法である。距離データ３６＿ｋを占有格子によって表現すれば、重なりの度合いを評価することができる。このマッチング処理にはＩＣＰ（Iterative Closest Point）法などを用いてもよい。

このマッチング処理を行うためには、適当な座標系を割り当てる必要があり、座標系としては、例えば３６＿１の位置を原点、計測センサ１１の０°をｘ軸方向などとすればよい。このような割り当て方は、一例を示したものであり、例えば別の割り当て方としては、距離データを基準として座標を決定してもよい。例えば、計測した距離データに緯度経度が既知の物体が含まれれば、この点を基準として世界測地系の座標を用いてもよい。

一旦座標系が決定されれば、距離データ３６＿ｋ＋１から時刻ｔ＿ｋにおける測定用台車２０の位置が確定する。上記の距離データをｋ＝１から順に重ね合わせていき図６の右側に示すように、物体６０の配置図である地図６１ａを拡張しながら、位置姿勢６１ｂを算出していく。

上記のように、地図生成から位置姿勢推定のサイクルをログデータすべてに対して行うことで、測定用台車２０の位置姿勢（ｘｋ，ｙｋ，θｋ，ｔｋ）６１ｂを算出する。このようにして、測定用台車２０の位置姿勢推定データを算出する。

ここでは、計測センサ１１のデータのみを用いて二次元地図を生成し、位置の推定を行ったが、あらかじめ地図データが存在する場合は、これを用いて測定用台車２０の位置姿勢を推定すればよく、地図を生成する必要はない。
〈三次元地図の生成例〉

続いて、三次元地図の生成技術について説明する。

図７は、図１の三次元地図生成システム１０による三次元地図の生成の一例を示す説明図である。

図７において、測定用台車２０は、移動しながら周囲にある物体６０を計測し、計測センサ１１の距離データ（３６＿ｋ）と計測センサ１２の距離データ（３７＿ｋ）とを記憶装置３２に格納する。ここで、ｋ番目の距離データ角度ω２ｋｉのデータは、点７２を計測しており、その距離ｄ２ｋｉを距離７１とする。

すでに述べたように、計測センサ１１のログデータから、測定用台車２０の位置姿勢（ｘ＿ｋ，ｙ＿ｋ，θ＿ｋ，ｔ＿ｋ）６１ｂの系列を算出することができる。

このとき、距離（ｄ２ｋｉ）７１を以下の（式１）に適用すれば、計測センサ１２が計測した点７２の三次元座標（ｘ，ｙ，ｚ）を求めることができる。

（式１）におけるベクトルＸ’は、計測センサ１２のｋ番目の距離ｄ２ｋｉをｘ−ｙ平面直交座標系で表現したベクトルである。

ａ，ｂ，ｃは、計測センサ１１と計測センサ１２との位置の差を表し、α，β，γは、相対的な姿勢をそれぞれ表している。Ｕ（α，β，γ，ａ，ｂ，ｃ）は、原点を中心としてベクトルを回転、平行移動させる同次変換行列であり、これをＸ’に演算すると計測センサ１１を原点とした座標系に変換できる。

Ｕ（０，０，θ＿ｋ，ｘ＿ｋ，ｙ＿ｋ，０）は、計測センサ１１の位置姿勢、すなわち測定用台車２０の位置姿勢を表す同次変換行列であり、さらに、これを演算することによって、計測センサ１２の位置姿勢を地図の座標系に変換する。上記の変換を行うことによって実際に計測した物体の座標を算出することができる。
〈三次元地図生成システムの動作例〉

次に、三次元地図生成システム１０による三次元地図の生成について説明する。

図８は、図２の三次元地図生成システム１０による距離データの測定から三次元地図の生成までの処理の一例を示すフローチャートである。

まず、三次元地図生成システム１０による距離データの測定が開始されると（ステップＳ１０１）、処理装置１３のセンサ制御部３３は、計測センサ１１および計測センサ１２による計測データを取得する（ステップＳ１０２，Ｓ１０３）。このとき、センサ制御部３３は、ほぼ同時刻に計測したデータに同じインデックスｋを付与し、記憶装置３２に格納する。

地図を生成したい範囲において、測定用台車２０を移動させながら（ステップＳ１０４）すべての範囲の計測が終了するまで、ステップＳ１０２の処理およびステップＳ１０３の処理を繰り返す（ステップＳ１０５）。

続いて、測定用台車２０を移動させることによって計測したいすべての範囲の計測が終了すると、二次元地図生成・位置姿勢推定部３４による二次元地図生成および位置姿勢推定処理が実行される（ステップＳ１０６）。

このステップＳ１０６の処理では、記憶装置３２の第１のログデータ格納部３６に格納されているログデータを順次読み出し、地図生成処理と測定用台車２０の位置姿勢の推定処理を繰り返し、すべてのログデータに対して、位置姿勢推定データを生成する。二次元地図生成・位置姿勢推定部３４は、生成した位置姿勢推定データを地図データ・位置姿勢データ格納部３８に格納する。

その後、三次元地図生成部３５による三次元地図の生成処理が実行される（ステップＳ１０７）。この処理は、三次元地図生成部３５が、三次元座標変換プログラム４４に基づいて、記憶装置３２の第２のログデータ格納部３７に格納されたログデータおよび地図データ・位置姿勢データ格納部３８に格納された位置姿勢推定データから三次元座標（ｘ，ｙ，ｚ）を生成する。

上記した処理を実行することによって、三次元データを復元することが可能となる。しかし、計測センサ１２においては、その取り付け角度に応じて、得られる三次元地図の精度が異なってくる。
〈計測センサの傾き効果〉

以下、計測センサ１２に傾きをつけた場合の効果について説明を行う。

図９は、計測センサ１２による側壁面の計測例を示す説明図である。

屋内で物体を計測する場合、例えば壁面やロッカー、机などの様々な物体が配置されている。

図９において、測定用台車２０は、移動しながら計測を行っており、位置９１から位置９３まで移動するものとする。測定用台車２０は、位置９１から位置９２まで移動する。そして、該位置９２において旋回した後、位置９３まで移動する。

計測センサ１１，１２は、測定用台車２０によって移動しながら計測を行っているため、計測センサ１２のスキャンは、移動速度に応じた間隔で側壁面に照射されることになる。

このとき、計測センサ１２の取り付け角度を、俯角９０°、すなわち計測センサ１２の取り付け角度が走行路面に対して垂直であるとすると、レーザパルスによるスキャン面の間隔９４は、移動速度×スキャン時間によって表現される。

一方、計測センサ１２の取り付け角度を、俯角０°ではなく、走行路面に対してβ°だけ傾けて取り付けた場合、スキャン面の間隔９５は、移動速度×スキャン時間×ｃｏｓ（９０−β）となる。走行路面に対してβ°だけ傾けて計測センサ１２を取り付けた場合には、βが０でなければ、俯角０°にて計測した場合と比較すると間隔が狭くなることが分かる。

よって、計測センサ１２を俯角０°ではなく、傾けて取り付けることによって、側壁面を計測するデータ点９６の数を増加させることができる。その角度は、前述したように、俯角の場合、望ましくは走行路面に対して俯角５０°程度〜俯角７０°程度、特に望ましくは、俯角６０°程度であり、仰角の場合、望ましくは走行路面に対して仰角５０°程度〜仰角７０°程度、特に望ましくは、仰角６０°程度である。これらの角度にて計測センサ１２を取り付けることにより、データ点の数を効率よく増加させることができる。

データ点９６の数が増えると、データを利用するための処理、例えば側壁面抽出やノイズ処理なども行いやすくなり、より高精度な質の良いデータを計測することができる。

また、計測センサ１２を傾けて取り付けることによって、異なる時刻にて計測した三次元用距離データが、側壁面および床面で交差する。計測センサ１２のスキャン面が交差すると、計測データの整合性をチェックすることが可能となり、以下の誤差要因を排除することが可能となる。

１）距離データの測定開始時刻のずれ。
２）センサの取り付け誤差。

３）床面のうねりなどの影響による誤差。

計測センサ１２の取り付け角度が俯角９０°の場合、すなわち走行路面に対して垂直の場合には、測定用台車２０が旋回することによって走行面での交差は発生するが、側壁面では発生しないことになる。そのため、上記の誤差を排除することができない。

しかし、計測センサ１２の測定開始時刻に誤差がある場合であっても、計測センサ１２の交差の情報を用いれば、測定開始時刻の誤差を正確に算出することができる。

続いて、計測センサ１１および計測センサ１２における測定開始時刻のずれの影響について説明する。
〈測定時刻のずれの影響〉

図１０は、計測センサ１１と計測センサ１２とにおける測定開始時刻のずれの影響を示す説明図である。この図１０においては、図９における計測の様子を上方から俯瞰し、二次元的に示した図である。この図１０を用いて、計測センサ１１と計測センサ１２との測定開始時刻のずれの影響について説明する。

まず、移動経路１０４に沿って測定用台車２０を移動させて計測を行い、計測センサ１１の距離データ、および計測センサ１２の距離データを計測する。また、計測センサ１１によるこの計測結果に基づいて、測定用台車２０の位置姿勢推定結果１０５，１０６，１０１，１０２が得られている。

このとき、センサ制御部３３は、計測センサ１１の距離データと計測センサ１２の距離データとの時刻の対応がある程度ついている状態であるが正確な対応はついていない。

図９を用いて説明したように、計測センサ１２は、傾けて取り付けられているため、側壁面においてスキャン面が交差しており、該計測センサ１２の距離データと位置姿勢推定データの時刻的な対応が正しくついている場合、距離データ１０３ａ，１０７ａが正しく重なる。対応関係がずれている場合には、データを復元すると三次元点群データの計測面１０３ｂ，１０７ｂが側壁面において重ならない。

この計測センサ１１と計測センサ１２との測定開始時刻のずれ量Δｔは、全体の距離データがきれいに重なる値としてソフト的に検出可能である。よって、処理装置１３に汎用的なＯＳを用いることがでる。また、測定開始時刻の同期をとるための特別なシステムを必要としないため、システムを安価に構成することができる。
〈キャリブレーション処理例〉

続いて、計測センサ１１と計測センサ１２との測定開始時刻のずれを修正するキャリブレーション処理について説明する。

図１１は、図２の処理装置１３によるキャリブレーション処理の一例を示すフローチャートである。

このキャリブレーション処理は、処理装置１３における三次元地図生成部３５が、記憶装置３２に格納されているキャリブレーションプログラム４５に基づいて実行するものであり、計測時間のずれ量であるΔｔを算出する。前提条件として、図８に示すステップＳ１０６の処理である位置姿勢推定処理がすでに実施されているものとする。

以下においては、時刻ｔにおける位置姿勢推定結果をｘ（ｔ）で表す。また、要点を説明するため、計測周期は移動速度に対して十分高速であり、特に補間などが必要ない場合について説明する。

はじめにΔｔを−Ｔとして、時刻誤差を初期化する（ステップＳ２０１）。Ｔは、時刻誤差の範囲を表し、計測センサの計測周期によって異なる。例えば２５ｍｓ周期にて計測する場合には、２周期分の５０ｍｓ程度に設定する。

続いて、時刻誤差をΔｔとして三次元データの再構成を行う（ステップＳ１０２）。位置姿勢推定結果の測定開始時刻ｔ１＿１と計測センサ１２の測定開始時刻ｔ２＿１との差をＤとすれば、時刻ｔ２＿ｋにおける計測センサ１２の位置は、ｘ（ｔ２＿ｋ−Ｄ＋Δｔ），ｙ（ｔ２＿ｋ−Ｄ＋Δｔ），θ（ｔ２＿ｋ−Ｄ＋Δｔ）である。この値を後述する（式２）に代入し、三次元データを算出する。そして、計測センサ１２の距離データの交点の数を算出し、その数をＰとする（ステップＳ２０３）。

次に、ＰｍａｘとＰを比較し（ステップＳ２０４）、交点の数が多ければ、Δｔ＿ｏｐｔとＰｍａｘを更新する（ステップＳ２０５）。交点の数が少なければ、Δｔ＿ｏｐｔとＰｍａｘは更新せず、次のステップに移行する。ここで、Ｐｍａｘは、交点の数の最大値であり、Δｔ＿ｏｐｔは、交点の数が最大になるときの時刻のずれ量である。

続いて、Δｔの探索範囲内かどうかを比較し（ステップＳ２０６）、探索範囲内であればΔｔを更新する（ステップＳ２０７）。Ｕは、更新幅であり、キャリブレーションの精度によって変える。

上記の処理を−Ｔ≦Δｔ≦Ｔの範囲で行い、最適なずれ量ΔＴを算出し（ステップＳ２０８）、処理を終了する。

三次元地図生成部３５は、ステップＳ２０８の処理にて算出した最適なずれ量ΔＴを用いて計測センサ１１と計測センサ１２との測定開始時刻の誤差を修正し、三次元点群データを生成する。これにより、歪みなどがない高精度な三次元地図を生成することができる。

測定用台車２０の移動速度に対して計測周期が遅い場合や、計測センサ１２の距離データに欠損がある場合には、位置姿勢推定結果を補間する必要がある。例えば、計測センサ１２のスキャン周期が２５ｍｓであり、キャリブレーションの精度を１ｍｓで行うためには、位置姿勢推定結果を１ｍｓ間隔で求める必要があるため、はじめに補間処理を行う。

補間の方法は、例えば線形補間や、スプライン補間などで行う。また、位置推定結果に載っている誤差の影響を考慮したカルマンフィルタなど状態推定法による補間を行ってもよい。

図１１に示すフローチャートは、最適なΔｔを探す方法として、最も基本的な総当たりにて探しているが、二分探索や最急勾配法など、効率的な方法を用いて探索してもよい。

また、キャリブレーションの評価値としては、理解の簡単のため、交点の数で評価しているが、重なりを評価できるものであればどのような評価関数でもよい。他の評価関数としては、例えば以下のようなものがある。

空間を格子状に分割し、分割されたセルの中にある点の数ｐ１，ｐ２，…を用いて、式２の評価式Ｐを用いる。

この評価式は、計測データが重なれば重なるほどＰの値が大きくなる。セルの大きさはセンサのノイズによって決める。上記のキャリブレーション方法は、計測センサ１１と計測センサ１２との取り付け位置関係をキャリブレーションするのにも利用できる。

また、以下に示す（式３）におけるパラメータηｋ，ξｋ，Δθｋ，Δｘｋ，Δｙｋ，Δｚｋを変化させ、（式２）を用いてデータの重なりを評価すれば、床面のうねりの影響による誤差も自動的に補正することが可能である。

これによって、計測センサ１２を傾けて取り付けることによって、正確な三次原点群データを生成することができる。

以上により、測定用台車２０を移動させながら、三次元地図生成システム１０による距離データの計測を行うだけで、死角などがない高精度な三次元地図データを容易に生成させることができる。

それにより、複数回にわたる測定機器の設置や計測などを不要とすることができ、作業効率を向上させ、コストを削減することができる。

以上、本発明者によってなされた発明を実施の形態に基づき具体的に説明したが、本発明は前記実施の形態に限定されるものではなく、その要旨を逸脱しない範囲で種々変更可能であることはいうまでもない。

なお、本発明は上記した実施の形態に限定されるものではなく、様々な変形例が含まれる。例えば、上記した実施の形態は本発明を分かりやすく説明するために詳細に説明したものであり、必ずしも説明した全ての構成を備えるものに限定されるものではない。

また、ある実施の形態の構成の一部を他の実施の形態の構成に置き換えることが可能であり、また、ある実施の形態の構成に他の実施の形態の構成を加えることも可能である。また、各実施の形態の構成の一部について、他の構成の追加、削除、置換をすることが可能である。

その他、実施の形態に記載された内容の一部を以下に記載する。

（１）床面に対して水平方向の距離データを収集する第１の計測センサと、上下方向の距離データを収集する２つの第２の計測センサと、第１および第２の計測センサを移動させて取得した距離データから、三次元地図を生成する地図生成部と、を有する。一方の第２の計測センサは、床面に対して俯角が生じるように取り付けられる。他方の第２の計測センサは、床面に対して仰角が生じるように取り付けられる。地図生成部は、第１の計測センサが収集した水平方向の距離データに基づいて、第１の計測センサの位置および角度を同定し、同定した位置および角度に対して、第２の計測センサが収集した上下方向の距離データを対応付けして三次元地図を生成する。

（２）（１）の三次元地図生成システムにおいて、地図生成部は、第１の計測センサが収集した距離データに基づいて、二次元地図上における第１の計測センサの位置および角度を推定した位置姿勢推定データを出力する位置姿勢推定部と、位置姿勢推定部が推定した二次元地図上における位置姿勢推定データに基づいて、第２の計測センサが収集した距離データを三次元座標に変換し、三次元点群データからなる三次元地図を生成する三次元地図生成部と、を有する。

（３）（２）の三次元地図生成システムにおいて、さらに、第１の計測センサおよび第２の計測センサは、距離データの測定開始時刻を計測する第１および第２の時計部をそれぞれ有する。三次元地図生成部は、位置姿勢推定部が推定した位置姿勢推定データと第２の計測センサが収集した距離データとを第１の時計部および第２の時計部がそれぞれ計測した測定開始時刻に基づいて対応付けする。

（４）（３）の三次元地図生成システムにおいて、三次元地図生成部は、三次元点群データが交差している点の数が最も多い点における時間のずれ量を算出し、算出した時間のずれ量に基づいて、第１の時計部および第２の時計部が計測した測定開始時刻を補正する。

（５）（１）の三次元地図生成システムにおいて、第２の計測センサの俯角および仰角は、それぞれ５０°〜７０°である。

１０ 三次元地図生成システム
１１ 計測センサ
１２ 計測センサ
１３ 処理装置
１４ 入力部
１５ 表示部
１６ バッテリ
２０ 測定用台車
２１ 車輪
２５ 走行路面
２６ 側壁面
２７ 天井面
３０ プロセッサ
３１ メモリ
３２ 記憶装置
３３ センサ制御部
３４ 二次元地図生成・位置姿勢推定部
３５ 三次元地図生成部
３６ 第１のログデータ格納部
３７ 第２のログデータ格納部
３８ 地図データ・位置姿勢データ格納部
３９ 三次元地図格納部
４０ ＯＳ
４１ 初期化プログラム
４２ レーザ距離センサ制御プログラム
４３ 二次元地図生成・位置姿勢推定プログラム
４４ 三次元座標変換プログラム
４５ キャリブレーションプログラム
５０ 発光部
５１ リフレクタ
５２ リフレクタ
５３ 受光部
５４ 回転部
５５ エンコーダ
５６ 内部時計
６０ 物体

Claims (5)

1. 床面に対して水平方向の距離データを収集する第１の計測センサと、
   前記床面から前記床面に対向する天井面にかけての方向である上下方向の距離データを収集する第２の計測センサと、
   前記第１および前記第２の計測センサを移動させて取得した前記距離データから、三次元地図を生成する地図生成部と、
   前記第１の計測センサがデータ取得した際の時間のずれと前記第２の計測センサがデータ取得した際の時間のずれとをキャリブレーションするプログラムを格納する記憶装置と、
   を有し、
   前記第２の計測センサは、照射するレーザパルスが前記床面に対して俯角または仰角となるように取り付けられ、
   前記地図生成部は、前記第１の計測センサが収集した水平方向の前記距離データに基づいて、前記第１の計測センサの位置および角度を同定し、同定した前記位置および前記角度に対して、前記第２の計測センサが収集した前記上下方向の距離データを対応付けして前記三次元地図を生成し、前記三次元地図を生成する際に前記記憶装置に格納される前記プログラムを用いて前記第１の計測センサが収集した前記距離データと前記第２の計測センサが収集した前記距離データとが重なる測定時間のずれ量を算出し、算出した前記ずれ量を前記第１の計測センサによる前記距離データの測定開始時刻と前記第２の計測センサによる前記距離データの測定開始時刻とのずれとして補正する、三次元地図生成システム。
2. 請求項１記載の三次元地図生成システムにおいて、
   前記地図生成部は、
   前記第１の計測センサが収集した前記距離データに基づいて、二次元地図上における前記第１の計測センサの位置および角度を推定した位置姿勢推定データを出力する位置姿勢推定部と、
   前記位置姿勢推定部が推定した二次元地図上における前記位置姿勢推定データに基づいて、前記第２の計測センサが収集した前記距離データを三次元座標に変換し、三次元点群データからなる三次元地図を生成する三次元地図生成部と、
   を有する、三次元地図生成システム。
3. 請求項２記載の三次元地図生成システムにおいて、
   さらに、前記第１および前記第２の計測センサは、前記距離データの測定開始時刻を計測する第１および第２の時計部をそれぞれ有し、
   前記三次元地図生成部は、前記位置姿勢推定部が推定した前記位置姿勢推定データと前記第２の計測センサが収集した前記距離データとを前記第１および前記第２の時計部がそれぞれ計測した測定開始時刻に基づいて対応付けする、三次元地図生成システム。
4. 請求項１記載の三次元地図生成システムにおいて、
   前記第２の計測センサの俯角は、５０°〜７０°である、三次元地図生成システム。
5. 請求項１記載の三次元地図生成システムにおいて、
   前記第２の計測センサの仰角は、５０°〜７０°である、三次元地図生成システム。

Images