JP6075343B2

JP6075343B2 - 走行ロボット、その動作計画方法及びプログラム

Info

Publication number: JP6075343B2
Application number: JP2014177931A
Authority: JP
Inventors: 達礒部
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2014-09-02
Filing date: 2014-09-02
Publication date: 2017-02-08
Anticipated expiration: 2034-09-02
Also published as: JP2016049616A; DE102015114584A1; DE102015114584B4; CN105382833A; US20160059408A1; CN105382833B; US10065306B2

Description

本発明は、目標位置まで走行しアームを用いてタスクを実行する走行ロボット、その動作計画方法及びプログラムに関するものである。

車輪を有する移動体と、移動体に支持されると共に移動体に対して相対変位可能に連結されたアームと、を備える走行ロボットが知られている。

特開２０１３−０９４９３４号公報

上記走行ロボットは、走行中、重心位置を一定の所定範囲内に制限して、各動作部の動作を計画している。この場合、所定範囲が広いと移動体の車輪の滑りなどによる走行ロボットの位置決め精度の悪化が懸念される。一方、所定範囲が狭いと走行ロボットの動作が制限され、各動作部を高速に動作させることが困難となる。

本発明は、このような問題点を解決するためになされたものであり、走行ロボットの位置決め精度と動作性を両立できる走行ロボット、その動作計画方法及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、車輪を有する移動体と、前記移動体に支持されると共に前記移動体に対して相対変位可能に連結されたアームと、当該走行ロボットが目標とする目標位置を設定する目標位置設定手段と、当該走行ロボットの重心位置を算出する重心位置算出手段と、前記目標位置設定手段により設定された目標位置までの前記移動体及び／又はアームの動作部の動作を計画する動作計画手段と、を備える走行ロボットであって、前記動作計画手段は、当該走行ロボットが前記目標位置設定手段により設定された目標位置に近い場合、遠い場合と比較して、前記重心位置の範囲に対する制限を緩和して、前記移動体及び／又はアームの動作部の動作を計画する、ことを特徴とする走行ロボットである。
この一態様において、前記動作計画手段は、前記重心位置算出手段により算出された重心位置を所定範囲内に制限して当該走行ロボットの各動作部の動作を計画しており、当該走行ロボットの位置が前記目標位置設定手段により設定された目標位置から所定距離以内で、前記重心位置に対する所定範囲内の制限を解除して前記移動体及び／又はアームの動作部の動作を計画してもよい。
この一態様において、前記動作計画手段は、前記重心位置算出手段により算出された重心位置を所定範囲内に制限して前記移動体及び／又はアームの動作部の動作を計画しており、当該走行ロボットの位置と前記目標位置設定手段により設定された目標位置との距離に応じて前記所定範囲を変化させてもよい。
この一態様において、前記動作計画手段は、当該走行ロボットの位置が前記目標位置設定手段により設定された目標位置に近付くに従がって前記所定範囲を大きくしてもよい。
この一態様において、前記動作計画手段は、前記移動体及び前記アームの動作部が協調する動作を計画してもよい。
この一態様において、前記アームは、操作対象物を把持するための把持部を有しており、前記動作計画手段は、前記アームの把持部が前記目標位置設定手段により設定された目標位置から所定距離以内で、前記重心位置に対する所定範囲内の制限を解除して前記移動体及び／又はアームの動作部の動作を計画してもよい。
上記目的を達成するための本発明の一態様は、車輪を有する移動体と、前記移動体に支持されると共に前記移動体に対して相対変位可能に連結されたアームと、を備える走行ロボットの動作計画方法であって、前記走行ロボットが目標とする目標位置を設定するステップと、前記走行ロボットの重心位置を算出するステップと、前記走行ロボットが前記設定された目標位置に近い場合、遠い場合と比較して、前記重心位置の範囲に対する制限を緩和して、前記移動体及び／又はアームの動作部の動作を計画するステップと、を含む、ことを特徴とする走行ロボットの動作計画方法であってもよい。
上記目的を達成するための本発明の一態様は、車輪を有する移動体と、前記移動体に支持されると共に前記移動体に対して相対変位可能に連結されたアームと、を備える走行ロボットのプログラムであって、前記走行ロボットが目標とする目標位置を設定する処理と、前記走行ロボットの重心位置を算出する処理と、前記走行ロボットが前記設定された目標位置に近い場合、遠い場合と比較して、前記重心位置の範囲に対する制限を緩和して、前記移動体及び／又はアームの動作部の動作を計画する処理と、をコンピュータに実行させる、ことを特徴とする走行ロボットのプログラムであってもよい。

本発明によれば、走行ロボットの位置決め精度と動作性を両立できる走行ロボット、その動作計画方法及びプログラムを提供することができる。

