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

Abstract

【課題】自走式作業装置が目的地に向けて移動して作業を行う際、従来よりも自己位置推定および環境地図構築の精度を向上させることが可能な自走制御プログラムと、このプログラムを実行可能なコンピュータを備えた自走式作業装置を提供する。【解決手段】自走制御プログラムは、SLAM技術を用いて環境地図における自己の方向および相対的な位置関係について大域的な自己位置の推定を行う大域的自己位置推定手段、環境地図のデータを更新する環境地図更新手段、環境地図更新手段によって更新された環境地図において、SLAM技術を用いて現在の自己位置の推定を行う現在位置推定手段、自走部の所定の運動モデルから自己の移動量を予測する移動量予測手段、として機能し、現在位置推定手段は、移動量予測手段によって予測された移動量を利用した進化戦略に基づくSLAM技術を用いて自己位置の推定を行うことを特徴とする。【選択図】図５

Description

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

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

例えば、特許文献１には、SLAM技術を用いて、初期位置を含む現在位置の推定（自己位置推定）および周囲環境の地図作成（環境地図構築）を行いながら、目的地（例えば、照度測定地点）に向けて自走する自走式作業装置が開示されている。

特開２０２０－６４４００号公報

しかしながら、上記特許文献１の自走式作業装置では、照度測定地点が比較的少数であったり、走行前および走行初期であったりする場合、自己位置を比較的高い精度で推定できるものの、比較的多数の照度測定地点が存在する大規模な作業現場において照度計測作業を行う場合、走行中に自己位置推定の誤差が発生・蓄積し、SLAM技術を活かせなくなることがあった。これに伴い、上記特許文献１の自走式作業装置では、構築する環境地図に実環境とのずれが生じてしまうことがあり、その結果、実際の照度測定地点と自走式作業装置の移動位置とにずれが生じ、照度計測を十分に行うことができない場合があった。

そこで、本発明は、自走式作業装置が目的地に向けて移動して作業を行う際、従来よりもSLAM技術を活かして、自己位置推定および環境地図構築の精度を向上させることが可能な自走制御プログラム、および、この自走制御プログラムを実行可能なコンピュータを備えた自走式作業装置を提供することを目的とする。

（１） 本発明は、 複数の駆動輪を有した自走部と、環境地図に基づいて前記自走部の動作を制御する動作制御部と、作業現場領域内における自己の周囲の実環境データを検出する実環境データ検出部と、前記実環境データ検出部によって得られた前記実環境データと、作業現場の環境を示す図面データとを少なくとも記憶する記憶部と、を備えた自走式作業装置における前記動作制御部を、
前記作業現場の環境を示す図面データから、前記環境地図の基となる基礎データを抽出する基礎データ抽出手段、
前記基礎データに基づいて、前記環境地図を構築する環境地図構築手段、
前記基礎データと前記実環境データとに基づき、SLAM技術を用いて前記環境地図における自己の方向および相対的な位置関係について大域的な自己位置の推定を行う大域的自己位置推定手段、
スケール調整パラメータを推定するスケール調整パラメータ推定手段、
前記スケール調整パラメータ推定手段によって推定されたスケール調整パラメータに基づいて、前記環境地図と前記実環境データとのスケールを合致させる調整を行うスケール調整手段、
前記スケール調整手段による前記環境地図と前記実環境データとのスケールを合致させる調整が行われた後に、前記環境地図のデータを更新する環境地図更新手段、
前記環境地図更新手段によって更新された環境地図において、SLAM技術を用いて現在の自己位置の推定を行う現在位置推定手段、
前記自走部の前記駆動輪についての所定の運動モデルに基づいて、自己の移動量を予測する移動量予測手段、
として機能させ、
前記現在位置推定手段は、前記移動量予測手段によって予測された移動量の予測値から次時刻における自己の予測位置を求め、前記予測位置付近で個体群の初期化を行う進化戦略に基づくSLAM技術を用いて前記自己位置の推定を行うことを特徴とする自走制御プログラムである。

