JP2018084995A - 経路データ生成装置、それを備える移動体及び経路データ生成方法 - Google Patents

Abstract

【課題】Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術を改良することで比較的に誤差の小さい経路データを生成する。【解決手段】経路データ生成装置は、環境情報記憶部と、センサ位置情報記憶部と、センサノードと、センサノード間の相対的位置関係を規定するセンサエッジと、環境ノードと、環境ノードと対応するセンサノードの相対的位置関係を規定する環境エッジから構成されるグラフ構造データを生成するグラフ構造データ生成部と、計測物の位置の真値情報を記憶する真値情報記憶部と、真値情報が規定する計測物の位置と対応する環境ノードが規定する計測物の位置とのずれを規定する計測物エッジをグラフ構造データに追加する計測物エッジ追加部と、各エッジから算出される誤差関数の和が最小となるようにグラフ構造データを最適化するグラフ構造データ最適化部と、最適化されたグラフ構造データ内のセンサノードを用いて経路データを作成する経路データ生成部を備えている。【選択図】図１

Description

本明細書に開示する技術は、移動体を対象領域内で所望の経路上を移動させるための経路データを生成する経路データ生成装置及びそれを備える移動体に関する。

対象領域内の所望の経路を自動で移動する移動体が開発されている。移動体が所望の経路を移動するためには、経路データを生成する必要がある。経路データは、例えば、経路上の複数の点の座標値（ｘ_ｉ，ｙ_ｉ）（ｉ＝１〜ｎ）で表される。経路データを生成するには、例えば、経路上の複数の点の座標値を測量器で計測する方法が考えられる。しかしながら、経路上の複数の点の座標値を測量器で計測する方法では、精度の高い経路データが生成できるものの、測量作業に多大な時間を要するという問題が生じる。そこで、環境地図作成時に取得される移動体の自己位置推定座標を利用して経路データを生成する技術が開発されている（特許文献１）。特許文献１の技術では、ユーザの操作によって移動体を対象領域内で移動させ、その移動中にレーザレンジファインダ（ＬＲＦ）等のセンサによって取得された環境情報から対象領域の環境地図を作成する。また、ユーザの操作によって移動体が移動する際、移動体が所定の設定ポイントに位置しているときの自己位置推定座標を記憶（教示）する。そして、作成された環境地図と、記憶した設定ポイントの自己位置推定座標を用いて、移動体が移動するための経路データを作成する。この方法によると、移動体の自己位置推定座標を用いて経路データを生成するため、短時間で効率的に経路データを生成することができる。

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

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

特許文献１の技術では、環境地図と自己位置推定座標を用いて経路データを生成する。このため、経路データの精度は、環境地図の精度に依存することとなる。ＬＲＦ等のセンサによって取得した環境情報に基づいて環境地図を生成する場合、センサの計測誤差等によって、生成された環境地図に誤差が生じる。誤差が含まれた環境地図を用いるため、生成された経路データにも誤差が含まれ、精度の高い経路データを生成することが難しいという問題があった。本明細書は、Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術を改良することで、比較的に誤差の小さい経路データを生成する技術を開示する。

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

上記の経路データ生成装置では、真値情報と環境情報とのずれを規定する計測物エッジをグラフ構造データに追加することによって、グラフ構造データに真値情報が取込まれる。そして、真値情報が取込まれたグラフ構造データに含まれる誤差情報から算出される誤差関数の和が最小となるようにグラフ構造データを最適化する。そして、真値情報が取込まれて最適化されたグラフ構造データ内のセンサノードに基づいて経路データを生成する。これにより、従来のＧｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術を用いて作成した経路データと比較して精度の高い経路データ作成することができる。

また、本明細書に開示する経路データ生成方法は、移動体を対象領域内で所望の経路上を移動させるための経路データを生成する。この方法は、環境情報を計測するセンサを備えた移動体を経路上を移動させる移動工程と、移動体が経路上を移動するときに、経路上の複数の計測点のそれぞれにおいて、センサによって環境情報を計測する環境情報計測工程と、複数の計測点のそれぞれにおいて、計測時の前記センサの位置に関するセンサ位置情報を取得するセンサ位置情報取得工程と、計測した環境情報と取得したセンサ位置情報に基づいて、複数の計測点のそれぞれにおけるセンサの位置を規定するセンサノードと、隣接するセンサノード間におけるセンサの相対的位置関係を規定するセンサエッジと、複数の計測点のそれぞれにおいてセンサで計測された計測物の位置を規定する環境ノードと、環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する環境エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成工程と、対象領域内に位置する計測物の中から選択された少なくとも１つの計測物の位置に関する真値情報を取得する真値情報取得工程と、取得した真値情報と、その真値情報に対応する環境ノードに基づいて、真値情報が規定する計測物の位置と対応する環境ノードが規定する計測物の位置とのずれを規定する計測物エッジをグラフ構造データに追加する計測物エッジ追加工程と、計測物エッジが追加されたグラフ構造データについて、各エッジから算出される誤差関数の和が最小となるようにグラフ構造データを最適化するグラフ構造データ最適化工程と、最適化されたグラフ構造データ内のセンサノードを用いて経路データを作成する経路データ作成工程と、を備えている。この方法によると、精度の高い経路データを生成することができる。

