JP6974189B2 - 地図作成装置 - Google Patents

Description

本明細書に開示する技術は、対象領域の環境地図を作成する地図作成装置に関する。

例えば、移動体に対象領域内を自律的に移動させる場合、対象領域内の障害物（例えば、壁等）を記憶する環境地図が必要となる。対象領域内の環境地図を作成する技術としては、例えば、Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ）技術が知られている。Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術では、通常、レーザレンジファインダ（ＬＲＦ）等のセンサを搭載した移動体を対象領域内で移動させ、移動経路上の複数の計測点においてセンサにより環境情報を取得する。そして、地図作成装置を用いて、複数の計測点で取得した環境情報に基づいて環境地図を作成する。すなわち、地図作成装置には、複数の計測点のそれぞれについて、センサによって計測された環境情報と、各計測点における計測時のセンサの位置を規定するセンサ位置情報が入力される。次に、入力された環境情報とセンサ位置情報に基づいて、地図作成装置は、計測点のセンサ位置を規定するノードと、関連する２つのノード間の相対的位置関係を規定するエッジから構成されるグラフ構造データを生成する。地図作成装置は、複数のエッジが表す想定誤差を最小化することによってグラフ構造データを最適化する。Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術を用いることによって、地図作成装置は、誤差の小さい環境地図を作成することができる。

ＬＲＦ等のセンサによって取得した環境情報に基づいて環境地図を作成する場合、センサの計測誤差等によって、作成された環境地図に誤差が生じる。このため、作成された環境地図を、真値情報を用いて修正する技術が知られている。例えば、特許文献１には、ＧＰＳ測位機能によって取得した真値情報を用いて地図の精度を向上させる技術が開示されている。特許文献１の技術では、まず、ＧＰＳ測位機能によって基準点（真値情報）を取得する。次に、地図作成装置は、３点の基準点によって囲まれた三角形に基づいて、元の地図において基準点に対応する３点によって囲まれる三角形を補正する。この補正は、３点の基準点によって囲まれた三角形ごとに行われる。３点の基準点によって囲まれた三角形ごとに元の地図を補正することによって、元の地図は部分ごとに精度の高い地図に変換される。

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

精度の高い環境地図を作成するためには、例えば、対象領域内に配置される計測物を測量機器で計測し、その計測データに基づいて地図を作成する方法がある。しかしながら、対象領域内に配置される多数の計測物を全て測量機器で計測することは、時間や手間がかかるという問題があった。

Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術では、対象領域内の計測物を測量機器で計測していないため、簡易に環境地図を作成できる。しかしながら、センサによって複数の計測点で計測物を計測するため、複数の計測点ごとに計測誤差が生じる。このため、Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術を用いた地図作成装置は、グラフ構造データの最適化によって比較的誤差の小さい環境地図を作成することができるものの、測量機器で計測したデータに基づいて作成した地図と比較すると作成された環境地図に歪みが生じるという問題があった。

また、特許文献１の技術では、ＧＰＳ測位機能によって取得した３点の基準点に基づいて元の地図を補正しているため、補正した部分については地図の精度を高くすることができる。しかしながら、３点の基準点によって囲まれた三角形ごとに元の地図を補正するため、部分ごとの地図の精度については高くすることができる一方、地図全体としては歪みが生じ易いという問題があった。また、特許文献１の技術では、補正のためにＧＰＳ測位機能を用いているため、倉庫等の屋内環境においては地図の補正ができない場合がある。

本明細書は、Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術を用いながら、比較的に精度の高い地図を作成する技術を開示する。

本明細書に開示する地図作成装置は、対象領域の環境地図を作成する。地図作成装置は、対象領域内の複数の計測点のそれぞれについて、センサによって計測された環境情報を記憶する環境情報記憶部と、複数の計測点のそれぞれについて、センサの移動量に基づいて算出される計測時のセンサの推定位置に関するセンサ推定位置情報を記憶するセンサ推定位置情報記憶部と、環境情報及びセンサ推定位置情報に基づいて、複数の計測点のそれぞれにおけるセンサの位置を規定するセンサノードと、隣接するセンサノード間におけるセンサの相対的位置関係を規定するセンサエッジとを備える第１のグラフ構造データを生成する第１グラフ構造データ生成部と、対象領域内の複数の設置点のそれぞれについて、センサ位置計測装置が設置される設置位置に関する設置位置情報を記憶する設置位置情報記憶部と、複数の計測点の中から選択された複数の計測点において、センサ位置計測装置を用いて計測されるセンサの計測位置に関するセンサ計測位置情報を記憶するセンサ計測位置情報記憶部と、センサ計測位置情報及び設置位置情報に基づいて、複数の設置点のそれぞれにおけるセンサ位置計測装置の設置位置を規定する計測装置ノードと、複数の設置点間におけるセンサ位置計測装置の相対的位置関係を規定する計測装置エッジと、複数の設置点のそれぞれにおいてセンサ位置計測装置で計測されたセンサの位置を規定するセンサ計測ノードと、センサ計測ノードとそのセンサ計測ノードに対応する計測装置ノードの相対的位置関係を規定するセンサ計測エッジとを備える第２のグラフ構造データを生成する第２グラフ構造データ生成部と、計測装置エッジ及びセンサ計測エッジから算出される誤差関数の和が最小となるように第２のグラフ構造データを最適化することにより、センサの真位置を算出するセンサ真位置算出部と、センサ真位置算出部により算出されたセンサの真位置と、そのセンサ真位置に対応するセンサノードに基づいて、算出されたセンサの真位置が規定するセンサの位置と対応するセンサノードが規定するセンサの位置とのずれを規定するセンサ位置エッジを第１のグラフ構造データに追加するセンサ位置エッジ追加部と、センサ位置エッジが追加された第１のグラフ構造データについて、各エッジから算出される誤差関数の和が最小となるように第１のグラフ構造データを最適化するグラフ構造データ最適化部と、を備えている。

上記の地図作成装置では、センサ真位置が規定するセンサの位置とセンサ推定位置情報に基づくセンサの位置とのずれを規定するセンサ位置エッジを第１のグラフ構造データに追加することによって、第１のグラフ構造データに真値又は真値に近似する情報（真値情報）が取込まれる。そして、真値情報が取込まれたグラフ構造データに含まれる誤差情報から算出される誤差関数の和が最小となるようにグラフ構造データを最適化する。また、センサ計測位置情報及びセンサ位置計測装置の設置位置情報に基づいて、センサ位置計測装置によって計測されたセンサの計測位置を最適化するため、第１のグラフ構造データに取り込む真値情報の精度をより高くすることができる。これにより、従来のＧｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術を用いて作成した地図と比較して地図の精度を向上できる。また、センサ位置エッジを導入したグラフ構造データを全体として最適化するため、真値情報を用いて地図を部分的に補正する場合と比較して地図全体の精度を高めることができる。

実施例に係る地図作成装置のシステム構成を示す図。 地図作成処理を示すフローチャート。 移動体に搭載されるセンサが対象領域内の計測物を計測する状態を模式的に示す図。 測量器が対象領域内に設置された基準点の位置を計測する状態を模式的に示す図。 測量器が移動体に搭載されるセンサの位置を計測する状態を模式的に示す図。 設置位置を変更された測量器が、対象領域内に設置された基準点の位置及び移動体に搭載されるセンサの位置を計測する状態を模式的に示す図。 センサノード及びセンサエッジを模式的に示す図。 測量器ノード、測量器エッジ、センサ計測ノード及びセンサ計測エッジにより構成される第２のグラフ構造データを模式的に示す図。 センサ計測ノード及びセンサ位置エッジが追加された第１のグラフ構造データを模式的に示す図。 ループクローズエッジが追加された第１のグラフ構造データを模式的に示す図。 センサが倉庫内の環境情報を計測する状態の一例を模式的に示す図。