（２） 上記（１）に記載の自走制御プログラムにおいて、前記所定の運動モデルは、少なくとも前記自走部の旋回角速度と旋回半径とに基づくものであることが好ましい。

（３） 上記（１）または（２）に記載の自走制御プログラムにおいて、前記所定の運動モデルは、下記式によるものであることが好ましい。
ここで、ｌは前記自走部の旋回半径、ｈは前記駆動輪軸上の中心点と実環境データ検出部の中心位置との距離、ωは前記自走部の旋回角速度、である。

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

本発明によれば、自走式作業装置が地図情報に基づき目的地に向けて移動して作業を行う際、従来よりも自己位置推定および環境地図構築の精度を向上させることが可能な自走制御プログラム、および、この自走制御プログラムを実行可能なコンピュータを備えた自走式作業装置を提供することができる。また、自走式作業装置は、各目的地に対して同じ作業を繰り返し行う場合に好適に用いることができる。例えば、建築現場での墨出し作業、照明の照度測定作業、物流倉庫での運搬作業などに対して、本発明の自走式作業装置は利用可能である。

本発明の実施形態に係る自走式作業装置を示す斜視図である。 （ａ）は図１の自走式作業装置の右側面図、（ｂ）は図１の自走式作業装置の背面図である。 本発明の実施形態に係る自走式作業装置および携帯通信端末の構成を示すブロック図である。 図３に示した動作制御部における機能的構成を示すブロック図である。 図３に示した動作制御部が実行する図面データからの自己位置推定処理を示すフローチャートである。 図３に示した動作制御部が実行する進化戦略による自己位置推定処理を示すフローチャートである。 （ａ）は図１の自走式作業装置の旋回時の運動モデルを示す概念図、（ｂ）は図１の自走式作業装置の直進時の運動モデルを示す概念図である。 本発明の実施形態に係る自走式作業装置の一変形例を示す斜視図である。 （ａ）は図８の自走式作業装置の右側面図、（ｂ）は図８の自走式作業装置の背面図である。 本発明の実施形態に係る自走式作業装置の一変形例を示す斜視図である。 実施例１に係る走行実験における実験環境を示す図である。 実施例１に係る走行実験における実験条件を示す図である。 実施例１に係る走行実験おいて運動モデルを利用したSLAM手法を用いた場合の適応度の推移を示すグラフである。 実施例１に係る走行実験おいて従来のSLAM手法を用いた場合の適応度の推移を示すグラフである。

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

＜装置構成＞
図１に示すように、本実施形態に係る自走式作業装置１００は、左右一対の駆動輪１ａ、１ｂを有する自走部１と、扁平な直方体形状の前方部２ａおよび前方部２ａに連結する直方体形状の後方部２ｂを有する本体２と、本体２の前方部２ａの上面に設けられた２次元または３次元の距離計測センサ（Laser Range Finder。以下、ＬＲＦ）３および照度計４の受光部４ａと、本体２の後方部２ｂの背面に設けられた照度計４の表示部４ｂおよびバッテリ５と、本体２の後方部２ｂの上面に設けられた非常停止ボタン６および無線通信部７と、本体２の前方部２ａのバンパ８に設けられたセンサ９と、を備えている。

自走部１の左右一対の駆動輪１ａ、１ｂは、本体２の後方部２ｂの下部に設けられており、左右一対の駆動モータ１０ａ、１０ｂ（図３参照）によりそれぞれ独立に駆動する。したがって、自走式作業装置１００は、各駆動モータ１０ａ、１０ｂが後述する動作制御部２１で駆動制御されることにより、前進、後退、旋回、加減速などの走行ができるようになっている。また、図２（ａ）、（ｂ）に示すように、本体２の前方部２ａの底面のバンパ８側には、キャスター１１が設けられており、このキャスター１１は、各駆動輪１ａ、１ｂの駆動に倣って回転する。

