JP2019191498A

JP2019191498A - 地図作成装置

Info

Publication number: JP2019191498A
Application number: JP2018087110A
Authority: JP
Inventors: 渚小山; Nagisa Koyama; 亮暢藤井; Akinobu Fujii
Original assignee: Toyota Central R&D Labs Inc
Current assignee: Toyota Central R&D Labs Inc
Priority date: 2018-04-27
Filing date: 2018-04-27
Publication date: 2019-10-31
Anticipated expiration: 2038-04-27
Also published as: JP7047576B2

Abstract

【課題】正確な地図を作成可能な地図作成装置を提供する。【解決手段】地図作成装置１０は、計測情報を取得したセンサの位置・姿勢を示すセンサノードと、部分地図の位置・姿勢を規定する環境ノードと、環境ノードとセンサノードの相対的位置関係を規定する第１エッジと、を備えたグラフ構造データを生成する。複数の部分地図の信頼度を算出し、複数の部分地図について、部分地図と複数の計測情報との一致判定を行い、一致度が所定のしきい値を超えた特定部分地図を抽出する。部分地図の信頼度が高いほど、所定のしきい値が小さくされる。抽出された特定部分地図について、環境ノードとセンサノードの相対的位置関係を規定する第２エッジを生成し、グラフ構造データに追加する。特定部分地図の信頼度が高いほど、追加する際の重み付けを大きくし、グラフ構造データを最適化する。【選択図】図１

Description

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

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

特許第５６４７２９４号公報

環境が変化する場合がある。例えば、工場や倉庫などにおいて、駐車されているトラックや床に仮置きされる荷物が存在する場合である。このような環境下では、グラフ構造データに誤ったエッジが追加されることで，そのエッジを用いた最適化により地図の精度が低下する。精度の低下した地図を参照した移動体は、自己位置の推定に失敗してしまう。その結果、移動体を自律的に移動させることが困難になる。本明細書は、環境が変化する場合においても、正確な地図を作成可能な技術を開示する。

本明細書に開示する地図作成装置の一実施形態は、対象領域の地図を作成する地図作成装置である。移動体に搭載された環境計測センサによって対象領域内の複数の計測点のそれぞれについて測定された複数の計測情報を取得する計測情報取得部であって、計測情報は、計測点の周囲に存在する複数の計測物の計測点からの相対位置または距離を示すデータである計測情報取得部を、地図作成装置は備える。複数の計測情報に基づいて、複数の部分地図を生成する部分地図生成部であって、部分地図は計測点の周囲における計測物の状態を示す局所的な地図である部分地図生成部を、地図作成装置は備える。グラフ構造データを生成するグラフ構造データ生成部を、地図作成装置は備える。グラフ構造データは、複数の計測点のそれぞれにおける環境計測センサの位置および姿勢を示すセンサノードと、部分地図の位置および姿勢を規定する環境ノードと、環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する第１エッジと、を備える。複数の部分地図の各々の信頼度を算出する信頼度算出部であって、信頼度は、部分地図の確かさが高いほど高くなる指標である信頼度算出部を、地図作成装置は備える。複数の部分地図の各々について、部分地図と複数の計測情報との一致判定を行い、一致度が所定のしきい値を超えた部分地図である特定部分地図を抽出する一致判定部であって、部分地図の信頼度が高いほど所定のしきい値が小さくされる一致判定部を、地図作成装置は備える。特定部分地図に対応する環境ノードと、一致判定部にてその特定部分地図と一致判定された計測情報に対応するセンサノードとの相対的位置関係を規定する第２エッジを生成し、生成した第２エッジをグラフ構造データに追加するエッジ追加部であって、第２エッジの元になる特定部分地図の信頼度が高いほど追加する際の重み付けを大きくして第２エッジを追加するエッジ追加部を、地図作成装置は備える。第２エッジが追加されたグラフ構造データについて、各エッジから算出される誤差関数の和が最小となるようにグラフ構造データを最適化するグラフ構造データ最適化部を、地図作成装置は備える。

上記の地図作成装置では、複数の部分地図の各々の信頼度を算出することができる。そして、移動体で測定された計測情報に一致する特定部分地図を、信頼度に基づいて抽出することができる。抽出された特定部分地図について作成したエッジを、グラフ構造データに追加することができる。このとき、エッジの元になる特定部分地図の信頼度が高いほど、追加する際の重み付けを大きくすることができる。これにより、環境が変化する場合においても、移動体によって測定された計測情報に基づいて正確な地図を生成することができる。

信頼度算出部が算出する信頼度は、部分地図生成部によって部分地図が生成された時刻から、当該部分地図と一致判定される計測情報が計測情報取得部によって取得された時刻までの経過時間の絶対値が大きくなるほど単調減少してもよい。効果の詳細は実施例で説明する。

部分地図は、複数のグリッドで表されていてもよい。グリッドが物体で占有されているか否かが、複数のグリッドの各々について占有確率で示されていてもよい。信頼度算出部が算出する信頼度は、予め定められた２値化しきい値と占有確率との差の絶対値を複数のグリッドの各々について求めた場合に、差の絶対値の総和が大きくなるほど高くなってもよい。２値化しきい値は、占有確率から、物体がグリッドで占有されているか否かに２値化するための値であってもよい。効果の詳細は実施例で説明する。