実施例１に係る移動体の構成を示す図。 環境地図及び経路データを生成する処理を示すフローチャート。 移動体に搭載されるセンサが対象領域内の計測物を計測する構成を模式的に示す図。 センサノード及びセンサエッジを模式的に示す図。 グラフ構造データを模式的に示す図。 測量器が対象領域内の計測物の特徴的な形状を示す地点を計測する構成を模式的に示す図。 測量器で計測した真値情報及び計測物エッジが追加されたグラフ構造データを模式的に示す図。 実施例１の移動体が自動で移動する処理を示すフローチャート。 移動経路の変形例を示す図。 測量用レーザスキャナが対象領域内の計測物の特徴的な形状部分（マーカー）を計測する構成を模式的に示す図。 測量用レーザスキャナで計測した真値情報及び計測物エッジを追加したグラフ構造データを模式的に示す図。

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

（特徴１）本明細書に開示する経路データ生成装置では、最適化されたグラフ構造データ内の環境ノードを用いて環境地図を作成する環境地図作成部をさらに備えていてもよい。真値情報が取り込まれて最適化されたグラフ構造データを用いるため、精度の高い環境地図を作成することができる。

（特徴２）本明細書に開示する経路データ生成装置では、真値情報は、測量、測定及び図面の少なくとも１つから取得されてもよい。

（特徴３）本明細書が開示する経路データ生成装置では、真値情報記憶部は、少なくとも３つの座標で示される真値情報を記憶していてもよい。このような構成によると、少なくとも３点の真値情報に基づいて計測物ノードを生成することができる。このため、より誤差の小さい経路データを生成することができる。

（特徴４）本明細書が開示する経路データ生成装置では、真値情報記憶部は、位置、姿勢及び形状で示される真値情報を記憶していてもよい。このような構成によると、位置、姿勢及び形状を備える真値情報に基づいて計測物ノードを生成することができる。このため、より誤差の小さい経路データを生成することができる。

以下、実施例１に係る地図及び経路データ生成装置１０を備える移動体４０について説明する。移動体４０は、車輪駆動型の移動体である。移動体４０は、地図及び経路データ生成装置１０と、センサ４２と、操作部４４と、制御演算部４６と、駆動部４８と、自己位置推定部５０を備えている。

地図及び経路データ生成装置１０は、例えば、ＣＰＵ，ＲＯＭ，ＲＡＭ等を備えたコンピュータによって構成することができる。コンピュータがプログラムを実行することで、地図及び経路データ生成装置１０は、図１に示すグラフ構造データ生成部１４、計測物エッジ追加部１８、グラフ構造データ最適化部２０、環境地図生成部２２、経路データ生成部２４等として機能する。地図及び経路データ生成装置１０の各部１４、１８、２０、２２、２４の処理については、後で詳述する。

また、図１に示すように、地図及び経路データ生成装置１０は、環境計測データ記憶部１２と、真値情報記憶部１６を備えている。環境計測データ記憶部１２は、センサ４２及び駆動部４８のエンコーダによって計測された対象領域内の環境情報及びセンサ位置情報を記憶する。移動体４０は対象領域内を移動しながら、センサ４２によって対象領域内の環境（例えば、壁等の計測物）を計測する。センサ４２によって取得された環境情報は、その環境情報が取得されたときのセンサ４２の位置と共に環境計測データ記憶部１２に記憶される。真値情報記憶部１６は、測量器５６によって計測された対象領域内の真値情報を記憶する。環境計測データ記憶部１２に記憶される環境情報及びセンサ位置情報と、真値情報記憶部１６に記憶される真値情報については、後で詳述する。なお、地図及び経路データ生成装置１０は、「経路データ生成装置」の一例である。

センサ４２は、環境情報を計測するセンサであり、レーザレンジファインダ（ＬＲＦ）である。ＬＲＦは、レーザ光を射出し、射出したレーザ光が物体で反射して返ってくるまでの時間を計測する。ＬＲＦで計測された時間から、ＬＲＦから物体までの距離が計測される。また、ＬＲＦからレーザ光を射出した方向（すなわち、物体から反射されるレーザ光の入射角度）は既知であるため、ＬＲＦに対する物体の方位を決定することができる。ＬＲＦは、計測地点から見た壁や柱等（以下、「計測物６２」という）の位置、姿勢及び形状等の環境情報を計測することができる。なお、センサ４２はＬＲＦに限定されるものではない。センサ４２は、環境情報を計測できればよく、例えば、センサ４２は画像情報によって計測物６２を計測するものであってもよい。センサ４２は、計測した環境情報を環境計測データ記憶部１２及び自己位置推定部５０に出力する。

操作部４４は、移動体４０を操縦するための入力手段であり、ジョイスティック等のリモコンである。操作部４４は、作業者の指示に基づいて移動体４０を操作する操作信号を生成し、制御演算部４６に出力する。なお、本実施例では、操作部４４はリモコンであるが、このような構成に限定されない。例えば、操作部４４は作業者が運転することによって移動体４０を操縦するハンドルであってもよい。また、操作部４４を用いずに、作業者が移動体４０を手で押してもよい。

制御演算部４６は、移動体４０の移動を制御する。制御演算部４６は、操作部４４から出力される操作信号や経路データ生成部２４から出力される経路データに基づいて、移動体４０の車輪を駆動する制御信号を生成する。生成された制御信号は、制御演算部４６から駆動部４８に出力される。

駆動部４８は、車輪と、モータと、エンコーダを備えている。駆動部４８は、制御演算部４６から出力される制御信号に基づいてモータを動作させ、モータの動作によって車輪を駆動させる。車輪が駆動すると、エンコーダが車輪の回転角を検出する。駆動部４８は、エンコーダで検出した車輪の回転角を環境計測データ記憶部１２及び自己位置推定部５０に出力する。