本発明の実施形態１に係る走行ロボットの概略的構成を示す斜視図である。本発明の実施形態１に係る走行ロボットの概略的システム構成を示すブロック図である。本発明の実施形態１に係る制御装置の概略的システム構成を示すブロック図である。走行ロボットの移動例を示す図である。重心位置を制約する所定範囲の一例を示す図である。本発明の実施形態１に係る走行ロボットの動作計画方法を示すフローチャートである。本発明の実施形態２に係るノード探索方法のフローを示すフローチャートである。オムニホイールを有する構成の重心位置の所定範囲を示す図である。重心位置を制約する所定範囲の設定方法を示す図である。

実施の形態１
以下、図面を参照して本発明の実施の形態について説明する。
図１は、本発明の実施形態１に係る走行ロボットの概略的構成を示す斜視図である。図２は、本発明の実施形態１に係る走行ロボットの概略的システム構成を示すブロック図である。本実施形態１に係る走行ロボット１は、移動体２と、移動体２に支持されたアーム３と、を備えている。

移動体２は、胴体部２１と、頭部２２と、移動装置２３と、を有している。胴体部２１には、肩関節部３３を介してアーム３が相対変位可能に連結されている。頭部２２は、首関節部２５を介して胴体部２１に回動可能に設けられている。

移動装置２３は、胴体関節部２６を介して胴体部２１の下部に旋回可能に連結されている。移動装置２３は、例えば、一対の駆動車輪２７と、一対の左右方向に回動可能な従動車輪２８と、が設けられたアクティブキャスタ式の全方位台車として構成されている（図５）。移動装置２３は、各駆動車輪２７を回転駆動することで、走行ロボット１の前後進、左右旋回、加減速、停止などの任意の走行を行うことができる。

アーム３は、例えば、多関節型アームとして構成されている。アーム３は、手首関節部３１、肘関節部３２、肩関節部３３などの各関節部を介して相互に連結された複数のリンク３４と、リンク３４の先端に連結され、操作対象物の把持、吸引、引掛け等を行うエンドエフェクタ（ハンド機構、吸引機構、引掛け機構などを含む）３５と、を有している。なお、上記走行ロボット１の構成は一例であり、これに限定されない。

本実施形態１に係る走行ロボット１は、各駆動車輪２７を駆動する一対のモータ４と、各関節部を駆動するサーボモータなど複数のアクチュエータ５と、制御装置６と、複数の回転センサ７と、複数の角度センサ８と、を備えている。

各モータ４は、移動体２の移動装置２３に設けられている。各モータ４は、制御装置６から制御信号に応じて各駆動車輪２７を駆動する。各駆動車輪２７には回転センサ７が夫々設けられている。各回転センサ７は、各駆動車輪２７の回転情報を検出し、制御装置６に出力する。制御装置６は、各回転センサ７から出力される回転情報に基づいて、各モータ４を制御する。

各アクチュエータ５は、アーム３、移動体２の胴体部２１、および頭部２２の各関節部に設けられている。アクチュエータ５は、制御装置６から制御信号に応じて各関節部を回転駆動する。各関節部には、各関節部の回転角情報を検出する、ポテンショメータ、エンコーダなどの角度センサ８が設けられている。各角度センサ８は、各関節部の回転角情報を検出し、制御装置６に出力する。制御装置６は、各角度センサ８から出力される回転角情報に基づいて、各関節部のアクチュエータ５のフィードバック制御などを行う。

図３は、本発明の実施形態１に係る制御装置の概略的システム構成を示すブロック図である。本実施形態１に係る制御装置６は、目標位置算出部６１と、動作算出部６２と、軌道補完部６６と、を有している。

制御装置６は、例えば、制御処理、演算処理等と行うＣＰＵ（Central Processing Unit）６ａ、ＣＰＵ６ａによって実行される制御プログラム、演算プログラム等が記憶されたＲＯＭ（Read Only Memory）６ｂ、処理データ等を記憶するＲＡＭ（Random Access Memory）６ｃ等からなるマイクロコンピュータを中心にして、ハードウェア構成されている。

目標位置算出部６１は、目標位置設定手段の一具体例であり、ユーザにより設定された操作情報に従って、走行ロボット１の初期の位置及び姿勢（以下、初期位置と称す）と、終端の位置及び姿勢（目標位置の一例であり、以下、終端位置と称す）と、を算出する。ここで、終端位置は、走行ロボット１が操作対象物に対してタスクを実現するためのエンドエフェクタ３５の位置及び姿勢を含む。