本体２の内部には、図３に示すように、制御用パーソナルコンピュータ（以下、制御用ＰＣ）２０が設けられている。制御用ＰＣ２０は、動作制御部２１および記憶部２２を有している。動作制御部２１は、ＣＰＵおよびＲＡＭを含み、ＬＲＦ３、照度計４、センサ９、および自走部１の動作を制御する。

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

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

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

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

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

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

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

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

移動量予測部２１ｈは、自走部１の後述する運動モデルから自己の移動量を予測し、予測した自己の移動量のデータを記憶部２２に送信する。

記憶部２２は、ＲＯＭまたはハードディスクなどを含み、自走式作業装置１００の各種機能を実現するために必要な情報（実環境データ、図面データ、演算結果のデータなどを含む）、および、各種制御プログラム、などを記憶する。ここで、自走式作業装置１００の各種機能としては、例えば、現在位置の推定（自己位置推定）および周囲環境の地図作成（環境地図構築）を行う機能、障害物を回避して走行する機能、照明の照度測定を行う機能などである。また、各種制御プログラムとは、上記各機能を実現するためのプログラムであり、初期位置を含む現在位置の推定（自己位置推定）および周囲環境の地図作成（環境地図構築）の制御を行うモジュール（後述するMulti-resolution Mapを用いたSLAMアルゴリズム、後述する運動モデルを用いた進化戦略に基づく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 を用いた未知環境の探査」 戸田 雄一郎、久保田 直行」に掲載されている未知環境における探査手法で用いた２次元環境地図の一例である占有格子空間地図である。

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

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

なお、ステップＳ３のスケール調整処理に用いるスケール調整アルゴリズムには、たとえば、本願の出願人が出願した特願２０１８－１９５１８９（特開２０２０－６４４００号公報）に記載されているスケール調整アルゴリズムを用いることができる。このスケール調整アルゴリズムにおいては、Multi-resolution Mapを用い解像度の低い環境地図から初期位置の探索を開始し、高い解像度へ環境地図を遷移させていくことによって、初期位置の探索における精度の向上を図っている。また、Multi-resolution Mapは、環境地図の解像度が更新可能であるため、初期位置の探索における計算時間を削減することができる。

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

ここで、図６に示すフローチャートを用いて、移動中の局所的位置推定処理について説明する。まず、図６のステップＳ１１の移動量の予測においては、移動量予測部２１ｈが、後述の運動モデルを用いて次時刻における自走式作業装置１００の移動量の予測値を算出する。以下、具体的に移動量予測部２１ｈによる移動量の予測処理について説明する。移動量予測部２１ｈは、自走式作業装置１００の運動モデルとして、図７に示す対向二輪型モデルを用いる。なお、図７（ａ）は自走式作業装置１００の旋回時の運動モデルを示す概念図、図７（ｂ）は自走式作業装置１００の直進時の運動モデルを示す概念図である。

まず、移動量予測部２１ｈは、各駆動輪１ａ、１ｂの回転角速度ω_Ｌ、ω_Ｒを以下の式（１）によって計算する。なお、式（１）において、Ａ_Ｌ、Ａ_Ｒは、左右の駆動モータ制御量ｕ_Ｌ、ｕ_Ｒから各駆動輪１ａ、１ｂの回転角速度ω_Ｌ、ω_Ｒへの変換係数を示す。

次に，移動量予測部２１ｈは、車輪半径Ｒから各駆動輪１ａ、１ｂの移動速度ｖ_Ｌ、ｖ_Ｒを以下の式（２）によって計算する。

次に、移動量予測部２１ｈは、各駆動輪１ａ、１ｂの移動速度ｖ_Ｌ、ｖ_Ｒから自走式作業装置１００（自走部１）の平均移動速度ｖ、旋回角速度ω、旋回半径ｌ（自走部１が旋回したとき、駆動輪１ａ、１ｂ軸上の中心点によってできる円の半径）を以下の式（３）～（５）によって計算する。ここで、Ｄは駆動輪１ａの中心と１ｂの中心との距離である。

