JP6893200B2

JP6893200B2 - 自走制御プログラム、および、自走式作業装置

Info

Publication number: JP6893200B2
Application number: JP2018195189A
Authority: JP
Inventors: 誠辻元
Original assignee: 株式会社きんでん
Priority date: 2018-10-16
Filing date: 2018-10-16
Publication date: 2021-06-23
Anticipated expiration: 2038-10-16
Also published as: JP2020064400A

Description

本発明は、自走制御プログラム、および、この自走制御プログラムを実行可能なコンピュータを備えた自走式作業装置に関するものである。

従来、周囲の障害物などを検出するセンサを備え、自律的に移動して作業を行う自走式作業装置が知られている。例えば、掃除機、空気清浄機、警備用の装置などがある。また、建築現場において、墨出し（マーキング）作業に利用可能なものが知られている（例えば、特許文献１参照）。

特許文献１の自走式作業装置では、自己の位置座標を求めて予め入力した移動順序に従って目的地まで自走し、停止後、搭載した作業機器の目的地からのずれを修正し、その後、作業機器による作業を実施することができるようになっている。なお、ずれの修正は、自走式作業装置の台車上に設けられたレーザー測距測角儀を用いて、床面の基準点に配置した反射ターゲットまでの距離を測定して台車の自己位置を求め、環境モデル内に設定した作業地点から台車までのずれ量を演算することにより行っている。

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

しかしながら、上記特許文献１の自走式作業装置では、作業現場において、台車の位置ずれを修正する際、実環境の床面に複数の反射ターゲットを設置する必要があるとともに、当該反射ターゲットの設置位置を正確に測定する必要がある。また、作業現場が変わるごとに、当該反射ターゲットの設置位置を変更する必要が生じる。これらのため、上記特許文献１の自走式作業装置では、設置作業の手間および時間がかかるだけでなく、運用に必要なコストが高騰化することがあった。

そこで、本発明は、自走式作業装置が目的地に向けて移動して作業を行う際、目的地の登録と、その目的地からの位置ずれを修正する際の作業の手間、時間、およびコストと、を従来よりも削減することができる機能を発揮する自走制御プログラム、および、この自走制御プログラムを実行可能なコンピュータを備えた自走式作業装置を提供することを目的とする。

（１）本発明は、自走部と、環境地図に基づいて前記自走部の動作を制御する動作制御部と、作業現場領域内における自己の周囲の実環境データを検出する実環境データ検出部と、前記実環境データ検出部によって得られた前記実環境データと、作業現場の環境を示す図面データとを少なくとも記憶する記憶部と、を備えた自走式作業装置における前記動作制御部を、
前記作業現場の環境を示す図面データから、前記環境地図の基となる基礎データを抽出する基礎データ抽出手段、
前記基礎データに基づいて、前記環境地図を構築する環境地図構築手段、
前記基礎データと前記実環境データとに基づき、SLAM技術を用いて前記環境地図における自己の方向および相対的な位置関係について大域的な自己位置の推定を行う大域的自己位置推定手段、
前記環境地図と前記実環境データとのスケールの一致度を計算し、前記環境地図と前記実環境データとを一致させる補正に用いられるスケール調整パラメータを探索するスケール調整パラメータアルゴリズムを用いて、真のスケール調整パラメータを推定するスケール調整パラメータ推定手段、
前記スケール調整パラメータ推定手段によって推定されたスケール調整パラメータに基づいて、前記環境地図と前記実環境データとのスケールを合致させる調整を行うスケール調整手段、
前記スケール調整手段による前記環境地図と前記実環境データとのスケールを合致させる調整が行われた後に、前記環境地図のデータを更新する環境地図更新手段、
前記環境地図更新手段によって更新された環境地図において、SLAM技術を用いて自己の現在位置を推定する現在位置推定手段、
として機能させる自走制御プログラムである。

（２）上記（１）の自走制御プログラムにおいて、前記環境地図構築手段は、前記基礎データに基づいて、前記環境地図について解像度が異なる複数の地図を構築するものであり、
前記大域的自己位置推定手段は、前記基礎データと前記実環境データとに基づき、SLAM技術を用いて前記複数の地図それぞれにおける自己の方向および相対的な位置関係について大域的な自己位置の推定を行うものであり、
前記スケール調整パラメータ推定手段は、前記複数の地図のそれぞれと前記実環境データとのスケールの一致度を計算し、前記一致度が所定以上となる範囲において、前記複数の地図のうち最も低い解像度の地図から順に高い解像度の地図について徐々に範囲を絞り込みながら真のスケール調整パラメータを探索する遺伝的アルゴリズムを用いて、真のスケール調整パラメータを推定するものであることが好ましい。

（３）上記（２）の自走制御プログラムにおいて、前記スケール調整パラメータ推定手段は、
前記大域的自己位置推定手段によって推定された自己位置の情報に基づいて自己の周辺に少なくとも１つの仮想の自己を配置し、
前記複数の地図のそれぞれと前記実環境データとのスケールの一致度が最大となる範囲を前記真のスケール調整パラメータが含まれる範囲とし、
前記真のスケール調整パラメータが含まれる範囲において、最大のスケール調整パラメータと最小のスケール調整パラメータとの平均値を、次に解像度が高い地図において真のスケール調整パラメータを推定する場合に用いることが好ましい。

（４）本発明の自走式作業装置は、上記（１）〜（３）に記載の自走制御プログラムを記憶した記憶部と、前記自走制御プログラムを実行する動作制御部を含むコンピュータと、を備えたものである。