自己位置推定部５０は、移動体４０の自己位置を推定する。自己位置推定部５０は、センサ４２から環境情報を取得し、駆動部４８から車輪の回転角を取得し、環境地図生成部２２から環境地図を取得する。自己位置推定部５０は、取得した環境情報及び車輪の回転角に基づいて、環境地図生成部２２で生成された環境地図上の自己位置を推定する。自己位置を推定する方法としては、公知の方法を用いることができる。

図２〜図７を参照して、地図及び経路データ生成装置１０が行う処理について説明する。図２は、地図及び経路データ生成装置１０が行う環境地図及び経路データの生成処理を示すフローチャートである。図２に示すように、まず、地図及び経路データ生成装置１０は、対象領域内の複数の計測地点Ａ〜Ｈ（図３参照）においてセンサ４２で計測された環境情報及び駆動部４８のエンコーダで検出された車輪の回転角を取得し、各計測地点Ａ〜Ｈにおける環境情報及びセンサ位置情報を環境計測データ記憶部１２に記憶する（Ｓ１０）。

環境情報は、センサ４２から取得される。センサ位置情報は、駆動部４８のエンコーダで検出された車輪の回転角に基づいて以下のように算出される。上述したように、駆動部４８のエンコーダが車輪の回転角を検出する。エンコーダによって車輪の回転角を検出することで、移動体４０の移動方向及び移動量を算出することができる。このため、移動体４０は、算出された移動体４０の移動方向及び移動量に基づいて、各計測地点における移動体４０の姿勢及び位置を算出することができる。移動体４０へのセンサ４２の設置状態は既知であるため、算出された各計測地点における移動体４０の姿勢及び位置に基づいて、各計測地点におけるセンサ４２の姿勢及び位置を算出することができる。算出された各計測地点におけるセンサ４２の姿勢及び位置は、その計測点においてセンサ４２で計測された環境情報に対応付けて記憶される。本実施例では、センサ４２が計測する環境情報及び算出されるセンサ位置情報を合わせて「環境計測データ」と称することがある。センサ４２によって計測された計測物６２の環境情報は、環境計測データ記憶部１２に時系列で記憶される。なお、センサ４２の位置及び姿勢は、エンコーダによって検出された車輪の回転角から算出する方法に限定されない。例えば、ＧＰＳセンサ等を用いて検出するようにしてもよい。

ここで、環境情報及びセンサ位置情報を取得する手順について説明する。図３は、移動体４０に搭載されるセンサ４２が、対象領域内の計測物６２を計測する状態を模式的に示している。移動体４０は、対象領域内に設定される移動経路６０上を移動する。具体的には、作業者は、移動体４０が所望の移動経路６０上を移動するように操作部４４を操作する。すなわち、作業者は操作部４４を操作して、所望の移動経路６０上を移動体４０が実際に移動するようにする。操作部４４が、作業者の指示に基づいて操作信号を生成すると、その生成された操作信号は制御演算部４６に出力され、制御演算部４６は制御信号を生成する。生成された制御信号は駆動部４８に出力され、駆動部４８は制御信号に基づいて移動経路６０上を移動する。対象領域内には、壁や柱等（すなわち、計測物６２）が配置されており、センサ４２は、計測地点から計測物６２までの距離及び計測物６２の形状等を計測する。図３に示される矢印は、移動体４０が計測地点Ｂに移動したときにセンサ４２から計測物６２ａに射出されるレーザと、移動体４０が計測地点Ｄに移動したときにセンサ４２から計測物６２ｂ、６２ｃに射出されるレーザを示している。なお、以下の明細書において、計測物６２を区別する必要があるときは計測物６２ａ、６２ｂ、６２ｃのように沿字のアルファベットを用いて記載し、区別する必要のないときは単に計測物６２と記載する場合がある。また、他の構成要素についても同一構成のものについて区別する必要がないときは、上記と同様に沿字のアルファベットを省略して単に数字で記載することがある。

図３に示すように、例えば、センサ４２は、地点Ｂにおいて計測可能な計測物６２ａの環境情報を計測する。また、駆動部４８のエンコーダが検出した車輪の回転角に基づいて、地点Ｂにおける計測時のセンサ位置情報を算出する。計測された環境情報と算出されたセンサ位置情報は環境計測データ記憶部１２に記憶される。また、移動体４０が地点Ｄに移動すると、センサ４２は、地点Ｄにおいて計測可能な計測物６２ｂの環境情報及び計測物６２ｃの環境情報を計測し、また、地点Ｄにおける計測時のセンサ位置情報が算出される。このようにして、移動体４０は、移動経路６０上を移動し、センサ４２は、対象領域内の計測物６２を計測し、その計測物６２を計測した地点のセンサ位置情報が算出される。計測された環境情報と算出されたセンサ位置情報は、時系列順に環境計測データ記憶部１２に蓄積される。なお、環境計測データ記憶部１２は、「環境情報記憶部」及び「センサ位置情報記憶部」の一例である。

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