そして、移動量予測部２１ｈは、以下の条件式（６）を用いて自走式作業装置１００の直進判定を行う。この直進判定は、たとえば、左右の駆動モータ出力がほぼ等しい（ｖ_Ｒ≒ｖ_Ｌ）場合、すなわち左右の駆動モータ出力差が０付近の場合に直進と判定するものである。なお、条件式（６）において、ｖ_ｔｈは直進判定用閾値を示す。

ここで、移動量予測部２１ｈにより上記条件式（６）を満たしたと判定された場合、移動量予測部２１ｈは、図７（ｂ）に示すＸ軸に沿った直進時の運動モデルに基づく以下の式（７）によって、次時刻における自走式作業装置１００の移動量の予測値ｄｘ、ｄｙ、ｄθを算出する。ここで、自走式作業装置１００の自己位置を（ｘ，ｙ）、自走式作業装置１００の予測自己位置を(ｘ´,ｙ´)、ＬＲＦ３の計測方向をθ、時間をｔとした場合、その座標系での移動量の予測値(ｄｘ,ｄｙ,ｄθ)は，以下のように計算可能である。

また、移動量予測部２１ｈにより上記条件式（６）を満たしていないと判定された場合、移動量予測部２１ｈは、図７（ａ）に示す旋回時の運動モデルに基づく以下の式（８）によって、次時刻における自走式作業装置１００の移動量の予測値(ｄｘ、ｄｙ、ｄθ)を算出する。なお、式（８）において、ｈは、駆動輪軸上の中心点とＬＲＦ３の中心位置との距離を示す。

図６のステップＳ１１の処理後、現在位置推定部２１ｇは、移動量予測部２１ｈによる予測結果に基づいて個体群の初期化を行う（ステップＳ１２）。具体的には、現在位置推定部２１ｇは、移動量予測部２１ｈにより算出された移動量の予測値から次時刻における自走式作業装置１００の予測位置を求め、この予測位置付近で個体群の初期化を行う。なお、従来から自己位置推定で利用している進化戦略では、個体群は現在位置付近で初期化されているため、初期化範囲外に最適解が存在する場合に最適解を導出できる可能性が低くなってしまう。これに対して、本実施形態の進化戦略では、個体群は予測位置付近で初期化されているため、個体の初期化範囲内に最適解が存在する確率が高くなり、最適解を導出できる可能性も高くなり、自己位置推定の精度を向上させることができる。

ステップＳ１２の処理後、現在位置推定部２１ｇは、子となる個体を１つ生成し，個体の評価を行ない、（μ＋１)個の個体群から次世代の親個体となるμ個を選択する（ステップＳ１３）。そして、各個体を現時刻における自走式作業装置１００の自己位置（ｇ_ｋ,１、ｇ_ｋ,２）と姿勢（ｇ_ｋ,３）として表現し、各遺伝子に対して、交叉および突然変異といった遺伝的操作を行なっていく（ステップＳ１４）。なお、交叉処理では、最良個体とランダムに選択された個体から交叉を行なうエリート交叉を用いる。また、突然変異処理では、以下の式（９）によって計算される適応的突然変異を用いる。なお、式（９）において、ｆｉｔ_ｋは、ｋ番目の個体の適応度（計測されたＬＲＦデータと環境地図との一致度）を示す。また、ｆｉｔ_ｍａｘとｆｉｔ_ｍｉｎは、それぞれ個体群中の最大、最小の適応度を示す。また、Ｎ（０,１）は、平均０、分散１の正規乱数、α_ｈとβ_ｈは、それぞれ係数とオフセットを示す。また、式（１０）により、自己位置推定解の探索において、適応度は、基本的に地図の占有度の総和によって決定される。適応度が高くなるほど、ＬＲＦ３によって計測された実環境データと現時刻までに構築された地図情報との一致度合いが高くなるため、より精確に自己位置を推定できることが期待される。そのため，本問題は最大化問題として帰着できる。ここで，p_t ^OCCはペナルティ項を表す。基本的には、地図の占有度の総和のみでも自己位置推定が可能であるが、環境によっては、探索が局所解に陥ってしまう場合が存在する。ペナルティ項は、多くの非占有状態のセルに対して、ＬＲＦ３の計測データが地図情報と一致した時に、ペナルティ項が低くなるように設計されており、自己位置推定が失敗することを防ぐための係数となっている。

