JP5169273B2 - 移動ロボットの制御装置および移動ロボットシステム - Google Patents

Description

本発明は，内界センサから得られる情報と外界センサから得られる情報とを組み合わせて推定した推定現在位置（並進位置と姿勢）をもとに環境中の目標位置に移動ロボットを位置決め制御する移動ロボットの制御装置および移動ロボットシステムに関する。

移動ロボットを環境中の目標位置に誘導制御するためには，まず環境中の移動ロボットの「並進位置と姿勢」を求める必要がある。なお、以降、移動ロボットの「並進位置と姿勢」を単に「位置」として総称する。求めた現在の移動ロボット位置と目標位置との関係にもとづいて移動ロボットを駆動制御することで，移動ロボットを指定した目標位置まで位置決めするのである。あるいは，環境中の目標位置と移動ロボット位置との関係を直接求め，その結果にもとづいて移動ロボットを駆動制御するという方法もある。

環境中の移動ロボット位置を求めるには，車輪の回転状態を検出するロータリーエンコーダやジャイロセンサといった内界センサにもとづいて移動ロボット位置を推定する手段(オドメトリ)と，レーザーレンジファインダやカメラ・超音波センサといった外界センサにもとづいて位置を求める手段とがある。しかし，内界センサにもとづく位置推定結果には誤差が蓄積するため，外界センサにもとづく位置推定結果を用いるか，内界センサにもとづく位置推定結果と外界センサにもとづく位置推定結果を組み合わせて用いるのが一般的である。以下に例を示す。

特許文献１は，内界センサにもとづく位置推定結果と外界センサにもとづく位置推定結果を組み合わせて用いる例である。本文献では，走行方向に垂直した上方のイメージを撮像できるように本体上に設けられた上方カメラと，所定の走行パターンにより作業領域内を走行するよう駆動部を制御しつつ上方カメラにより撮像されたイメージを分析して走行経路を補正する制御部とを備えたロボット掃除機が開示されている。図４は特許文献１のロボット掃除機システムを示したブロック図である。図において４０１は図示しない車輪駆動用モータに付属するエンコーダであって，内界センサに相当する。４０２は走行方向に垂直上方のイメージを撮像できるよう本体上に設けられた上方カメラであって，外界センサに相当する。また，４０３は車輪駆動用モータを駆動する駆動部，４０４は上方カメラによって事前に作成された上方領域に対する映像マップを保存するための記憶装置，４０５はエンコーダ４０１や上方カメラ４０２などから得られる信号を処理し駆動部を制御するための制御部である。制御部４０５は，予め記憶してある映像マップと上方カメラから入力された現在のイメージを比較しながらロボット掃除機の現位置を認識し，認識された位置から目的とする走行経路に対応するよう駆動部４０３を制御する。目的とする走行経路に沿って移動させるためには，エンコーダ４０１を利用して逐次推定現在位置を算出するとともに，上方カメラ４０２で現在撮像されたイメージと映像マップとの比較により認識された推定現在位置を用いて走行エラーを算出し，エラーを補償して目的とする走行経路を追跡するよう駆動部を制御する。

特許文献２は，外界センサにもとづく位置推定結果を用いる例である。本文献では，環境中に存在する複数の既知の構造をランドマークとして観測することによって移動ロボットの位置および姿勢を推定する方法が開示されている。その流れは以下の通りである。まず，移動ロボットに備えた視覚装置において，実時間三次元物***置決め方法によりランドマークの物体モデルと観測した環境のデータとを照合し，物体モデルに対応するランドマークの三次元空間中における位置と姿勢を実時間で連続的に算出する。次いで自己位置推定処理用のコンピュータにおいて，算出されたランドマークの位置と姿勢にもとづくロボットの自己位置推定処理を実行する。そして移動ロボットの制御装置において，現在の推定された自己位置および姿勢を利用してロボットの移動制御を実行する。
特開２００２−３２５７０８号公報 特開２００４−０３０４４５号公報

