JP6548452B2 - 地図生成装置および地図生成方法 - Google Patents

Description

本発明は、地図生成装置および地図生成方法に関する。

近年、工場における効率的な改修やレイアウト確認あるいは美術館や商業施設などにおけるインテリアの変更などの様々な場面において、三次元地図が利用されている。この三次元地図は、三次元レーザスキャナなどを用いて計測されたデータに基づいて生成される。三次元レーザスキャナは、レーザパルスを測定対象物に照射し、その反射光のデータを取得する。三次元レーザスキャナにより取得されたデータは、三次元座標の点群データであり、該点群データによって、被測定対象の表面形状が形成される。

例えば、特開２０１４−１６８５８号公報（特許文献１）には、測距部によって取得された測距データに基づいて環境地図を生成する無人移動体が記載されている。

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

レーザスキャナなどを用いて地図を生成する場合において、特許文献１に記載された技術では、無人移動体の速度や加速度が高くなると、測距部によって取得される測距データに誤差が生じ、生成される環境地図の精度が低下する可能性がある。

本発明の目的は、地図を生成する精度を向上可能にする技術を提供することである。

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

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

本発明の一実施の形態の地図生成装置は、周囲にレーザを照射し、前記レーザを照射した角度と前記レーザが当たった点までの距離とを含む計測データを収集する計測センサを有する。また、前記計測センサを移動させて取得した前記計測データから地図を生成する地図生成部を有する。そして、前記地図生成部が、前記計測センサが移動される速度と加速度と角速度と角加速度との少なくともいずれか一つに基づいて、前記計測データを補間し、または前記計測センサが移動される速度を制御する。

また、本発明の一実施の形態の地図生成方法は、周囲にレーザを照射し、前記レーザを照射した角度と前記レーザが当たった点までの距離とを含む計測データを収集する計測センサと、前記計測センサを移動させて取得した前記計測データから地図を生成する地図生成部とを有する地図生成装置における地図生成方法であって、前記地図生成部が、前記計測センサが移動される速度と加速度と角速度と角加速度との少なくともいずれか一つに基づいて、前記計測データを補間し、または前記計測センサが移動される速度を制御する制御ステップを有する。

本願において開示される発明のうち、代表的なものによって得られる効果を簡単に説明すれば以下のとおりである。

本発明の代表的な実施の形態によれば、地図を生成する精度が向上する。

実施の形態１に係る地図生成装置における構成の一例を示す説明図である。 実施の形態１に係る地図生成装置が有する処理装置の構成の一例を示す説明図である。 実施の形態１に係る地図生成装置の処理装置が有する記憶装置に格納されるソフトウェア構成の一例を示す説明図である。 実施の形態１に係る地図生成装置に備えられる計測センサの構成の一例を示す説明図である。 実施の形態１に係る地図生成装置による計測の一例を示す説明図である。 実施の形態１に係る地図生成装置の計測センサによる測定用台車の位置姿勢の推定処理の一例を示す説明図である。 実施の形態１に係る地図生成装置による三次元地図の生成の一例を示す説明図である。 実施の形態１に係る地図生成装置による距離データの測定から三次元地図を生成するまでの処理の一例を示すフローチャートである。 実施の形態１に係る地図生成装置の計測センサにおける計測時刻のずれの影響を示す説明図である。 実施の形態２に係る地図生成装置による距離データの測定から未計測領域表示画面を表示するまでの処理の一例を示すフローチャートである。 （ａ）は、実施の形態２に係る地図生成装置による計測の一例を示す説明図であり、（ｂ）は、実施の形態２に係る地図生成装置の表示部が表示する未計測領域表示画面の説明図である。

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

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

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

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

また、実施の形態を説明するための全図において、同一の部材には原則として同一の符号を付し、その繰り返しの説明は省略する。なお、図面をわかりやすくするために平面図であってもハッチングを付す場合がある。