図６のステップＳ１４の処理後、現在位置推定部２１ｇは、終了条件を満たしたか否か（ここでは、任意に設定した回数または進化戦略における最大世代数と同じ回数、ステップＳ１３からステップＳ１４を繰り返して処理したか否か）を判定する（ステップＳ１５）。現在位置推定部２１ｇにより終了条件を満たしていないと判定された場合（ステップＳ１５：ＮＯ）、ステップＳ１３に戻る。一方、現在位置推定部２１ｇにより終了条件を満たしたと判定された場合（ステップＳ１５：ＹＥＳ）、現在位置推定部２１ｇは、最良個体を選択し（ステップＳ１６）、最良個体の遺伝子を現時刻における自己位置として推定する。そして、SLAMアルゴリズムによって地図を更新し、処理を終了する。このような移動中の局所的位置推定処理を自己位置の推定の必要がなくなるまで（たとえば、任意の回数まで、または、照度計測作業が完了するまで）繰り返す。

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

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

上記構成の自走式作業装置１００によれば、目的地に向けて移動して作業を行う際、比較的多数の照度測定地点が存在する大規模な作業現場において照度計測作業を行う場合であっても、運動モデルによる移動量予測を利用した進化戦略に基づくSLAM技術を用いることで、従来よりもSLAM技術を活かして、自己位置推定および環境地図構築の精度を向上させることができる。

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

また、上記実施形態で示した自走式作業装置の一変形例として、図８および図９に示した構成の自走式作業装置２００であってもよい。以下、具体的に説明する。なお、本変形例において、上記実施形態と下二桁が同じ番号の符号は、同様のものであるので、説明を省略することがある。また、特に説明しない部分に関しては、上記実施形態と同様であるので、説明および図示を省略することがある。

図８および図９に示したように、自走式作業装置２００においては、左右一対の駆動輪１０１ａ、１０１ｂが本体１０２の前方部１０２ａの下部に設けられている点と、キャスター１１１が本体１０２の後方部１０２ｂの底面に設けられている点と、照度計１０４の表示部１０４ｂおよびバッテリ１０５が本体１０２の後方部１０２ｂの上面に設けられている点と、無線通信部１０７が本体１０２の前方部１０２ａの上面に設けられている点で、上記実施形態の自走式作業装置１００と異なっている。

ここで、本変形例の自走式作業装置２００の動作制御部が、局所的位置推定処理を行う場合に用いる運動モデルによる移動量予測について説明する。自走式作業装置２００においては、駆動輪軸上の中心点とＬＲＦ１０３の中心位置との距離が比較的短いため、駆動輪軸上の中心点とＬＲＦ１０３の中心位置とがほぼ一致する場合の運動モデルを用いて、自走式作業装置２００の移動量の予測値を近似的に計算しても実用上問題ないと考えられる。したがって、本変形例に係る旋回時の運動モデルに基づく自走式作業装置２００の移動量の予測値は、上記実施形態で示した式（８）において、ｈ＝０を代入した以下の式（１１）によって計算可能である。なお、本変形例に係る直進時の運動モデルに基づく自走式作業装置２００の移動量の予測値は、上記実施形態で示した式（７）によって計算可能である。

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

また、自走式作業装置２００は、床面に近い位置の照度を計測できるように、照度計１０４の受光部１０４ａが本体１０２の前方部１０２ａの上面に設けられていたが、図１０に示したように、比較的高い位置の照度を計測できるように構成したものであってもよい。以下、具体的に説明する。