なお，一般にロータリーエンコーダやジャイロセンサのような内界センサの出力信号は比較的短い周期で得られ，それにもとづく位置推定も同様に短い周期で実行できる。一方，レーザーレンジファインダやカメラ・超音波センサといった外界センサにもとづいて位置を求める場合，センサ出力の伝送および処理に時間を要することが多く，内界センサにもとづく位置推定よりも長い周期で位置推定が実行される。

以上に示したように，従来の移動ロボットの制御装置は外界センサにもとづく位置推定結果をそのまま利用して移動ロボットを目標位置まで誘導制御している。また，外界センサによる計測結果から直接目標位置を求める場合も，求めた目標位置をそのまま利用して移動ロボットとを目標位置まで誘導制御している。

移動ロボットを正確に目標位置に位置決めするため外界センサにもとづく位置推定結果を制御ループに組み込む場合，外界センサにもとづく位置推定処理は時間を要するため，制御ループに遅れ要素が発生するという問題がある。制御ループの遅れ要素は目標位置付近での振動現象を招く。これを抑制するためには
(対策1)目標位置と現在位置との許容偏差を大きくする，
(対策2)外界センサにもとづく位置推定処理の速度を上げて遅れを減らす，
といった対策があるが，(対策1)を実施すると移動ロボットを目標位置に正確に位置決めできなくなる。特許文献１の掃除ロボットシステムは要求される停止位置精度が比較的低いため，(対策1)が有効である。しかし，配膳作業や充電器との接続など，停止位置精度が要求される場合には(対策1)は現実的ではない。(対策2)の実施には高性能の処理装置が必要であり，追加コストが発生する。他に，位置推定処理のアルゴリズムを工夫して計算量を減らすなどの措置が考えられるが，特定の外界センサや位置推定手法に特化せざるを得ず，一般的ではない。特許文献２の実現には(対策2)が必要であったが，システム構築コストが高くなってしまう。

本発明は，上記のような外界センサにもとづく位置推定処理の遅れの影響を軽減し，目標位置への正確な位置決めを可能とし，多くの外界センサや位置推定処理アルゴリズムを利用可能であり，なおかつ追加コストのかからない移動ロボットの制御装置を提供するものである。

上記問題を解決するため、本発明は、次のように構成したのである。
本発明は、内界センサで得られる情報と外界センサで得られる情報とから推定した推定現在位置にもとづいて環境中の目標位置に移動ロボットを所定の制御周期で位置決め制御する移動ロボットの制御装置において，前周期の推定現在位置と前記内界センサの観測結果とから短い周期で第１位置推定値を更新する第１位置同定部と，前記外界センサの観測結果から前記第１位置同定部よりも長い周期で第２位置推定値を更新する第２位置同定部と，前記外界センサによる環境計測時の前記推定現在位置である計測推定位置を記憶する計測推定位置記憶部と，前記第１位置推定値，前記第２位置推定値および前記計測推定位置記憶部が記憶した計測推定位置を統合して推定現在位置を求める位置更新演算部と，前記推定現在位置にもとづいて前記移動ロボットの動作を制御する動作制御部と，を備えたことを特徴とするものである。
また、本発明は、前記位置更新演算部は，前記第２位置推定値が得られた周期のみ前記計測推定位置に対する前周期の推定現在位置の相対変位量を第２位置推定値に加算した結果を推定現在位置として出力し，第２位置推定値が得られない周期では第１位置推定値を推定現在位置として出力することを特徴とするものである。
また、本発明は、前周期の推定現在位置と内界センサから得られる情報とから現在位置を推定しつつ外界センサによるランドマーク観測結果にもとづいて算出した環境中の目標位置に移動ロボットを所定の制御周期で位置決め制御する移動ロボットの制御装置において，前周期の推定現在位置と前記内界センサの観測結果とから短い周期で推定現在位置を更新する位置同定部と，前記外界センサによる環境計測時の前記推定現在位置である計測推定位置を記憶する計測推定位置記憶部と，前記外界センサによるランドマーク観測結果から得られる移動ロボットから目標位置までの相対変位量と前記計測推定位置とにもとづいて前記位置同定部よりも長い周期で目標位置を算出する目標位置更新演算部と，前記推定現在位置と前記目標位置にもとづいて移動ロボットの動作を制御する動作制御部と，を備えたことを特徴とするものである。
また、本発明は、前記目標位置更新演算部は，前記計測推定位置に前記移動ロボットから目標位置までの相対変位量を加算した値を目標位置として出力することを特徴とするものである。
また、本発明は、前記動作制御部は，前記目標位置と前記推定現在位置との偏差にもとづいて移動ロボットの速度指令を生成することを特徴とするものである。
また、本発明は、前記動作制御部は，前記目標位置と前記推定現在位置との偏差にもとづいて移動ロボットの加速度指令を生成することを特徴とするものである。
また、本発明は、前記推定現在位置，前記目標位置，およびそれらの時間履歴を画面上に地図と重ねて表示することを特徴とするものである。
また、本発明は、上記した移動ロボットの制御装置と，前記制御装置によって制御される移動ロボットと，を備えたことを特徴とするものである。