すなわち、図４に示すように、グラフ構造データ生成部１４は、まず、環境計測データ記憶部１２が記憶するセンサ位置情報に基づいて、各計測地点Ａ〜Ｈにおける計測時のセンサ４２の位置や姿勢を規定するセンサノード３０ａ〜３０ｈをそれぞれ生成する。なお、隣接する計測地点の間隔は、対象領域内に設置されている複数の計測物６２の環境情報を計測できる間隔であればよい。隣接する計測地点の間隔が狭すぎると、計測地点の数が多くなり、後述するように各計測地点の姿勢誤差の蓄積が大きくなる。このため、隣接する計測地点の間隔は、対象領域内に設置される複数の計測物６２を計測することができる範囲で広くすることができる。

次に、グラフ構造データ生成部１４は、環境計測データ記憶部１２に記憶される時系列に蓄積されたセンサ位置情報及び環境情報に基づいて、隣接するセンサノード３０間におけるセンサ４２の相対的位置関係を規定するセンサエッジ３２を生成する。センサエッジ３２は、センサエッジ３２の両端にある２つのセンサノード３０の各姿勢についての制約を表しており、具体的には、運動誤差の共分散行列と相対位置・姿勢変換行列から表される公知の方法を用いて表すことができる。

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

次に、グラフ構造データ生成部１４は、環境情報に基づいて、各計測地点Ａ〜Ｈにおいて計測された計測物６２の特徴（例えば、位置、姿勢及び形状等）を規定する環境ノード３４を生成する。また、グラフ構造データ生成部１４は、環境情報及びセンサ位置情報に基づいて、環境ノード３４とその環境ノード３４を規定する環境情報を計測した位置のセンサノード３０との相対的位置関係を規定する環境エッジ３６を生成する。環境エッジ３６は、センサノード３０とそのセンサノード３０の位置から計測された環境情報によって規定された環境ノード３４との間の姿勢の制約を表している。具体的には、環境エッジ３６は、計測誤差の共分散行列と相対位置・姿勢変換行列から表される公知の方法を用いて表すことができる。

具体的には、図５に示すように、グラフ構造データ生成部１４は、計測地点Ｂにおいてセンサ４２によって計測された環境情報から、環境ノード３４ａを生成する。環境ノード３４ａは、計測地点Ｂから計測された計測物６２ａの特徴（例えば、位置、姿勢及び形状等）を規定している。また、グラフ構造データ生成部１４は、計測地点Ｂにおける環境情報及びセンサ位置情報から、センサノード３０ｂと環境ノード３４ａの間に環境エッジ３６ａを生成する。環境エッジ３６ａは、環境ノード３４ａとセンサノード３０ｂの相対的位置関係を規定している。すなわち、環境エッジ３６ａは、計測地点Ｂから環境ノード３４ａを計測したという事象を表している。同様にして、グラフ構造データ生成部１４は、計測地点Ｃにおいてセンサ４２によって計測された環境情報から、環境ノード３４ｂを生成する。また、グラフ構造データ生成部１４は、計測地点Ｃにおける環境情報及びセンサ位置情報から、センサノード３０ｃと環境ノード３４ｂの間に環境エッジ３６ｂを生成する。環境エッジ３６ｂは、環境ノード３４ｂとセンサノード３０ｃとの相対的位置関係を規定しており、計測地点Ｃから環境ノード３４ｂを計測したという事象を表している。

上記の説明から明らかなように、計測物６２ａは、計測地点Ｂから計測されると共に、計測地点Ｃからも計測される。しかしながら、図５に示すように、計測地点Ｂから計測した計測物６２ａの特徴を規定する環境ノード３４ａと、計測地点Ｃから計測した計測物６２ａの特徴を規定する環境ノード３４ｂが一致しないことがある。これは、計測地点Ｂからセンサ４２によって計測された環境情報の姿勢誤差等と、計測地点Ｃからセンサ４２によって計測された環境情報の姿勢誤差等の蓄積によるものである。

同様にして、グラフ構造データ生成部１４は、計測地点Ａから計測した計測物６２ｅの特徴を規定する環境ノード３４ｃを生成し、環境ノード３４ｃとセンサノード３０ａの相対的位置関係を規定する環境エッジ３６ｃを生成する。また、グラフ構造データ生成部１４は、計測地点Ｄから計測した計測物６２ｂの特徴を規定する環境ノード３４ｄを生成し、環境ノード３４ｄとセンサノード３０ｄの相対的位置関係を規定する環境エッジ３６ｄを生成する。さらに、グラフ構造データ生成部１４は、計測地点Ｄから計測した計測物６２ｃの特徴を規定する環境ノード３４ｅを生成し、環境ノード３４ｅとセンサノード３０ｄの相対的位置関係を規定する環境エッジ３６ｅを生成する。このようにして、グラフ構造データ生成部１４は、各計測地点から計測した計測物６２の特徴を規定する環境ノード３４を生成し、環境ノード３４とその環境ノード３４とを関連付ける環境エッジ３６を生成する。センサノード３０、センサエッジ３２、環境ノード３４及び環境エッジ３６を生成する際には、グラフ構造データ内に仮の原点を設定し、仮の原点を基準に環境情報及びセンサ位置情報を座標変換すればよい。