対象領域には、時間が経過しても計測物の数、位置、形状が変化しない定常領域が含まれていてもよい。信頼度算出部は、定常領域に対応している部分地図の信頼度を、定常領域に対応していない部分地図の信頼度よりも高くしてもよい。効果の詳細は実施例で説明する。

対象領域には、時間が経過しても計測物の数、位置、形状が変化しない定常領域が含まれていてもよい。部分地図は、複数のグリッドで表されていてもよい。グリッドが物体で占有されているか否かが、複数のグリッドの各々について占有確率で示されていてもよい。信頼度算出部が定常領域に対応している部分地図の信頼度を算出する場合には、定常領域に対応している部分地図の信頼度を、定常領域に対応していない部分地図の信頼度よりも高くし、予め定められた２値化しきい値と占有確率との差の絶対値が小さくなるほど信頼度を低くしてもよい。２値化しきい値は、占有確率から、物体がグリッドで占有されているか否かに２値化するための値であってもよい。差の絶対値は、複数のグリッドの各々について求められてもよい。信頼度算出部が定常領域に対応していない部分地図の信頼度を算出する場合には、部分地図生成部によって部分地図が生成された時刻から、計測情報取得部によって当該部分地図と一致判定を行う計測情報が取得された時刻までの経過時間の絶対値が大きくなるほど信頼度を単調減少させ、予め定められた２値化しきい値と占有確率との差の絶対値が小さくなるほど信頼度を低くしてもよい。効果の詳細は実施例で説明する。

第１エッジおよび第２エッジは、環境ノードとその環境ノードに対応するセンサノードとの相対位置・姿勢および計測誤差の共分散行列を表していてもよい。エッジ追加部は、第２エッジが表す計測誤差の共分散行列を信頼度が大きいほど小さく設定してもよい。効果の詳細は実施例で説明する。

地図作成装置はネットワーク上に配置されたサーバであってもよい。地図作成装置は、移動体からネットワークを介して複数の計測情報を受信可能であってもよい。効果の詳細は実施例で説明する。

計測情報取得部は、時刻ｔにおいてｉ番目（ｉは２以上の自然数）の計測点で測定された計測物の位置と、時刻ｔ−１においてｉ−１番目の計測点で測定された計測物の位置とを比較した場合に、時刻ｔにおける位置と時刻ｔ−１における位置とが一致している計測物の位置のみを示す情報を、時刻ｔにおける計測情報として受信してもよい。効果の詳細は実施例で説明する。

実施例１に係る地図生成システムの構成を示す図。全体地図の具体例を示す図。部分地図の具体例を示す図。グラフ構造データの具体例を示す図。初期地図生成モードの具体的な処理を示すフローチャート。地図生成・更新処理の具体的な処理を示すフローチャート。グラフ構造データの具体例を示す図。単調減少する関数の例を示す図。部分地図の内部状態を示す図。信頼度の算出のための第３の方法の具体例を示す図。グラフ構造データの具体例を示す図。地図生成処理の具体的な処理を示すフローチャート。実施例２に係る地図生成システムの構成を示す図。フィルタ処理の具体的な内容を示すフローチャート。フィルタ処理の一例を説明する図。

（地図生成システム１の構成）
図１のブロック図を用いて、地図生成システム１の構成を説明する。地図生成システム１は、地図作成装置１０、ロボット１１および１２を備えている。地図生成システム１は、「初期地図生成モード」で用いるシステムである。「初期地図生成モード」では、ロボット１１および１２を操作し、ロボット１１および１２から得る計測情報に基づき、地図作成装置１０を用いて全体地図を生成する。なお、「初期地図生成モード」で生成された全体地図を用いてロボットの自律制御を行う、「運用モード」も存在する。しかし本明細書では、「運用モード」の説明については省略する。

（ロボット１１および１２の構成）
ロボット１１および１２は、センサ４０、計測情報取得部４１、データ送信部４２、移動制御部４３を備える。ロボット１１および１２は対象領域内を移動しながら、センサ４０によって対象領域内の環境（例えば、壁等の計測物）を計測する。これにより計測情報取得部４１は、センサ４０から計測情報を取得する。センサ４０の一例としては、LiDAR、カメラ、RADAR、超音波センサ等が挙げられる。計測情報の一例としては、センサ４０で測定される相対位置情報・距離情報や、ＩＭＵ（Inertial Measurement Unit）やエンコーダ等により得られるロボットの速度情報・角速度情報等が挙げられる。また、ロボット１１および１２は、車輪の回転角を検出する不図示のセンサ（例えば、エンコーダ）を備えている。このセンサによって車輪の回転角を検出することで、ロボット１１および１２の移動方向及び移動量を算出することができる。よって、各計測地点におけるロボット１１および１２の姿勢及び位置を算出することができる。

データ送信部４２は、計測情報取得部４１で取得した計測情報に、ロボットを識別するためのロボットＩＤを付与して地図作成装置１０に送信する。移動制御部４３は、外部コントローラの操作に基づき、自身の移動を制御する。

（地図作成装置１０の構成）
地図作成装置１０は、対象領域の地図を作成する装置である。地図作成装置１０は、例えば、ＣＰＵ、ＲＯＭ、ＲＡＭ等を備えたコンピュータによって構成することができる。コンピュータがプログラムを実行することで、地図作成装置１０は、図１に示す計測情報取得部２１〜初期地図構成要素群書出部３２等として機能する。