本発明によれば，外界センサにもとづく位置推定結果に含まれる遅れ分の補償が，例え位置推定に要する時間が変化しても，可能となる。様々な外界センサにもとづく様々な位置推定アルゴリズムを同じ枠組みで実装できるので，システムの柔軟性が高まる。
また、本発明によれば，外界センサにもとづく位置推定結果に含まれる遅れ分の補償が単純な演算で実現できる。
また、本発明によれば，移動ロボットから目標位置までの相対変位量に含まれる遅れ分の補償が，例え相対変位量算出に要する時間が変化しても，可能となる。様々な外界センサにもとづく様々な相対変位量算出アルゴリズムを同じ枠組みで実装できるので，システムの柔軟性が高まる。
また、本発明によれば，移動ロボットから目標位置までの相対変位量に含まれる遅れ分の補償が単純な演算で実現できる。
また、本発明によれば，外界センサにもとづく位置推定結果にもとづいて，移動ロボットを目標位置まで正確に位置決めできる。
また、本発明によれば，保守管理が容易となる。

以下，本発明の実施の形態について図を参照して説明する。

図１は本発明の第１実施例である移動ロボットの制御装置の構成を示したブロック図である。図１中のブロックは移動ロボットの制御装置の構成要素を，矢印は構成要素間の信号の流れを示している。１００は制御対象である移動ロボット本体，１０１は移動ロボット本体１００の動作を計測するための内界センサ，１０２は移動ロボット本体１００の周囲環境を計測する外界センサ，１０３は内界センサ１０１と前周期の現在推定位置にもとづいて移動ロボット本体１００の位置を推定する位置同定部１，１０４は外界センサ１０２にもとづいて移動ロボット本体１００の位置を推定する位置同定部２，１０５は外界センサ１０２が計測を実施した時の推定現在位置である計測推定位置を記憶する計測推定位置記憶部，１０６は位置推定値１と位置推定値２および計測推定位置から推定現在位置１１７を導く位置更新演算部，１０７は与えられた目標位置１１０と推定現在位置１１７にもとづいて移動ロボット本体１００を駆動制御する動作制御部である。
また，１１０は移動ロボットの操作者あるいは上位制御器から送られる移動ロボットの目標位置，１１１は内界センサ計測結果，１１２は外界センサ計測結果，１１３は外界センサが計測を実行したことを通知する計測実施通知信号，１１４は位置推定値１，１１５は位置推定値２，１１６は計測推定位置，１１７は推定現在位置，１１８は移動ロボット本体１００への動作指令である。

続いて，図１中の構成要素について詳述する。