本発明によれば、自走式作業装置が地図情報に基づき目的地に向けて移動して作業を行う際、その目的地からの位置ずれを自動的に修正できるとともに、作業の手間および時間を削減することができる機能を発揮する自走制御プログラム、および、この自走制御プログラムを実行可能なコンピュータを備えた自走式作業装置を提供することができる。また、本発明の自走式作業装置は、実環境データ検出部（たとえば２次元距離計測センサ）を備え、この実環境データ検出部で検出された実環境データと、建物などの作業現場の寸法図などに係る図面データ（たとえばＣＡＤ図またはＣＡＤ図から変換された画像ファイルなど）とに基づいて、自己位置推定および環境地図構築を行う機能を有している。したがって、自走式作業装置は、各目的地に対して同じ作業を繰り返し行う場合に好適に用いることができる。例えば、建築現場での墨出し作業、照明の照度測定作業、物流倉庫での運搬作業などに対して、本発明の自走式作業装置は利用可能である。

本発明の実施形態に係る自走式作業装置を示す斜視図である。本発明の実施形態に係る自走式作業装置および携帯通信端末の構成を示すブロック図である。図２に示した動作制御部における機能的構成を示すブロック図である。図２に示した動作制御部が実行する図面データからの自己位置推定処理を示すフローチャートである。図２に示した動作制御部が実行するスケール調整処理を示すフローチャートである。本発明の実施形態に係る環境地図における仮想の自走式作業装置の配置を示す概念図である。本発明の実施形態に係る自走式作業装置についての自己位置の推定の変形例の説明に用いた図である。本発明の実施形態に係る自走式作業装置の変形例を示す斜視図である。実施例１に係る照度計測実験における実験環境を示す図である。実施例１に係る自走式作業装置の自己位置推定の実験に関する環境地図を示した図である。実施例１に係る照度計測実験におけるスケール調整パラメータ推定および環境地図構築に関する結果を示す図である。

以下、本発明の自走式作業装置の一例として、照明の照度測定を自動的に行う自走式作業装置について図面を参照しつつ説明する。

＜装置構成＞
図１に示すように、本実施形態に係る自走式作業装置１００は、円盤形状の自走部１と、自走部１の上面に設けられた円柱形状の本体２と、本体２の上面に設けられた三脚３および２次元距離計測センサ（Laser Range Finder。以下、ＬＲＦ）１１と、照度測定を行うための照度計１２と、センサ１３と、を備えている。

自走部１は、下部に設けられた複数の車輪４（図１では図示せず）と、内部に設けられ、各車輪４をそれぞれ独立に駆動する複数のモータ５（図１では図示せず）と、を有している。したがって、自走式作業装置１００は、自走部１の駆動によって、配置された場所の床面上において前後左右の任意方向に移動できるようになっている。

本体２の内部には、図２に示すように、制御用パーソナルコンピュータ（以下、制御用ＰＣ）２０およびバッテリ６が設けられている。また、バッテリ６は、制御用ＰＣ２０、ＬＲＦ１１および自走部１に接続されており、電源供給を行うようになっている。

制御用ＰＣ２０は、動作制御部２１、記憶部２２、および無線通信部２３を有している。動作制御部２１は、ＣＰＵおよびＲＡＭを含み、ＬＲＦ１１、照度計１２、センサ１３、および自走部１の動作を制御する。

動作制御部２１では、ＣＰＵが、後述する記憶部２２に予め格納されＲＡＭに展開された各種制御プログラムに基づいて処理を実行することで、各構成部を有機的に動作させて自走式作業装置１００の各種機能を実行することができる。なお、動作制御部２１は、図３に示したように、基礎データ抽出部２１ａ、環境地図構築部２１ｂ、大域的自己位置推定部２１ｃ、スケール調整パラメータ推定部２１ｄ、スケール調整部２１ｅ、環境地図更新部２１ｆ、現在位置推定部２１ｇを備えているが、これらの他に、図示しない自走部制御部などの各種の機能部を備えている。

基礎データ抽出部２１ａは、作業現場の環境を示す図面データ（ＣＡＤ図面など）から、環境地図の基となる基礎データを抽出し、抽出した基礎データを記憶部２２に送信する。

環境地図構築部２１ｂは、様々な環境地図データを構築することが可能であり、本実施形態では、基礎データ抽出部２１ａで得た基礎データに基づいて、環境地図について解像度が異なる複数の地図データを構築し、構築した地図データを記憶部２２に送信する。

大域的自己位置推定部２１ｃは、上記基礎データと後述する実環境データとに基づき、SLAM（Simultaneous Localization and Mapping）技術を用いて、上記複数の地図データそれぞれにおける自己の方向および相対的な位置関係について大域的な自己位置の推定を行う。なお、大域的自己位置推定部２１ｃは、大域的な自己位置の推定結果のデータを記憶部２２に送信する。

スケール調整パラメータ推定部２１ｄは、上記複数の地図データのそれぞれと後述する実環境データとのスケールの一致度が所定以上となる範囲において、上記複数の地図データのうち最も低い解像度の地図データから順に高い解像度の地図データについて徐々に範囲を絞り込みながら、真のスケール調整パラメータを探索する後述の遺伝的アルゴリズムを用いて、真のスケール調整パラメータを推定する。なお、スケール調整パラメータ推定部２１ｄは、スケール調整パラメータの推定結果を記憶部２２に送信する。

スケール調整部２１ｅは、スケール調整パラメータ推定部２１ｄによって推定された後述のスケール調整パラメータに基づいて、環境地図と上記実環境データとのスケールを合致させる調整を行う。

環境地図更新部２１ｆは、スケール調整部２１ｅによる環境地図と上記実環境データとのスケールを合致させる調整が行われた後に、環境地図のデータを更新するとともに、更新した環境地図のデータを記憶部２２に送信する。