終端位置は、例えば、操作対象物の形状、特性、目的、実施するタスクなどによって異なる値をとり得る。本実施形態１において、目標位置算出部６１は、例えば、走行ロボット１の物取りタスクを想定して初期位置及び終端位置を算出している。目標位置算出部６１は、図４に示す如く、走行ロボット１が初期位置から操作対象物に十分接近した終端位置Ａに到達し、その終端位置Ａから把持位置（終端位置Ｂ）に到達するまでの各関節部の軌道を示す関節軌道を算出する。目標位置算出部６１は、例えば、初期位置から終端位置Ａまでについて、手先（エンドエフェクタ）姿勢の拘束が存在しない関節軌道を算出する。一方で、目標位置算出部６１は、終端位置Ａから終端位置Ｂまでについて、手先姿勢の拘束が存在する関節軌道を算出する。

なお、上記一例では、走行ロボット１は物取りタスクを実行しているが、これに限らず、例えば、物を置くタスク、物を押すタスク、などの手先の位置決めが必要な任意のタスクを実行可能である。目標位置算出部６１は、算出した初期位置及び終端位置を動作算出部６２に出力する。

動作算出部６２は、目標位置算出部６１により算出された初期位置と終端位置とをつなぐ走行ロボット１の関節空間における各関節部の関節軌道を各種の制約条件を考慮して探索することで、オフラインで各関節部の動作を計画する。動作算出部６２は、例えば、移動体２及びアーム３の各関節部が協調する動作を計画する。動作算出部６２は、動作計画部６３と、重心位置算出部６４と、制約条件部６５と、を有している。

制約条件部６５は、走行ロボット１と環境などとの干渉、走行ロボット１の各関節部の関節角可動限界、走行ロボット１の重心位置などの各種の制約条件を設定する。

動作計画部６３は、動作計画手段の一具体例であり、例えば、ＲＲＴ（Rapidly-exploring Random Tree）、Ａ＊（Ａ−star）、ダイクストラ法等のサンプリングベースの探索手法あるいは非線形計画法などを用いてノード（ｑ_ｎｅｗ）を探索することで、各関節部の関節軌道を計画する。

動作計画部６３は、例えば、制約条件部６５により設定された各種制約条件を満たすか否かを判定しつつ、上記ノード探索を実施する。そして、動作計画部６３は、終端位置に十分近いノードを得たときノード探索を終了する。

動作計画部６３は、制約条件部６５により設定された干渉の制約条件に基づいて、関節角空間でノード探索を実施する。このため、動作計画部６３は、逐次順運動学計算を用いて操作対象物と環境、および、自己干渉がないノード集合を獲得する。

重心位置算出部６４は、重心位置算出手段の一具体例であり、動作計画部６３により探索されたノード（ｑ_ｎｅｗ）に基づいて、走行ロボット１の重心位置を算出する。重心位置算出部６４は、例えば、動作計画部６３により探索されたノード（ｑ_ｎｅｗ）、アーム３の各リンク３４の質量、幾何学構造、エンドエフェクタ３５が把持する把持物体の質量、現在の各関節部の姿勢（回転角）などに基づいて、走行ロボット１の重心位置を算出する。重心位置算出部６４は、算出した走行ロボット１の重心位置を動作計画部６３に出力する。

ところで、駆動車輪を有する移動体と、移動体に連結されたアームと、を備える走行ロボットが物体を把持するなどのマニュピュレーションタスクを実行する場合、移動体の位置精度が非常に重要となる。しかしながら、従来の走行ロボットにおいては、移動体の各駆動車輪の滑りや各駆動車輪に対する荷重のアンバランスによって、走行ロボットの位置精度に誤差が生じている。特に、移動体が大きな距離を移動する場合、その誤差は累積し大きくなる。さらに、アームの姿勢や把持する物体の重量によっても、走行ロボットの重心位置は変化する。

これに対して、本実施形態１に係る走行ロボット１においては、当該走行ロボット１が目標位置算出部６１により設定された終端位置に近い場合、遠い場合と比較して、重心位置の範囲に対する制限を緩和して、当該走行ロボット１の各動作部の動作を計画する。

これにより、走行ロボット１が終端位置に接近する前までは、走行ロボット１の重心位置を通常どおり制限する。このため、上述した移動体２の各駆動車輪２７の滑りや各駆動車輪２７に対する荷重のアンバランスなどを小さく抑制した状態で走行できる。したがって、走行ロボット１の位置精度の誤差を小さく抑え、オドメトリ精度を向上させることができる。

一方、走行ロボット１が終端位置に接近した後は、重心位置の制限をより緩和する。このため、より少ない制約条件でノードを探索でき、走行ロボット１の動作が制約されない。したがって、高速に最終姿勢に到達できる可能性が高くなる。特に、終端位置付近では、手先位置姿勢がより大きく制約されるため、上述の如く、重心位置の制約を緩和することで、手先位置姿勢の制約条件を満たす解を高速に得ることができる。