計測情報取得部２１は、ロボットＩＤの付与された計測情報を受信する。計測情報取得部２１は、ロボットＩＤの付与された計測情報をロボット１１および１２から受信する。そして、センサ４０の位置と共に、計測情報を記憶する。

部分地図生成部２２は、計測情報をロボットＩＤ毎に所定の間隔で蓄積して部分地図を生成する。また、蓄積の過程で得られる部分地図とロボットとの相対位置・姿勢を、グラフ構造データ生成部２７へ出力する。部分地図記憶部２３は、部分地図生成部２２が作成した部分地図を記憶する。信頼度算出部２４は、部分地図記憶部２３中の部分地図について、その信頼度を算出する。

一致判定部２５は、部分地図記憶部中の部分地図と計測情報との一致判定を行う。そして、一致度が所定のしきい値を超えた部分地図を抽出する。エッジ追加部２６は、一致判定部２５で抽出された部分地図に関するエッジを生成し追加する。

グラフ構造データ生成部２７は、ノード（頂点）とエッジ（枝）で構成されるグラフ構造データを生成する。ノードは、ロボット１１および１２の位置・姿勢、部分地図の位置・姿勢を表す情報である。エッジは、部分地図とロボット１１および１２との相対位置・姿勢と、その計測誤差を表す情報である。

グラフ最適化部２８は、グラフ構造データを最適化する。グラフ記憶部２９は、グラフ構造データを保存する。地図生成部３０は、部分地図および信頼度に基づき、全体地図を生成する。全体地図記憶部３１は、地図生成部３０で生成された全体地図を保存する。初期地図構成要素群書出部３２は、部分地図記憶部２３、グラフ記憶部２９、全体地図記憶部３１の内容を、読込可能な形式で書き出す。

（地図の構成要素の説明）
地図の構成要素には、全体地図、部分地図、グラフ構造データが含まれる。これらの構成要素について説明する。全体地図は、地図作成装置１０にて最終的に出力する情報である。出力された全体地図は、ロボットの経路計画や、ロボットの自己位置推定に利用される。全体地図の具体例を図２に示す。全体地図はグリッド表現されている。すなわち、複数のグリッドに区分けされている。各グリッドは、「１」または「０」によって、そのグリッドが物体に占有されているか否かを表現している。図２では、ハッチングで示されているグリッドが「１」であり、物体に占有されていることを示している。また白色で示されているグリッドが「０」であり、物体に占有されていないことを示している。全体地図には、１つの固定の座標系である、全体地図座標系ＥＣ１が設定されている。全体地図の生成方法は後述する。

部分地図は、地図作成装置１０にて中間情報として生成される情報である。部分地図は、環境の計測情報から取得した特徴量として用いられる。部分地図の具体例を図３に示す。部分地図は、ロボットが一定間隔（例えば、一定移動距離）で生成する局所的な地図である。すなわち部分地図は、複数の計測点の各々について、計測点の周囲における計測物の状態を示している。部分地図は、全体地図と同様にグリッド表現されている。グリッドの数や部分地図の形状は、部分地図により異なっていてもよい。部分地図は、それぞれ固有の座標系である部分地図座標系ＰＣ１〜ＰＣ４を備えている。部分地図座標系の方向は、部分地図により異なっている。部分地図の生成方法は後述する。

グラフ構造データは、地図作成装置１０にて中間情報として生成される情報である。グラフ構造データは、ロボットの走行軌跡の誤差を抑制するために用いられる。グラフ構造データの具体例を図４に示す。グラフ構造データは、センサノードＳＮ１〜ＳＮ４、環境ノードＥＮ１〜ＥＮ３、エッジＥ１〜Ｅ６を備えている。センサノードＳＮ１〜ＳＮ４は、ロボット１１が備えるセンサ４０の、複数の計測点のそれぞれにおける位置および姿勢を示す情報である。環境ノードＥＮ１〜ＥＮ３は、複数の計測点のそれぞれにおける、センサ４０から得られた計測情報を蓄積して生成した部分地図の位置および姿勢を規定する情報である。エッジＥ１〜Ｅ６は、環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する情報である。具体的には、エッジＥ１〜Ｅ６は、環境ノードとその環境ノードに対応するセンサノードとの相対位置・姿勢、および、計測誤差の共分散行列を表している。センサノードと環境ノードは、全結合するわけではない。また、処理の過程でマッチングされたロボットと部分地図との関係が、グラフ構造データに逐次追加されていく。グラフ構造データの生成方法は後述する。

（初期地図生成モードの動作）
図５のフローチャートを基に、「初期地図生成モード」の具体的な処理について説明する。Ｓ１０において、ロボット１１および１２が送信する計測情報を受信する、計測情報受信処理を行う。この処理は、計測情報取得部２１にて実施する。

Ｓ１５において、地図生成・更新処理を行う。処理の詳細は、図６を用いて後述する。Ｓ２０において、地図生成・更新処理の終了指示が入力されたか否かを判断する。否定判断される場合（Ｓ２０：ＮＯ）にはＳ１０へ戻り、処理を続行する。一方、肯定判断される場合（Ｓ２０：ＹＥＳ）にはＳ２５へ進む。