移動ロボット本体１００は，図示しない車輪と車輪を駆動するモータおよびモータ駆動装置を備えており，動作指令１１８にしたがって平面環境中を移動する。動作指令１１８は，本実施例では移動ロボット本体１００の速度指令であるが，加速度指令であってもよい。

内界センサ１０１は，本実施例では車輪を駆動するモータに付属するロータリーエンコーダであるが，移動ロボット本体１００の動作を計測するものであれば何でもよく，他にジャイロセンサや加速度センサなどを組み合わせた慣性航法モジュールなどが挙げられる。

外界センサ１０２は，本実施例では移動ロボット上に備えた画像センサであるが，移動ロボット本体１００の周囲環境を計測するものであれば何でもよく，他にレーザーレンジファインダや超音波センサアレイなどが挙げられる。外界センサ１０２は，外界センサ計測結果１１２に加えて，環境計測を実行した瞬間に計測実施通知信号１１３を出力する。本実施例における外界センサ計測結果１１２は，画像センサでとらえられた環境画像情報である。

位置同定部１は内界センサ計測結果１１１にもとづいて移動ロボット本体１００の位置を推定する。具体的には，ロータリーエンコーダから得られる車輪の回転速度から算出した移動ロボット本体の移動速度を前周期の推定現在位置１１７を初期値として積分し，位置推定値１として出力する。この処理はオドメトリとも呼ばれ，一般に計算負荷が小さく，ミリ秒オーダの比較的短い周期で実行する。本処理によって移動ロボット本体１００の動作を反映した時間・空間分解能の高い位置推定値１が得られるが，内界センサ計測結果１１１に含まれるバイアス誤差が時間の経過とともに蓄積するという欠点がある。

位置同定部２は外界センサ計測結果１１２にもとづいて移動ロボット本体１００の位置を推定する。具体的には，画像センサで得られた環境画像情報からランドマーク画像を抽出し，ランドマーク画像と予め図示しないデータベースに登録しておいたランドマーク情報とを照合して移動ロボット本体１００の位置を推定，位置推定値２として出力する。この処理は情報量の大きな画像を扱うため，一般に計算負荷が大きく，汎用的な演算装置では数十から数百ミリ秒オーダの比較的長い周期で実行する。本処理によって得られる位置推定値２は時間分解能が低く処理に要する時間遅れが大きいという欠点があるが，内界センサにもとづく位置推定にみられるようなバイアス誤差の蓄積が起こらないため，位置推定値１の補正に使用することができる。

計測推定位置記憶部１０５は，計測実施通知信号１１３を受けた瞬間の推定現在位置１１７を計測推定位置１１６として記憶する。

位置更新演算部１０６は，位置推定値１，位置推定値２および計測推定位置１１６を統合して推定現在位置１１７を算出し出力する。位置更新演算部１０６での処理内容は後述する。

動作制御部１０７は，目標位置１１０と推定現在位置１１７との偏差にもとづいて移動ロボット本体１００への動作指令１１８を算出し出力する。動作指令１１８は，具体的には移動ロボット本体１００の速度指令であって，目標位置１１０と推定現在位置１１７との偏差が零となる点で移動ロボット本体１００を停止できるような指令である。

次に，位置更新演算部１０６の処理について，その流れ図(図２)を用いて説明する。図２の処理は，位置推定値１又は位置推定値２の何れかが得られたときに毎回実行する。図２において，Ｓ１は得られた位置推定値が位置推定値２であるか否かを判断するステップ，Ｓ２は推定現在位置を位置推定値１とするステップ，Ｓ３は推定現在位置を位置推定値２および計測推定位置から算出するステップ，Ｓ４は算出した指定現在位置を出力するステップである。

ステップＳ３で推定現在位置を算出する方法を以下に示す。

まず，数式記述のための変数を定義する。移動ロボット本体１００の現在位置Ｐ［ｉ］は，環境に固定した座標軸x-yおよびx軸に対する移動ロボットの向きaで攻勢されるベクトルである。すなわち，