例えば、動作計画部６３は、重心位置算出部６４により算出された重心位置を所定範囲内に制限して走行ロボット１の各関節部の動作を計画する。さらに、動作計画部６３は、目標位置算出部６１により設定された終端位置から所定距離以内で、重心位置に対する所定範囲内の制限を解除して走行ロボット１の各関節部の動作を計画する。

この場合、走行ロボット１が終端位置から所定距離以内に接近する前までは、走行ロボット１の重心位置を所定範囲内に制限する。これにより、上述した移動体２の各駆動車輪２７の滑りなどを小さく抑制した状態で走行でき、走行ロボット１の位置精度を向上させることができる。一方、走行ロボット１が終端位置から所定距離以内に接近した後は、重心位置に対する所定範囲内の制限を解除する。これにより、より少ない制約条件でノードを探索でき、走行ロボット１の動作が制約されないため、高速に最終姿勢に到達できる可能性が高くなる。

このように、本実施形態１に係る走行ロボット１は、操作対象物に接近するまでは重心位置を重視した各関節部の動作を計画することで、その位置精度を向上させることができる。そして、走行ロボット１は、操作対象物に十分に接近した後、重心位置の制約を緩和し動作性を重視した各関節部の動作を計画することで、各関節部を高速に動作させることができる。すなわち、走行ロボット１の位置決め精度と動作性を両立できる。

また、ドア開けのような大きな手先移動が必要なタスクや遠くある操作対象物を高速に把持する場合において、本実施形態１に係る走行ロボット１は、移動装置２３の併進自由度を積極的に活用して移動を行いながら、操作対象物を把持できる。これにより、大作業域への対応、作業スピード向上、アーム自由度の簡略化によるコスト低減などの効果が期待できる。

動作計画部６３は、重心位置算出部６４により算出された走行ロボット１の重心位置が制約条件部６５により設定された所定範囲内にあり制約条件を満たすか否かを判定する。移動装置２３が、上述の如く、アクティブキャスタ式の全方位台車として構成されている場合、所定範囲は、図５に示す如く、一対の駆動車輪２７の中心付近に位置する楕円となる。

動作計画部６３は、例えば、重心位置算出部６４により算出された走行ロボット１の重心位置（ｐ_ｇ１、ｐ_ｇ２）が制約条件部６５により設定された下記式の制約条件を満たし、所定範囲（上記楕円）内に存在するように制限して、ノードの探索を行う。
（ｐ_ｇ１−α）^２／ａ^２＋（ｐ_ｇ２−β）^２／ｂ^２＜１

但し、上記式において、ｐ_ｇ１、ｐ_ｇ２は、走行ロボット１の略中心を原点とした２次元座標系における走行ロボット１の重心位置のｘ、ｙ座標である。ａは上記所定範囲を示す楕円の長軸長さであり、ｂは短軸長さである。α、βは、走行ロボット１の機構によって任意にとり得る値である。本実施形態１のように、移動装置２３の駆動車輪２７が対向している構成の場合、αは駆動軸の近くの値に設定し、βは０付近に設定するのが好ましい。なお、上記重心位置に対する所定範囲は一例であり、任意に設定変更できる。

ここで、上記重心位置は、２次元座標系で表現されているが、３次元座標系で表現されてもよい。この場合、重心位置を制約する所定範囲は、例えば、楕円体となる。この所定範囲を表す楕円体のＸ軸及びＹ軸方向の成分には、例えば厳しい（より小さい）値が設定され、Ｚ軸方向の成分にはより緩い（より大きな）値（あるいは制限が無視される大きな値）が設定されてもよい。

動作計画部６３は、走行ロボット１から操作対象物までの距離が所定距離より大きいか否かを判定する。動作計画部６３は、例えば、アーム３のエンドエフェクタ（把持部）３５から操作対象物までの距離が所定距離より大きいか否かを判定する。そして、動作計画部６３は、エンドエフェクタ３５（の先端あるいは重心）から操作対象物までの距離が所定距離以内と判定すると、重心位置に対する所定範囲内の制限を解除して各関節部の動作を計画する。最終的に距離の変位が生じ易くなるエンドエフェクタ３５と操作対象物との距離に基づいて、重心位置に対する所定範囲の制限を解除するため、エンドエフェクタ３５を目標位置に素早く到達させることができる。

動作計画部６３は、例えば、下記条件式を満たした場合、上記式の制約条件を満たすようにノード探索を行い、走行ロボット１の各関節部の動作を計画する。下記条件式を満たす場合、走行ロボット１から終端位置までの距離が所定距離γより大きく、走行ロボット１が操作対象物から離れている。
｜Ｐ_ｏｂｊ−Ｐ_ｅｅ｜＞γ