現在位置推定部２１ｇは、環境地図更新手段によって更新された環境地図において、SLAM技術を用いて自己の現在位置を推定し、推定した自己の現在位置のデータを記憶部２２に送信する。

記憶部２２は、ＲＯＭまたはハードディスクなどを含み、自走式作業装置１００の各種機能を実現するために必要な情報（実環境データ、図面データ、演算結果のデータなどを含む）、および、各種制御プログラム、などを記憶する。ここで、自走式作業装置１００の各種機能としては、例えば、現在位置の推定（自己位置推定）および周囲環境の地図作成（環境地図構築）を行う機能、障害物を回避して走行する機能、照明の照度測定を行う機能などである。また、各種制御プログラムとは、上記各機能を実現するためのプログラムであり、初期位置を含む現在位置の推定（自己位置推定）および周囲環境の地図作成（環境地図構築）の制御を行うモジュール（後述するMulti-resolution Mapを用いたSLAMアルゴリズム、後述するスケール値調整アルゴリズムなどを含む）、障害物を回避して走行する制御を行うモジュール、照明の照度測定の制御を行うモジュールなどを含むものである。また、無線通信部２３は、外部の機器と通信する無線通信回路である。

なお、制御用ＰＣ２０は、自走式作業装置１００の各種機能を実現するために必要な情報を、ＬＲＦ１１、照度計１２、およびセンサ１３の各構成部との間で、シリアル通信により送受信を行うようになっている。また、制御用ＰＣ２０と自走部１との間においても、シリアル通信により情報の送受信を行うようになっている。また、制御用ＰＣ２０は、ＵＳＢメモリ、ＳＤカードなどの記憶媒体から各種情報を読み込み、かつ、当該記憶媒体に書き込み可能な読込・書込装置（図示せず）を有している。

照度計１２は、三脚３の雲台３ａに取り付けられ、固定されている。この三脚３は、伸縮可能であり、測定条件に合わせて高さの調整が可能である。また、三脚３は、本体２の上面から取り外し可能であるので、自走式作業装置１００を分解して、容易に運搬することができるようになっている。

センサ１３は、本体２の側面であって、ＬＲＦ１１の測定方向側（自走式作業装置１００の進行方向側）に設けられている。

ＬＲＦ１１、照度計１２、およびセンサ１３は、それぞれが制御用ＰＣ２０の動作制御部２１と接続されている。ＬＲＦ１１は、レーザーを照射し、物体に反射して戻ってくるまでの時間から当該物体までの距離を計測するものであり、高精度で長距離まで計測可能なものである。ＬＲＦ１１の計測データは、実環境データとして、制御用ＰＣ２０の動作制御部２１へ送信される。また、照度計１２は、一般に市販されているものを使用可能である。なお、照度計１２において、出力されるアナログ値は、Ａ／Ｄ変換器を備えるマイクロコンピュータ（図示略）を介してデジタル値に変換されて、制御用ＰＣ２０の動作制御部２１へ送信されるようになっている。また、センサ１３は、ロボット進行方向前方の障害物を検知するものであり、例えば、ＣＣＤ、ＣＭＯＳなどのような固体撮像素子を用いるイメージセンサ、または、超音波や赤外線を用いるセンサを採用することができる。センサ１３により取得されたデータは、制御用ＰＣ２０の動作制御部２１へ送信される。

携帯通信端末３０は、端末制御部３１、端末記憶部３２、端末表示部３３、端末操作部３４、および端末通信部３５を備えている。なお、携帯通信端末３０の具体例としては、タブレットＰＣ端末、スマートフォンなどが挙げられる。

端末制御部３１は、ＣＰＵおよびＲＡＭを含み、携帯通信端末３０の動作を制御する。端末記憶部３２は、具体的にはフラッシュメモリなどの不揮発性半導体メモリである。端末表示部３３は、具体的には液晶表示装置を有しており、照度値、環境地図、操作用ボタンなどを表示する。端末操作部３４は、具体的には電源オンオフなどのスイッチと、端末表示部３３の表示面に配されるタッチパネルと、を有しており、作業者からの各種入力を受付可能になっている。端末通信部３５は、外部の機器と通信する無線通信回路であり、制御用ＰＣ２０が備える無線通信部２３と同様の無線通信方式を備えている。例えば、制御用ＰＣ２０と携帯通信端末３０との間でデータを送受信するために、ＴＣＰ／ＩＰを用いた通信方式を使用することができる。

なお、携帯通信端末３０において、作業者により開始ボタンが押されると、この操作信号が端末通信部３５を介して制御用ＰＣ２０に送信される。そして、制御用ＰＣ２０の動作制御部２１は、無線通信部２３を介してこの操作信号を受信し、ＬＲＦ１１を作動させて自走式作業装置１００自身と周囲の物体間の距離を計測して得たデータ、および、図面データ（ＣＡＤ図またはＣＡＤ図を他の方式の画像ファイルに変換して得たデータ）を用いて、Multi-resolution Mapを用いたSLAMアルゴリズムを含むプログラムによって、自己位置同定の処理および環境地図の構築の処理を行う。

＜動作制御部２１の制御処理＞
上記構成を有した自走式作業装置１００が照度測定作業を行う場合、動作制御部２１は、自己位置同定の処理および環境地図の構築の処理を行う。以下、具体例を用いながら説明する。