Ｓ２５において、初期地図構成要素群の書出処理を行う。この処理は、初期地図構成要素群書出部３２にて実施する。具体的には、部分地図記憶部２３、グラフ記憶部２９、全体地図記憶部３１の記憶内容を保存する。

（地図生成・更新処理の内容（Ｓ１５））
Ｓ１５の地図生成・更新処理の詳細内容について、図６のフローチャートを用いて説明する。Ｓ１００において、ロボットの現在位置および現在姿勢の概算処理を行う。この処理は、部分地図生成部２２にて、ロボット１１および１２の各々について実施する。概算したロボットの現在位置および現在姿勢は、Ｓ１０５のスキャンマッチ処理の初期値として用いられる。ロボットの位置・姿勢の概算には、過去２地点の位置・姿勢から外挿しても良い。またロボットの速度や各情報を参照し、概算しても良い。

Ｓ１０５において、スキャンマッチ処理を行う。この処理は、部分地図生成部２２にて、ロボット１１および１２の各々について実施する。スキャンマッチ処理では、ロボットが時刻ｔ−１にて生成中の部分地図と、時刻ｔの計測情報とのマッチングを行い、時刻tでのロボットの位置・姿勢を出力する。具体的には、スキャンマッチ初期値算出処理（Ｓ１００）により得たロボットの位置・姿勢を初期値として、ロボットの計測情報と生成中の部分地図との誤差の総和が最小となるようなロボットの位置・姿勢を求める。また、マッチングの過程で算出する部分地図とロボットとの相対位置・姿勢と、その計測誤差共分散を出力する。

Ｓ１１０において、部分地図生成処理を行う。この処理は、部分地図生成部２２にて、ロボット１１および１２の各々について実施する。部分地図生成処理では、スキャンマッチ処理（Ｓ１０５）により得られた部分地図座標系でのロボットの位置・姿勢を用いて時刻tの計測情報を部分地図に蓄積することで、生成中の部分地図を更新する。一定数の計測情報を蓄積すると，部分地図の生成を完了し、新規に部分地図の生成を開始する。新規に生成を開始する部分地図の位置・姿勢は、時刻tでのロボットの位置・姿勢としてもよい。生成を完了した部分地図は、部分地図記憶部２３に記憶される。

Ｓ１１５において、グラフ構造データ生成処理を行う。具体的には、スキャンマッチ処理（Ｓ１０５）の結果と、部分地図生成処理（Ｓ１１０）の結果に基づき、グラフ構造データの更新を行う。この処理は、グラフ構造データ生成部２７にて、ロボット１１および１２の各々について実施する。処理の具体例を、図４および図７を用いて説明する。なお、図４および図７では，重なりを避けるために環境ノードとセンサノードをずらして描画している。図４のグラフ構造データに対して、新規に生成した部分地図の位置および姿勢を環境ノードＥＮ４として追加するとともに、ロボットの最新の位置および姿勢を、センサノードＳＮ５として追加する。これにより、グラフ構造データは図７の状態になる。また、ロボットが生成中の部分地図に対する、ロボットの相対位置・姿勢をエッジＥ７として追加する。エッジＥ７には、スキャンマッチにより得た相対位置・姿勢・計測誤差共分散を加える。

Ｓ１２０において、部分地図の信頼度算出処理を行う。この処理は、信頼度算出部２４にて、ロボット１１および１２の各々について、また、部分地図記憶部２３内の複数の部分地図の各々について実施する。信頼度は、部分地図の確からしさを示す指標である。信頼度は、部分地図の確からしさが高くなるほど高くなってもよい。

信頼度の算出方法の具体例としては、以下の３つの方法が挙げられる。第１の方法は、部分地図生成処理（Ｓ１１０）によって部分地図が生成された時刻から、計測情報受信処理（Ｓ１０）によって当該部分地図との一致判定を試みる計測情報が取得された時刻までの経過時間の絶対値が大きくなるほど、信頼度を単調減少させる方法である。具体的には、時間変数の増加に伴い単調減少する関数を用いることで、信頼度を算出することができる。図８に、単調減少する関数の例を示す。横軸（経過時間ｔ）は、部分地図が生成された時刻から計測情報が取得された時刻までの経過時間の絶対値である。信頼度ｃは、例えば、「ｃ＝−ａｔ＋ｂ」という、係数ａが負である１次関数Ｆ１で表すことができる。また例えば、「ｃ＝−ｄｔ^２＋ｂ」という、係数ｄが負である二次関数Ｆ２で表すことができる。また例えば、「ｃ＝ｆ^ｔ＋ｂ−１」という、係数を正とする指数関数Ｆ３で表すことができる。これにより、部分地図が生成された時刻から、その部分地図と一致判定を試みる計測情報が取得された時刻までの経過時間の絶対値が増加するに伴って、部分地図の信頼度ｃを低下させることができる。

信頼度の算出方法の第２の方法は、部分地図の確率的な確からしさに基づいて算出する方法である。具体例を図９（Ａ）および図９（Ｂ）に示す。部分地図の内部状態は、複数のグリッドで表されている。複数のグリッドの各々が物体で占有されているか否かが、占有確率で示されている。占有確率は、占有されている状態を「１」、占有されていない状態を「０」とし、その間の状態を数値で確率的に示したものである。図９（Ａ）および図９（Ｂ）では、占有確率は、ハッチングの密度が高いほど大きいとする。図９（Ａ）の部分地図ＰＭ１は、図９（Ｂ）の部分地図ＰＭ２に比して、占有確率を示すハッチング密度のコントラスト差が、各グリッド間で大きい。すなわち、部分地図ＰＭ１の方が、部分地図ＰＭ２よりも「確からしさ」が高いと言える。第２の方法では、このような「確からしさ」の大きい部分地図ほど信頼度を上げる。