以下に説明する実施例の主要な特徴を列記しておく。なお、以下に記載する技術要素は、それぞれ独立した技術要素であって、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時の請求項に記載の組合せに限定されるものではない。

（特徴１）本明細書に開示する地図作成装置は、最適化された第１のグラフ構造データを構成する複数のセンサノードから選択された少なくとも２つのセンサノード間におけるセンサの相対的位置関係を規定するループクローズエッジを最適化された第１のグラフ構造データに追加し、各エッジから算出される誤差関数の和が最小となるように第１のグラフ構造データを最適化するループクローズ部をさらに備えていてもよい。このような構成によると、最適化された第１のグラフ構造データに、センサノード間におけるセンサの相対的位置関係を規定するループクローズエッジを追加することによって、ループクローズされるセンサ間の相対的な姿勢のずれを第１のグラフ構造データに取り込むことができる。第１のグラフ構造データがさらに最適化されるため、さらに地図全体の精度を高くすることができる。

（特徴２）本明細書に開示する地図作成装置は、最適化された第１のグラフ構造データ内のセンサノードと、センサノードが規定するセンサの位置から計測された環境情報を用いて環境地図を作成する環境地図作成部をさらに備えていてもよい。このような構成によると、真値情報が取込まれ、最適化された第１のグラフ構造データ内のセンサノードが規定するセンサの位置から計測された環境情報に基づいて、精度の高い地図を作成することができる。

（特徴３）本明細書に開示する地図作成装置では、センサ計測位置情報は、センサ位置計測装置によって、複数の計測点の中から選択された２以上の計測点において計測されたセンサの位置に関する情報であってもよい。２以上の計測点間の距離は、センサによって環境情報を計測可能な距離より短くてもよい。このような構成によると、センサで計測可能な距離より短い間隔で、センサの位置の真値情報が第１のグラフ構造データに追加される。このため、地図全体の精度を好適に高くすることができる。

以下、実施例に係る地図作成装置１０について説明する。地図作成装置１０は、対象領域の環境地図を作成する。地図作成装置１０は、例えば、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータによって構成することができる。コンピュータがプログラムを実行することで、地図作成装置１０は、図１に示すグラフ構造データ生成部１４、センサ真位置算出部１８、センサ位置エッジ追加部２０、グラフ構造データ最適化部２２、ループクローズ部２４、環境地図作成部２６等として機能する。地図作成装置１０の各部１４、１８、２０、２２、２４、２６の処理については、後で詳述する。

また、図１に示すように、地図作成装置１０は、環境計測データ記憶部１２と、測量データ記憶部１６を備えている。環境計測データ記憶部１２は、センサ５２によって計測された対象領域内の環境情報及びセンサ推定位置情報を記憶する。後述するようにセンサ５２は移動体５０に搭載される。移動体５０は対象領域内を移動しながら、センサ５２によって対象領域内の環境（例えば、壁等の計測物）を計測する。センサ５２によって取得された環境情報は、その環境情報が取得されたときのセンサ５２の位置（センサ５２の推定位置ともいう）と共に環境計測データ記憶部１２に記憶される。測量データ記憶部１６は、測量器５６によって計測されたセンサ５２の位置（センサ５２の計測位置ともいう）に関するセンサ計測位置情報及び測量器５６の設置位置に関する設置位置情報を記憶する。環境計測データ記憶部１２に記憶される環境情報及びセンサ推定位置情報と、測量データ記憶部１６に記憶されるセンサ計測位置情報及び設置位置情報については、後で詳述する。

図２〜図１０を参照して、地図作成装置１０が行う処理について説明する。図２は、地図作成装置１０が行う地図作成処理を示すフローチャートである。図２に示すように、まず、地図作成装置１０は、移動体５０から対象領域内の複数の計測地点Ａ〜Ｊ（図３参照）において計測された環境情報及びセンサ推定位置情報を取得し、環境計測データ記憶部１２に記憶する（Ｓ１０）。ここで、移動体５０によって環境情報を計測する手順について説明する。

移動体５０は、車輪駆動型の移動体である。移動体５０は、図示しない操作装置を備えており、例えば、ジョイスティック等のリモコンで操縦される。なお、本実施例では、移動体５０はリモコンで操縦されるが、このような構成に限定されない。例えば、移動体５０は、作業者がハンドル操作によって運転することによって移動してもよいし、作業者が手で押すことによって移動してもよい。また、移動体５０は飛翔体であってもよい。

移動体５０は、センサ５２と記憶部５４を備えている（図１参照）。本実施例では、移動体５０が搭載するセンサ５２は、レーザレンジファインダ（ＬＲＦ）である。ＬＲＦは、レーザ光を射出し、射出したレーザ光が物体で反射して返ってくるまでの時間を計測する。ＬＲＦで計測された時間から、ＬＲＦから物体までの距離が計測される。また、ＬＲＦからレーザ光を射出した方向（すなわち、物体から反射されるレーザ光の入射角度）は既知であるため、ＬＲＦに対する物体の方位を決定することができる。ＬＲＦは、計測地点から見た壁や柱等（以下、「計測物６２」という）の位置、姿勢及び形状等の環境情報を計測することができる。ＬＲＦで計測された環境情報は記憶部５４に記憶される。また、センサ５２には、図示しない測量用の反射板が設置されている。測量用の反射板は、測量器５６によってセンサ５２の位置を計測するために用いられる。測量器５６によるセンサ５２の位置の計測については、後で詳述する。

各計測地点におけるセンサ５２の姿勢及び位置は、各計測地点においてセンサ５２によって計測された環境情報を用いて算出される。詳細には、１つの計測地点においてセンサ５２が計測した環境情報と、次の計測地点においてセンサ５２が計測した環境情報をマッチングさせることによって、両者の間の位置及び姿勢の変位を算出する。ここで算出された変位が、１つの計測地点から次の計測地点までのセンサ５２の移動量といえる。マッチングの方法としては、パターンマッチングやＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）等の公知の方法を用いることができる。このように、各計測地点の間のセンサ５２の移動量を算出することによって、各計測地点におけるセンサ５２の姿勢及び位置を算出することができる。また、移動体５０は、車輪の回転角を検出するセンサ（例えば、エンコーダ）を備えていてもよい。このセンサによって車輪の回転角を検出することで、移動体５０の移動方向及び移動量を算出することができる。このため、移動体５０は、算出された移動体５０の移動方向及び移動量に基づいて、各計測地点における移動体５０の姿勢及び位置を算出することができる。移動体５０へのセンサ（ＬＲＦ）５２の設置状態は既知であるため、算出された各計測地点における移動体５０の姿勢及び位置に基づいて、各計測地点におけるセンサ５２の姿勢及び位置を算出してもよい。さらに、センサ５２が計測した環境情報をマッチングさせる方法と、車輪の回転角を検出するセンサによって算出する方法を組み合わせてもよい。