自走式作業装置１００における自己位置推定では、上述したとおり、動作制御部２１において、ＬＲＦ１１を作動させて自走式作業装置１００自身と周囲の物体間の距離を計測して得た実環境データ、および、図面データ（ＣＡＤ図またはＣＡＤ図を他の方式の画像ファイルに変換して得たデータ）を環境地図として読み込み、初期位置同定を行われなければならない。動作制御部２１において、図面データ（ＣＡＤ図またはＣＡＤ図を他の方式の画像ファイルに変換して得たデータ（たとえばＰＤＦ、ＪＰＥＧ、ＴＩＦＦ、ＢＭＰ、またはＧＩＦなどのファイル）が読み込まれた場合、図面データにおける画像ファイルのサイズ（ＣＡＤ図上でのスケール）はピクセル単位で表されるのに対して、計測されたＬＲＦデータは実空間の大きさであるｍｍ単位となっている。そのため、取り込んだ図面データ上で視覚的に目的地を設定し、かつ、目的地への到達誤差を小さくするためには、ピクセルの大きさを実空間上に合わせなければならない。ピクセル単位から実空間のｍｍ単位へ変換するパラメータであるスケール値を事前に設定することによって初期位置同定を実現できるが、当該設定に慣れない作業者が当該設定を行うことは困難である。また、ＣＡＤ図を他の方式の画像ファイルに変換して得た図面データについては、画像ファイル変換時の解像度変化により誤差が生じてしまう問題点も存在する。そこで、動作制御部２１は、図４に示すフローチャートに基づいて、上記問題点を解消した自己位置推定および環境地図構築の自動処理を行うものである。以下、具体的に当該処理について説明する。なお、図４中のｋは、後述するMulti-resolution Mapにおける環境地図の解像度のレベルを示し、ｋ＝１は、予め設定した複数の解像度のレベルのうち最も解像度のレベルが高いことを示す。すなわち、ｋの値が大きいほど、環境地図の解像度のレベルが低いということでもある。

まず、制御用ＰＣ２０に、たとえば、ＵＳＢメモリが装着され接続された場合、動作制御部２１は、ＵＳＢメモリ内に保存されている作業現場の図面データ（ＣＡＤ図またはＣＡＤ図を他の方式の画像データに変換して得たデータ）を読み込む。その後、基礎データ抽出部２１ａは、読み込んだ図面データから上記基礎データを抽出して記憶部２２に記憶させる。また、動作制御部２１は、ＬＲＦ１１を作動させて自走式作業装置１００自身と周囲の物体間の距離を計測して得た実環境データを読み込み、記憶部２２に記憶させる。続いて、環境地図構築部２１ｂによって、上記基礎データおよび実環境データに基づいて、解像度の異なる複数の環境地図を構築しておく。なお、上記図面データの読み込みは、上記実環境データの読み込みの後であってもよい。次に、これらの処理後、大域的自己位置推定部２１ｃは、後述する環境地図の解像度が更新可能なMulti-resolution Mapと進化戦略とを用いた大域的位置推定において、スケール値に誤差が生じている環境地図上において暫定的な初期位置を推定する（ステップＳ１）。

ここで、Multi-resolution Mapとは、たとえば、論文「No.11-5 Proceedings of the 2011 JSME Conference on Robotics and Mechatronics, Okayama, Japan, May 26-28, 2011 2P1-K07「Multi-resolution map を用いた未知環境の探査」戸田雄一郎、久保田直行」に掲載されている未知環境における探査手法で用いた２次元環境地図の一例である占有格子空間地図である。なお、一般的に占有格子空間地図を構築する際に、この地図の粒度は細かいほうが精確な地図を得ることができる。しかしながら，当該地図を用いて探査計画または経路計画などの行動計画を行うには、当該地図の粒度が細かく、一つ一つのセル（小区画）の値を参照していては計算コストが高くなってしまうという問題点が存在する。上記Multi-resolution Mapを用いれば、上記問題点を解決でき、自走式作業装置１００の動作制御部２１が、効率よく行動計画を立てるためにセルの粒度を適切なものにすることができる。また、構築されたMulti-resolution Mapを探査計画に利用することにより、効率のよい未知環境の探査ができる。

また、大域的位置推定に用いる進化戦略（Evolution Strategy：以下、ＥＳ）とは、自己位置推定解の探索に用いる進化計算の１つである。基本的にＥＳは、個体群を解集合とした多点探索手法の１つであり、交叉および突然変異といった遺伝的操作と適者生存に基づく選択を行なっていくことによって、解集合の更新を行ない、探索を行なっていくものである（遺伝的アルゴリズム）。大域的位置推定では、μ個の親個体、λ個の子個体から構成される個体群から次世代の親個体となるμ個体を選択する連続世代交代モデルの１つである（μ＋λ)-ＥＳを用いる。

ステップＳ１の処理の後、スケール調整パラメータ推定部２１ｄは、スケール調整における環境地図の解像度のレベルが最も高い解像度のレベル（ｋ＝１）となっているかどうか判定する（ステップＳ２）。スケール調整パラメータ推定部２１ｄによりｋ＝１でないと判定された場合（ステップＳ２：ＮＯ）、スケール調整パラメータ推定部２１ｄは、ステップＳ１の処理で推定された暫定的な初期位置を用いて、後述するスケール調整アルゴリズムによりパラメータα^Ｓを推定し、スケール調整部２１ｅは、推定されたパラメータα^Ｓを用いて環境地図のスケール調整処理を行う（ステップＳ３）。

ここで、図５に示すフローチャートを用いて、ステップＳ３のスケール調整処理におけるスケール調整アルゴリズムを用いた処理について説明する。スケール調整アルゴリズムは、進化計算に基づく探索として、上述した（μ＋λ)-ＥＳにおけるλ＝１とした（μ＋１)-ＥＳを用い、各個体をスケール調整のパラメータ（ｇ^Ｓ _ｋ）として表現し、各遺伝子に対して、遺伝的操作を行なっていくことによって、探索を行なっていくものである。また、スケール調整アルゴリズムは、Multi-resolution Mapにより、最初の場合を除いて解像度が高いものへ遷移した際に、１つ前の解像度で探索した結果に基づき探索範囲を設定し、探索の範囲を狭めることが可能である。