以下、本発明の実施の形態１を、図１〜図９を用いて詳細に説明する。
（実施の形態１）
＜地図生成装置の構成例＞

図１は、実施の形態１に係る地図生成装置１０における構成の一例を示す説明図である。

地図生成装置１０は、図１に示すように、計測センサ１１，１２、処理装置１３、入力部１４、表示部１５、およびバッテリ１６を有する。これら計測センサ１１，１２、処理装置１３、入力部１４、表示部１５、バッテリ１６は、測定用台車２０に搭載されている。なお、測定用台車２０に、図示しない速度センサ、加速度センサ、角速度センサが搭載されるようにしても良い。

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

図２は、実施の形態１に係る地図生成装置１０が有する処理装置１３の構成の一例を示す説明図である。

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

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

また、プロセッサ３０は、センサ制御部３３、二次元地図生成・位置姿勢推定部３４、および三次元地図生成部３５を有する。

センサ制御部３３は、計測センサ１１，１２の動作を制御する。また、センサ制御部３３は、計測センサ１１および計測センサ１２による計測データを取得する。また、センサ制御部３３は、二次元地図生成・位置姿勢推定部３４により算出される計測センサ１１，１２が移動される速度と加速度と角速度と角加速度とを取得する。なお、センサ制御部３３は、測定用台車２０に取り付けられる、速度センサ、加速度センサ、角速度センサなどから計測センサ１１，１２が移動される速度と加速度と角速度と角加速度とを取得しても良い。

センサ制御部３３は、前回スキャンが完了してから、スキャン周期時間（２５ｍｓ）が経過した場合に、１周期分のスキャンが完了したと判定する。そして、センサ制御部３３は、計測センサ１１，１２が移動される速度と加速度と角速度と角加速度とに基づいて、１周期の間に計測されている計測データの精度が低下しているのを判定する。例えば、センサ制御部３３は、計測センサ１１，１２が移動される速度と加速度と角速度と角加速度との少なくともいずれか一つが所定の閾値を超えている場合に、１周期の間に計測されている計測データの精度が低下していると判定する。

位置姿勢推定部である二次元地図生成・位置姿勢推定部３４は、計測センサ１１による距離データに基づいて、測定用台車２０の位置姿勢を推定し、位置姿勢推定データを算出する。二次元地図生成・位置姿勢推定部３４は、算出した位置姿勢推定データを記憶装置３２の後述する地図データ・位置姿勢データ格納部３８に格納する。また、測定用台車２０の位置姿勢とは、二次元地図上において位置（ｘ，ｙ）および方向θの３つのパラメータで表現される状態を意味する。方向θは、例えばＸ軸を基準として計測センサ１１が何度の方向を向いているかなどと定義する。

二次元地図生成・位置姿勢推定部３４は、推定した測定用台車２０の位置姿勢に基づき、計測センサ１１および計測センサ１２が移動される速度と加速度と角速度と角加速度と算出する。例えば、二次元地図生成・位置姿勢推定部３４は、測定用台車２０の移動前の位置と移動後の位置との差分と、移動に要した時間とに基づき、速度と加速度とを算出する。また、二次元地図生成・位置姿勢推定部３４は、測定用台車２０の旋回前の方向（姿勢）と旋回後の方向との差分と、旋回に要した時間とに基づき、角速度と角加速度と算出する。

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

１周期の間に計測されている計測データの精度が低下していないと判定される場合、二次元地図生成・位置姿勢推定部３４は、センサ制御部３３により割り振られる周期を識別するための番号であるインデックスｋと、１周期分のスキャンを開始した際の位置姿勢データと、１周期の間に計測された各計測データとを対応付けて記憶装置３２に格納する。