記憶部５４には、各計測点においてセンサ５２で計測された環境情報に対応付けて、その計測点におけるセンサ５２の姿勢及び位置が記憶される。本実施例では、センサ５２が計測する環境情報及び算出されるセンサ推定位置情報を合わせて「環境計測データ」と称することがある。なお、センサ５２はＬＲＦに限定されるものではない。センサ５２は、環境情報を計測できればよく、例えば、センサ５２は画像情報によって計測物６２を計測するものであってもよい。センサ５２によって計測された計測物６２の環境情報は、移動体５０の記憶部５４に時系列で記憶される。また、センサ５２の位置及び姿勢も上記の方法（いわゆる、オドメトリ）に限られず、例えば、ＧＰＳセンサ等を用いて検出するようにしてもよい。

図３は、移動体５０に搭載されるセンサ５２が、対象領域内の計測物６２を計測する状態を模式的に示している。移動体５０は、対象領域内に設定される移動経路６０上を移動する。詳細には、移動体５０は、地点Ａを始点として移動経路６０上を移動し、地点Ａの近傍の地点Ｊまで（すなわち、地点Ｊを終点として）移動する。対象領域内には、壁や柱等（すなわち、計測物６２）が配置されており、センサ５２は、計測地点から計測物６２までの距離及び計測物６２の形状等を計測する。図３に示される矢印は、移動体５０が計測地点Ｂに移動したときにセンサ５２から計測物６２ａに射出されるレーザと、移動体５０が計測地点Ｄに移動したときにセンサ５２から計測物６２ｂ、６２ｃに射出されるレーザを示している。なお、以下の明細書において、計測物６２を区別する必要があるときは計測物６２ａ、６２ｂ、６２ｃのように沿字のアルファベットを用いて記載し、区別する必要のないときは単に計測物６２と記載する場合がある。また、他の構成要素についても同一構成のものについて区別する必要がないときは、上記と同様に沿字のアルファベットを省略して単に数字で記載することがある。

図３に示すように、例えば、センサ５２は、地点Ｂにおいて計測可能な計測物６２ａ及び計測物６２ｄの環境情報を計測し、また、移動体５０（詳細には、移動体５０に搭載された演算装置）は地点Ｂにおける計測時のセンサ推定位置情報を算出する。計測された環境情報と算出されたセンサ推定位置情報は記憶部５４に記憶される。また、移動体５０が地点Ｅに移動すると、センサ５２は、地点Ｅにおいて計測可能な計測物６２ｂの環境情報、計測物６２ｃ及び計測物６２ｄの環境情報を計測し、また、地点Ｅにおける計測時のセンサ推定位置情報が算出される。このようにして、移動体５０は、移動経路６０上を移動し、センサ５２は、対象領域内の計測物６２を計測し、その計測物６２を計測した地点のセンサ推定位置情報が算出される。計測された環境情報と算出されたセンサ推定位置情報は、時系列順に記憶部５４に蓄積される。記憶部５４に蓄積された環境情報とセンサ推定位置情報は、地図作成装置１０に入力され、環境計測データ記憶部１２に記憶される。なお、環境計測データ記憶部１２は、「環境情報記憶部」及び「センサ推定位置情報記憶部」の一例である。

次に、地図作成装置１０は、測量器５６で計測されたセンサ５２の位置に関する情報（すなわち、対象領域内の複数の計測地点Ａ〜Ｊの中から選択された地点において測量器５６を用いて計測されたセンサ５２の位置に関する情報（以下、センサ計測位置情報ともいう））と、測量器５６の設置位置に関する情報（以下、設置位置情報ともいう）を取得し、測量データ記憶部１６に記憶する（Ｓ１２）。ここで、測量器５６によってセンサ計測位置情報を計測する手順について説明する。

センサ計測位置情報は、基本測量や公共測量に用いられるトータルステーション等の測量器５６によって計測される。測量器５６は、図示しないレーザセンサを備えており、反射板までの距離及び向きを計測することによって、反射板が設置される位置を精度良く測量する。本実施例では、測量器５６は、対象領域内に設置された複数の反射板（基準用反射板ともいう）の位置と、上述のセンサ５２に設置された反射板の位置を計測する。計測された基準用反射板の位置に基づいて、測量器５６で計測する際の基準点（例えば、座標の原点）と座標軸の軸方向を決定することができる。なお、基準用反射板に代わり、例えば、プリズムを設置して、その位置を計測してもよいし、対象領域内に存在する特徴的な模様等の位置を計測してもよい。また、測量器５６は、決定された基準点と座標軸の軸方向に基づいて、測量器５６の設置位置と、測量器５６によって計測されたセンサ５２の位置（すなわち、センサ５２の計測位置）を算出することができる。また、測量器５６は、記憶部５８を備えている。記憶部５８には、測量器５６の設置位置に対応付けて、その設置位置に設置された測量器５６から計測されたセンサ５２の位置が記憶される。本実施例では、測量器５６の設置位置情報及びセンサ計測位置情報を合わせて、「測量データ」と称することがある。なお、本実施例では、測量データは、測量器５６を用いて計測しているが、このような構成に限定されない。センサ５２の位置を計測する精度が高い方法であればよく、例えば、ＡＲマーカーやＱＲコード（登録商標）等をカメラで計測してもよいし、ＧＰＳ測量や航空測量を用いてもよい。また、メジャーやコンベックス等を用いて測定してもよいし、ＣＡＤ図等の座標値を示す図面を用いてもよい。また、測量器５６は、「センサ位置計測装置」の一例である。

図４〜図６は、測量器５６が対象領域内に設置された基準用反射板の位置とセンサ５２に設置された反射板の位置を計測する状態を模式的に示している。本実施例では、測量器５６を用いてセンサ５２の位置を計測する地点として、センサ５２が環境情報を計測する１０箇所の計測地点Ａ〜Ｊの中から５箇所の地点Ａ、Ｃ、Ｅ、Ｇ、Ｊを選択している。測量器５６が計測するセンサ５２の位置の数は特に限定されないが、測量器５６で計測するセンサ５２間の距離が、センサ５２によって環境情報を計測可能な距離より短くすることが好ましい。測量器５６で計測するセンサ５２間の距離をセンサ５２によって環境情報を計測可能な距離より短くすることによって、地図全体の精度を好適に高くできる。一方、測量器５６で計測するセンサ５２間の距離をセンサ５２によって環境情報を計測可能な距離より長くすると、測量器５６で計測する作業が増大することを抑制できる。したがって、測量器５６で計測するセンサ５２間の距離は、対象領域の環境によって作業者が適宜決定することができる。以下に、測量器５６を用いてセンサ５２の位置を計測する手順について説明する。

まず、センサ５２によって環境情報を取得する前（すなわち、移動体５０が移動経路６０上を移動する前）に、測量器５６を用いて基準用反射板の位置を計測する。例えば、図４に示すように、対象領域内の地点Ｋに測量器５６を設置し、対象領域内の地点Ｌ及び地点Ｍに基準用反射板を設置する。測量器５６は、見通しの良い位置に配置することが好ましく、センサ５２の位置を計測する５箇所の地点Ａ、Ｃ、Ｅ、Ｇ、Ｊのうち、できるだけ多くの地点を計測可能な位置に設置するとよい。また、基準用反射板は、地点Ｋに設置された測量器５６によって計測可能な位置に配置する。

次いで、測量器５６は、地点Ｌ及び地点Ｍに設置された基準用反射板の位置を計測する。測量器５６（詳細には、測量器５６に搭載された演算装置）は、計測された基準用反射板の位置に基づいて、原点と座標軸の軸方向を決定する。測量器５６は、決定した原点と座標軸の軸方向に基づいて、測量器５６の設置地点Ｋの位置（例えば、地点Ｋの座標）及び姿勢を算出する。決定された原点及び座標軸の軸方向と、算出された測量器５６の設置地点Ｋの位置は、記憶部５８に記憶される。