そこで、図５のステップＳ１１の個体の初期化においては、初期の解像度における探索と初期以外の探索とにおいて、個体の初期化で異なる戦略を用いる。具体的には、初期の解像度において、スケール値調整のパラメータα^Ｓの真値がどこに存在するかが不明であるため、スケール調整パラメータ推定部２１ｄは、解空間全体に対して、初期個体の生成を行う。一方で、初期以外の解像度以降の探索においては、前ステップにおけるスケール調整パラメータにおいて、ある程度の探索がなされていると仮定したものに予め設定しているため、スケール調整パラメータ推定部２１ｄは、初期個体の生成範囲として、（１−ｒ,１＋ｒ）の範囲内において個体を生成することによって、探索の効率化および探索が別の局所解に陥るのを防ぐ処理を行う。また、スケール調整パラメータ推定部２１ｄは、生成範囲に用いる範囲ｒはステップごとに、ｒ←γｒとし、範囲を減少させていく処理を行う（０＜γ≦１）。

次に、図５のステップＳ１２の個体の選択処理について説明する。ここで、本処理の説明の前に、スケール調整アルゴリズムにおける、遺伝的操作に関して説明を行う。スケール値調整のパラメータα^Ｓの真値付近の適応度（計測されたＬＲＦデータと環境地図との一致度）の値は、必ずしも１つの値をとるとは限らず、特に解像度の低い環境地図においては、範囲の広い複数の値において最良個体を取り得ることが考えられる。このような場合、次のステップにおいて探索範囲を適切に設定するためには、最良個体の１つを探索するよりも、最良個体の範囲を探索し、その範囲を次ステップの探索に活かす方が、効率が良い。そこで、ステップＳ１２の個体の選択処理において、スケール調整パラメータ推定部２１ｄは、ステップＳ１１の個体の初期化で得られた複数の個体から、最良個体とランダムに選択された個体とを採択し、記憶部２２に記憶する。

続いて、図５のステップＳ１３の交叉・突然変異処理においては、まず、交叉処理を行う。具体的に説明すると、スケール調整パラメータ推定部２１ｄは、遺伝的操作の１つである交叉に、ステップＳ１２で選択した最良個体とランダムに選択された個体とから、交叉を行うエリート交叉と複数の最良個体とが現れた場合、その最小値と最大値との外挿をとり、最良の適応度を持つ範囲の探索を行う。具体的には、スケール調整パラメータ推定部２１ｄは、以下の式（１）で示される外分点を取る処理を行うことによって交叉処理を行う。

ここで、式（１）のｇ^Ｓ _ｍｉｎおよびｇ^Ｓ _ｍａｘは、それぞれ、最良個体の中で、最も小さい・大きいスケール調整パラメータを持つ個体を示す。スケール調整パラメータ推定部２１ｄは、この式（１）のように外分点をとる演算を行い、これまで得られていた最良個体群の外側の解空間を探索していくことによって、最良個体群の範囲を探索する処理を行う。

続いて、突然変異処理では、スケール調整パラメータ推定部２１ｄは、以下の式（２）によって計算される世代数により探索範囲の幅を変更する突然変異を用いて処理を行う。なお、式（２）のＴは進化戦略における最大世代数（終了条件）、ｔは現在の世代数を示す。また、Ｎ（０,１）は、平均０、分散１の正規乱数、εは係数を示す。

ステップＳ１３の処理後、スケール調整パラメータ推定部２１ｄにおいて、ｉ番目の解像度の地図の適応度ｆ^Ｓ _ｉを、以下の式（３）によって計算する（ステップＳ１４）。なお、式（３）において、Ｍ＝{(ｎ,ｍ) | ｎ＝{−δ, −δ＋１, …, δ−１, δ }、ｍ＝{−δ, −δ＋１, …, δ−１, δ}}の集合を示す。

なお、上記式（３）の適応度は、図６に示す仮想の自走式作業装置２００の配置範囲δ＝１の場合の概念図のように、スケール調整パラメータ推定部２１ｄにおいて、大域的位置推定によって推定された自己位置情報をもとに現在の解像度のMulti-resolution Mapの周辺に仮想の自走式作業装置２００を仮想の地図上に配置した状態とし、各位置で算出されたＬＲＦデータとMulti-resolution Mapの一致度とを計算し、その最大値を適応度として計算するものである。すなわち、スケール値の調整にMulti-resolution Mapを用いることで、領域２^{（ｋ＋１）}・ｃｓ・δの適応度を計算可能となっている。ここで、ｃｓは、環境地図構築における格子空間のセルサイズを示す。

ステップＳ１４の処理後、スケール調整パラメータ推定部２１ｄは、終了条件を満たしたか否か（ここでは、進化戦略における最大世代数Ｔと同じ回数、ステップＳ１２からステップＳ１４を繰り返して処理したか否か）を判定する（ステップＳ１５）。スケール調整パラメータ推定部２１ｄにより終了条件を満たしていないと判定された場合（ステップＳ１５：ＮＯ）、ステップＳ１２に戻る。一方、スケール調整パラメータ推定部２１ｄにより終了条件を満たしたと判定された場合（ステップＳ１５：ＹＥＳ）、スケール調整パラメータ推定部２１ｄは、複数の最良個体の中央値の算出処理を行う（ステップＳ１６）。そして、ステップＳ３のスケール調整処理におけるスケール調整アルゴリズムを用いた処理は終了する。

なお、ステップＳ１６での最良個体の中央値の算出処理は、たとえば、スケール調整パラメータ推定部２１ｄにおいて、上述の探索手法において推定された最良個体の中で最大・最小のスケール値を持つ個体同士の平均値をとる演算を行うものである（以下の式（４）参照）。