一方、１周期の間に計測されている計測データの精度が低下していると判定される場合、二次元地図生成・位置姿勢推定部３４は、１ｍｓ間隔で計測センサ１１，１２の位置姿勢データを算出する。そして、二次元地図生成・位置姿勢推定部３４は、０ｍｓ時（測定開始時刻）の測定用台車２０の位置姿勢の位置姿勢データから、１ｍｓ間隔で２５ｍｓ時の測定用台車２０の位置姿勢の位置姿勢データまでを算出する。このようにして、二次元地図生成・位置姿勢推定部３４は、測定用台車２０の位置姿勢推定データを補間する。そして、二次元地図生成・位置姿勢推定部３４は、センサ制御部３３により割り振られるインデックスと、０ｍｓ時における位置姿勢データから、１ｍｓ間隔で２５ｍｓ時における位置姿勢データまでを、１周期の間に計測された各計測データと対応付けて記憶装置３２に格納する。

詳細には、二次元地図生成・位置姿勢推定部３４は、計測センサが移動される速度と加速度と角速度と角加速度との少なくともいずれか一つに基づいて、１ｍｓ間隔で０ｍｓ時（測定開始時刻）の測定用台車２０の位置姿勢から、２５ｍｓ時の測定用台車２０の位置姿勢までの軌跡（１周期の間に移動した軌跡）を算出する。例えば、二次元地図生成・位置姿勢推定部３４は、速度と加速度とに基づいて、各位置（軌跡の構成する各位置）を推定することで、測定開始時刻の測定用台車２０の位置姿勢から、２５ｍｓ時の測定用台車２０の位置姿勢までの軌跡を算出する。

そして、二次元地図生成・位置姿勢推定部３４は、算出した軌跡を構成するすべての位置姿勢の位置姿勢データを、１周期の間に計測された各計測データと対応付けて記憶装置３２に格納する。

なお、二次元地図生成・位置姿勢推定部３４は、位置姿勢推定データを補間することに替えて、速度と加速度と角速度と角加速度とのいずれか一つが所定の閾値を超えた際に計測された計測データを記憶部３２に格納せずに、破棄しても良い。例えば、二次元地図生成・位置姿勢推定部３４は、１ｍｓ間隔で、速度と加速度と角速度と角加速度とのいずれか一つが所定の閾値を越えたかを判定し、閾値を越えたかと判定された際に照射されたレーザによって計測された計測データを破棄する。または、二次元地図生成・位置姿勢推定部３４は、速度と加速度と角速度と角加速度とのいずれか一つが所定の閾値を超えた周期に計測された計測データをすべて破棄しても良い。

センサ制御部３３は、１周期の間に計測された計測データに対応付けするための番号（周期を識別するための番号）であるインデックスｋを付与する。そして、センサ制御部３３は、番号を付与したその計測データをログデータとして、記憶装置３２の後述する第１のログデータ格納部３６と第２のログデータ格納部３７にそれぞれ格納する。

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

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

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

地図データ・位置姿勢データ格納部３８には、二次元地図生成・位置姿勢推定部３４が生成した二次元地図および推定した測定用台車２０の位置姿勢推定データが格納される。三次元地図格納部３９には、前述したように三次元地図生成部３５による算出結果が格納される。

表示部１５は、速度と加速度と角速度と角加速度とのいずれか一つが所定の閾値を超えた場合に、計測センサ１１，１２により取得されている計測データの精度が低下していることを警告する警告画面を表示する。

また、処理装置１３は、計測センサ１１，１２が移動される速度と加速度と角速度と角加速度との少なくともいずれか一つに基づいて、計測センサ１１，１２が移動される速度を制御する。例えば、処理装置１３は、計測センサ１１，１２が移動される速度と加速度と角速度と角加速度との少なくともいずれかが所定の閾値を超える場合、計測センサ１１，１２が移動される速度が遅くなるように、モータを制御する。
＜ソフトウェアの構成例＞

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

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

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

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

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

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

図４は、実施の形態１に係る地図生成装置１０に備えられる計測センサ１１における構成の一例を示す説明図である。なお、図４では、計測センサ１１の構成例を示しているが、計測センサ１２における構成も同じである。

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

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

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

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

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

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

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

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

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

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

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