次いで、測量器５６は、センサ５２の位置を計測する。測量器５６は、移動体５０に搭載されたセンサ５２が複数の計測地点Ａ〜Ｊにおける環境情報を計測しながら移動経路６０上を移動する際に、複数の計測地点Ａ〜Ｊの中から選択した５箇所の地点Ａ、Ｃ、Ｅ、Ｇ、Ｊにおけるセンサ５２の位置をそれぞれ計測する。測量器５６によってセンサ５２の位置を計測する際には、移動経路６０上を移動する移動体５０を停止させ、移動体５０（すなわち、センサ５２）を停止させた状態で測量器５６はセンサ５２の位置を計測する。

図５は、地点Ｋに設置された測量器５６が地点Ａ、Ｃ、Ｅにおけるセンサ５２の位置を計測する状態を模式的に示している。まず、移動体５０が移動経路６０の始点である地点Ａに位置する状態で、移動体５０を停止させる。そして、移動体５０を停止させた状態で、地点Ａにおけるセンサ５２の位置を地点Ｋに設置された測量器５６で計測する。図５に示すように、地点Ｋに設置された測量器５６と地点Ａに位置するセンサ５２の間には計測物６２等の測量器５６による計測を妨害するものが配置されていないため、測量器５６は地点Ａに位置するセンサ５２の位置を計測することができる。計測された地点Ａにおけるセンサ５２の位置は、記憶部５８に記憶される。なお、移動体５０が地点Ａに位置する間に、移動体５０に搭載されるセンサ５２は、地点Ａから計測物６２（すなわち、環境情報）を計測する。地点Ａにおけるセンサ５２の位置の計測が終了すると、移動体５０を移動させる。移動体５０が地点Ａから地点Ｂを介して地点Ｃまで移動する間、センサ５２は、地点Ｂ及び地点Ｃから計測物６２を計測する。以下の説明では、センサ５２による計測物６２の計測についての説明は省略するが、移動体５０が移動経路６０を移動する間、上述したように、センサ５２は地点Ａ〜Ｊにおいて計測物６２を計測する。このように、移動体５０が移動経路６０上を移動する間、測量器５６によるセンサ５２の位置の計測と、センサ５２による計測物６２の計測を同時に行う。

移動体５０が地点Ｃに位置すると、移動体５０を停止させる。そして、移動体５０を停止させた状態で、地点Ｃにおけるセンサ５２の位置を地点Ｋに設置された測量器５６で計測する。地点Ｋに設置された測量器５６と地点Ｃに位置するセンサ５２の間には計測物６２等が配置されていないため、測量器５６は地点Ｃに位置するセンサ５２の位置を計測することができる。計測された地点Ｃにおけるセンサ５２の位置は、記憶部５８に記憶される。地点Ｃにおけるセンサ５２の位置の計測が終了すると、移動体５０を移動させる。同様に、移動体５０を次の計測地点である地点Ｅまで移動させ、地点Ｋに設置された測量器５６を用いて地点Ｅにおけるセンサ５２の位置を計測する。地点Ｋに設置された測量器５６と地点Ｅに位置するセンサ５２の間には計測物６２等が配置されていないため、測量器５６は地点Ｅに位置するセンサ５２の位置を計測することができる。計測された地点Ｅにおけるセンサ５２の位置は、記憶部５８に記憶される。

地点Ｅにおけるセンサ５２の計測が終了すると、移動体５０を次の計測地点である地点Ｇまで移動させ、移動体５０を停止させる。しかしながら、地点Ｋに設置された測量器５６と地点Ｇに位置するセンサ５２の間には計測物６２ｄが配置されているため、地点Ｋに設置された測量器５６は地点Ｇに位置するセンサ５２の位置を計測することができない。このため、地点Ｇに位置するセンサ５２の位置を計測できるように、測量器５６の設置位置を変更する。例えば、図６に示すように、測量器５６を地点Ｋから地点Ｎに移動させる。

測量器５６を地点Ｎに設置すると、再び測量器５６を用いて基準用反射板の位置を計測する。このとき、先に設置された基準用反射板の位置（すなわち、地点Ｌと地点Ｍに設置された基準用反射板の位置）を地点Ｎから計測できない場合には、地点Ｎから計測できる位置に基準用反射板をさらに設置する。測量器５６は、計測された基準用反射板の位置に基づいて、原点と座標軸の軸方向を決定し、測量器５６の設置地点Ｎの位置を算出する。決定された原点及び座標軸の軸方向と、算出された測量器５６の設置地点Ｎの位置は、記憶部５８に記憶される。

次いで、地点Ｎに設置された測量器５６を用いて、地点Ｇにおけるセンサ５２の位置を計測する。地点Ｎに設置された測量器５６と地点Ｇに位置するセンサ５２の間には計測物６２等が配置されていないため、測量器５６は地点Ｇに位置するセンサ５２の位置を計測することができる。そして、地点Ｇにおけるセンサ５２の計測が終了すると、次の計測地点である地点Ｊまで移動体５０を移動させ、地点Ｎに設置された測量器５６を用いて、地点Ｊに位置するセンサ５２の位置を計測する。計測された地点Ｇ及び地点Ｊにおけるセンサ５２の位置は、記憶部５８に記憶される。このようにして、測量器５６を用いて、複数の計測地点Ａ〜Ｊの中から選択した５箇所の地点Ａ、Ｃ、Ｅ、Ｇ、Ｊにおけるセンサ５２の位置をそれぞれ計測する。センサ５２の計測位置は、測量器５６によって計測されるため、記憶部５８には、センサ５２の位置に関する真値又は真値に近似する情報（真値情報）が記憶される。記憶部５８に蓄積された測量器５６の設置位置情報とセンサ計測位置情報は、地図作成装置１０に入力され、測量データ記憶部１６に記憶される。なお、測量データ記憶部１６は、「センサ計測位置情報記憶部」及び「設置位置情報記憶部」の一例である。

なお、本実施例では、測量器５６の設置位置を１回変更し（すなわち、設置位置を地点Ｋから地点Ｎに変更し）、５箇所の地点Ａ、Ｃ、Ｅ、Ｇ、Ｊにおけるセンサ５２の位置を計測したが、このような構成に限定されない。例えば、１つの設置位置から５箇所の計測地点おけるセンサ５２の位置の全てを計測可能であれば、測量器５６を移動させることなく、その１つの設置位置から５箇所の計測地点におけるセンサ５２の位置の全てを計測してもよい。また、例えば、壁等の計測物６２が多く配置されている場合や対象領域が広域である場合には、測量器５６の設置位置を２回以上変更してもよい。また、本実施例では、１台の測量器５６で５箇所の計測地点におけるセンサ５２の位置の全てを計測しているが、このような構成に限定されない。例えば、２台以上の測量器５６を用いてセンサ５２の位置を計測してもよい。また、本実施例では、測量器５６は２箇所の地点Ｋ、Ｍに設置した基準用反射板の位置を計測しているが、このような構成に限定されない。測量器５６で計測する基準用反射板の位置や数は上記の例に限定されるものではなく、３箇所以上の地点に設置した基準用反射板の位置を計測してもよい。