したがって、ステップＳ１６での最良個体の中央値の算出処理により、現在の解像度のMulti-resolution Mapにおける最終的なパラメータの推定結果を得ることができる。また、このように最良個体群の中央値をとるような処理を行うのは、低い解像度を持つMulti-resolution Mapでは、一定の範囲内に同一の適応度を持つ最良個体群が存在することが考えられ、中央値を用いることによって、次の解像度での探索に活かすことができるためである。

このように、図５に示したスケール調整アルゴリズムにおいては、Multi-resolution Mapを用い、得られた環境地図のうち解像度の低い環境地図から初期位置の探索を開始していくため、初期のスケール値に大きな誤差が含まれており、自走式作業装置１００の位置の修正量が大きい場合においても、対応可能な適応度関数となっている。このように仮想的な自走式作業装置１００をMulti-resolution Map上に配置することによって、スケール調整における自走式作業装置１００の自己位置の修正量を適応度関数の中に組み込み、スケール値調整パラメータの推定に伴う自走式作業装置１００の位置の修正量を明示的に考慮せず、スケール値調整パラメータの推定のみを行うものである。

なお、スケール調整アルゴリズムにおいては、Multi-resolution Mapを用い解像度の低い環境地図から初期位置の探索を開始し、高い解像度へ環境地図を遷移させていくことによって、初期位置の探索における精度の向上を図っている。また、Multi-resolution Mapは、環境地図の解像度が更新可能であるため、初期位置の探索における計算時間を削減することができる。

次に、図４のフローチャートに戻って、ステップＳ４に進み、環境地図更新部２１ｆは、ステップＳ３において推定されたスケール値調整のパラメータα^Ｓを用いてMulti-resolution Mapのスケール値を更新し、Multi-resolution Mapの再構築を行う。その後、ステップＳ１に戻り、スケール調整パラメータ推定部２１ｄは、再構築されたMulti-resolution Map上において、再度、大域的位置推定を行う。そして、ステップＳ２において、スケール調整パラメータ推定部２１ｄによりｋ＝１であると判定されるまで、ステップＳ１〜ステップＳ４までを繰り返す。その後、スケール調整パラメータ推定部２１ｄによりｋ＝１であると判定された場合（ステップＳ２：ＹＥＳ）、現在位置推定部２１ｇは、局所的位置推定処理（SLAMアルゴリズムによる処理）を行って（ステップＳ５）、自走式作業装置１００の初期位置同定を行う。そして、処理を終了する。

このようにして得た自己位置および環境地図の情報により、自走部１のモータ５の回転角度および回転速度を制御することで、自走式作業装置１００は、目的地（たとえば、照度測定位置）に向けて自走するようになる。さらに、走行時において、動作制御部２１が、自己位置推定と環境地図構築とを同時に行うSLAM手法を用いて自己位置と環境地図とを更新することで、自走式作業装置１００は、最適経路を走行するようになる。

また、動作制御部２１は、たとえば、照度測定位置において、自走部１のモータ５を停止させ、照度計１２を作動させて照度の計測を行ない、その計測結果を、記憶部２２に送信する。その後、動作制御部２１は、次の照度測定位置まで自走式作業装置１００を移動させ、上記同様に、照度計１２を作動させて照度の計測を行ない、その計測結果を、記憶部２２に送信する。以上の動作制御部２１における照度測定処理を繰り返し、すべての照度測定位置の測定終了後、記憶部２２に記録された計測結果の情報は、制御用ＰＣ２０を介してＵＳＢメモリに書き込むことで回収することができるだけでなく、無線通信部２３を介して携帯通信端末３０に送信する処理を行うことでも回収できる。これにより、照度測定作業が自動的に行われ、作業現場の全ての照度測定が完了することになる。

上記構成の自走式作業装置１００によれば、目的地に向けて移動して作業を行う際、その目的地からの位置ずれを修正する際の作業の手間および時間を従来よりも削減することができる。

以上、本発明の実施形態について図面に基づいて説明したが、具体的な構成は、これらの実施形態に限定されるものではないと考えられるべきである。本発明の範囲は、上記した実施形態の説明ではなく特許請求の範囲によって示され、さらに特許請求の範囲と均等の意味および範囲内でのすべての変更が含まれる。例えば、上記実施形態では、本発明の自走式作業装置の一例として、自走式作業装置について説明を行なったが、本発明の自走式作業装置は、墨出し作業、運搬作業、設備機器類の監視および点検作業などの各種作業に利用可能である。

また、上記実施形態においては、自走式作業装置の自己位置について、スケール調整パラメータの推定後、解像度が低い環境地図から徐々に解像度が高い環境地図について、推定を行うものを示したが、これに限られない。たとえば、以下の２つの方法がある。

ＬＲＦによる計測対象が分かっており、計測対象に応じて基準となる座標系の設定が可能である場合においても、スケール調整パラメータの推定後であれば、自走式作業装置３００の自己位置の推定が可能である。たとえば、図７（ａ）では基準座標系の原点として、左下の占有部分を取ったものである。ここで、スケール調整前の自走式作業装置３００の位置を(ｘ ,ｙ)、スケール調整後の自走式作業装置３００の位置を(ｘ´,ｙ´)とすると，その座標系での移動量(ｄｘ, ｄｙ)は，以下のように計算可能である。これにより、自走式作業装置３００の自己位置の推定が可能である。