グラフ構造データ生成部１４は、生成したセンサノード３０、センサエッジ３２、環境ノード３４及び環境エッジ３６から、対象領域内のグラフ構造データを生成する。また、グラフ構造データ生成部１４は、グラフ構造データ内の地点Ｉ〜Ｋに対応する地点（以下、「グラフ構造データ内の地点Ｉ〜Ｋ」ともいう）について、仮の原点を基準とする座標を算出する。地点Ｉ〜Ｋは、グラフ構造データを構成する環境ノード３４のうち、凹凸がある等の特徴的な形状となっている部分に対応する計測物６２の形状を示す地点である。すなわち、地点Ｉ〜Ｋは、計測物６２の特徴的な形状を示す地点となる。例えば、地点Ｉは、計測物６２ｅの角部と一致する地点を示しており、地点Ｊは、計測物６２ｂの角部と一致する地点を示しており、地点Ｋは、計測物６２ｃの角部と一致する地点を示している。図５に示す実施例においては、仮の原点（０，０）を基準にすると、グラフ構造データ内の地点Ｉの座標は（９，１０）となり、グラフ構造データ内の地点Ｊの座標は（５３，１０）となり、グラフ構造データ内の地点Ｋの座標は（５０，−５）となっている。

次に、地図及び経路データ生成装置１０は、真値情報記憶部１６に、対象領域内の計測物６２の特徴的な形状を示す地点Ｉ〜Ｋの真値情報を記憶させる（Ｓ１４）。真値情報記憶部１６は、真値情報を測量器５６から取得する。

真値情報は、基本測量や公共測量に用いられる測量器５６によって計測される。測量器５６は、図示しないレーザセンサを備えており、反射板までの距離を計測することによって、反射板が設置される位置を精度良く測量する。なお、本実施例では、真値情報は、測量器５６を用いて計測しているが、このような構成に限定されない。真値情報を計測する精度が高い方法であればよく、例えば、ＡＲマーカーやＱＲコード（登録商標）等をカメラで計測してもよいし、ＧＰＳ測量や航空測量を用いてもよい。また、メジャーやコンベックス等を用いて測定してもよいし、ＣＡＤ図等の座標値を示す図面を用いてもよい。

測量器５６を用いて計測する際は、まず、図６に示すように、対象領域内の地点Ｉ〜Ｋに図示しない反射板をそれぞれ設置する。次いで、測量器５６を用いて、地点Ｉ〜Ｋに設置される反射板の位置（すなわち、測量器５６からの距離と方位）を計測する。そして、計測結果を用いて、グラフ構造データの仮の原点の位置と対応する位置を真値の原点として、計測地点Ｉ〜Ｋの座標を算出する。例えば、図６では、測量器５６の計測結果に基づいて算出される地点Ｉの座標は（１０，８）となり、測量器５６の計測結果に基づいて算出される地点Ｊの座標は（５５，８）となり、測量器５６の計測結果に基づいて算出される地点Ｉの座標は（５０．４，−７．３）となる。なお、本実施例では、測量器５６によって３つの地点Ｉ〜Ｋを計測しているが、測量器５６によって計測する地点の数はこれに限定されない。例えば、測量器５６を用いて３つより多くの地点を計測してもよい。真値情報記憶部１６は、測量器５６から地点Ｉ〜Ｋの真値情報を取得し記憶する。また、本実施例では、ステップＳ１４はステップＳ１２の後に行われているが、これに限定されない。ステップＳ１４は，ステップＳ１０の前に行われてもよいし、ステップＳ１０とステップＳ１２の間に行われてもよい。

次に、地図及び経路データ生成装置１０は、計測物エッジ追加部１８によって、計測物エッジ３８をグラフ構造データ内に追加する（Ｓ１６）。計測物エッジ３８は、真値情報による計測物６２の位置と、その位置に対応するグラフ構造データ内の位置とのずれを規定する。図７に示すように、計測物エッジ追加部１８は、まず、真値情報記憶部１６に記憶される地点Ｉ〜Ｋの真値の座標を、グラフ構造データ生成部１４によって生成されたグラフ構造データに追加する。次いで、計測物エッジ追加部１８は、追加された地点Ｉ〜Ｋの真値の座標と、グラフ構造データ内の地点Ｉ〜Ｋの座標に基づいて、両座標のずれを規定する計測物エッジ３８を生成する。計測物エッジ追加部１８は、生成した計測物エッジ３８をグラフ構造データに追加する。

図７では、グラフ構造データ内の地点Ｉの座標は（９，１０）となっている一方、追加した真値情報による地点Ｉの座標は（１０，８）となっている。計測物エッジ３８ａは、この両座標のずれを規定している。同様にして、計測物エッジ３８ｂは、グラフ構造データ内の地点Ｊの座標（５３，１０）と、追加した真値情報による地点Ｊの座標（５５，８）のずれを規定している。計測物エッジ３８ｃは、グラフ構造データ内の地点Ｋの座標（５０，−５）と、追加した真値情報による地点Ｋの座標（５０．４，−７．３）のずれを規定している。計測物エッジ３８を追加することによって、グラフ構造データに真値情報を取り込むことができる。

次に、地図及び経路データ生成装置１０は、グラフ構造データ最適化部２０によって、真値情報及び計測物エッジ３８が追加されたグラフ構造データを最適化する（Ｓ１８）。具体的には、グラフ構造データ最適化部２０は、グラフ構造データ内のセンサエッジ３２、環境エッジ３６及び計測物エッジ３８に基づいて算出される誤差関数の和が最小となるようにグラフ構造データを最適化する。誤差関数は、Ｇｒａｐｈ−ｂａｓｅｄ ＳＬＡＭ技術で用いられている、公知の方法を用いて生成することができ、また、最適化方法についても、最急降下法や共役勾配法等の最小二乗法等の公知の方法を用いることができる。グラフ構造データ最適化部２０は、まず、グラフ構造データ内の全てのエッジ（センサエッジ３２、環境エッジ３６及び計測物エッジ３８）から誤差関数を算出し、これら算出された誤差関数を加算し、次いで、この誤差関数の和が最小となるように最適化する。最適化されたグラフ構造データでは、真値の原点がグラフ構造データの原点となる。センサエッジ３２及び環境エッジ３６に加え、計測物エッジ３８を用いて最適化することによって、真値情報を取り込んだグラフ構造データを最適化することができる。