次に、地図作成装置１０では、ステップＳ１０で環境計測データ記憶部１２に記憶された環境情報及びセンサ推定位置情報に基づいて、グラフ構造データ生成部１４が対象領域内のグラフ構造データ２８（以下、第１のグラフ構造データ２８ともいう）を生成する（Ｓ１４）。第１のグラフ構造データ２８は、センサノード３０及びセンサエッジ３２によって構成される。グラフ構造データ生成部１４は、環境情報及びセンサ推定位置情報に基づいてセンサノード３０及びセンサエッジ３２を生成し、生成したセンサノード３０がセンサエッジ３２により関連付けられた第１のグラフ構造データ２８を生成する。

すなわち、図７に示すように、グラフ構造データ生成部１４は、まず、環境計測データ記憶部１２が記憶するセンサ推定位置情報に基づいて、各計測地点Ａ〜Ｊにおける計測時のセンサ５２の位置や姿勢を規定するセンサノード３０ａ〜３０ｊをそれぞれ生成する。なお、隣接する計測地点の間隔は、対象領域内に設置されている複数の計測物６２の環境情報を計測できる間隔であればよい。

次に、グラフ構造データ生成部１４は、環境計測データ記憶部１２に記憶される時系列に蓄積されたセンサ推定位置情報及び環境情報に基づいて、隣接するセンサノード３０間におけるセンサ５２の相対的位置関係を規定するセンサエッジ３２を生成する。センサエッジ３２は、センサエッジ３２の両端にある２つのセンサノード３０の各姿勢についての制約を表しており、具体的には、上述したように、各計測地点においてセンサ５２が計測した環境情報をマッチングさせることによって算出することができる。

例えば、グラフ構造データ生成部１４は、環境計測データ記憶部１２に記憶される時系列に蓄積された計測地点Ａ、Ｂの環境情報及びセンサ推定位置情報から、計測地点Ａ、Ｂ間のセンサ５２の移動量（すなわち、移動体５０が地点Ａ、Ｂ間を移動したことによって移動したセンサ５２の移動量）を算出する。この移動量と計測地点Ａ、Ｂのセンサ推定位置情報から、グラフ構造データ生成部１４は、センサノード３０ａとセンサノード３０ｂとを連結するセンサエッジ３２ａを生成する。センサエッジ３２ａは、センサノード３０ａ、３０ｂ間におけるセンサ５２の相対的位置関係を規定する。すなわち、センサエッジ３２ａは、センサ５２が計測地点Ａから計測地点Ｂまで移動したという事象を表している。同様にして、グラフ構造データ生成部１４は、時系列に蓄積された計測地点Ｂ、Ｃの環境情報及びセンサ推定位置情報から、センサノード３０ｂとセンサノード３０ｃとを連結するセンサエッジ３２ｂを生成する。このようにして、グラフ構造データ生成部１４は、全ての隣接するセンサノード３０間に、センサエッジ３２を生成する。そして、グラフ構造データ生成部１４は、生成したセンサノード３０及びセンサエッジ３２から第１のグラフ構造データ２８を生成する。なお、グラフ構造データ生成部１４は、「第１グラフ構造データ生成部」の一例である。

次に、地図作成装置１０では、ステップＳ１２で測量データ記憶部１６に記憶された測量器５６の設置位置情報及びセンサ計測位置情報に基づいて、センサ真位置算出部１８が、測量器５６によって計測されたセンサ５２の位置についてセンサ真位置を算出する（Ｓ１６）。センサ真位置の算出は、以下の手順で実行される。

まず、センサ真位置算出部１８は、測量データ記憶部１６に記憶された測量器５６の設置位置情報及びセンサ計測位置情報に基づいて、グラフ構造データ３８（以下、第２のグラフ構造データ３８ともいう）を生成する。第２のグラフ構造データ３８は、測量器ノード４０、測量器エッジ４２、センサ計測ノード４４及びセンサ計測エッジ４６によって構成される。センサ真位置算出部１８は、測量器５６の設置位置情報及びセンサ計測位置情報に基づいて測量器ノード４０、測量器エッジ４２、センサ計測ノード４４及びセンサ計測エッジ４６を生成し、生成した測量器ノード４０及びセンサ計測ノード４４が測量器エッジ４２及びセンサ計測エッジ４６により関連付けられた第２のグラフ構造データ３８を生成する。なお、測量器ノード４０は、「計測装置ノード」の一例であり、測量器エッジ４２は、「計測装置エッジ」の一例である。

図８に示すように、センサ真位置算出部１８は、測量データ記憶部１６に記憶された測量器５６の設置位置情報に基づいて、測量器５６の設置位置を規定する測量器ノード４０ａ、４０ｂを生成する。測量器ノード４０ａは、地点Ｋに設置された測量器５６の位置を規定し、測量器ノード４０ｂは、地点Ｎに設置された測量器５６の位置を規定する。そして、センサ真位置算出部１８は、地点Ｋに設置された測量器５６から計測された基準用反射板の位置である地点Ｌ、Ｍの位置と、地点Ｎに設置された測量器５６から計測された基準用反射板の位置である地点Ｌ、Ｍの位置に基づいて、各地点Ｋ、Ｎに設置された測量器５６間の相対的位置や姿勢関係を規定する測量器エッジ４２を生成する。

次いで、センサ真位置算出部１８は、測量データ記憶部１６に記憶されたセンサ計測位置情報に基づいて、センサ５２の計測位置を規定するセンサ計測ノード４４ａ〜４４ｅを生成する。また、センサ真位置算出部１８は、測量データ記憶部１６に記憶された測量器５６の設置位置情報及びセンサ計測位置情報に基づいて、センサ計測ノード４４とそのセンサ計測ノード４４を規定するセンサ５２の位置を計測した地点の測量器ノード４０との相対的位置関係を規定するセンサ計測エッジ４６ａ〜４６ｅを生成する。

具体的には、センサ計測ノード４４ａは、地点Ｋに設置された測量器５６が計測した地点Ａにおけるセンサ５２の位置を規定する。また、センサ計測エッジ４６ａは、測量器ノード４０ａとセンサ計測ノード４４ａの間の相対位置を規定する。同様に、センサ計測ノード４４ｂ、４４ｃは、地点Ｋに設置された測量器５６が計測した地点Ｃ、Ｅにおけるセンサ５２の位置をそれぞれ規定し、センサ計測エッジ４６ｂ、４６ｃは、測量器ノード４０ａとセンサ計測ノード４４ｂ、４４ｃの間の相対位置をそれぞれ規定する。また、センサ計測ノード４４ｄ、４４ｅは、地点Ｎに設置された測量器５６が計測した地点Ｇ、Ｊにおけるセンサ５２の位置をそれぞれ規定し、センサ計測エッジ４６ｄ、４６ｅは、測量器ノード４０ｂとセンサ計測ノード４４ｄ、４４ｅの間の相対位置や姿勢関係をそれぞれ規定する。

さらに、センサ真位置算出部１８は、測量データ記憶部１６に記憶された測量器５６の設置位置情報及び基準用反射板の位置に関する情報に基づいて、基準用反射板の位置を規定するノードを生成すると共に、基準用反射板の位置を規定するノードとそのノードを規定する基準用反射板の位置を計測した地点の測量器ノード４０との相対的位置関係を規定するエッジを生成する。本実施例では、測量器５６の設置位置情報及び基準用反射板の位置に関する情報に基づいて生成されるノード及びエッジについても「センサ計測ノード４４」及び「センサ計測エッジ４６」と称する。具体的には、センサ計測ノード４４ｆ、４４ｇは、地点Ｋに設置された測量器５６が計測した地点Ｌ、Ｍに設置される基準用反射板の位置をそれぞれ規定し、センサ計測エッジ４６ｆ、４６ｇは、測量器ノード４０ａとセンサ計測ノード４４ｆ、４４ｇの間の相対位置や姿勢関係をそれぞれ規定する。同様に、センサ計測ノード４４ｈ、４４ｉは、地点Ｎに設置された測量器５６が計測した地点Ｌ、Ｍに設置される基準用反射板の位置をそれぞれ規定し、センサ計測エッジ４６ｈ、４６ｉは、測量器ノード４０ｂとセンサ計測ノード４４ｈ、４４ｉの間の相対位置や姿勢関係をそれぞれ規定する。センサ真位置算出部１８は、測量器５６の設置位置情報、センサ計測位置情報及び基準用反射板の位置に関する情報に基づいて生成した測量器ノード４０、測量器エッジ４２、センサ計測ノード４４及びセンサ計測エッジ４６から、第２のグラフ構造データ３８を生成する。