また、図７（ｂ）に示したように、基準となる座標系と自走式作業装置３０１の姿勢情報から基準となるＬＲＦの計測点を決定し、その距離情報をもとに、移動量の修正量を決定し、自走式作業装置の自己位置を推定することも可能である。この場合、自走式作業装置３０１の移動量(ｄｘ, ｄｙ)は，以下のようにして計算可能である。これにより、自走式作業装置３０１の自己位置の推定が可能である。

また、上記実施形態で示した自走式作業装置は、図８に示した構成の自走式作業装置２００であってもよい。以下、具体的に説明する。自走式作業装置２００は、自走部１０１と、自走部１０１の上面に設けられた長方体形状（箱形状）の本体１０２と、本体１０２の上面に設けられた三脚１０３およびＬＲＦ１１１と、照度測定を行うための照度計１１２（上部照度計１１２Ａおよび下部照度計１１２Ｂ）と、本体１０２の前面に取り付けられた第１実施形態におけるセンサ１３と同様のセンサ１１３と、を備えている。

自走部１０１は、本体１０２の下部に設けられた一対の車輪１０１ａと、一対の車輪１０１ａのそれぞれを駆動する一対のモータ１０１ｂと、車輪１０１ａの駆動に倣って回転する一対の車輪１０１ｃと、を備えている。なお、一対の車輪１０１ａは、本体１０２内に設けられた後述の動作制御部（図示せず）により、必要に応じて自走式作業装置２００の進行方向を変化させるべく、回転方向が操舵される（ハンドリングされる）ようになっている。

本体１０２は、上記実施形態で示した本体２と形状が異なるだけであって、その他の点は同様のものである。すなわち、本体１０２は、内部に、上記実施形態で示した本体２と同様の動作制御部およびバッテリ（図示せず）などを有しており、上記実施形態と同様の制御を行うことができる。

ＬＲＦ１１１は、上記実施形態で示したＬＲＦ１１と同様のものであり、本体１０２の前方側に設けられている。

上部照度計１１２Ａおよび下部照度計１１２Ｂは、上記実施形態で示した照度計１２と同様のものである。なお、上部照度計１１２Ａは、三脚１０３の上部に設けられた載置台１１４の上に固定されており、比較的高い位置の照度を計測できるようになっている。これに対して、下部照度計１１２Ｂは、床面に近い位置の照度を計測できるように、本体１０２の前方面に固定され床面に近い位置に設けられた載置台１１５の上に固定されている。

上記構成の自走式作業装置２００によれば、上記実施形態で示した自走式作業装置１００と同様の作用効果を奏することができる。

（実施例）
以下、実施例を挙げて本発明を具体的に説明するが、本発明は下記の実施例に何ら限定されるものではない。

（実施例１）
本発明に係る自走式作業装置について、体育館における照度計測実験を行った。なお、実験に用いた自走式作業装置における自走部には、iRobot Corporation（アイロボット・コーポレーション）製のiRobot Create２を使用した。また、ＬＲＦには、北陽電機株式会社製のＵＴＭ−３０ＬＸを使用し、照度計には、コニカミノルタジャパン株式会社製のＴ−３０を使用した。

まず、本実施例の自走式作業装置の動作制御部におけるスケール調整を行うことによる自己位置推定誤差の改善効果の検証実験として、図９（ａ）の図面データ（ＣＡＤ図をＪＰＥＧに変換した画像ファイル）において、図面右下を基準としてＸ方向（横方向）に10800ｍｍ、Ｙ方向（縦方向）に4400ｍｍの位置に本実施例の自走式作業装置を配置し、本実施例の自走式作業装置に自己の初期位置を推定させた。その結果、スケール調整を行わない場合（1ピクセル＝30.5ｍｍで固定）、本実施例の自走式作業装置の初期位置をＸ＝13920ｍｍ、Ｙ＝4500ｍｍと推定し実際の位置との誤差が約3121ｍｍでた（図１０（ａ）参照）が、上記実施形態で示したスケール調整を行った結果、1ピクセル＝34ｍｍに調整され、本実施例の自走式作業装置の初期位置はＸ＝10680ｍｍ、Ｙ＝4400ｍｍと推定され、実際の位置との誤差が約121ｍｍとなった（図１０（ｂ）参照）。

続いて、次の実験として、図９（ａ）の体育館の図面データ（ＣＡＤ図をＪＰＥＧに変換した画像ファイル）を環境地図として読み込み、図９（ｂ）に示すように、体育館の半面に照度計測地点（１５点）を含む走行経路を設定し実験を行った。なお、図９（ａ）、（ｂ）は、実験環境を示すものである。また、上記実施形態で述べたスケール調整パラメータアルゴリズムにおける（μ＋１)−ＥＳのパラメータとしては、親個体数μ＝３００、最大世代数Ｔ＝１０００、仮想ロボットの配置範囲δ＝２、初期のMulti-resolution Mapの解像度レベルをｋ＝４とした。また、環境地図構築における格子空間のセルサイズは６０（ｍｍ）、スケール値調整パラメータの真値α^Ｓを約１．１として実験を行った。照度計測実験におけるスケール調整パラメータ推定および環境地図構築に関する結果を図１１に示す。

図１１の結果から、図１１（ａ）においては、スケール調整前のスケール値に誤差が生じている環境地図上に対して、大まかな姿勢推定を行うための初期位置同定を行っており、この大まかな姿勢情報をもとにスケール調整を行っていき（図１１（ｂ）参照）、スケール調整終了後の環境地図構築開始時点では、ＬＲＦの計測データと図面データとから生成された環境地図の占有空間とほぼ一致していることが分かる。その後、自走式作業装置は環境地図の更新を行いながら、照度計測のための移動を開始し、各計測地点を通過しながら計測終了地点まで移動する。この移動時における、環境地図の更新過程（図１１（ｄ）〜（ｆ）参照）においても、各ＬＲＦの計測データは、読み込まれた図面データの環境地図とほぼ一致しており、最終的な環境地図である図１１（ｆ）においては、体育館全体の環境地図の構築を行えていることが分かる。