なお、上記式において、Ｐ_ｏｂｊは、走行ロボット１の座標系における操作対象物の位置である。Ｐ_ｅｅは、走行ロボット１の座標系におけるアーム３の手先位置である。所定距離γは、例えば、アーム動作範囲、タスク内容などに基づいて設定されており、より小さい値を設定するのが好ましい。動作計画部６３は、上記条件式を満たす解が得られない場合、所定距離γを大きくして再度、ノード探索を行ってもよい。

一方、動作計画部６３は、下記条件式を満たさない場合、上記式の制約条件を解除して、ノード探索を行う。なお、動作計画部６３は、走行ロボット１から終端位置までの距離が所定距離より大きいか否かを判定して、上記式の制約条件を満たすように、ノード探索を行ってもよい。動作計画部６３は、上述のようにして探索したノード集合を軌道補完部６６に出力する。

軌道補完部６６は、動作計画部６３からのノード集合を滑らかな時間に関する関節軌道に変換する。制御装置６は、変換された関節軌道に基づいて各関節部を制御するための制御信号を生成する。そして、制御装置６は、生成した制御信号を各関節部のアクチュエータ５に出力することで、各関節部を制御する。なお、動作算出部６２は、上述の如く、オフラインで各関節部の動作を計画しているが、これに限らず、オンライン（走行ロボット１の動作中）で各関節部の動作を計画してもよい。

図６は、本実施形態１に係る走行ロボットの動作計画方法を示すフローチャートである。
目標位置算出部６１は、ユーザにより設定された操作情報に従って、走行ロボット１の初期位置および終端位置を算出し（ステップＳ１０１）、算出した初期位置及び終端位置を動作算出部６２に出力する。

動作算出部６２の動作計画部６３は、目標位置算出部６１により算出された初期位置と終端位置とをつなぐ走行ロボットの関節空間において、例えば、サンプリングベースの探索手法を用いて、新しいノード候補（ｑ_ｎｅｗ）を取得する（ステップＳ１０２）。

動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、走行ロボット１の手先と操作対象物との距離が所定距離γより大きいか否かを判定する（ステップＳ１０３）。

動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、走行ロボット１の手先と操作対象物との距離が所定距離γより大きいと判定したとき（ステップＳ１０３のＹＥＳ）、走行ロボット１の重心位置が所定範囲内にあり制約条件を満たすか否かを判定する（ステップＳ１０４）。一方、動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、走行ロボット１の手先と操作対象物との距離が所定距離γ以下であると判定したとき（ステップＳ１０３のＮＯ）、後述の（ステップＳ１０５）の処理に移行する。

動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、走行ロボット１の重心位置が所定範囲内にあり制約条件を満たすと判定したとき（ステップＳ１０４のＹＥＳ）、走行ロボット１の干渉、走行ロボット１の各関節部の関節角可動限界などの他の制約条件を満たすか否かを判定する（ステップＳ１０５）。一方、動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、走行ロボット１の重心位置が所定範囲内になく制約条件を満たさないと判定したとき（ステップＳ１０４のＮＯ）、そのノード候補（ｑ_ｎｅｗ）を削除し、上記（ステップＳ１０２）に戻る。

動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、他の制約条件を満たすと判定したとき（ステップＳ１０５のＹＥＳ）、そのノード候補（ｑ_ｎｅｗ）を探索木に加える（ステップＳ１０６）。一方、動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、他の制約条件を満たさないと判定したとき（ステップＳ１０５のＮＯ）、そのノード候補（ｑ_ｎｅｗ）を削除し、上記（ステップＳ１０２）に戻る。

動作計画部６３は、目標位置算出部６１により算出された終端位置とノード候補（ｑ_ｎｅｗ）が十分に近いか否かを判定する（ステップＳ１０７）。動作計画部６３は、終端位置とノード候補（ｑ_ｎｅｗ）が十分に近いと判定したとき（ステップＳ１０７のＹＥＳ）、ノード探索を終了し、得られたノード候補の集合を軌道補完部６６に出力する（ステップＳ１０８）。一方、動作計画部６３は、終端位置とノード候補（ｑ_ｎｅｗ）が十分に近くないと判定したとき（ステップＳ１０７のＮＯ）、上記（ステップＳ１０２）に戻る。
軌道補完部６６は、動作計画部からのノード候補の集合を滑らかな時間に関する各関節部の関節軌道に変換する（ステップＳ１０９）。

以上、本実施形態１に係る走行ロボット１においては、当該走行ロボット１が目標位置算出部６１により設定された終端位置に近い場合、遠い場合と比較して、重心位置の範囲に対する制限を緩和して、当該走行ロボット１の各動作部の動作を計画する。
これにより、走行ロボット１は、目標位置に接近するまでは重心位置を重視した各動作部の動作を計画することで、位置精度を向上させることができる。そして、走行ロボット１は、操作対象物に十分に接近した後、重心位置の制約を緩和し動作性を重視した各動作部の動作を計画することで、各動作部を高速に動作させることができる。すなわち、走行ロボット１の位置決め精度と動作性を両立できる。