信頼度の算出方法の一例を説明する。「２値化しきい値」を予め定める。２値化しきい値は、０〜１の間の数値をとる占有確率を、物体がグリッドで占有されているか否かを示す「１」または「０」に２値化するための値である。２値化しきい値は、例えば「０．５」に設定してもよい。予め定められた２値化しきい値と占有確率との「差の絶対値」を、複数のグリッドの各々について求める。信頼度は、この「差の絶対値」が大きくなるほど高くなる。
具体的には、下式（１）によって求めることができる。

ここで、ｃは部分地図の信頼度である。ｌ_ｉｊはグリッド（ｉ，ｊ）の内部状態である。ｌ_ｔｈは２値化しきい値である。ｎは部分地図内のグリッドの総数である。αは係数である。

信頼度の算出のための第３の方法は、信頼度が高くなる領域を予め認識しておく方法である。例えば、時間が経過しても計測物の数、位置、形状が変化しない定常領域に対応する部分地図の信頼度を、定常領域に対応しない部分地図よりも高く設定する。定常領域の具体例としては、物体設置が禁止された領域が挙げられる。具体例を図１０に示す。図１０において、定常領域Ｒ１内の部分地図ＰＭ１２およびＰＭ１３の信頼度を、定常領域Ｒ１に含まれない部分地図ＰＭ１１、ＰＭ１４、ＰＭ１５の信頼度よりも高くする。

信頼度の算出のための第１〜第３の方法の組み合わせ例を説明する。例えば、定常領域に少なくとも一部が含まれている部分地図の信頼度を算出する場合には、第２および第３の方法を組み合わせて使用してもよい。一方、定常領域に含まれていない部分地図の信頼度を算出する場合には、第１および第２の方法を組み合わせて使用してもよい。なお、１つの部分地図に対して複数の信頼度が算出された場合には、最小値や最大値を選択してもよいし、複数の信頼度の積や平均値を算出してもよい。

Ｓ１２５において、部分地図と計測情報との一致判定処理を行う。この処理は、一致判定部２５にて、ロボット１１および１２の各々について実施する。また、部分地図記憶部２３内の部分地図ごとに実施する。

Ｓ１２５の一致判定処理の具体的内容は、Ｓ１０５のスキャンマッチ処理と同様である。すなわち、複数の部分地図の各々について、部分地図と計測情報との一致判定（スキャンマッチ）を行う。そして、マッチング度合を示す指標（スコア）が所定のしきい値を超えた部分地図である特定部分地図を抽出する。このとき、所定のしきい値は、部分地図の信頼度が高いほど小さくする。これにより、信頼度の低い部分地図が計測情報と一致すると判断されてしまい、抽出されてしまう事態を防止することができる。

式（２）に、一致判定処理で用いられる数式の具体例を示す。
τ_ｌ＝η／Ｃ_ｌ・・・式（２）
ここで、τ_ｌは部分地図ｌの判定閾値、Ｃ_ｌは部分地図ｌの信頼度、ηは定数である。

Ｓ１３０において、一致判定に成功したか否かが判断される。否定判断される場合（Ｓ１３０：ＮＯ）にはＳ１４０へ進み、肯定判断される場合（Ｓ１３０：ＹＥＳ）にはＳ１３５へ進む。

Ｓ１３５において、エッジ追加処理を行う。この処理は、一致判定処理（Ｓ１２５）の結果に基づいてグラフ構造データの更新を行うための処理である。具体的には、Ｓ１２５で抽出された特定部分地図に対応する環境ノードと、その特定部分地図と一致判定された計測情報に対応するセンサノードとの間にエッジ（制約）を生成する。この処理は、エッジ追加部２６にて、ロボット１１および１２の各々について実施する。また、部分地図記憶部２３内の部分地図ごとに実施する。

図７および図１１を用いて、エッジ追加処理を具体的に説明する。図１１に示すように、過去の部分地図である環境ノードＥＮ３と、最新のロボット位置・姿勢であるセンサノードＳＮ５の相対的位置関係を規定するエッジＥ８を追加生成する。これにより、グラフ構造データを図７の状態から図１１の状態にすることができる。追加生成したエッジＥ８には、エッジＥ８の元となる特定部分地図に対する当該ロボットの相対位置・姿勢と、計測誤差の共分散行列を設定する。このとき、エッジＥ８が表す計測誤差の共分散行列を、部分地図の信頼度が大きいほど小さく設定する。これにより、エッジＥ８の元になる特定部分地図の信頼度が高いほど、エッジＥ８をグラフ構造データに追加する際の重み付けを大きくすることができる。換言すると、特定部分地図の信頼度が高いほど、エッジＥ８の寄与度が大きくなるように調整することができる。

式（３）に、共分散行列の具体例を示す。

ここで、左辺のΣはロボット位置ｓと過去の部分地図ｌの間に制約として設定する計測誤差共分散である。右辺のΣはロボット位置ｓと部分地図ｌとのスキャンマッチによる誤差共分散である。ｃ_ｌは部分地図ｓの信頼度である。εは定数である。