続いて、設定した計測地点と自走式作業装置における照度計測時の測定点の座標値、およびその時の照度値を下記表１に示す。

表１の結果から、設定した計測地点と測定地点の誤差の平均値は、約７４（ｍｍ）、誤差の最大値は、１３４（ｍｍ）であり、設定した計測地点からの位置ずれは、照度測定における許容誤差内に収まっていることが分かる。したがって、本発明に係る自走式作業装置は、ある程度の精度により位置決め制御が可能であり、スケール値に誤差が生じた環境地図を用いても照度測定が可能であると考えられる。

（実施例２）
次に、作業現場（約６５ｍ×３６ｍ）を想定したシミュレーション環境において、本発明に係る自走式作業装置を用いて作業員１名で照度測定を行う場合の作業時間を調べた。また、比較例として、作業員２名で照度測定を行う場合についても作業時間を調べた。なお、作業時間の比較は、一般照明の照度測定および非常照明の照度測定それぞれについて行った。また、一般照明の照度測定点数は２９点、非常照明の照度測定点数は３３点に設定した。結果を下記表２に示す。
（比較例）

（実施例）

表２、３の結果から、照度測定の作業時間を比較すると、一般照明と非常照明との両方において、自走式作業装置を用いて照度測定を行う場合は、作業員２名で照度測定を行う場合よりも約半分の作業時間で済むことが分かる。したがって、照度測定の作業に本発明に係る自走式作業装置を用いることで、作業効率の向上、労力および人件費の削減などを図ることができる。

１、１０１自走部
２、１０２本体
３、１０３三脚
４、１０１ａ、１０１ｃ車輪
５、１０１ｂモータ
６バッテリ
１１、１１１ＬＲＦ
１２、１１２照度計
１３、１１３センサ
２０制御用ＰＣ
２１動作制御部
２１ａ基礎データ抽出部
２１ｂ環境地図構築部
２１ｃ大域的自己位置推定部
２１ｄスケール調整パラメータ推定部
２１ｅスケール調整部
２１ｆ環境地図更新部
２１ｇ現在位置推定部
２２記憶部
２３無線通信部
３０携帯通信端末
３１端末制御部
３２端末記憶部
３３端末表示部
３４端末操作部
３５端末通信部
１００、２００自走式作業装置
１１４、１１５載置台

Claims

自走部と、環境地図に基づいて前記自走部の動作を制御する動作制御部と、作業現場領域内における自己の周囲の実環境データを検出する実環境データ検出部と、前記実環境データ検出部によって得られた前記実環境データと、作業現場の環境を示す図面データとを少なくとも記憶する記憶部と、を備えた自走式作業装置における前記動作制御部を、
前記作業現場の環境を示す図面データから、前記環境地図の基となる基礎データを抽出する基礎データ抽出手段、
前記基礎データに基づいて、前記環境地図を構築する環境地図構築手段、
前記基礎データと前記実環境データとに基づき、SLAM技術を用いて前記環境地図における自己の方向および相対的な位置関係について大域的な自己位置の推定を行う大域的自己位置推定手段、
前記環境地図と前記実環境データとのスケールの一致度を計算し、前記環境地図と前記実環境データとを一致させる補正に用いられるスケール調整パラメータを探索するスケール調整パラメータアルゴリズムを用いて、真のスケール調整パラメータを推定するスケール調整パラメータ推定手段、
前記スケール調整パラメータ推定手段によって推定されたスケール調整パラメータに基づいて、前記環境地図と前記実環境データとのスケールを合致させる調整を行うスケール調整手段、
前記スケール調整手段による前記環境地図と前記実環境データとのスケールを合致させる調整が行われた後に、前記環境地図のデータを更新する環境地図更新手段、
前記環境地図更新手段によって更新された環境地図において、SLAM技術を用いて自己の現在位置を推定する現在位置推定手段、
として機能させる自走制御プログラム。
前記環境地図構築手段は、前記基礎データに基づいて、前記環境地図について解像度が異なる複数の地図を構築するものであり、
前記大域的自己位置推定手段は、前記基礎データと前記実環境データとに基づき、SLAM技術を用いて前記複数の地図それぞれにおける自己の方向および相対的な位置関係について大域的な自己位置の推定を行うものであり、
前記スケール調整パラメータ推定手段は、前記複数の地図のそれぞれと前記実環境データとのスケールの一致度を計算し、前記一致度が所定以上となる範囲において、前記複数の地図のうち最も低い解像度の地図から順に高い解像度の地図について徐々に範囲を絞り込みながら真のスケール調整パラメータを探索する遺伝的アルゴリズムを用いて、真のスケール調整パラメータを推定するものであることを特徴とする請求項１に記載の自走制御プログラム。
前記スケール調整パラメータ推定手段は、
前記大域的自己位置推定手段によって推定された自己位置の情報に基づいて自己の周辺に少なくとも１つの仮想の自己を配置し、
前記複数の地図のそれぞれと前記実環境データとのスケールの一致度が最大となる範囲を前記真のスケール調整パラメータが含まれる範囲とし、
前記真のスケール調整パラメータが含まれる範囲において、最大のスケール調整パラメータと最小のスケール調整パラメータとの平均値を、次に解像度が高い地図において真のスケール調整パラメータを推定する場合に用いることを特徴とする請求項２に記載の自走制御プログラム。
請求項１〜３のいずれか１項に記載の自走制御プログラムを記憶した記憶部と、前記自走制御プログラムを実行する動作制御部を含むコンピュータと、を備えた自走式作業装置。