実施形態２．
本発明の実施形態２に係る動作計画部は、写像を用いて、ノード候補（ｑ_ｎｅｗ）を走行ロボット１の重心位置が制約条件を満たすものに明示的に近づける点を特徴とする。これにより、ノードをより高速に探索できる。

図７は、本実施形態２に係るノード探索方法のフローを示すフローチャートである。
動作算出部６２の動作計画部６３は、目標位置算出部６１により算出された初期位置と終端位置とをつなぐ走行ロボット１の関節空間において、例えば、サンプリングベースの探索手法を用いて、新しいノード候補（ｑ_ｎｅｗ）を取得する（ステップＳ２０１）。

動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、走行ロボット１の手先と操作対象物との距離が所定距離γより大きいか否かを判定する（ステップＳ２０２）。

動作計画部６３は、取得したノード候補（ｑ_ｎｅｗ）に関して、走行ロボット１の手先と操作対象物との距離が所定距離γより大きいと判定したとき（ステップＳ２０２のＹＥＳ）、ノード候補を修正し、走行ロボット１の重心位置が制約条件を満たすもの近づける。ここで、走行ロボット１の重心位置Ｐｇ（ｑ）の制約条件を下記式で定義する。
Ｐ_ｇｍｉｎ＜Ｐ_ｇ（ｑ）＜Ｐ_ｇｍａｘ

但し、上記制約条件式において、Ｐ_ｇｍｉｎ、Ｐ_ｇ（ｑ）、Ｐ_ｇｍａｘは夫々３次元のベクトルとし、そのｉ番目の要素をＰ_ｇｉと表記する。Δｘ＝（Δｘ１、Δｘ２、Δｘ３）として、下記式で定義する。

一方、重心速度と関節軸速度との関係は、一般に下記式で表すことができる。

上記式において、Δｑ＝Ｊｇ^＋Δｘ（Ｊｇ^＋はＪｇの疑似逆行列）として、下記式が導出できる。
Δｑ＝Ｊ_ｇ（ｑ_ｎｅｗ）^＋Δｘ

動作計画部６３は、下記式を用いて、ノード候補（ｑ_ｎｅｗ）をノード候補（ｑ’_ｎｅｗ）に修正する（ステップＳ２０３）。これにより、ノード（ｑ_ｎｅｗ）を走行ロボット１の重心位置が制約条件を満たすものに明示的に近づけることができる。
ｑ’_ｎｅｗ＝ｑ_ｎｅｗ−Δｑ

動作計画部６３は、（ｑ’_ｎｅｗ−ｑ_ｎｅｗ）のノルムが所定値以下で小さいか否かを判定する（ステップＳ２０４）。動作計画部６３は、（ｑ’_ｎｅｗ−ｑ_ｎｅｗ）のノルムが小さいと判定したとき（ステップＳ２０４のＹＥＳ）、そのノード候補ｑ’_ｎｅｗに関して、走行ロボット１の重心位置Ｐ_ｇ（ｑ）が制約条件式を満たすと判断する。そして、動作計画部６３は、走行ロボット１の干渉、走行ロボット１の各関節部の関節角可動限界などの他の制約条件を満たすか否かを判定する（ステップＳ２０５）。一方、動作計画部６３は、ｑ’_ｎｅｗ−ｑ_ｎｅｗのノルムが小さいと判定したとき（ステップＳ２０４のＮＯ）、そのノード候補（ｑ_ｎｅｗ）を削除し、上記（ステップＳ２０１）に戻る。

動作計画部６３は、ノード候補（ｑ’_ｎｅｗ）に関して、他の制約条件を満たすと判定したとき（ステップＳ２０５のＹＥＳ）、そのノード候補（ｑ’_ｎｅｗ）を探索木に加える（ステップＳ２０６）。一方、動作計画部６３は、他の制約条件を満たさないと判定したとき（ステップＳ２０６のＮＯ）、そのノード候補（ｑ_ｎｅｗ）を削除し、上記（ステップＳ２０１）に戻る。

動作計画部６３は、目標位置算出部６１により算出された終端位置とノード候補（ｑ’_ｎｅｗ）が十分に近いか否かを判定する（ステップＳ２０７）。動作計画部６３は、終端位置とノード候補（ｑ’_ｎｅｗ）が十分に近いと判定したとき（ステップＳ２０７のＹＥＳ）、ノード探索を終了し、得られたノード候補の集合を軌道補完部６６に出力する（ステップＳ２０８）。一方、動作計画部６３は、終端位置とノード候補（ｑ’_ｎｅｗ）が十分に近くないと判定したとき（ステップＳ２０７のＮＯ）、上記（ステップＳ２０１）に戻る。