次に、地図及び経路データ生成装置１０は、環境地図生成部２２によって、グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて環境地図を生成させる（Ｓ２０）。具体的には、環境地図生成部２２は、グラフ構造データ最適化部２０によって最適化されたグラフ構造データを構成する環境ノード３４を用いて環境地図を生成する。グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて環境地図を生成することによって、環境地図全体の精度を高めることができる。

最後に、経路データ生成部２４は、グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて移動体４０の経路データを生成する（Ｓ２２）。具体的には、経路データ生成部２４は、グラフ構造データ最適化部２０によって最適化されたグラフ構造データを構成するセンサノード３０を用いて経路データを生成する。このとき、隣接するセンサノード間を結んで経路データを生成する。すなわち、移動経路６０上の計測点が教示ポイントとなり、各教示ポイントを通過するための経路データが生成される。隣接する計測点の間の経路データは、公知の方法（例えば、内挿法等）で生成することができる。グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて経路データを生成することによって、経路データ全体の誤差を小さくすることができる。なお、本実施例では、ステップＳ２２はステップＳ２０の後に行われているが、これに限定されない。ステップＳ２２は，ステップＳ２０の前に行われてもよいし、ステップＳ２２とステップＳ２０は、同時に行われてもよい。

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

図８を参照して、移動体４０が地図及び経路データ生成装置１０で生成された環境地図及び経路データに基づいて移動する手順を説明する。図８に示すように、まず、自己位置推定部５０は、環境地図生成部２２で生成された環境地図を取得する（Ｓ３０）。また、制御演算部４６は、経路データ生成部２４で生成された経路データを取得する（Ｓ３２）。

次に、自己位置推定部５０は、センサ４２から環境情報を取得する（Ｓ３４）。そして、自己位置推定部５０は、センサ４２から取得した環境情報とステップＳ３０で取得した環境地図に基づいて、環境地図上における移動体４０の位置を推定する（Ｓ３６）。

次に、制御演算部４６は、自己位置推定部５０で推定された自己位置を取得する（Ｓ３８）。制御演算部４６は、取得した推定自己位置とステップＳ３２で取得した経路データに基づいて、移動体４０が位置している経路データ上の地点を算出する。そして、制御演算部４６は、経路データに基づいて移動体４０が移動するように、制御信号を生成する（Ｓ４０）。

次に、制御演算部４６は、生成した制御信号を駆動部４８に出力し、駆動部４８を駆動する（Ｓ４２）。ステップＳ４０で生成された制御信号に基づいて駆動部４８が駆動することによって、移動体４０は経路データに従って対象領域内を移動する。

次に、制御演算部４６は、移動体４０が最終目的位置に到達したか否かを判断する（Ｓ４４）。移動体４０が最終目的位置に到達していない場合（ステップＳ４４でＮＯの場合）、制御演算部４６は、自己位置推定部５０にセンサ４２からの環境情報を取得させる。すなわち、ステップＳ３４に戻って、ステップＳ３４からの処理が繰り返される。移動体４０が最終目的位置に到達した場合（ステップＳ４４でＹＥＳの場合）、制御演算部４６は、駆動部４８の駆動を終了し、移動体４０の移動が終了する。

本実施例の移動体４０は、地図及び経路データ生成装置１０で生成された環境地図に基づいて自己位置を推定すると共に、地図及び経路データ生成装置１０で生成された経路データに基づいて対象領域内を移動する。地図及び経路データ生成装置１０で生成された環境地図及び経路データは、真値情報が取込まれた状態で最適化されたグラフ構造データに基づいて生成されているため、精度が高くなっている。このため、移動体４０は、精度の高い環境地図に基づいて自己位置を推定することができ、推定される自己位置の誤差を小さくすることができる。また、移動体４０は、精度の高い経路データと誤差の小さい推定自己位置に基づいて移動するため、移動経路の誤差を小さくすることができる。

なお、移動体４０の構成は、本実施例のような構成に限定されない。例えば、移動体４０は、自己位置推定部５０を備えていなくてもよい。また、地図及び経路データ生成装置１０によって経路データを生成した移動体４０とは異なる移動体を移動させるために、生成した経路データを用いてもよい。

また、本実施例では、移動経路６０（図３参照）が周回経路であるため、生成される経路データは環状の経路データとなるが、このような構成に限定されない。例えば、図９に示すように、移動体４０の移動経路は、始点と終点を有する線状の移動経路６１であってもよい。このとき、生成される経路データも線状の経路データとなる。また、生成される経路データが線状である場合には、移動体４０は、経路データに従って往復して移動するように制御されてもよいし、始点から終点まで移動した後に移動を終了するように制御されてもよい。