図５は、実施の形態１に係る測定用台車２０に搭載された地図生成装置１０による計測の一例を示した説明図である。

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

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

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

この推定処理は、二次元地図生成・位置姿勢推定部３４が二次元地図生成・位置姿勢推定プログラム４３に基づいて、実行する処理である。

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

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

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

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

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

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

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

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

補間処理を行った場合、二次元地図生成・位置姿勢推定部３４は、０ｍｓ（測定開始時刻）から２５ｍｓまで１ｍｓ間隔で計測センサ１１，１２の位置姿勢と、各位置姿勢にて計測された計測データとを算出する。すなわち、二次元地図生成・位置姿勢推定部３４は、測定開始時刻における測定用台車２０の位置姿勢（ｘｋ０，ｙｋ０，θｋ０，ｔｋ０）から、２５ｍｓ経過時における測定用台車２０の位置姿勢（ｘｋ２５，ｙｋ２５，θｋ２５，ｔｋ２５）まで１ｍｓ間隔で算出する。このようにして、測定用台車２０の位置姿勢推定データを補間する。

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

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

図７は、実施の形態１に係る地図生成装置１０による三次元地図の生成の一例を示す説明図である。

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

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

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

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

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

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

次に、地図生成装置１０による三次元地図の生成について説明する。

図８は、実施の形態１に係る地図生成装置１０による距離データの計測開始から三次元地図の生成までの処理の一例を示すフローチャートである。

まず、地図生成装置１０による距離データの計測が開始される（Ｓ８０１）。

次に、処理装置１３のセンサ制御部３３は、計測センサ１１および計測センサ１２による計測データを取得する（Ｓ８０２，Ｓ８０３）。

次に、地図を生成したい範囲において、測定用台車２０が移動される（Ｓ８０４）。そして、センサ制御部３３は、計測センサ１１および計測センサ１２が移動される速度と加速度と角速度と角加速度とを取得する。

次に、センサ制御部３３は、計測センサ１１、１２による１周期分のスキャンが完了したか判定する（Ｓ８０５）。センサ制御部３３が、計測センサ１１、１２による１周期分のスキャンが完了していないと判定する場合（Ｓ８０５−Ｎｏ）、Ｓ８０２へ戻る。一方、センサ制御部３３が、計測センサ１１、１２による１周期分のスキャンが完了したと判定する場合（Ｓ８０５−Ｙｅｓ）、Ｓ８０６へ進む。例えば、センサ制御部３３は、測定開始時刻から、スキャン周期時間（２５ｍｓ）が経過した場合に１周期分のスキャンが完了したと判定する。

次に、センサ制御部３３は、計測センサ１１，１２により取得された、計測データの精度が低下しているか否かを判定する（Ｓ８０６）。センサ制御部３３が、計測センサ１１，１２により取得されている計測データの精度が低下していないと判定する場合（Ｓ８０６−Ｎｏ）、Ｓ８０９へ進む。一方、センサ制御部３３が、計測センサ１１，１２により取得された計測データの精度が低下していると判定する場合（Ｓ８０６−Ｙｅｓ）、Ｓ８０７へ進む。例えば、Ｓ８０４にて取得した速度と加速度と角速度と角加速度との少なくともいずれかが所定の閾値を超えている場合に、センサ制御部３３は、計測センサ１１，１２により取得された計測データの精度が低下していると判定する。

次に、二次元地図生成・位置姿勢推定部３４は、１ｍｓ間隔で計測センサ１１，１２の位置姿勢データを算出する。そして、二次元地図生成・位置姿勢推定部３４は、０ｍｓ時（測定開始時刻）の測定用台車２０の位置姿勢の位置姿勢データから、１ｍｓ間隔で２５ｍｓ時の測定用台車２０の位置姿勢の位置姿勢データまでを算出する（Ｓ８０７）。