Ｓ１４０において、全ての部分地図を処理したか否かを判断する。否定判断される場合（Ｓ１４０：ＮＯ）にはＳ１４５へ進み、次の部分地図が選択される。そしてＳ１２０へ戻る。一方、肯定判断される場合（Ｓ１４０：ＹＥＳ）には、Ｓ１５０へ進む。

Ｓ１５０において、全てのロボットについて処理を実行したか否かを判断する。否定判断される場合（Ｓ１５０：ＮＯ）にはＳ１５５へ進み、次のロボットが選択される。そしてＳ１００へ戻る。一方、肯定判断される場合（Ｓ１５０：ＹＥＳ）にはＳ１６０へ進む。

Ｓ１６０において、グラフ最適化処理を行う。この処理は、グラフ最適化部２８にて実施する。グラフ最適化では、グラフ構造データ全体で、エッジが表す制約（すなわち、相対位置・姿勢と誤差共分散）から誤差量を表す誤差関数を作成する。そして、各エッジの誤差関数の和が最小となるように、各ノードの位置・姿勢を補正する。そして、最適化後のグラフ構造データをグラフ最適化部２８記憶部に保存する。誤差関数としては、各エッジに対する計測誤差の総和を用いることができる。計測誤差の一例としては、エッジ両端のノード間の相対位置・姿勢と、エッジに設定された相対位置・姿勢との誤差をエッジの計測誤差共分散の逆行列で重み付けしたもの（マハラノビス距離）が挙げられる。

Ｓ１６５において、地図生成処理を行う。この処理は、地図生成部３０にて実施する。図１２のフローチャートを用いて、地図生成処理の具体的な内容を説明する。Ｓ２００において、部分地図記憶部２３のすべての部分地図を、その位置・姿勢に基づき配置する。また、全ての部分地図の外接矩形から、全体地図のサイズを調整する。Ｓ２０５において、全体地図の各グリッドのうちのｉ＝１番目のグリッドを選択する。Ｓ２１０において、選択されたｉ番目のグリッドと重なる部分地図の数を算出する。Ｓ２１５において、選択されたｉ番目のグリッドの内部状態を、部分地図の信頼度の重み付けによって算出する。例えば下式（４）によって算出できる。

ここで、Ｌ_ｉは全体地図のｉ番目のグリッドの内部状態である。Ｎ_ｉは全体地図のｉ番目のグリッドと重なる部分地図の数である。ｃ_ｊはｊ番目の部分地図の信頼度である。ｌ_ｊはｊ番目の部分地図の内部状態である。

Ｓ２２０の内部状態の閾値処理において、全体地図のｉ番目のグリッドの占有状態を「０」または「１」で出力する。Ｓ２２５において、全てのグリッドについて処理を実行したか否かが判断される。肯定判断される場合（Ｓ２２５：ＹＥＳ）にはフローを終了する。否定判断される場合（Ｓ２２５：ＮＯ）にはＳ２０３へ進み、次のｉ＋１番目のグリッドを選択した上で、Ｓ２１０へ戻る。

（効果）
ロボットの正しい位置・姿勢（走行軌跡）が分かれば、その位置・姿勢に対応する計測情報を重ねてグリッド化することで、正確な地図をロボットを用いて生成することができる。しかし実際には、ロボットの走行軌跡の推定値には誤差が生じるため、ロボットの走行軌跡を最適化する必要がある。最適化の方法として、部分地図を特徴量（環境の局所的な特徴を表現する情報）としたGraph-based SLAMがある。しかし、特徴量である部分地図は、時間経過に伴う環境の変化（例：倉庫の床に置かれる荷物の位置や数の変化）に従ってその信頼度が変化する。そのため、信頼度の低い部分地図を用いた最適化処理が発生してしまい、ロボットの走行軌跡の誤差が増大してしまう場合があった。そこで本実施形態の地図作成装置１０では、複数の部分地図の各々の信頼度を算出することができる（Ｓ１２０）。そして、移動体に搭載された環境計測センサで測定された計測情報に一致する特定部分地図を、信頼度に基づいて抽出することができる（Ｓ１２５）。抽出された特定部分地図について作成したエッジを、グラフ構造データに追加することができる（Ｓ１３５）。このとき、エッジの元になる特定部分地図の信頼度が高いほど、追加する際の重み付けを大きくすることができる。これにより、状態が変化する環境下においても、移動体によって測定された計測情報に基づいて正確な地図を生成することが可能となる。

実施例１の地図生成システム１は、ロボットによって作成された部分地図の良否を、信頼度を用いて判断するシステムであった。実施例２の地図生成システム１ａは、計測情報のうちから不要な情報を取り除くことで、精度よく部分地図を作成する機能をさらに備えたシステムである。図１３に、実施例２の地図生成システム１ａのブロック図を示す。実施例２の地図生成システム１ａ（図１３）と、実施例１の地図生成システム１（図１）とで、共通する部位には共通の符号を付している。また実施例２に特有の部位には、符号の末尾に「ａ」を付している。

実施例２の地図生成システム１ａは、ロボット１１ａおよび１２ａを備えている。ロボット１１ａおよび１２ａは、フィルタ部４４ａをさらに備えている。計測情報取得部４１から出力された計測情報は、フィルタ部４４ａを介してデータ送信部４２へ入力される。