軌道補完部６６は、動作計画部６３からのノード候補の集合を滑らかな時間に関する各関節部の関節軌道に変換する（ステップＳ２０９）。

以上、本実施形態２によれば、ノードを走行ロボット１の重心位置が制約条件を満たすものに明示的に近づけることから、ノードをより高速に探索でき、処理速度を高速化できる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

上記実施形態において、動作計画部６３は、目標位置算出部６１により設定された終端位置から所定距離以内で、重心位置に対する所定範囲内の制限を解除して走行ロボット１の各関節部の動作を計画しているが、これに限らない。動作計画部６３は、走行ロボット１の位置と目標位置算出部６１により設定された終端位置との距離に応じて、重心位置に対する所定範囲を連続的あるいは段階的にに変化させてもよい。例えば、動作計画部６３は、走行ロボット１の位置が目標位置算出部６１により設定された終端位置に近付くに従がって重心位置に対する所定範囲を連続的あるいは段階的に大きくして重心位置に対する所定範囲内の制限を緩和してもよい。この場合は、動作計画部６３は、走行ロボット１の位置と終端位置との距離と、重心位置の範囲との関係を示す関数あるいはテーブルを用いて、重心位置に対する所定範囲を連続的あるいは段階的に変化させてもよい。

さらに、動作計画部６３は、上記走行ロボット１の位置と目標位置算出部６１により設定された終端位置との距離に応じて、重心位置に対する所定範囲を連続的あるいは段階的に変化させ、目標位置算出部６１により設定された終端位置から所定距離以内で、重心位置に対する所定範囲内の制限を解除して走行ロボット１の各関節部の動作を計画してもよい。例えば、動作計画部６３は、走行ロボット１の位置が目標位置算出部６１により設定された終端位置に近付くに従がって重心位置に対する所定範囲を大きくし、目標位置算出部６１により設定された終端位置から所定距離以内で、重心位置に対する所定範囲内の制限を解除して走行ロボット１の各関節部の動作を計画してもよい。

上記実施形態において、移動装置２３は、一対の駆動車輪２７及び一対の従動車輪２８を有するアクティブキャスタ式の全方位台車として構成されているが、これに限らない。例えば、移動装置は、一対の駆動車輪及び一つの従動車輪を有する、あるいは、左右方向に回動する４つの駆動車輪を有するアクティブキャスタ式の全方位台車であってもよい。さらに、移動装置は、３つのオムニホイール２９を有する構成（図８）であってもよい。この場合、上記重心位置を制約する所定範囲は、３輪の中心点を中心とした円となる。

さらに、上記重心位置を制約する所定範囲は、例えば、三角形状や台形状であってもよく、任意の多角形として設定してもよい。例えば、一対の駆動車輪２７に必要な荷重を算出する。各駆動車輪２７に剛性を設定して、各従動車輪２８及び各駆動車輪２７の力の釣合いを求めるなどして、算出した各駆動車輪２７に必要な荷重が掛かる重心位置の集合を求める（図９）。そして、求めた重心位置の集合に対して、動的な要素（移動装置２３の加速度、遠心力など）、や床面の凹凸、各車輪等の誤差を吸収するためのマージンＳ１を差し引いて、重心位置に対する所定範囲Ｓ２を求める。

走行ロボット１は、旋回時に直進時に比して、各駆動車輪の滑りなどが生じて位置精度に誤差が生じ易い。従がって、例えば、走行ロボット１が旋回状態となるときに重心位置対する所定範囲の制限をより厳しく（所定範囲を小さく）設定してもよい。これにより、走行ロボット１の走行状態を考慮して、走行ロボット１の位置精度をより向上させることができる。

走行ロボット１の状態（バランス、構造など）、あるいは、走行路面の状態（傾斜、凹凸など）や横風などの走行環境により、一定方向に走行ロボット１の駆動車輪２７及び従動車輪２８に滑りが生じ易くなる場合がある。この場合、その滑りが生じ易い方向に対して、重心位置に対する所定範囲の制限をより厳しく設定してもよい。これにより、走行ロボット１の状態及び走行環境を考慮して、走行ロボット１の位置精度をより向上させることができる。

また、本発明は、例えば、図６及び図７に示す処理を、ＣＰＵ６ａにコンピュータプログラムを実行させることにより実現することも可能である。