である。ここで［ｉ］は，値が現周期ｉのものであることを示す。また，その推定値である推定現在位置１１７は

位置推定値２は，

計測推定位置１１６は，

と表す。なお，計測推定位置１１６は外界センサによる環境計測実行時の推定現在位置なので，

である。ここでｊは環境計測実行時から位置推定値２が得られるまでに要する時間に相当する正数である。

ステップＳ３では，まず計測推定位置１１６に対する前周期の推定現在位置１１７の相対変位量Δ［ｉ］を求める。

次いで，式（１）の演算により推定現在位置１０７を算出する。

以上のような処理によれば，外界センサにもとづく位置推定に要する時間分の遅れを補償した推定現在位置が得られるため，これを動作制御部で使用することで制御ループに含まれる遅れ要素を除去でき，したがって停止位置付近での振動現象を誘発することなく正確に移動ロボットを目標位置に位置決めできるのである。なお，この処理は内界／外界センサの種類，位置推定に用いるアルゴリズムおよびその所要時間に非依存であり，したがって様々なセンサやアルゴリズムに対応可能である。

本発明は，従来技術では問題とされていなかった外界センサにもとづく位置推定結果に含まれる遅れ要素を補償し，結果として遅れ要素に起因する振動現象の発生を抑制しつつ目標位置への正確な位置決めを可能とするものである。センサや位置推定アルゴリズムに非依存であるため，システムの柔軟性は極めて高い。位置推定処理のために高速な演算装置を特別に設ける必要がないのも利点である。

図３は本発明の第２実施例である移動ロボットの制御装置の構成を示したブロック図である。図中の構成要素の多くは実施例１の図１と共通であるが，本実施例における新たな構成要素について以下で説明する。図において３０１は目標位置更新演算部，３０２は内界センサにもとづく位置同定処理を実行する位置同定部，３１０は移動ロボットの操作者あるいは上位制御器から送られるランドマークと目標位置の位置関係を記述した目標位置情報，３１１は位置同定部３０２が出力する，内界センサにもとづく推定現在位置である。

目標位置更新演算部３０１は，外界センサ計測結果１１２と計測推定位置１１６および目標位置情報３１０を用いて目標位置１１０を逐次算出し動作制御部１０７に出力する。外界センサ１０２は本実施例においても画像センサであり，外界センサ計測結果１１２は画像センサで得られた環境画像情報である。目標位置更新演算部３０１では，まず環境画像情報からランドマーク画像を抽出し，その移動ロボット本体１００に対する相対位置を特定する。この処理は大きな情報を扱うため計算負荷が大きく，時間を要する。そして，目標位置情報３１０から得られる該ランドマークと目標位置の位置関係を用いて，目標位置の移動ロボット本体１００に対する相対位置Ｄ_ｒｅｌ［ｉ］を求める。ランドマーク画像抽出および位置特定処理に要する時間が短ければ，Ｄ_ｒｅｌ［ｉ］は移動ロボット本体１００と目標位置との偏差と等価であるから，Ｄ_ｒｅｌ［ｉ］をそのまま利用して移動ロボット本体１００の動作を制御することも可能である。しかし，本実施例のように処理に要する時間が長い場合，遅れにより動作が振動的となってしまう。そこで，

を用いて目標位置Ｄ［ｉ］を次式により算出し，動作制御部１０７に送る。

前記Ｄ_ｒｅｌ［ｉ］が移動ロボット本体１００の移動に伴って変化する量であるのに対し，数９で算出した目標位置Ｄ［ｉ］は理論上移動の影響を受けない定数である。したがって以上のような処理によれば，外界センサにもとづくランドマーク画像抽出および位置特定に要する時間分の遅れを補償し，正しい目標位置が安定して得られるため，これを動作制御部で使用することで制御ループに含まれる遅れ要素を除去でき，したがって停止位置付近での振動現象を誘発することなく正確に移動ロボットを目標位置に位置決めできるのである。なお，この処理は内界／外界センサの種類，ランドマーク／目標位置特定に用いるアルゴリズムおよびその所要時間に非依存であり，したがって様々なセンサやアルゴリズムに対応可能である。