次に、センサ制御部３３は、インデックスｋと、１ｍｓ間隔ごとに算出した各位置姿勢データと、１周期の間に計測された各計測データとを対応付けて記憶装置３２に格納する（Ｓ８０８）。Ｓ８０８の次は、Ｓ８１０へ進む。

Ｓ８０６にてＮｏだった場合Ｓ８０９にて、二次元地図生成・位置姿勢推定部３４は、センサ制御部３３により割り振られるインデックスｋと、１周期分のスキャンを開始した際の位置姿勢データと、１周期の間に計測された各計測データとを対応付けて記憶装置３２に格納する（Ｓ８０９）。

次に、地図を生成したいすべての範囲の計測が終了するまで、上述した処理が繰り返される（Ｓ８１０）測定用台車２０を移動させることによって地図を生成したいすべての範囲の計測が終了すると（Ｓ８１０−Ｙｅｓ）、三次元地図生成部３５による三次元地図の生成処理が実行される（Ｓ８１１）。この処理は、三次元地図生成部３５が、三次元座標変換プログラム４４に基づいて、記憶装置３２の第２のログデータ格納部３７に格納されたログデータおよび地図データ・位置姿勢データ格納部３８に格納された位置姿勢推定データから三次元座標（ｘ，ｙ，ｚ）を生成する。

上記した処理を実行することによって、三次元データを復元することが可能となる。
＜測定時刻のずれの影響＞

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

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

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

補間処理では、二次元地図生成・位置姿勢推定部３４は、１周期（２５ｍｓ）の間に計測センサ１１，１２が移動された軌跡を算出する。すなわち、１ｍｓ間隔で計測センサ１１，１２の位置姿勢データを算出する。そして、二次元地図生成・位置姿勢推定部３４は、０ｍｓ時（測定開始時刻）の測定用台車２０の位置姿勢の位置姿勢データから、１ｍｓ間隔で２５ｍｓ時の測定用台車２０の位置姿勢の位置姿勢データまでを算出する。

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

以上説明した実施の形態１によれば、計測センサ１１，１２が移動される速度と加速度と角速度と角加速度との少なくともいずれか一つに基づいて、計測データを補間し、または計測センサ１１，１２が移動される速度を制御することで、測定用台車２０の速度や加速度が変化することで、生成される地図の制度が低下することを抑止できるようになる。

また、速度と加速度と角速度と角加速度との少なくともいずれか一つが閾値を超えている場合に、計測センサ１１，１２により取得されている計測データ１１，１２の精度が低下していることを警告する警告画面を表示することで、計測データ１１，１２の精度が低下していることをユーザに認識させることができる。

また、速度と加速度と角速度と角加速度との少なくともいずれか一つが閾値を超えている際に取得された計測データを破棄することで、計測の失敗を防止でき、再計測なく短時間で地図を生成できる。
（実施の形態２）

図１１（ａ）に示されるように、柱や壁の裏側など測定用台車２０から見て死角になる領域には、計測センサ１１、１２から照射されるレーザパルスが通過できない。そのため、レーザパルスが通過していない領域に未計測領域１１０が形成されることがある。レーザパルスが通過していない未形成領域が存在する場合、地図に欠損が発生する可能性がある。

実施の形態２では、図１１（ｂ）に示されるように、計測センサ１１、１２から照射されるレーザパルスが通過した計測領域１２０とレーザパルスが通過していない未計測領域１１０とを異なる態様で表示する。これによって、未計測領域１１０があることをユーザに認識させることができ、地図を生成する精度を向上可能にしている。以下、本発明の実施の形態２を、実施の形態１と異なる点を主に、図１０〜図１２を用いて説明する。
＜地図生成装置の動作例＞

図１０は、実施の形態２に係る地図生成装置１０による距離データの計測開始から三次元地図の生成までの処理の一例を示すフローチャートである。

まず、地図生成装置１０による距離データの計測が開始される（Ｓ１００１）。

次に、処理装置１３のセンサ制御部３３は、計測センサ１１および計測センサ１２による計測データを取得する（Ｓ１００２，Ｓ１００３）。