プログラムは、様々なタイプの非一時的なコンピュータ可読媒体（non-transitory computer readable medium）を用いて格納され、コンピュータに供給することができる。非一時的なコンピュータ可読媒体は、様々なタイプの実体のある記録媒体（tangible storage medium）を含む。非一時的なコンピュータ可読媒体の例は、磁気記録媒体（例えばフレキシブルディスク、磁気テープ、ハードディスクドライブ）、光磁気記録媒体（例えば光磁気ディスク）、ＣＤ−ＲＯＭ（Read Only Memory）、ＣＤ−Ｒ、ＣＤ−Ｒ／Ｗ、半導体メモリ（例えば、マスクＲＯＭ、ＰＲＯＭ（Programmable ROM）、ＥＰＲＯＭ（Erasable PROM）、フラッシュＲＯＭ、ＲＡＭ（random access memory））を含む。

また、プログラムは、様々なタイプの一時的なコンピュータ可読媒体（transitory computer readable medium）によってコンピュータに供給されてもよい。一時的なコンピュータ可読媒体の例は、電気信号、光信号、及び電磁波を含む。一時的なコンピュータ可読媒体は、電線及び光ファイバ等の有線通信路、又は無線通信路を介して、プログラムをコンピュータに供給できる。

１走行ロボット、２移動体、３アーム、４モータ、５アクチュエータ、６制御装置、７回転センサ、８角度センサ、２１胴体部、２２頭部、２３移動装置、６１目標位置算出部、６２動作算出部、６３動作計画部、６４重心位置算出部、６５制約条件部、６６軌道補完部

Claims

車輪を有する移動体と、前記移動体に支持されると共に前記移動体に対して相対変位可能に連結されたアームと、を備える走行ロボットであって、
前記走行ロボットが目標とする目標位置を設定する目標位置設定手段と、
前記走行ロボットの重心位置を算出する重心位置算出手段と、
前記目標位置設定手段により設定された目標位置までの前記移動体及び／又はアームの動作部の動作を計画する動作計画手段と、
を備え、
前記動作計画手段は、前記走行ロボットが前記目標位置設定手段により設定された目標位置に近い場合、遠い場合と比較して、前記重心位置の範囲に対する制限を緩和して、前記移動体及び／又はアームの動作部の動作を計画する、ことを特徴とする走行ロボット。
請求項１記載の走行ロボットであって、
前記動作計画手段は、前記重心位置算出手段により算出された重心位置を所定範囲内に制限して当該走行ロボットの各動作部の動作を計画しており、
当該走行ロボットの位置が前記目標位置設定手段により設定された目標位置から所定距離以内で、前記重心位置に対する所定範囲内の制限を解除して前記移動体及び／又はアームの動作部の動作を計画する、ことを特徴とする走行ロボット。
請求項１又は２記載の走行ロボットであって、
前記動作計画手段は、前記重心位置算出手段により算出された重心位置を所定範囲内に制限して前記移動体及び／又はアームの動作部の動作を計画しており、
当該走行ロボットの位置と前記目標位置設定手段により設定された目標位置との距離に応じて前記所定範囲を変化させる、ことを特徴とする走行ロボット。
請求項３記載の走行ロボットであって、
前記動作計画手段は、当該走行ロボットの位置が前記目標位置設定手段により設定された目標位置に近付くに従がって前記所定範囲を大きくすることを特徴とする走行ロボット。
請求項１乃至４のうちいずれか１項記載の走行ロボットであって、
前記動作計画手段は、前記移動体及び前記アームの動作部が協調する動作を計画する、ことを特徴とする走行ロボット。
請求項２記載の走行ロボットであって、
前記アームは、操作対象物を把持するための把持部を有しており、
前記動作計画手段は、前記アームの把持部が前記目標位置設定手段により設定された目標位置から所定距離以内で、前記重心位置に対する所定範囲内の制限を解除して前記移動体及び／又はアームの動作部の動作を計画する、ことを特徴とする走行ロボット。
車輪を有する移動体と、前記移動体に支持されると共に前記移動体に対して相対変位可能に連結されたアームと、を備える走行ロボットの動作計画方法であって、
前記走行ロボットが目標とする目標位置を設定するステップと、
前記走行ロボットの重心位置を算出するステップと、
前記走行ロボットが前記設定された目標位置に近い場合、遠い場合と比較して、前記重心位置の範囲に対する制限を緩和して、前記移動体及び／又はアームの動作部の動作を計画するステップと、を含む、ことを特徴とする走行ロボットの動作計画方法。
車輪を有する移動体と、前記移動体に支持されると共に前記移動体に対して相対変位可能に連結されたアームと、を備える走行ロボットのプログラムであって、
前記走行ロボットが目標とする目標位置を設定する処理と、
前記走行ロボットの重心位置を算出する処理と、
前記走行ロボットが前記設定された目標位置に近い場合、遠い場合と比較して、前記重心位置の範囲に対する制限を緩和して、前記移動体及び／又はアームの動作部の動作を計画する処理と、をコンピュータに実行させる、ことを特徴とする走行ロボットのプログラム。