また，推定現在位置と目標位置およびそれらの履歴を移動ロボット上あるいは遠隔地に設置した画面上で地図と重ねて表示するようにすれば，操作者が逐次移動ロボットの状態を確認することができ，保守管理上有益である。

外界センサにもとづく位置推定値に含まれる遅れ要素を除去することによって移動ロボットを目標位置に正確に位置決めできるので，物品自動搬送ロボットや案内ロボットなど，複数点間の移動を含む作業を遂行する様々なロボットに広く適用できる。

本発明の第１実施例である移動ロボットの制御装置の構成を示したブロック図 位置更新演算部における処理の流れ図 本発明の第２実施例である移動ロボットの制御装置の構成を示したブロック図 特許文献１のロボット掃除機システムを示したブロック図

符号の説明

１００ 移動ロボット本体
１０１ 内界センサ
１０２ 外界センサ
１０３ 位置同定部１
１０４ 位置同定部２
１０５ 計測推定位置記憶部
１０６ 位置更新演算部
１０７ 動作制御部
１１０ 目標位置
１１１ 内界センサ計測結果
１１２ 外界センサ計測結果
１１３ 計測実施通知信号
１１４ 位置推定値１
１１５ 位置推定値２
１１６ 計測推定位置
１１７ 推定現在位置
１１８ 動作指令
３０１ 目標位置更新演算部
３０２ 位置同定部
３１０ 目標位置情報
３１１ 推定現在位置
４０１ エンコーダ
４０２ 上方カメラ
４０３ 駆動部
４０４ 記憶装置
４０５ 制御部

Claims (6)

1. 前周期の推定現在位置と内界センサから得られる情報とから現在位置を推定しつつ外界センサによるランドマーク観測結果にもとづいて算出した環境中の目標位置に移動ロボットを所定の制御周期で位置決め制御する移動ロボットの制御装置において，
   前周期の推定現在位置と前記内界センサの観測結果とから所定の周期で推定現在位置を更新する位置同定部と，
   前記外界センサによる環境計測時の前記推定現在位置である計測推定位置を記憶する計測推定位置記憶部と，
   前記外界センサによるランドマーク観測結果から得られる移動ロボットから目標位置までの相対変位量と前記計測推定位置とにもとづいて前記位置同定部よりも長い周期で目標位置を更新する目標位置更新演算部と，
   前記推定現在位置と前記目標位置更新演算部によって更新された前記目標位置とにもとづいて移動ロボットの動作を制御する動作制御部と，を備えたことを特徴とする移動ロボットの制御装置。
2. 前記目標位置更新演算部は，前記計測推定位置に前記移動ロボットから目標位置までの相対変位量を加算した値を目標位置として出力することを特徴とする請求項１に記載の移動ロボットの制御装置。
3. 前記動作制御部は，前記目標位置と前記推定現在位置との偏差にもとづいて移動ロボットの速度指令を生成することを特徴とする請求項１または２に記載の移動ロボットの制御装置。
4. 前記動作制御部は，前記目標位置と前記推定現在位置との偏差にもとづいて移動ロボットの加速度指令を生成することを特徴とする請求項１，２または３に記載の移動ロボットの制御装置。
5. 前記推定現在位置と，前記目標位置と，前記推定現在位置および前記目標位置の時間履歴とを画面上に地図と重ねて表示することを特徴とする請求項１〜４のいずれか一つに記載の移動ロボットの制御装置。
6. 請求項１〜５のいずれか一つに記載の移動ロボットの制御装置と，
   前記制御装置によって制御される移動ロボットと，を備えたことを特徴とする移動ロボットシステム。