次に、二次元地図生成・位置姿勢推定部３４は、レーザパルスが照射されて測距されたポイント（点）の物体座標を算出し、算出した物体座標を保持する（Ｓ１００４）。

次に、二次元地図生成・位置姿勢推定部３４は、計測センサ１１，１２から、レーザパルスが照射されて測距されたポイント（点）までの空間に含まれるすべての座標を計測済空間座標として推定し、推定した計測済空間座標を保持する（Ｓ１００５）。

次に、地図を生成したい範囲において、測定用台車２０が移動される（Ｓ１００６）。

次に、センサ制御部３３は、計測センサ１１、１２による１周期分のスキャンが完了したか判定する（Ｓ１００７）。センサ制御部３３が、計測センサ１１、１２による１周期分のスキャンが完了していないと判定する場合（Ｓ１００７−Ｎｏ）、Ｓ１００２へ戻る。一方、センサ制御部３３が、計測センサ１１、１２による１周期分のスキャンが完了したと判定する場合（Ｓ１００７−Ｙｅｓ）、Ｓ１００８へ進む。

次に、二次元地図生成・位置姿勢推定部３４は、保持している物体座標と計測済空間座標以外の座標を未計測空間座標として算出する（Ｓ１００８）。

次に、二次元地図生成・位置姿勢推定部３４は、互いに隣り合う未計測空間座標と計測済空間座標とを境界座標として算出する（Ｓ１００９）。なお、二次元地図生成・位置姿勢推定部３４は、未計測空間座標と計測済空間座標との間に位置する座標を境界座標として算出しても良い。

次に、二次元地図生成・位置姿勢推定部３４は、Ｓ１００９にて算出した境界座標に基づく境界の中心点と直行する直線を算出する。そして、二次元地図生成・位置姿勢推定部３４は、算出した直線を構成する座標と、計測済空間座標とに基づき、疑似画像座標を算出する（Ｓ１０１０）。例えば、二次元地図生成・位置姿勢推定部３４は、計測領域に含まれる座標であって、かつ算出した直線を構成する座標から、所定間隔ごとに座標を抽出し、抽出した座標を疑似画像座標として算出する。

次に、二次元地図生成・位置姿勢推定部３４は、計測データから計測センサ１１、１２の現在位置を示す現在位置座標を算出する（Ｓ１０１１）。

次に、二次元地図生成・位置姿勢推定部３４は、Ｓ１００４にて算出された物体座標と、Ｓ１００５にて算出された計測済空間座標と、Ｓ１００８にて算出した未計測空間座標と、Ｓ１００９にて算出した境界座標と、Ｓ１０１０にて算出した疑似画像座標と、Ｓ１０１１にて算出した現在位置座標とに基づき、図１１（ｂ）に示される未計測領域表示画面を生成し、生成した未計測領域表示画面を表示部１５に表示させる（Ｓ１０１２）。例えば、二次元地図生成・位置姿勢推定部３４は、Ｓ１００４にて算出した物体座標に基づいて、側壁面１５０の画像を生成し、Ｓ１００５にて算出した計測済空間座標に基づいて計測領域１２０を第１の態様で表示する画像を生成し、Ｓ１００８にて算出した未計測空間座標に基づいて未計測領域１１０を第二態様で表示する画像を生成し、Ｓ１００９にて算出した境界座標に基づいて境界１３０の画像を生成し、Ｓ１０１０にて算出した疑似画像表示座標に表示される疑似画像１４０を生成し、Ｓ１０１１にて算出した現在位置座標に表示される測定用台車画像１６０を生成し、生成した各画像を含む未計測領域表示画面を表示部１５に表示させる。なお、二次元地図生成・位置姿勢推定部３４は、レーザパルスを未計測領域１１０に通過させるための計測センサ１１，１２の姿勢を示す（例えば、測定用台車２０が境界１３０に接近または、測定用台車２０が境界１３０と並行して走行していることを示す）疑似画像を生成する。また、二次元地図生成・位置姿勢推定部３４は、未計測領域１１０については、なんら画像を生成しないようにしても良い。