図１０に示したように、自走式作業装置３００においては、伸縮可能な支持棒２４０が台座２４１を介して本体２０２の前方部２０２ａの上面に設けられている点と、照度計２０４の受光部２０４ａが支持棒２４０の上部に設けられた取付部材２４２を介して固定されている点で、上記自走式作業装置２００と異なっている。なお、自走式作業装置３００の移動量の予測値は、上記自走式作業装置２００と同じ運動モデルを用いて計算可能である。したがって、上記構成の自走式作業装置３００においても、上記実施形態で示した自走式作業装置１００と同様の作用効果を奏することができる。

なお、上記実施形態および変形例の自走式作業装置にセンサ（たとえば、ロータリーエンコーダ、タコメータ、加速度センサなど）をさらに設けておいて、本センサによって得られた自走式作業装置（自走部）の旋回角速度と旋回半径とを用いて、移動量予測部によって移動量を予測してもよい。

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

（実施例１）
本発明に係る自走式作業装置の運動モデルを利用した進化戦略に基づくSLAM手法（以下、運動モデルを利用したSLAM手法）の有効性を検証するために、比較的広大な体育館において走行実験を行った。この走行実験において、位置推定における蓄積誤差によりSLAMが破綻したか否かを判定し評価した。なお、SLAMの破綻の判定には、計測されたＬＲＦデータと環境地図との一致度を示す適応度を用いた。

また、走行実験に用いた自走式作業装置は、上記自走式作業装置２００と同様の構成のものを使用した。なお、この場合の運動モデルに基づく自走式作業装置の移動量の予測値は、上述した式（７）または／および（１１）によって計算されるものである。また、比較例として、進化戦略に自走式作業装置の運動モデルを利用しない場合のSLAM手法（以下、従来のSLAM手法）を用いた自走式作業装置についても、走行実験を行った。なお、図１１は、実験環境を示すものであり、体育館の図面データ（ＣＡＤ図をＰＮＧまたはＪＰＥＧに変換した画像ファイル）である。

実施例１に係る走行実験は、図１１に示した実験環境において、運動モデルを利用したSLAM手法および従来のSLAM手法について、それぞれ１０回ずつ走行実験を行った。具体的には、図１１の体育館の図面データを環境地図として読み込み、図１２に示すように、自走式作業装置の初期位置および照度計測地点（３６点）を設定し走行実験を行った。なお、図１２において、Ｓは照度計測開始地点、Ｇは照度計測終了地点を示す。また、下記表１に運動モデルを利用したSLAM手法を用いた自走式作業装置の走行実験パラメータを示す。

次に、下記表２に運動モデルを利用したSLAM手法を用いた自走式作業装置の走行実験の結果を示す。

表２の結果から、運動モデルを利用したSLAM手法を用いた自走式作業装置では、１０回の走行実験すべてにおいてSLAMが成功しており、SLAMの成功率が１００％と十分な結果が得られた。また、照度計測全体の平均実行時間は、１２分３０秒であることがわかった。

続いて、下記表３に従来のSLAM手法を用いた自走式作業装置の走行実験の結果を示す。

表３の結果から、従来のSLAM手法を用いた自走式作業装置では、１０回の走行実験のうち１回のみしかSLAMが成功しておらず、SLAMの成功率が１０％であることがわかった。また、照度計測全体の平均実行時間は、１２分１０秒であることがわかった。

表２および表３の結果から、従来のSLAM手法よりも運動モデルを利用したSLAM手法のほうが、SLAMの成功率が高く、照度計測のやり直しが必要なくなるという点で優れていることがわかった。一方、従来のSLAM手法よりも運動モデルを利用したSLAM手法のほうが、照度計測に要する時間は多少増えてしまうことがわかった。しかし、照度計測のやり直しが発生する場合は、２倍以上の実行時間がかかると考えると、数％程度の実行時間の増加は実用上大きな問題ではないと考えられる。したがって、運動モデルを利用したSLAM手法を用いた自走式作業装置によれば、照度計測の効率化を図ることができる。

次に、図１３に運動モデルを利用したSLAM手法を用いた自走式作業装置の走行実験における適応度の推移を示す。また、図１４に従来のSLAM手法を用いた自走式作業装置の走行実験における適応度の推移を示す。