図１４のフローチャートを用いて、フィルタ部４４ａで行われるフィルタ処理を説明する。Ｓ３００において、時刻ｔ−１と時刻ｔの計測情報を位置合わせする。Ｓ３０３において、ｉ＝１番目の計測情報を選択する。Ｓ３０５において、時刻ｔ−１の計測情報と時刻ｔの計測情報の位置が一致するか否かを判断する。肯定判断される場合（Ｓ３０５：ＹＥＳ）にはＳ３１５へ進み、選択された計測情報をデータ送信部４２へ送信する。そしてＳ３２０へ進む。一方、否定判断される場合（Ｓ３０５：ＮＯ）には、Ｓ３１５をスキップしてＳ３２０へ進む。

Ｓ３２０において、全ての計測情報について処理を実行したか否かが判断される。肯定判断される場合（Ｓ３２０：ＹＥＳ）にはフローを終了する。否定判断される場合（Ｓ３２０：ＮＯ）にはＳ３２５へ進み、次のｉ＋１番目の計測情報を選択した上で、Ｓ３０５へ戻る。

図１５の計測情報の例を用いて、フィルタ処理の内容を説明する。図１５では、時刻ｔ−１および時刻ｔの各々におけるロボット１１ａの位置を、位置Ｐ１１およびＰ１２とする。時刻ｔ−１および時刻ｔの各々における壁の計測情報を、ＭＤ１１およびＭＤ１２とする。時刻ｔ−１および時刻ｔの各々における歩行者の脚の計測情報を、ＭＤ２１およびＭＤ２２とする。時刻ｔにおいて検出開始された走行中のフォークリフトの計測情報を、ＭＤ３２とする。図１５において、時刻ｔ−１の計測情報は破線で示し、時刻ｔの計測情報は実線で示している。計測情報ＭＤ１１およびＭＤ１２は壁の計測情報であるため、時刻ｔ−１と時刻ｔとで位置が一致する（Ｓ３０５：ＹＥＳ）。よって、計測情報ＭＤ１２はデータ送信部４２へ送信される（Ｓ３１５）。一方、計測情報ＭＤ２１とＭＤ２２は、歩行者の脚の計測情報であるため、時刻ｔ−１と時刻ｔとで位置が一致しない（Ｓ３０５：ＮＯ）。また計測情報ＭＤ３２は、時刻ｔで検出開始されている。従って、計測情報ＭＤ２２およびＭＤ３２は、データ送信部４２へ送信されない。

（効果）
実施例１において部分地図を生成するために行うスキャンマッチ処理（Ｓ１０５）は、静的な環境を前提にした技術である。移動する物体（例：作業者やフォークリフトなど）が存在する動的な環境では、過去の計測情報とのマッチングが適切に一致しない場合がある。すると、特徴量とする部分地図が正確に生成されなくなる結果、全体地図の精度が悪化してしまう場合がある。そこで実施例２に係る地図生成システム１ａでは、時刻ｔおよび時刻ｔ−１において測定された計測情報が示す計測物の位置を比較し（Ｓ３０５）、両者が一致している計測情報のみを地図作成装置１０へ送信する（Ｓ３１５）。これにより、複数の計測情報の中から移動する物体（動的物体）を計測したデータを除き、静的物体（例：壁、柱など）および準静的物体（例：停止中のトラックなど）を計測したデータのみを地図作成装置１０に送信することができる。全体地図の精度を向上させることが可能となる。

以上、本明細書が開示する技術の実施例について詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。

（変形例）
地図作成装置１０はコンピュータに限られず、例えば、インターネットなどのネットワーク上に配置されたサーバであってもよい。地図作成装置１０は、ネットワークを介して、ロボット１１および１２から計測情報を受信してもよい。地図作成装置１０の汎用性を高めることができる。

地図生成システム１は、ロボット１１および１２の２台の移動体を備える場合を説明したが、この形態に限られない。３台以上のロボットを備える場合においても、本明細書の技術を適用可能である。

式（１）〜式（４）は一例である。本実施例の技術では、様々なタイプの数式を使用することが可能である。

実施例２の地図生成システム１ａにおいて、フィルタ部４４ａの配置位置は自由に設定できる。例えば、地図作成装置１０において、計測情報取得部２１の前後に配置してもよい。また、実施例２の地図生成システム１ａでは、全体地図の精度を向上させることができるため、信頼度を用いなくてもよい。すなわち実施例２では、信頼度算出部２４を省略してもよい。また、一致判定部２５およびエッジ追加部２６では、信頼度を用いた処理を実行しなくてもよい。

本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時請求項記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数目的を同時に達成するものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。

ロボット１１、１２は、移動体の一例である。センサ４０は、環境計測センサの一例である。グラフ最適化部２８は、グラフ構造データ最適化部の一例である。

１：地図生成システム１０：地図作成装置１１、１２：ロボット２１：計測情報取得部２２：部分地図生成部２４：信頼度算出部２５：一致判定部２６：エッジ追加部２７：グラフ構造データ生成部２８：グラフ最適化部４０：センサ

Claims