そして、図１１（ｂ）に示されるように、表示部１５は、計測センサ１１、１２から照射されるレーザパルスが通過した計測領域１２０と、レーザパルスが通過していない未計測領域１１０との境界１３０を表示する。また、表示部１５は、計測領域１２０と未計測領域１１０とを異なる態様で表示する。また、表示部１５は、境界１３０の中心点と直行する直線上に所定間隔ごとに疑似画像１４０を表示するとともに、計測センサの現在位置を示す測定用台車画像１６０を表示する。
＜実施の形態２の効果＞

以上説明した実施の形態２によれば、実施の形態１と異なる効果として、計測センサ１１，１２から照射されるレーザが通過した計測領域１２０とレーザが通過していない未計測領域１１０との境界を表示することで、未計測領域１１０があることをユーザに認識させることができ、漏れのない地図を生成可能になる。

また、計測センサ１１，１２の現在位置と、レーザを未計測領域１１０に通過させるための計測センサ１１，１２の位置や姿勢とを表示することで、どのように測定用台車２０を移動させれば未計測領域１１０を計測できるのかをユーザに示すことが可能になる。そして、未計測領域１１０の計測が容易になる。

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

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

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

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

Claims (7)

1. 周囲にレーザを照射し、前記レーザを照射した角度と前記レーザが当たった点までの距離とを含む計測データを収集する計測センサと、
   前記計測センサを移動させて取得した前記計測データから地図を生成する地図生成部と、
   表示部と、
   を有し、
   前記地図生成部は、前記計測センサが移動される速度と加速度と角速度と角加速度との少なくともいずれか一つに基づいて、前記計測データを補間し、または前記計測センサが移動される速度を制御し、
   前記表示部は、速度と加速度と角速度と角加速度との少なくともいずれか一つが閾値を超えている場合に、前記計測センサにより取得されている前記計測データの精度が低下していることを警告する警告画面を表示する、地図生成装置。
2. 請求項１に記載の地図生成装置において、
   前記地図生成部は、速度と加速度と角速度と角加速度との少なくともいずれか一つが閾値を超えている際に取得された前記計測データを破棄する、地図生成装置。
3. 請求項１に記載の地図生成装置において、
   前記表示部は、前記計測センサから照射されるレーザが通過した計測領域と前記レーザが通過していない未計測領域との境界を表示する、地図生成装置。
4. 請求項１に記載の地図生成装置において、
   前記表示部は、前記計測センサから照射されるレーザが通過した計測領域と、前記レーザが通過していない未計測領域とを異なる態様で表示する、地図生成装置。
5. 請求項３または請求項４に記載の地図生成装置において、
   前記表示部は、前記計測センサの現在位置と、前記レーザを前記未計測領域に通過させるための前記計測センサの位置とを表示する、地図生成装置。
6. 請求項５に記載の地図生成装置において、
   前記表示部は、前記レーザを前記未計測領域に通過させるための前記計測センサの姿勢を表示する、地図生成装置。
7. 周囲にレーザを照射し、前記レーザを照射した角度と前記レーザが当たった点までの距離とを含む計測データを収集する計測センサと、前記計測センサを移動させて取得した前記計測データから地図を生成する地図生成部と、表示部とを有する地図生成装置における地図生成方法であって、
   前記地図生成部が、前記計測センサが移動される速度と加速度と角速度と角加速度との少なくともいずれか一つに基づいて、前記計測データを補間し、または前記計測センサが移動される速度を制御する制御ステップと、
   前記表示部が、速度と加速度と角速度と角加速度との少なくともいずれか一つが閾値を超えている場合に、前記計測センサにより取得されている前記計測データの精度が低下していることを警告する警告画面を表示する表示ステップと、を有する、地図生成方法。

Images