第２のグラフ構造データ３８を生成すると、センサ真位置算出部１８は、第２のグラフ構造データ３８を最適化する。具体的には、センサ真位置算出部１８は、第２のグラフ構造データ３８内の測量器エッジ４２及びセンサ計測エッジ４６に基づいて算出される誤差関数の和が最小となるように第２のグラフ構造データ３８を最適化する。誤差関数は、Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術で用いられている、公知の方法を用いて生成することができ、また、最適化方法についても、最急降下法や共役勾配法等の最小二乗法等の公知の方法を用いることができる。センサ真位置算出部１８は、第２のグラフ構造データ３８内の全てのエッジ（測量器エッジ４２及びセンサ計測エッジ４６）から誤差関数を算出し、これら算出された誤差関数を加算し、次いで、この誤差関数の和が最小となるように最適化する。

センサ真位置算出部１８は、最適化された第２のグラフ構造データ３８を構成するセンサ計測ノード４４により規定されるセンサ５２の計測位置を算出する。ここで算出されたセンサ５２の計測位置が、第１のグラフ構造データ２８においてセンサ５２の真位置として用いられる。センサ５２の位置（計測位置）は測量器５６によって計測されるため、真値に近い正確な位置を計測することができる。しかしながら、上述したように、本実施例では、測量器５６を用いてセンサ５２を計測する際に、測量器５６の設置位置を地点Ｋから地点Ｎに移動させている。このように測量器５６の設置位置を移動させると、測量器５６が計測するセンサ５２の計測位置に誤差が生じることがある。測量器５６の設置位置情報及びセンサ計測位置情報に基づいて生成した第２のグラフ構造データ３８を最適化することによって、センサ５２の計測位置を全体としてより真値に近づけることができる。このため、センサ５２の位置についての真値情報の精度をより高くすることができる。なお、センサ真位置算出部１８は、「第２グラフ構造データ生成部」及び「センサ真位置算出部」の一例である。

次に、地図作成装置１０は、センサ位置エッジ追加部２０によって、センサ位置エッジ４８を第１のグラフ構造データ２８内に追加する（Ｓ１８）。センサ位置エッジ４８は、ステップＳ１６で算出されたセンサ５２の真位置と、その位置に対応する第１のグラフ構造データ２８内のセンサ５２の位置とのずれを規定する。図９に示すように、センサ位置エッジ追加部２０は、まず、ステップＳ１６で最適化されたセンサ計測ノード４４（すなわち、算出されたセンサ５２の真位置）を、グラフ構造データ生成部１４によって生成された第１のグラフ構造データ２８に追加する。次いで、センサ位置エッジ追加部２０は、追加されたセンサ計測ノード４４と、そのセンサ計測ノード４４に対応する第１のグラフ構造データ２８内のセンサノード３０に基づいて、両位置のずれを規定するセンサ位置エッジ４８を生成する。センサ位置エッジ追加部２０は、生成したセンサ位置エッジ４８を第１のグラフ構造データ２８に追加する。

図９に示すように、地点Ａにおける計測時のセンサ５２の推定位置を規定するセンサノード３０ａと、地点Ａにおけるセンサ５２の真位置を規定するセンサ計測ノード４４ａは一致しないことがある。センサ位置エッジ４８ａは、センサノード３０ａとセンサ計測ノード４４ａのずれを規定している。同様にして、センサ位置エッジ４８ｂは、センサノード３０ｃとセンサ計測ノード４４ｂのずれを規定しており、センサ位置エッジ４８ｃは、センサノード３０ｅとセンサ計測ノード４４ｃのずれを規定しており、センサ位置エッジ４８ｄは、センサノード３０ｇとセンサ計測ノード４４ｄのずれを規定しており、センサ位置エッジ４８ｅは、センサノード３０ｊとセンサ計測ノード４４ｅのずれを規定している。センサ位置エッジ４８を追加することによって、第１のグラフ構造データ２８に真値情報を取り込むことができる。

次に、地図作成装置１０は、グラフ構造データ最適化部２２によって、センサ計測ノード４４及びセンサ位置エッジ４８が追加された第１のグラフ構造データ２８を最適化する（Ｓ２０）。具体的には、グラフ構造データ最適化部２２は、第１のグラフ構造データ２８内のセンサエッジ３２及びセンサ位置エッジ４８に基づいて算出される誤差関数の和が最小となるように第１のグラフ構造データ２８を最適化する。なお、第１のグラフ構造データ２８の最適化については、ステップＳ１６においてセンサ真位置算出部１８が第２のグラフ構造データ３８を最適化する手順と同様であるため、詳細な説明は省略する。センサエッジ３２に加え、センサ位置エッジ４８を用いて最適化することによって、真値情報を取り込んだ第１のグラフ構造データ２８を最適化することができる。

次に、地図作成装置１０では、ループクローズ部２４がループクローズエッジ４９を追加して第１のグラフ構造データ２８をさらに最適化する（Ｓ２２）。ループクローズエッジ４９は、ステップＳ２０で最適化された第１のグラフ構造データ２８を構成する複数のセンサノード３０の中から選択された２つのセンサノード３０を規定するセンサ５２間の相対位置や姿勢を規定する。本実施例では、移動経路６０の始点である地点Ａにおける推定位置を規定するセンサノード３０ａと、終点である地点Ｊにおける推定位置を規定するセンサノード３０ｊを選択する。選択する２つのセンサノード３０は、２つのセンサノード３０によって規定されるセンサ５２間の距離が比較的近く、かつ、２つのセンサノード３０によって規定されるセンサ５２が環境情報を計測した時間が比較的離れているほうがよい。センサ５２間の距離が近いと、２つの位置から同一の計測物６２を計測することができる。また、センサ５２が環境情報を計測した時間が離れると、２つの位置から計測された同一の計測物６２の姿勢誤差等が大きくなる。このため、２つのセンサノード３０が規定するセンサ５２から計測された同一の計測物６２間にずれが生じ易くなる。ループクローズエッジ４９は、これら２つのセンサノード３０を規定するセンサ５２間の相対位置や姿勢を規定することで、上記したずれが生じることを抑制する。