対象領域の地図を作成する地図作成装置であって、
移動体に搭載された環境計測センサによって前記対象領域内の複数の計測点のそれぞれについて測定された複数の計測情報を取得する計測情報取得部であって、前記計測情報は、前記計測点の周囲に存在する複数の計測物の前記計測点からの相対位置または距離を示すデータである、前記計測情報取得部と、
複数の前記計測情報に基づいて、複数の部分地図を生成する部分地図生成部であって、前記部分地図は前記計測点の周囲における前記計測物の状態を示す局所的な地図である、前記部分地図生成部と、
グラフ構造データを生成するグラフ構造データ生成部であって、前記グラフ構造データは、複数の前記計測点のそれぞれにおける前記環境計測センサの位置および姿勢を示すセンサノードと、前記部分地図の位置および姿勢を規定する環境ノードと、前記環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する第１エッジと、を備える、前記グラフ構造データ生成部と、
複数の前記部分地図の各々の信頼度を算出する信頼度算出部であって、前記信頼度は、前記部分地図の確かさが高いほど高くなる指標である、前記信頼度算出部と、
複数の前記部分地図の各々について、前記部分地図と複数の前記計測情報との一致判定を行い、一致度が所定のしきい値を超えた部分地図である特定部分地図を抽出する一致判定部であって、前記部分地図の前記信頼度が高いほど前記所定のしきい値が小さくされる、前記一致判定部と、
前記特定部分地図に対応する前記環境ノードと、前記一致判定部にてその特定部分地図と一致判定された計測情報に対応する前記センサノードとの相対的位置関係を規定する第２エッジを生成し、生成した第２エッジを前記グラフ構造データに追加するエッジ追加部であって、前記第２エッジの元になる前記特定部分地図の信頼度が高いほど追加する際の重み付けを大きくして前記第２エッジを追加する、前記エッジ追加部と、
前記第２エッジが追加された前記グラフ構造データについて、各エッジから算出される誤差関数の和が最小となるように前記グラフ構造データを最適化するグラフ構造データ最適化部と、
を備える、地図作成装置。
前記信頼度算出部が算出する前記信頼度は、前記部分地図生成部によって前記部分地図が生成された時刻から、当該部分地図と一致判定される前記計測情報が前記計測情報取得部によって取得された時刻までの経過時間の絶対値が大きくなるほど単調減少する、請求項１に記載の地図作成装置。
前記部分地図は、複数のグリッドで表されており、
前記グリッドが物体で占有されているか否かが、複数の前記グリッドの各々について占有確率で示されており、
前記信頼度算出部が算出する前記信頼度は、予め定められた２値化しきい値と前記占有確率との差の絶対値を複数の前記グリッドの各々について求めた場合に、前記差の絶対値の総和が大きくなるほど高くなり、
前記２値化しきい値は、前記占有確率から、前記物体が前記グリッドで占有されているか否かに２値化するための値である、請求項１に記載の地図作成装置。
前記対象領域には、時間が経過しても計測物の数、位置、形状が変化しない定常領域が含まれており、
前記信頼度算出部は、前記定常領域に対応している部分地図の信頼度を、前記定常領域に対応していない部分地図の信頼度よりも高くする、請求項１に記載の地図作成装置。
前記対象領域には、時間が経過しても計測物の数、位置、形状が変化しない定常領域が含まれており、
前記部分地図は、複数のグリッドで表されており、
前記グリッドが物体で占有されているか否かが、複数の前記グリッドの各々について占有確率で示されており、
前記信頼度算出部が前記定常領域に対応している部分地図の前記信頼度を算出する場合には、
前記定常領域に対応している部分地図の信頼度を、前記定常領域に対応していない部分地図の信頼度よりも高くし、
予め定められた２値化しきい値と前記占有確率との差の絶対値が小さくなるほど前記信頼度を低くし、
前記２値化しきい値は、前記占有確率から、前記物体が前記グリッドで占有されているか否かに２値化するための値であり、
前記差の絶対値は、複数の前記グリッドの各々について求められ、
前記信頼度算出部が前記定常領域に対応していない部分地図の前記信頼度を算出する場合には、
前記部分地図生成部によって前記部分地図が生成された時刻から、前記計測情報取得部によって当該部分地図と一致判定を行う前記計測情報が取得された時刻までの経過時間が大きくなるほど前記信頼度を単調減少させ、
予め定められた２値化しきい値と前記占有確率との差の絶対値が小さくなるほど前記信頼度を低くする、請求項１に記載の地図作成装置。
前記第１エッジおよび前記第２エッジは、前記環境ノードとその環境ノードに対応する前記センサノードとの相対位置・姿勢および計測誤差の共分散行列を表しており、
前記エッジ追加部は、前記第２エッジが表す前記計測誤差の共分散行列を前記信頼度が大きいほど小さく設定する、請求項１〜５の何れか１項に記載の地図作成装置。
前記地図作成装置はネットワーク上に配置されたサーバであり、
前記移動体から前記ネットワークを介して複数の前記計測情報を受信可能である、請求項１〜６の何れか１項に記載の地図作成装置。
前記計測情報取得部は、時刻ｔにおいてｉ番目（ｉは２以上の自然数）の計測点で測定された前記計測物の位置と、時刻ｔ−１においてｉ−１番目の計測点で測定された前記計測物の位置とを比較した場合に、前記時刻ｔにおける位置と前記時刻ｔ−１における位置とが一致している計測物の位置のみを示す情報を、前記時刻ｔにおける前記計測情報として受信する、請求項１〜７の何れか１項に記載の地図作成装置。