図１３に示す適応度の推移から、運動モデルを利用したSLAM手法では、適応度が常に高い状態を維持していることが確認できた。また、図１４に示す適応度の推移から、従来のSLAM手法では、適応度が急激に下がりSLAMが破綻したことが確認できた。したがって、運動モデルを利用したSLAM手法は、位置推定における蓄積誤差によるSLAMの破綻を防止する手法として有効であることがわかった。

１ 自走部
１ａ、１ｂ 駆動輪
２ 本体
２ａ 前方部
２ｂ 後方部
３ ＬＲＦ
４ 照度計
４ａ 受光部
４ｂ 表示部
５ バッテリ
６ 非常停止ボタン
７ 無線通信部
８ バンパ
９ センサ
１０ａ、１０ｂ 駆動モータ
１１ キャスター
２０ 制御用ＰＣ
２１ 動作制御部
２１ａ 基礎データ抽出部
２１ｂ 環境地図構築部
２１ｃ 大域的自己位置推定部
２１ｄ スケール調整パラメータ推定部
２１ｅ スケール調整部
２１ｆ 環境地図更新部
２１ｇ 現在位置推定部
２１ｈ 移動量予測部
２２ 記憶部
３０ 携帯通信端末
３１ 端末制御部
３２ 端末記憶部
３３ 端末表示部
３４ 端末操作部
３５ 端末通信部
１００ 自走式作業装置

Claims (4)

1. 複数の駆動輪を有した自走部と、環境地図に基づいて前記自走部の動作を制御する動作制御部と、作業現場領域内における自己の周囲の実環境データを検出する実環境データ検出部と、前記実環境データ検出部によって得られた前記実環境データと、作業現場の環境を示す図面データとを少なくとも記憶する記憶部と、を備えた自走式作業装置における前記動作制御部を、
   前記作業現場の環境を示す図面データから、前記環境地図の基となる基礎データを抽出する基礎データ抽出手段、
   前記基礎データに基づいて、前記環境地図を構築する環境地図構築手段、
   前記基礎データと前記実環境データとに基づき、SLAM技術を用いて前記環境地図における自己の方向および相対的な位置関係について大域的な自己位置の推定を行う大域的自己位置推定手段、
   スケール調整パラメータを推定するスケール調整パラメータ推定手段、
   前記スケール調整パラメータ推定手段によって推定されたスケール調整パラメータに基づいて、前記環境地図と前記実環境データとのスケールを合致させる調整を行うスケール調整手段、
   前記スケール調整手段による前記環境地図と前記実環境データとのスケールを合致させる調整が行われた後に、前記環境地図のデータを更新する環境地図更新手段、
   前記環境地図更新手段によって更新された環境地図において、SLAM技術を用いて現在の自己位置の推定を行う現在位置推定手段、
   前記自走部の前記駆動輪についての所定の運動モデルに基づいて、自己の移動量を予測する移動量予測手段、
   として機能させ、
   前記現在位置推定手段は、前記移動量予測手段によって予測された移動量の予測値から次時刻における自己の予測位置を求め、前記予測位置付近で個体群の初期化を行う進化戦略に基づくSLAM技術を用いて前記自己位置の推定を行うことを特徴とする自走制御プログラム。
2. 前記所定の運動モデルは、少なくとも前記自走部の旋回角速度と旋回半径とに基づくものであることを特徴とする請求項１に記載の自走制御プログラム。
3. 前記所定の運動モデルは、下記式によるものであることを特徴とする請求項１または２に記載の自走制御プログラム。
   ここで、ｌは前記自走部の旋回半径、ｈは前記駆動輪軸上の中心点と実環境データ検出部の中心位置との距離、ωは前記自走部の旋回角速度、である。
   

   
4. 請求項１～３のいずれか１項に記載の自走制御プログラムを記憶した記憶部と、前記自走制御プログラムを実行する動作制御部を含むコンピュータと、を備えた自走式作業装置。
   

Images