図１０に示すように、まず、ループクローズ部２４は、移動体５０の移動経路６０の始点である地点Ａに対応するセンサノード３０ａと、移動経路６０の終点である地点Ｊに対応するセンサノード３０ｊの間に、ループクローズエッジ４９を生成する。具体的には、ループクローズ部２４は、地点Ａにおいてセンサ５２が計測した環境情報と、地点Ｊにおいてセンサ５２が計測した環境情報に基づいて、両者のずれを規定するループクローズエッジ４９を生成する。ループクローズエッジ４９は、センサノード３０ａとセンサノード３０ｊとの間の姿勢の制約を表す情報行列を含む。この情報行列の算出には、パターンマッチングやＩＣＰ等の公知の方法を用いることができる。なお、本実施例では、追加するループクローズエッジ４９は１つであるが、追加するループクローズエッジ４９の数は特に限定されるものではなく、２つ以上のループクローズエッジ４９を追加してもよい。また、本実施例では、ループクローズエッジ４９を生成するために、移動経路６０の始点である地点Ａに対応するセンサノード３０ａと、終点である地点Ｊに対応するセンサノード３０ｊを選択しているが、複数のセンサノード３０の中から選択する２つのセンサノード３０の組み合わせは特に限定されない。ループクローズ部２４は、生成したループクローズエッジ４９を第１のグラフ構造データ２８に追加する。

次いで、ループクローズ部２４は、ループクローズエッジ４９が追加された第１のグラフ構造データ２８を最適化する。具体的には、ループクローズ部２４は、ステップＳ２０において最適化された第１のグラフ構造データ２８内のセンサエッジ３２、センサ位置エッジ４８（図１０では図示省略）及びループクローズエッジ４９に基づいて算出される誤差関数の和が最小となるように第１のグラフ構造データ２８を最適化する。なお、ステップＳ２２における第１のグラフ構造データ２８の最適化については、ステップＳ１６において第２のグラフ構造データ３８を最適化する手順及びステップＳ２０において第１のグラフ構造データ２８を最適化する手順と同様であるため、詳細な説明は省略する。センサエッジ３２及びセンサ位置エッジ４８に加え、ループクローズエッジ４９を用いて最適化することによって、センサ５２間の相対的な姿勢のずれを取り込んだ第１のグラフ構造データ２８を最適化することができる。

最後に、地図作成装置１０は、環境地図作成部２６によって、グラフ構造データ最適化部２２及びループクローズ部２４によって最適化された第１のグラフ構造データ２８に基づいて環境地図を作成させる（Ｓ２４）。具体的には、環境地図作成部２６は、グラフ構造データ最適化部２２及びループクローズ部２４によって最適化された第１のグラフ構造データ２８を構成するセンサノード３０（以下では、最適化されたセンサノード３０ともいう）に基づいて、そのセンサノード３０に対応する位置から計測したセンサ５２の環境情報を用いて環境地図を作成する。グラフ構造データ最適化部２２及びループクローズ部２４によって最適化された第１のグラフ構造データ２８には、真値情報が組み込まれている。このため、最適化されたセンサノード３０が規定するセンサ５２の位置及び姿勢は、真値情報によって精度が高くなっている。環境地図作成部２６は、センサ５２が計測した環境情報に、最適化されたセンサノード３０が規定するセンサ５２の位置及び姿勢を適用する。これによって、環境情報の精度を高くすることができる。環境地図作成部２６は、各センサノード３０について、センサ５２が計測した環境情報に、最適化されたセンサノード３０が規定するセンサ５２の位置及び姿勢をそれぞれ適用する。そして、環境地図作成部２６は、最適化されたセンサノード３０が規定するセンサ５２の位置及び姿勢が適用された複数の環境情報をＩＣＰ等の公知の方法を用いてマッチングし、環境地図を作成する。このように、グラフ構造データ最適化部２２及びループクローズ部２４によって最適化された第１のグラフ構造データ２８に基づいて環境地図を作成することによって、環境地図全体の精度を高めることができる。

本実施例の地図作成装置１０は、センサ計測ノード４４が規定するセンサ５２の真位置と、その位置に対応するセンサノード３０が規定するセンサ５２の位置とのずれを規定するセンサ位置エッジ４８を第１のグラフ構造データ２８に追加する。これによって、第１のグラフ構造データ２８に真値情報を取込むことができる。また、真値情報は、測量器５６の設置位置情報及びセンサ計測位置情報に基づいて生成した第２のグラフ構造データ３８を最適化して算出される。これよって、センサ５２の計測位置を全体としてより真値に近づけることができ、センサ５２の位置についての真値情報の精度をより高くすることができる。また、真値情報が取り込まれた第１のグラフ構造データ２８の各エッジ３２、４８に基づいて算出される誤差関数の和が最小となるように第１のグラフ構造データ２８を最適化する。これによって、環境地図全体の精度を高めることができる。

（変形例）
上記の実施例では、対象領域内の中央に比較的小さい計測物６２ｄのみが配置されており、移動体５０は比較的広い領域を移動可能となっていたが、このような構成に限定されない。例えば、対象領域内の比較的広い範囲に計測物が配置されており、移動体５０が移動可能な範囲が狭くなっている場合であっても、本実施例の技術を適用して対象領域内の地図を作成することができる。具体的には、対象領域は、例えば工場や倉庫等であり、移動体５０は、対象領域（すなわち、工場や倉庫等）内の比較的狭い通路等を移動してもよい。

図１１は、センサ５２が倉庫７０内の環境情報を計測する状態の一例を模式的に示している。図１１に示すように、倉庫７０内には、３つの棚７２ａ〜７２ｃが設置されている。移動体５０は、移動経路７４を移動する。詳細には、移動経路７４は、区間７４ａ〜７４ｆに区分される。区間７４ａは、棚７２ａと倉庫７０の壁の間のＸ方向に伸びる区間である。区間７４ｂは、棚７２ａと倉庫７０の壁の間及び棚７２ｂと倉庫７０の壁の間のＹ方向に伸びる区間である。区間７４ｃは、棚７２ｂと倉庫７０の壁の間のＸ方向に伸びる区間である。区間７４ｄは、棚７２ｂと棚７２ｃの間のＹ方向に伸びる区間である。区間７４ｅは、棚７２ｃと倉庫７０の壁の間のＸ方向に伸びる区間である。区間７４ｆは、棚７２ｃと倉庫７０の壁の間及び棚７２ａと倉庫７０の壁の間のＹ方向に伸びる区間である。このような倉庫７０においては、例えば、移動体５０は、始点７６から、区間７４ａを＋Ｘ方向に移動する。そして、移動体５０は、区間７４ｂを−Ｙ方向に移動し、さらに区間７４ｃを−Ｘ方向に移動する。その後、移動体５０は、区間７４ｄを＋Ｙ方向に移動した後、同じ区間７４ｄを−Ｙ方向に移動する。そして、移動体５０は、区間７４ｅを−Ｘ方向に移動した後、区間７２ｆを＋Ｙ方向に移動し、始点７６の近傍に設定される終点７８まで移動し、移動を終了する。この間、センサ５２は複数地点で環境情報を計測する。

倉庫７０内のように、見通しが悪く、移動体５０が移動可能な領域（すなわち、通路等）が比較的狭い場合には、センサ５２による計測物（すなわち、壁や棚７２等）の計測地点が多いほうが作成される環境地図の精度を高くできる。このため、移動体５０が移動経路７４を移動する間、可能な限り多くの地点で環境情報を計測したほうがよい。具体的には、倉庫７０等では、例えば、移動体５０は１〜２ｍ程度移動する毎に環境情報を計測してもよく、移動経路７４の距離によるが、合計１０００箇所以上の地点で環境情報を計測してもよい。

測量器５６による計測についても、計測地点が多いほうが作成される環境地図の精度を高くできる。しかしながら、測量器５６による計測は、作業者によって行われるため、計測地点が多いと作業負担が増大する。このため、測量器５６による計測は、倉庫７０内において特徴を有する箇所を全て含むように計測すると共に、可能な限り少ない方がよい。