上述の実施例１では、真値情報記憶部１６が記憶する真値情報は、測量器５６によって計測された対象領域内の地点Ｉ〜Ｋの座標であったが、このような構成に限定されない。例えば、真値情報記憶部１６が記憶する真値情報は、位置及び姿勢等に関する情報を含む、対象領域内の特徴的な形状部分の情報であってもよい。以下、本実施例では、真値情報として用いる位置及び姿勢等に関する情報を含む特徴的な形状部分を「マーカー」と称することがある。図２、図１０及び図１１を参照して、本実施例において地図及び経路データ生成装置１０が行う処理について説明する。なお、本実施例では、地図及び経路データ生成装置１０で生成された環境地図及び経路データに基づいて移動体４０が移動する手順については実施例１と同様であるため、詳細な説明は省略する。また、図２のステップＳ１０とステップＳ１２についても実施例１と同様であるため、詳細な説明は省略する。

地図及び経路データ生成装置１０は、真値情報記憶部１６に、対象領域内の計測物６２が備える特徴的な形状部分（すなわち、マーカー）の真値情報を記憶させる（Ｓ１４）。真値情報記憶部１６は、真値情報を測量用レーザスキャナ５８（基本測量や公共測量に用いられる機器の一種）から取得することができる。

測量用レーザスキャナ５８は、図示しないレーザセンサを備えており、レーザセンサから計測物６２までの距離を計測する。測量用レーザスキャナ５８のレーザセンサが計測物６２を測量する高さは、移動体４０のセンサ４２が計測物６２を計測する高さと同一となっている。測量用レーザスキャナ５８は、計測物６２の形状を計測することができる。なお、本実施例では、真値情報は、測量用レーザスキャナ５８を用いて計測しているが、このような構成に限定されない。真値情報を座標だけでなく位置及び姿勢に関する情報を含む形状として計測することができる計測機器であれば用いることができる。

図１０に示すように、測量用レーザスキャナ５８によって計測された情報から、対象領域内の地点Ｉを含む計測物６２ｅの一部分の形状６４ａに関する情報（以下、マーカー６４ａともいう）、地点Ｊを含む計測物６２ｅの一部分の形状６４ｂに関する情報（以下、マーカー６４ｂともいう）、地点Ｋを含む計測物６２ｃの一部分の形状６４ｃに関する情報（以下、マーカー６４ｃともいう）を抽出する。このとき、対象領域内において、グラフ構造データの仮の原点の位置と対応する位置を真値の原点として、マーカー６４ａ〜６４ｃについて算出する。マーカー６４ａは、地点Ｉを含む計測物６２ｅの一部分の位置、姿勢及び形状等を規定しており、マーカー６４ｂは、地点Ｊを含む計測物６２ｂの一部分の位置、姿勢及び形状等を規定しており、マーカー６４ｃは、地点Ｋを含む計測物６２ｃの一部分の位置、姿勢及び形状を規定している。真値情報記憶部１６は、測量用レーザスキャナ５８からマーカー６４ａ〜６４ｃに関する情報を取得して記憶する。なお、本実施例では、測量用レーザスキャナ５８によって計測された情報から３つのマーカー６４ａ〜６４ｃを抽出しているが、これに限定されない。例えば、測量用レーザスキャナ５８によって計測された情報から３つより多くのマーカーを抽出してもよい。

次に、地図及び経路データ生成装置１０は、計測物エッジ追加部１８によって、計測物エッジ３８をグラフ構造データ内に追加する（Ｓ１６）。計測物エッジ３８は、真値情報による計測物６２の位置と、その位置に対応するグラフ構造データ内の位置とのずれを規定する。

図１１に示すように、計測物エッジ追加部１８は、真値情報記憶部１６に記憶されるマーカー６４ａ〜６４ｃに関する情報を、グラフ構造データ生成部１４によって生成されたグラフ構造データに追加する。計測物エッジ追加部１８は、追加されたマーカー６４と、グラフ構造データ内のマーカー６４に対応する部分を含む環境ノード３４に基づいて、マーカー６４と対応する環境ノード３４とのずれ（すなわち、位置と姿勢のずれ）を規定する計測物エッジ３８を生成する。具体的には、マーカー６４ａと地点Ｉを含む計測物６２ｅの特徴を規定する環境ノード３４ｃに基づいて、両者のずれを規定する計測物エッジ３８ｄを生成する。マーカー６４ｂと地点Ｊを含む計測物６２ｂの特徴を規定する環境ノード３４ｄに基づいて、両者のずれを規定する計測物エッジ３８ｅを生成する。マーカー６４ｃと地点Ｋを含む計測物６２ｃの特徴を規定する環境ノード３４ｅに基づいて、両者のずれを規定する計測物エッジ３８ｆを生成する。計測物エッジ３８は、追加されたマーカー６４と対応する環境ノード３４との間の姿勢の制約を表す情報行列が含まれる。この情報行列の算出には、パターンマッチングやＩＣＰ（Ｉｔｅｒａｔｉｖｅ Ｃｌｏｓｅｓｔ Ｐｏｉｎｔ）等の公知の方法を用いることができる。計測物エッジ追加部１８は、生成した計測物エッジ３８をグラフ構造データに追加する。計測物エッジ３８を追加することによって、グラフ構造データに真値情報を取り込むことができる。

次に、地図及び経路データ生成装置１０は、グラフ構造データ最適化部２０に、真値情報及び計測物エッジ３８が追加されたグラフ構造データを最適化させる（Ｓ１８）。そして、地図及び経路データ生成装置１０は、環境地図生成部２２に、グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて環境地図を生成させ（Ｓ２０）、経路データ生成部２４に、グラフ構造データ最適化部２０によって最適化されたグラフ構造データに基づいて経路データを生成させる（Ｓ２０）。なお、本実施例では、ステップＳ１８〜Ｓ２２の処理については実施例１と同様であるため、詳細な説明は省略する。