例えば、まず、測量器５６を区間７４ａと区間７４ｂの間の地点８０ａに設置する。測量器５６を地点８０ａに設置すると、区間７４ａに位置するときのセンサ５２の位置と、区間７４ｂに位置するときのセンサ５２の位置を計測できる。このとき、区間７４ａに位置するセンサ５２については始点７６を含む２〜３地点においてセンサ５２の位置を計測し、区間７４ｂに位置するセンサ５２については２〜３地点においてセンサ５２の位置を計測する。次に、測量器５６を区間７４ｂと区間７４ｄの間の地点８０ｂに設置する。測量器５６を地点８０ｂに設置すると、区間７４ｃに位置するときのセンサ５２の位置と、区間７４ｄに位置するときのセンサ５２の位置と、区間７４ｅに位置するときのセンサ５２の位置を計測できる。このとき、各区間７４ｃ、７４ｄ、７４ｅに位置するセンサ５２について、１〜３地点ずつセンサ５２の位置を計測する。最後に、測量器５６を区間７４ｆの中間付近の地点８０ｃに設置する。測量器５６を地点８０ｃに設置すると、区間７４ｆに位置するときのセンサ５２の位置を計測できる。このとき、終点７８を含む２〜３地点のセンサ５２の位置を計測する。このように計測することによって、各区間７４ａ〜７４ｆについてのセンサ計測情報を取得できると共に、作業者による作業負担の増大を抑制することができる。

上記の変形例では、倉庫７０内において、例えば、センサ５２によって合計１０００箇所以上の地点から環境情報を計測すると共に、測量器５６によって約１０〜２０箇所のセンサ５２の位置を計測する。センサ５２によって多くの地点から環境情報を計測することによって、倉庫７０内の環境情報を多く取得することができる。また、測量器５６によるセンサ５２の位置の計測地点を最小限まで減らすことによって、作業者による作業負担の増大を抑制できる。上記の変形例においても、センサ５２が計測した環境情報及び算出されたセンサ推定位置情報に基づいて生成された第１のグラフ構造データ２８に、測量器５６が計測したセンサ５２の位置から算出されるセンサ５２の真位置を取り込むことによって、環境地図の精度を高くすることができる。したがって、倉庫７０等のような見通しの悪い領域内であっても、精度の高い環境地図を作成することができる。

本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。例えば、上記の実施例では、ステップＳ１６において、センサ計測位置情報と測量器５６の設置位置情報に基づいて、センサ真位置を算出しているが、このような構成に限定されない。ステップＳ１６を省略してもよく、この場合には、ステップＳ１８において、測量器５６によって計測されたセンサ計測位置をセンサ５２の真位置として第１のグラフ構造データ２８に追加してもよい。また、上記の実施例では、ステップＳ２２において、ループクローズエッジ４９を追加して第１のグラフ構造データ２８をさらに最適化しているが、このような構成に限定されない。例えば、ステップＳ２０において最適化された第１のグラフ構造データ２８を構成するセンサノード３０に生じるずれが小さい場合には、ステップＳ２２を省略してもよい。この場合には、ステップＳ２０において最適化された第１のグラフ構造データ２８に基づいて環境地図を作成してもよい。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

１０：地図作成装置
１２：環境計測データ記憶部
１４：グラフ構造データ生成部
１６：測量データ記憶部
１８：センサ真位置算出部
２０：センサ位置エッジ追加部
２２：グラフ構造データ最適化部
２４：ループクローズ部
２６：環境地図作成部
２８：第１のグラフ構造データ
３０：センサノード
３２：センサエッジ
３８：第２のグラフ構造データ
４０：測量器ノード
４２：測量器エッジ
４４：センサ計測ノード
４６：センサ計測エッジ
４８：センサ位置エッジ
４９：ループクローズエッジ
５０：移動体
５２：センサ
５４：移動体の記憶部
５６：測量器
５８：測量器の記憶部
６０：移動体の移動経路
６２：計測物
７０：倉庫
７２：棚
７４：倉庫内の移動経路
８０：測量器の設置地点

Claims (4)

1. 対象領域の環境地図を作成する装置であって、
   前記対象領域内の複数の計測点のそれぞれについて、センサによって計測された環境情報を記憶する環境情報記憶部と、
   前記複数の計測点のそれぞれについて、前記センサの移動量に基づいて算出される計測時の前記センサの推定位置に関するセンサ推定位置情報を記憶するセンサ推定位置情報記憶部と、
   前記環境情報及び前記センサ推定位置情報に基づいて、前記複数の計測点のそれぞれにおける前記センサの位置を規定するセンサノードと、隣接するセンサノード間における前記センサの相対的位置関係を規定するセンサエッジとを備える第１のグラフ構造データを生成する第１グラフ構造データ生成部と、
   前記対象領域内の複数の設置点のそれぞれについて、センサ位置計測装置が設置される設置位置に関する設置位置情報を記憶する設置位置情報記憶部と、
   前記複数の計測点の中から選択された複数の計測点において、前記センサ位置計測装置を用いて計測されるセンサの計測位置に関するセンサ計測位置情報を記憶するセンサ計測位置情報記憶部と、
   前記センサ計測位置情報及び前記設置位置情報に基づいて、前記複数の設置点のそれぞれにおける前記センサ位置計測装置の設置位置を規定する計測装置ノードと、前記複数の設置点間における前記センサ位置計測装置の相対的位置関係を規定する計測装置エッジと、前記複数の設置点のそれぞれにおいて前記センサ位置計測装置で計測された前記センサの位置を規定するセンサ計測ノードと、前記センサ計測ノードとそのセンサ計測ノードに対応する計測装置ノードの相対的位置関係を規定するセンサ計測エッジとを備える第２のグラフ構造データを生成する第２グラフ構造データ生成部と、
   前記計測装置エッジ及び前記センサ計測エッジから算出される誤差関数の和が最小となるように前記第２のグラフ構造データを最適化することにより、センサの真位置を算出するセンサ真位置算出部と、
   前記センサ真位置算出部により算出されたセンサの真位置と、そのセンサ真位置に対応するセンサノードに基づいて、前記算出されたセンサの真位置が規定するセンサの位置と前記対応するセンサノードが規定するセンサの位置とのずれを規定するセンサ位置エッジを前記第１のグラフ構造データに追加するセンサ位置エッジ追加部と、
   前記センサ位置エッジが追加された前記第１のグラフ構造データについて、各エッジから算出される誤差関数の和が最小となるように前記第１のグラフ構造データを最適化するグラフ構造データ最適化部と、を備えている、地図作成装置。
2. 最適化された第１のグラフ構造データを構成する複数のセンサノードから選択された少なくとも２つのセンサノード間における前記センサの相対的位置関係を規定するループクローズエッジを前記最適化された第１のグラフ構造データに追加し、各エッジから算出される誤差関数の和が最小となるように前記第１のグラフ構造データを最適化するループクローズ部をさらに備える、請求項１に記載の地図作成装置。
3. 最適化された前記第１のグラフ構造データ内のセンサノードと、センサノードが規定する前記センサの位置から計測された環境情報を用いて環境地図を作成する環境地図作成部をさらに備える、請求項１又は２に記載の地図作成装置。
4. 前記センサ計測位置情報は、前記センサ位置計測装置によって、前記複数の計測点の中から選択された２以上の計測点において計測されたセンサの位置に関する情報であり、
   前記２以上の計測点間の距離は、前記センサによって環境情報を計測可能な距離より短い、請求項１〜３のいずれか一項に記載の地図作成装置。

Images