本実施例においても、地図及び経路データ生成装置１０は、真値情報が規定する計測物６２の一部を示すマーカー６４と、そのマーカー６４に対応する環境ノード３４とのずれを規定する計測物エッジ３８をグラフ構造データに追加することによって、グラフ構造データに真値情報を取込むことができる。また、真値情報が取り込まれたグラフ構造データの各エッジ３２、２６、２８に基づいて算出される誤差関数の和が最小となるようにグラフ構造データを最適化する。これによって、環境地図全体の精度を高めることができると共に、経路データ全体の誤差を小さくすることができる。

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

１０：地図及び経路データ生成装置
１２：環境計測データ記憶部
１４：グラフ構造データ生成部
１６：真値情報記憶部
１８：計測物エッジ追加部
２０：グラフ構造データ最適化部
２２：環境地図生成部
２４：経路データ生成部
３０：センサノード
３２：センサエッジ
３４：環境ノード
３６：環境エッジ
３８：計測物エッジ
４０：移動体
４２：ＬＲＦ
４４：操作部
４６：制御演算部
４８：駆動部
５０：自己位置推定部
５６：測量器
５８：測量用レーザスキャナ
６０、６１：移動体の移動経路
６２：計測物
６４：マーカー

Claims (7)

1. 移動体を対象領域内で所望の経路上を移動させるための経路データを生成する装置であって、
   前記経路上の複数の計測点のそれぞれについて、センサによって計測された環境情報を記憶する環境情報記憶部と、
   前記複数の計測点のそれぞれについて、計測時の前記センサの位置に関するセンサ位置情報を記憶するセンサ位置情報記憶部と、
   前記環境情報及び前記センサ位置情報に基づいて、前記複数の計測点のそれぞれにおける前記センサの位置を規定するセンサノードと、隣接するセンサノード間における前記センサの相対的位置関係を規定するセンサエッジと、前記複数の計測点のそれぞれにおいて前記センサで計測された計測物の位置を規定する環境ノードと、前記環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する環境エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成部と、
   前記対象領域内に位置する前記計測物の中から選択された少なくとも１つの計測物の位置に関する真値情報を記憶する真値情報記憶部と、
   前記真値情報と、その真値情報に対応する環境ノードに基づいて、前記真値情報が規定する計測物の位置と前記対応する環境ノードが規定する計測物の位置とのずれを規定する計測物エッジを前記グラフ構造データに追加する計測物エッジ追加部と、
   前記計測物エッジが追加された前記グラフ構造データについて、各エッジから算出される誤差関数の和が最小となるように前記グラフ構造データを最適化するグラフ構造データ最適化部と、
   最適化された前記グラフ構造データ内のセンサノードを用いて前記経路データを作成する経路データ生成部と、を備えている、経路データ生成装置。
2. 最適化された前記グラフ構造データ内の環境ノードを用いて環境地図を作成する環境地図作成部をさらに備える、請求項１に記載の経路データ生成装置。
3. 前記真値情報は、測量、測定及び図面の少なくとも１つから取得される、請求項１又は２に記載の経路データ生成装置。
4. 前記真値情報記憶部は、少なくとも３つの座標で示される真値情報を記憶している、請求項１〜３のいずれか一項に記載の経路データ生成装置。
5. 前記真値情報記憶部は、位置、姿勢及び形状で示される真値情報を記憶している、請求項１〜３のいずれか一項に記載の経路データ生成装置。
6. 請求項１〜５のいずれか一項に記載の経路データ生成装置を備える移動体。
7. 移動体を対象領域内で所望の経路上を移動させるための経路データを生成する方法であって、
   環境情報を計測するセンサを備えた移動体を前記経路上を移動させる移動工程と、
   前記移動体が前記経路上を移動するときに、前記経路上の複数の計測点のそれぞれにおいて、前記センサによって環境情報を計測する環境情報計測工程と、
   前記複数の計測点のそれぞれにおいて、計測時の前記センサの位置に関するセンサ位置情報を取得するセンサ位置情報取得工程と、
   計測した前記環境情報と取得した前記センサ位置情報に基づいて、前記複数の計測点のそれぞれにおける前記センサの位置を規定するセンサノードと、隣接するセンサノード間における前記センサの相対的位置関係を規定するセンサエッジと、前記複数の計測点のそれぞれにおいて前記センサで計測された計測物の位置を規定する環境ノードと、前記環境ノードとその環境ノードに対応するセンサノードの相対的位置関係を規定する環境エッジとから構成されるグラフ構造データを生成するグラフ構造データ生成工程と、
   前記対象領域内に位置する前記計測物の中から選択された少なくとも１つの計測物の位置に関する真値情報を取得する真値情報取得工程と、
   取得した前記真値情報と、その真値情報に対応する環境ノードに基づいて、前記真値情報が規定する計測物の位置と前記対応する環境ノードが規定する計測物の位置とのずれを規定する計測物エッジを前記グラフ構造データに追加する計測物エッジ追加工程と、
   前記計測物エッジが追加された前記グラフ構造データについて、各エッジから算出される誤差関数の和が最小となるように前記グラフ構造データを最適化するグラフ構造データ最適化工程と、
   最適化された前記グラフ構造データ内のセンサノードを用いて前記経路データを作成する経路データ作成工程と、を備えている、経路データ生成方法。

Images