JP5002609B2 - 移動マニピュレータの軌道生成システム - Google Patents

Description

本発明は、定められた軌道に沿って物品を操作するための移動台車及びマニピュレータ機構の軌道を生成する軌道生成システムに関する。

マニピュレータ機構を備えた自走式の移動ロボット（移動マニピュレータ）は、軌道生成システムにより生成された軌道に従って制御される。一例として、移動ロボットは、人の指示に従って、指示された物品にアクセスし、物品を把持して所定の位置まで運搬するように制御される。操作する対象の物品が環境に拘束されている場合、この拘束条件に従って物品の移動可能な軌道が定められる。例えば、ヒンジにより回転可能に支柱に支持されるドア（開き戸）では、ドアに設けられたドアノブの軌道は、ヒンジを軸とした円軌道に定められる。また、スライドレールにより移動可能に支持されるドア（引き戸）では、ドアに設けられたドアノブの軌跡は、スライドレールに沿った直線軌道に定められる。移動ロボットにおいては、定められた物品の軌跡に応じてマニピュレータ機構の位置及び姿勢が制御される。特許文献１には、レール上を走行する垂直多関節型で６軸のアームを有する移動ロボットにおいて、アーム先端の位置及び姿勢を制御する方法が開示されている。

物品の軌道がマニピュレータ機構の可動範囲外に及ぶ場合、マニピュレータ機構の位置及び姿勢以外に移動ロボット本体の位置及び姿勢を制御する必要がある。しかしながら、環境に拘束された物品を操作する際には、マニピュレータ機構及び移動台車の動きも拘束されるため、操作開始時におけるマニピュレータ機構及び移動台車の位置及び姿勢によっては、操作途中でマニピュレータ機構の操作限界に至って操作不能になる虞がある。

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

従って、移動マニピュレータの軌道生成システムには、物品を操作する際の移動台車の位置及び姿勢、マニピュレータ機構の関節角度、並びに、操作中の移動台車及びマニピュレータ機構の軌道が補完的に定められることが求められている。

本発明は、上記問題点を解決するためになされたものであり、その目的は、操作開始時における移動台車の位置及び姿勢並びに把持機構の位置及び姿勢、操作中の移動台車の軌道、マニピュレータ機構の軌道及び把持機構の軌道、さらに、操作終了時における把持機構の位置及び姿勢を決定することができる移動マニピュレータの軌道生成システムを提供することにある。

本発明によれば、
移動ロボット本体と、
前記移動ロボットを前後方向に移動可能且つ走行面に垂直な軸まわりに回転可能に載置する、ノンホロノミックな拘束を有する移動台車と、
前記移動ロボット本体に設けられ、各々が関節角を有する６以上の関節を備えるマニピュレータと、
前記マニピュレータの先端部に設けられ、操作する物品軌道を予め定められる物品を把持する把持機構と、
前記物品軌道に従って前記移動台車の台車位置及び姿勢並びに前記関節角を含む移動マニピュレータの制御軌道を生成する軌道生成部と、
を具備し、
前記軌道生成部は、前記物品軌道に設定される複数の演算点毎に、操作開始時における前記把持機構の初期位置及び姿勢と操作終了時における前記把持機構の最終位置及び姿勢とから前記把持機構の位置及び姿勢を次々に定め、前記演算点毎に定められた前記把持機構の位置及び姿勢から把持機構軌跡を定め、前記把持機構軌跡に従って、前記移動台車の前後方向の移動量及び前記走行面に垂直な軸まわりの回転量のいずれか一方と前記関節角の変化量とを前記演算点毎に算出することにより、前記移動台車の台車位置及び姿勢並びに前記関節角を定めることを特徴とする移動マニピュレータの軌道生成システムが提供される。

本発明によれば、物品を操作する移動量が大きい場合にも、定められた軌道に沿って物品を操作することができる。

本発明の実施の形態に係る移動ロボットの構成を示す概略図である。 図１に示したマニピュレータ機構を示す概略図である。 図１に示した移動ロボットが備える回路システムを概略的に示すブロック図である。 図１に示した移動ロボットが使用される実環境を模式的に示す斜視図である。 図５に示した冷蔵庫を示す斜視図である。 図４に示した環境に設定される座標系を示す斜視図である。 図１に示した移動台車及びマニピュレータ機構の軌道を定める手順を示すフローチャートである。 （ａ）は、図５に示したドアノブに対する移動台車の初期位置及び姿勢を示す上面図であり、（ｂ）は、（ａ）に示した初期位置での移動台車の初期姿勢を示す上面図である。 （ａ）は、図５に示したドアノブ位置に対する把持機構の初期位置及び姿勢を示す上面図であり、（ｂ）は、図５に示したドアノブに対する把持機構の最終位置及び姿勢を示す上面図である。 図５に示したドアノブの軌跡に従って移動する把持機構の軌跡を示す上面図である。 図７に示したフローチャートにおける移動台車の位置及び姿勢並びにマニピュレータ機構の位置及び姿勢を定める手順を詳細に示すフローチャートである。 図１１に示したマニピュレータ機構の手首位置を算出する工程を説明するための上面図である。 （ａ）は、図１１に示したヤコビ行列を選択する工程におけるヤコビ行列を選択する方法を説明するための上面図であり、（ｂ）は、（ａ）に示したヤコビ行列を選択する方法を説明するためのグラフである。

以下、必要に応じて図面を参照しながら、本発明の一実施の形態に係る移動マニピュレータの軌道生成システムを説明する。

図１は、本発明の実施の形態に係る移動ロボット（移動マニピュレータ）１００の概略構成を示している。この移動ロボット１００は、図１に示されるように、移動ロボット本体１０１を移動可能に載置する移動台車１０２を備えている。この移動台車１０２は、移動ロボット本体１０１の左右両側に配置される個別に駆動可能な移動輪を有し、前後方向に移動可能に、且つ、移動台車１０２の駆動中心まわりに回転可能に移動ロボット本体１０１を支持している。具体的には、移動ロボット本体１０１には、対向配置される同一形状に形成された移動輪が設けられており、これら移動輪が同一方向に同一回転数で回転駆動されることによって、移動ロボット本体１０１が前進又は後退され、これら駆動輪が互いに逆方向に同一回転数で回転駆動されることによって、移動ロボット本体１０１がその場で回転される。このように、移動台車１０２は、左右方向へ直接移動することが不可能なノンホロノミック台車であって、前後方向への移動及び回転が組み合わされて移動ロボット本体１０１の位置及び姿勢が制御される。即ち、前後方向への移動及び姿勢変更の２つの制御入力が適宜に組み合されて移動台車１０２の走行面上における移動ロボット本体１０１の位置及び姿勢が制御される。

また、図１に示した移動ロボット１００は、移動ロボット本体１０１の両側にマニピュレータ機構１０３(図１では、図を簡略化する目的から一方のマニピュレータ機構１０３のみが示されている。）を備え、これらマニピュレータ機構１０３の自由端に相当する先端には、操作又は運搬する対象になる物品（以下、対象物と称する）を把持するための把持機構１０４が設けられている。このマニピュレータ機構１０３は、第１乃至第７リンク部２０１〜２０７及び第１乃至第７関節２１１〜２１７から構成され、７自由度を有するように形成される。

図２は、移動ロボット本体１０１が備えるマニピュレータ機構１０３の概略構成を示している。図２には、マニピュレータ機構１０３が移動ロボット本体１０１の両側面に設けられる例が示されている。しかしながら、本発明の実施段階では、移動ロボット本体１０１の片側にマニピュレータ機構１０３が設けられればよい。また、図２に示される両マニピュレータは、互いに同様の構成に形成される。従って、実施の形態の説明では、説明を簡単にするために、マニピュレータ機構１０３が片側（移動ロボット本体１０１の右側）にのみ設けられるとして説明する。

マニピュレータ機構１０３は、図２に示されるように、移動ロボット本体１０１の側面に設けられ、第１乃至第７リンク部２０１〜２０７及び第１乃至第７関節２１１〜２１７から構成されている。第１リンク部２０１は、略Ｌ字形に形成され、一端を第１関節２１１に連結され、第１関節２１１を介して矢印方向Ｒ１に回転可能に本体１０１に連結されている。また、第１リンク部２０１は、他端を第２関節２１２に連結され、この第２関節２１２には、略直線状に形成された第２リンク部２０２の一端が連結されている。第２リンク部２０２は、第２関節２１２を介して矢印方向Ｒ２に回転可能に第１リンク部２０１に連結される。さらに、第２リンク部２０２は、他端を第３関節２１３に連結され、この第３関節２１３には、略直線状に形成された第３リンク部２０３の一端が連結されている。第３リンク部２０３は、第３関節２１３を介して矢印方向Ｒ３に回転可能に第２リンク部２１２に連結されている。

同様にして、略直線状に形成された第４リンク部２０４は、第４関節２１４を介して矢印方向Ｒ４に回転可能に第３リンク部２０３に連結され、略直線状に形成された第５リンク部２０５は、第５関節２１５を介して矢印方向Ｒ５に回転可能に第４リンク部２０４に連結される。略直線状に形成された第６リンク部２０６は、第６関節２１６を介して矢印方向Ｒ６に回転可能に第５リンク部２０５に連結され、略直線状に形成された第７リンク部２０７は、第７関節２１７を介して矢印方向Ｒ７に回転可能に第６リンク部２０６に連結されている。

マニピュレータ機構１０３は、先端に位置する第５関節２１５、第６関節２１６及び第７関節２１７の各々の回転軸（第５軸、第６軸及び第７軸）が１点で交差するように形成されている。マニピュレータ機構１０３においては、第５関節２１５に連結される第４リンク部２０４がマニピュレータ機構１０３の手首に相当する。マニピュレータ機構１０３の先端、即ち、第７リンク部２０７の第７関節２１７が連結されてない側の端部には、対象物を把持する把持機構１０４が設けられている。マニピュレータ機構１０３が６以上の自由度を有して形成されることから、対象物に対する把持機構１０４の位置及び姿勢を適切に定めることができる。この把持機構１０４は、対象物を把持することができればいかなるタイプの把持機構であってもよい。

各関節２１１〜２１７は、図３を参照して説明されるように、マニピュレータ機構駆動部３０７により駆動されて各リンク部２０１〜２０７が回転作動される。また、各関節２１１〜２１７は、対応するリンク部２０１〜２０７を特定の一方向Ｒ１〜Ｒ７に回転可能にするように形成されており、第１乃至第７関節２１１〜１７には、夫々の関節の回転軸をＺ１〜Ｚ７軸に定めた座標系Σ１〜Σ７が設定されている。また、移動ロボット本体１０１には、座標系Σ０が定められ、マニピュレータ機構１０３の先端に形成される把持機構１０４には、把持機構座標系Σｈが定められている。

また、移動ロボット本体１０１には、赤外線センサ及び超音波センサ等の非接触式センサ（図示せず）が設けられ、非接触式センサが搭載されるセンサ搭載部１０１Ａが２つの関節２１８，２１９を介して移動ロボット本体１０１に連結される。図２に示した移動ロボット１００においては、これら２つの関節２１８，２１９が駆動されてセンサ搭載部１０１Ａを上下及び左右方向に向くように制御される。移動ロボット１００は、非接触センサから出力されるセンサ信号に基づいて移動ロボット本体１０１の現在位置及び姿勢、障害物等を検出し、障害物を回避して目標位置に向けて自走することができる。

尚、マニピュレータ機構１０３は、７自由度に形成される場合に限らず、先端の位置及び姿勢を制御できるように６以上の自由度を有して形成されればよく、自由度に制限がないことは、明らかである。また、関節数及びリンク部の数は、マニピュレータ機構１０３の自由度に応じて定められる。即ち、目的に応じてリンク部及び関節の数を適宜変更してマニピュレータ機構１０３が形成されてもよい。さらに、マニピュレータ機構１０３は、先端に位置する３つの関節の回転軸が１点で交差するように形成されなくともよい。また、特定のリンク部にスライド伸縮機構が設けられ、このリンク部が伸縮自在に形成されてもよい。

図３は、移動ロボット１００が備える回路システムを示している。図２に示されるように、図１に示した移動ロボット１００には、外部からの無線信号が無線装置３０６で受信されて、或いは、移動ロボット本体１０１に設けられた入力表示パネル３０５がユーザによって操作されて操作すべき対象物の軌道及び後に説明される初期値（初期設定）等が入力される。この入力信号は、インターフェース３０４及びＣＰＵ３０１を介してＲＡＭ３０２に格納される。対象物の軌道及び初期値等の動作設定が入力されると、ＣＰＵ３０１は、ＲＯＭ３０３に記述された制御プログラムを読み出し、定められた軌道に沿って対象物を操作する移動台車１０２及びマニピュレータ機構１０３の軌道を含む移動マニピュレータ軌道を定めてこの移動マニピュレータ軌道をＲＡＭ３０２に記憶する。

移動台車１０２及びマニピュレータ機構１０３の軌道が決定されると、ＣＰＵ３０１は、インターフェース３０４を介して移動機構駆動部３０９に動作指示し、この動作指示に従って移動ロボット１００が目標位置にまで移動され、目標姿勢を保持して一時停止される。ＣＰＵ３０１は、インターフェースを介してマニピュレータ機構駆動部３０７及び把持機構駆動部３０８に動作指示し、この動作指示に従ってマニピュレータ機構１０３の各関節２１１〜２１７が回転駆動され、マニピュレータ機構１０３が目標位置及び姿勢に位置決めされ、マニピュレータ機構１０３が目標位置及び姿勢に位置決めされた状態で把持機構１０４が対象物を把持する。次に、移動台車１０２、マニピュレータ機構１０３及び把持機構１０５は、算出された移動台車１０２及びマニピュレータ機構１０３の軌道に従って各駆動部３０７，３０８，３０９により駆動され、定められた軌道に沿って対象物を操作するように制御される。

図４は、移動ロボット１００が使用される実環境の一例を示している。例えば、この移動ロボット１００は、図４に示されるように、操作する対象物、例えば、冷蔵庫１０に向けて移動し、この冷蔵庫１０の上段のドア１２に形成されたドアノブ１３を把持機構１０４で把持し、ドア１２を開けるように制御される。移動ロボット１００が移動する際には、移動ロボット本体１０１に設けられる非接触式センサ（図示せず）等によってテーブル２０及び椅子２１等の障害物が検出されることから、移動ロボット１００は、障害物を回避して目標位置まで到達することができる。

図４は、移動ロボットが操作する対象物の一例として冷蔵庫１０が示されている。図４に示されるように、冷蔵庫本体１１には、上段、中段及び下段の保冷室が設けられており、各保冷室には、ドアノブを有するドアで外気に対して密閉されている。上段の保冷室のドア（開き戸）１２は、このドア１２を冷蔵庫本体１１に支持するヒンジを軸として回動可能に形成され、中段及び下段のドア（引き戸）１４，１６は、夫々冷蔵庫本体１１の内表面に設けられたスライドレールにより引き出し自在に保持されている。即ち、上段のドア１２に形成されたドアノブ１３は、ドア１２の端部に設けられるヒンジを軸とした円運動するように軌道が定められ、中段及び下段の各々のドア１４，１６に形成されたドアノブ１５，１７は、スライドレールに沿って直線運動するように軌道が定められている。図１に示した移動ロボット１００は、このような予め定められた軌道に沿って移動する把持した対象物をその軌道に従って操作するように制御される。

尚、移動ロボット１００は、対象物が環境に拘束されて軌道が予め定められている場合に限らず、対象物を任意に定められた軌道に沿って運搬する場合等にも適応することができる。

図６は、冷蔵庫１０の上段ドア１２を開く操作を実行する移動台車１０２及びマニピュレータ機構１０３の軌道を算出する際に設定される座標系の一例を示している。以下では、冷蔵庫の上段のドア１２に形成されたドアノブ１３を所定の軌道に従って操作する移動台車１０２及びマニピュレータ機構１０３の軌道を算出する方法を説明する。図６に示される環境では、種々の要素の各々に対して座標系が定められている。ワールド座標系Σｗは、床面３０の特定の位置を原点としてＸｗ−Ｙｗ平面が床面３０に一致するように定められる。即ち、ワールド座標系Σｗは、そのＸｗ−Ｙｗ平面が移動台車１０２の走行面に一致するように設定され、この環境における基準の座標系に定められる。

また、移動台車１０２、マニピュレータ機構１０３、把持機構１０４及びドアノブ１３には、夫々台車座標系Σｒ、マニピュレータ座標系Σａ、把持機構座標系Σｈ及びドアノブ座標系Σｈｎが定められる。台車座標系Σｒにおいては、例えば、ワールド座標系ΣｗのＸｗ−Ｙｗ平面上における移動台車１０２の駆動中心が原点に定められ、移動ロボット本体１０１の正面方向にＸｒ軸が定められ、左側面（マニピュレータ機構１０３が設けられる側とは反対側の面）方向にＹｒ軸が定められる。また、Ｚｒ軸は、Ｚｗ軸と同一方向（鉛直方向）に定められる。マニピュレータ機構座標系Σａは、移動ロボット本体１０１であってマニピュレータ機構１０３の基端部に設定される。マニピュレータ機構座標系Σａ及び台車座標系Σｒが移動ロボット本体１０１を基準に設定されることから、マニピュレータ機構座標系Σａから見た台車座標系Σｒの位置及び姿勢は、常に固定されている。マニピュレータ機構座標系Σａから見た把持機構座標系Σｈの位置及び姿勢は、第１乃至第７関節２１１〜２１７の関節角により定められる。

移動台車１０２がワールド座標系ΣｗのＸｗ−Ｙｗ平面上を移動し、鉛直方向（Ｚｒ軸）まわりに回転することから、台車座標系Σｒの位置及び姿勢は、例えば、ワールド座標系ΣｗのＸｗ−Ｙｗ平面における移動台車１０２の位置（ｘ，ｙ）及び姿勢（Ｚｒ軸まわりの回転角）γを表わす３変数によって定められる。ここでは、回転角γは、右ねじ方向を正方向に選定される。台車座標系Σｒの位置及び姿勢は、任意の座標系から見た座標で記述されてもよく、以下では任意の座標系から見た台車座標系Σｒの位置及び姿勢を（ｘ，ｙ，γ）と表現する。

また、把持機構１０４は、３次元空間内でその位置及び姿勢が変更されることから、把持機構座標系の姿勢位置は、６変数で定められる。台車座標系Σｒと同様に、任意の座標系から見た把持機構１０４座標系の位置及び姿勢を（ｘ，ｙ，ｚ，ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）と表現する。（ｘ，ｙ，ｚ）は、例えば、ワールド座標系ΣｗのＸｗ−Ｙｗ−Ｚｗ空間における把持機構座標系Σｈの原点位置を示し、（ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）は、ワールド座標系Σｗに対する把持機構座標系Σｈの相対姿勢を示す。

次に、図７から図１３を参照して移動ロボット１００の制御軌道を算出する方法を説明する。ここで、移動ロボット１００の制御軌道とは、操作開始時の移動台車１０２の位置及び姿勢、操作開始時の把持機構１０４の位置及び姿勢、操作中の移動台車１０２の軌道、操作中のマニピュレータ機構１０３の軌道、操作中の把持機構１０４の軌跡、並びに、操作終了時の把持機構１０４の位置及び姿勢を意味する。従って、以下では、説明を簡単にするために単に移動ロボットの制御軌道と称して、操作開始時の移動台車１０２の位置及び姿勢、操作開始時の把持機構１０４の位置及び姿勢、操作中の移動台車１０２の軌道、操作中のマニピュレータ機構１０３の軌道、操作中の把持機構１０４の軌跡、並びに、操作終了時の把持機構１０４の位置及び姿勢を示すものとする。

図７は、ドア開け操作における移動ロボットの制御軌道を算出する手順を示している。図７のステップＳ０１に示されるように、ドアノブを定められた軌跡に従って操作する移動ロボット１００の制御軌道を算出するために必要な種々の初期値（初期設定）が設定される。初期値には、操作開始時の移動台車１０２の初期位置及び姿勢の候補、操作開始時の把持機構１０４の初期位置及び姿勢の候補、並びに操作終了時の把持機構１０４の最終位置及び姿勢の候補が含まれる。

ステップＳ０２に示されるように、ＣＰＵ３０１は、予め設定されるドアノブ１３の軌跡に従って把持機構１０４の軌跡を算出する。このステップＳ０２では、マニピュレータ機構１０３の初期位置及び姿勢の候補、並びにマニピュレータ機構１０３の最終位置及び姿勢の候補から夫々１つの候補が選択され、これら候補の組み合わせに対して把持機構１０４の軌跡が算出される。

予め設定されるドアノブ１３の軌跡には、複数の演算点が設定されおり、この演算点毎にドアノブ１３の軌跡に従って把持機構１０４の位置及び姿勢が算出されて把持機構１０４の軌跡が定められる。即ち、把持機構１０４の軌跡には、把持機構１０４の初期位置及び姿勢の候補並びに把持機構１０４の最終位置及び姿勢の候補のある組み合わせに対して各演算点において算出された把持機構１０４の一連の位置及び姿勢が含まれている。ここで、演算点とは、ステップＳ０２で把持機構１０４の位置及び姿勢を算出し、次のステップＳ０３で移動台車１０２の位置及び姿勢並びにマニピュレータ機構１０３の位置及び姿勢を算出するために設定される座標点であって、設定される演算点の数は、対象物を操作する軌道の複雑さ及び大きさに応じて適宜変更される。

把持機構１０４の軌跡の算出には、ステップＳ０１で設定されたマニピュレータ機構１０３の初期位置及び姿勢の候補並びにマニピュレータ機構１０３の最終位置及び姿勢の候補から夫々１つの候補が選択されて定められる複数の組み合わせに対して実行される。例えば、全ての組み合わせに対して把持機構１０４の軌跡が算出される場合、算出される把持機構１０４の軌跡の数は、マニピュレータ機構１０３の位置及び姿勢の候補の数とマニピュレータ機構１０３の最終位置及び姿勢の候補の数とを乗算して得られる数に一致する。

次に、ＣＰＵ３０１は、ステップＳ０３に示されるように、ドアノブの軌跡に設定される演算点毎に移動台車１０２の位置及び姿勢、並びにマニピュレータ機構１０３の位置及び姿勢を算出する。ここに記述されるマニピュレータ機構１０３の位置及び姿勢は、第１乃至第７関節２１１〜２１７の関節角θ１〜θ７を意味する。このステップＳ０３は、ステップＳ０２で算出された全ての把持機構１０４の軌跡に対して実施される。即ち、ステップＳ０２で算出された把持機構１０４の軌跡の数に応じて複数の解候補が算出される。

ステップＳ０４に示されるように、ＣＰＵ３０１は、予め用意される評価関数を使用して解候補から解を選定する。評価関数は、後に詳細に説明するように、システム全体の消費エネルギーを最小にする等の条件を含む関数に定められ、この評価関数を最小にする解候補が実際の移動ロボット１００の制御軌道に選定される。

次に、図８から図１３を参照して上述した各ステップの詳細な手順を説明する。

図７のステップＳ０１では、移動台車１０２の初期位置及び姿勢の候補、把持機構１０４の初期位置及び姿勢の候補、並びに把持機構１０４の最終位置及び姿勢の候補が初期値に設定される。図８（ａ）及び（ｂ）は、ステップＳ０１で設定される移動台車１０２の初期位置及び姿勢の候補を示している。移動台車１０２の初期位置及び姿勢の候補には、図８（ａ）に示されるように、操作対象となる冷蔵庫１０のドアノブ１３の位置に応じて、冷蔵庫１０正面にマトリックス状に配列されるマニピュレータ機構１０３の可動範囲内の位置Ｐｒ及びその位置Ｐｒでの姿勢Ｒｒが位置の候補及び姿勢の候補として設定される。図８（ｂ）に示されるように、移動台車１０２の各位置候補Ｐｒに対して複数の姿勢候補Ｒｒが設定される。

この位置及び姿勢の候補は、例えば、オペレータによって移動ロボット本体１０１に設けられた入力表示パネル３０５が操作されて入力されもよく、また、設定されたドアノブ１３の軌道に応じてＣＰＵ３０１が自動的に移動台車１０２の初期位置及び姿勢の複数の候補を生成し、これら候補が初期値に設定されてもよい。この移動台車１０２の位置及び姿勢の候補は、例えば、ワールド座標系Σｗに対する台車座標系Σｒの位置及び姿勢（ｘｒ，ｘｒ，γ）として設定される。

図９（ａ）は、ステップＳ０１で設定される把持機構１０４の初期位置及び姿勢の候補を示し、図９（ｂ）は、ステップＳ０１で設定される把持機構１０４の最終位置及び姿勢の候補を示している。把持機構１０４の初期位置候補及び姿勢候補には、図９（ａ）に示されるように、移動台車１０２の初期位置及び姿勢の候補を設定する方法と同様に、ドア開け開始時におけるドアノブ１３の位置に応じて複数の位置Ｐｈ１及びその位置Ｐｈ１での姿勢Ｒｈ１が設定される。移動台車１０２の最終位置候補及び姿勢候補には、図９（ｂ）に示されるように、同様に、ドア開け終了時におけるドアノブの位置に応じて複数の位置Ｐｈ２及びその位置Ｐｈ２での姿勢Ｒｈ２が設定される。これら把持機構１０４の初期位置及び姿勢の候補並びに把持機構１０４の最終位置及び姿勢の候補の入力は、オペレータにより手動で入力されてもよく、また、設定されたドアノブ１３の位置に応じてＣＰＵ３０１により自動的に設定されてもよい。把持機構１０４の位置及び姿勢の候補は、例えば、ワールド座標系Σｗに対する把持機構座標系Σｈの位置及び姿勢（Ｘｈ，Ｙｈ，Ｚｈ、ｒｏｌｌ，ｐｉｔｃｈ，ｙａｗ）で入力される、或いは、マニピュレータ機構１０３の関節角θ１〜θ７で入力されてもよい。

尚、ステップＳ０１で設定される初期値は、ワールド座標系Σｗで入力される場合に限定されず、任意の座標系で設定されてもよい。

図７に示したステップＳ０２では、ステップＳ０１で設定された把持機構１０４の初期位置及び姿勢候補と把持機構１０４の最終位置及び姿勢候補とからドア開け操作時の把持機構１０４の軌跡が算出される。図１０は、ドア開け操作におけるドアノブ１３の軌跡及び把持機構１０４の軌跡を示している。ドアノブ１３は、図１０に示されるように、ヒンジ取り付け位置１８を回転軸とし、ヒンジ及びドアノブ形成位置間の距離を半径とした円軌道に沿って、ポジションＰ１１からポジションＰ１２を経てポジションＰ１３まで移動される。ドアノブ１３の軌跡は、予め設定されており、この軌跡には、複数の演算点が定められている。

ステップＳ０２では、先ず、ステップＳ０１で設定された把持機構１０４の初期位置及び姿勢の候補から１つの候補が選択され、把持機構１０４の最終位置及び姿勢の候補から１つの候補が選択される。次に、この選択された候補の組み合わせに対してドアノブ１３の軌跡に従った把持機構１０４の位置及び姿勢が算出される。より詳細には、ドアノブ１３の軌道に定められた各演算点において、ドアノブ１３の軌跡に従うように把持機構１０４の位置が定められ、初期位置及び姿勢の候補並びに最終位置及び姿勢の候補を補完するように把持機構１０４の姿勢が定められる。図７のステップＳ０２に示される把持機構１０４の軌跡の算出は、任意の座標系で実施することができる。

次に、図７に示したステップＳ０３では、ステップＳ０２で算出された把持機構１０４の軌跡に対して軌跡毎に移動台車１０２の軌道及びマニピュレータ機構１０３の軌道が算出される。図１１は、移動台車１０２の位置及び姿勢並びにマニピュレータ機構１０３の位置及び姿勢を算出する手順を示している。ある演算点における移動台車１０２の位置及び姿勢並びにマニピュレータの位置及び姿勢は、図１１に示される手順に従って、直前の演算点で算出された移動台車１０２の位置及び姿勢並びにマニピュレータの位置及び姿勢に基づいて算出される。図１１に示される手順が繰り返し実行されて、移動台車１０２の軌道及びマニピュレータ機構１０３の軌道が算出される。即ち、移動台車１０２の初期位置及び姿勢の候補並びに把持機構１０４の初期位置及び姿勢の候補に基づいて、図１１に示した手順が繰り返して実行され、最終的にドア開け終了時の移動台車１０２の最終位置及び姿勢並びにマニピュレータの最終位置及び姿勢が算出される。

具体的には、先ず、ステップＳ０１で設定された移動台車１０２の初期位置及び姿勢の候補並びに把持機構１０４の初期位置及び姿勢候補から夫々１つの候補が選択される。第１の演算点では、これら選択された候補に基づいて、移動台車１０２の第１の位置及び第１の姿勢並びに把持機構１０４の第１の位置及び第１の姿勢が算出される。次に、第２の演算点では、算出された移動台車１０２の第１の位置及び姿勢、並びに把持機構１０４の第１の位置及び姿勢に基づいて、移動台車１０２の第２の位置及び姿勢、並びに把持機構１０４の第２の位置及び姿勢が算出される。同様に、第３の演算点では、算出された移動台車１０２の第２の位置及び姿勢、並びに把持機構１０４の第２の位置及び姿勢に基づいて、移動台車１０２の第３の位置及び姿勢、並びに把持機構１０４の第３の位置及び姿勢が算出される。このように、把持機構１０４の軌道上に設定された起動演算点の数だけ図１１に示される手順が繰り返し実行され、最終的に、ドア開け終了時の移動台車１０２の位置及び姿勢並びに把持機構１０４の位置及び姿勢が算出される。

図７に示したステップＳ０３では、ステップＳ０２で算出された把持機構１０４の軌跡に対して、対応する移動台車１０２の初期位置及び姿勢の候補並びに把持機構１０４の初期位置及び姿勢の候補に基づいて移動ロボット１００の制御軌道（移動台車１０２の軌道及びマニピュレータ機構１０３の軌道）が算出され、この移動ロボット１００の制御軌道が１つの解候補に定められる。従って、ステップＳ０２で算出された把持機構１０４の軌跡の数に応じて図１１に示される手順が繰り返し実行されて複数の解候補が算出される。

初めに、図１１のステップＳ１１に示されるように、ＣＰＵ３０１は、図７のステップＳ０２で算出され、任意の座標系で記述される把持機構１０４の位置及び姿勢をマニピュレータ座標系Σａへ変換し、ステップＳ０１で設定され、任意の座標系で記述される移動台車１０２の現在の位置及び姿勢の候補をマニピュレータ座標系Σａへ変換する。

後に説明するように、移動台車１０２及びマニピュレータ機構１０３の位置及び姿勢を算出する演算処理には、２つのヤコビ行列が使用され、これらヤコビ行列がマニピュレータ座標系で記述されている。このため、ステップＳ１１では、移動台車１０２及びマニピュレータの位置及び姿勢を記述する座標系がマニピュレータ座標系に統一される。即ち、ステップＳ１１では、ヤコビ行列が記述される座標系に応じて把持機構１０４の位置及び姿勢並びに移動台車の位置及び姿勢が適切な座標系へ変換される。

ステップＳ１２では、ＣＰＵ３０１は、第５乃至第７関節２１５〜２１７の現在の関節角（直前の演算点で算出された関節角）、並びにマニピュレータ座標系から見た把持機構座標系の位置及び姿勢からマニピュレータ機構１０３の手首位置を算出し、この手首位置を目標手首位置に定める。

次に、ＣＰＵ３０１は、ステップＳ１３に示されるように、用意された２つのヤコビ行列からいずれか一方を選択する。図１に示した移動ロボット１００には、移動台車１０２及びマニピュレータ機構１０３の軌道を決定する演算処理のために２つのヤコビ行列が用意されている。第１のヤコビ行列は、下記数式１のように表わされ、移動台車１０２の前後方向への直進成分（ｗｘ成分）、及びマニピュレータ機構１０３の第１乃至第４関節２１１〜２１４の回転成分（θ１〜θ４成分）から構成される。

また、第２の行列式は、下記数式２のように表わされ、移動台車１０２の回転成分（γ成分）及びマニピュレータ機構１０３の第１乃至第４関節２１１〜２１４の関節角成分（θ１〜θ４成分）から構成される。

図１１に示される移動台車１０２の位置及び姿勢並びにマニピュレータ機構１０３の位置及び姿勢を算出する工程では、これら２つのヤコビ行列からいずれか一方が選定され、選択されたヤコビ行列が逆運動学計算に使用される。即ち、図１２に示されるように、演算点（ポジションＰ２１〜Ｐ２３）毎に対象物の軌道に応じてヤコビ行列が選定され、選定されたヤコビ行列を利用して各関節２０１〜２０４の関節角θ１〜θ４が定められる。また、演算点Ｐ２１〜Ｐ２３毎に選定されたヤコビ行列に応じて、移動ロボット本体１０１が前後方向に直進するように制御される、或いは、その場で回転するように制御されるかが定められ、その移動量或いは回転量が算出される。

ヤコビ行列を選定する方法では、移動台車の姿勢及び対象物の軌道に応じて第１及び第２のヤコビ行列のいずれか一方が選定される。即ち、図１３（ａ）に示されるように、設定されたドアノブ１３の軌跡に従って演算点におけるドアノブ１３の移動ベクトルが台車座標系上に投影され、この投影ベクトルと台車座標系のｘｒ軸とがなす角度ψ（−１８０°＜α≦１８０°）に応じてヤコビ行列が選定される。図１３（ｂ）に示されるように、この角度ψの絶対値が４５度以下、即ち、投影ベクトルが領域Ａ１に含まれる場合、第１のヤコビ行列が選択される。この場合、ドアノブ１３が移動ロボット本体１０１に向けて近付くように移動するため、移動台車１０２は、後退してドアノブ１３から遠ざかるように制御される。また、角度ψの絶対値が４５度より大きく１３５度未満、即ち、投影ベクトルが領域Ａ２，Ａ３に含まれる場合、第２のヤコビ行列が選択される。この場合、移動台車１０２は、角度ψの絶対値を小さくする方向にその場で回転するように制御される。角度φが４５度より大きく１３５度未満、即ち、投影ベクトルが領域Ａ２に含まれる場合、移動台車１０２がその場で左回転される。また、角度ψが−１３５度より大きく−４５度未満即ち、投影ベクトルが領域Ａ３に含まれる場合、移動台車１０２がその場で右回転される。さらに、角度ψの絶対値が１３５度以上１８０度以下、即ち、投影ベクトルが領域Ａ４に含まれる場合には、第１のヤコビ行列が選択される。この場合、ドアノブが移動ロボット本体１０１から遠ざかるように移動するため、移動台車１０２は、前進してドアノブ１３に近付くように制御される。但し、領域Ａ１からＡ４を定める角度は、上記例に限定されず、任意の角度に設定することができる。

ロボット本体１０１が図１３に示したポジションＰ３１，Ｐ３３に位置する場合、ドアノブ１３の移動ベクトルｖ１，ｖ３を台車座標系に投影して得られる投影ベクトルｕ１，ｕ３は、領域Ａ１に含まれる。従って、ポジッションＰ３１，Ｐ３３では、第１のヤコビ行列が選択される。また、ロボット本体１０１が図１３に示したポジションＰ３２に位置する場合、ドアノブ１３の移動ベクトルｖ２を台車座標系に投影して得られる投影ベクトルｕ２は、領域Ａ２に含まれる。従って、ポジッションＰ３２では、第２のヤコビ行列が選択される。

このように、ステップＳ１３では、対象物の移動方向及び移動台車１０２の姿勢に応じて第１のヤコビ行列及び第２のヤコビ行列のいずれか一方が選択される。

ここで、上述した第１及び第２のヤコビ行列、従来から提案されているヤコビ行列の例として２つの比較例を示す。比較例１に係るヤコビ行列は、下記数式０．１に示されるように、マニピュレータ機構１０３の手首の位置及び姿勢（θ１〜θ４）、並びに移動台車１０２の左右夫々の移動輪の回転数（φ１，φ２）に関する偏微分を成分に有している。しかしながら、空中を移動するマニピュレータ機構１０３と異なり、移動台車１０２、移動輪及び床面３０間の摩擦に応じて数式０．１に記述される偏微分の値が異なる。即ち、摩擦係数が特定することが困難な実環境において、数式０．１で記述されるヤコビ行列を使用して移動台車１０２の軌道を算出すると、所望の位置及び姿勢を実現することが困難である問題がある。

また、第２の比較例に係るヤコビ行列は、下記数式０．２に示されるように、マニピュレータ機構１０３の手先の位置及び姿勢（θ１〜θ４）、並びに移動台車１０２の目標位置及び姿勢（ｗｘ，ｗｙ，γ）の偏微分を成分に有する。このようなヤコビ行列を使用すると、移動台車１０２は、目標位置及び姿勢へ到達するように移動輪が制御されるので、駆動輪と床面３０との摩擦が発生されても、所望の位置及び姿勢へ到達することができる。しかしながら、ノンホロノミック台車は、全方向台車と異なり位置及び姿勢を一工程で同時に目標位置及び姿勢に変更することは不可能である。そのため、数式０．２で示されるヤコビ行列を使用して得られる移動台車１０２の位置及び姿勢の変位量を台車制御コマンドに変換することが困難である問題がある。

比較例１及び比較例２にかかるヤコビ行列に生じる問題を回避するため、図１に示した移動ロボット１００には、数式１に示されるような移動台車１０２の直進成分（ｗｘ成分、破線の左側の要素）及びマニピュレータ機構１０３の関節角成分（θ１〜θ４成分、破線の右側の要素）を有する第１のヤコビ行列と、数式２に示されるような移動台車１０２の回転成分（γ成分、破線の左側の要素）及びマニピュレータ機構１０３の関節角成分（θ１〜θ４成分、破線の右側の要素）を有する第２のヤコビ行列とが用意されている。これにより、いずれの演算点においても移動台車１０２を前後方向に直進させる制御又はその場で回転させる制御のいずれか一方のみが実行されるため、算出される台車１０２の移動量及び回転量を制御コマンドへ容易に変換することできる。また、ヤコビ行列の要素の数が低減されることから行列計算にかかる処理時間を短縮することができる。

次に、ステップＳ１４に示されるように、マニピュレータの仮の手首位置（ｗｒｔｍｐ＝｛ｗｒｔｍｐ_ｘ，ｗｒｔｍｐ_ｙ，ｗｒｔｍｐ_ｚ｝^Ｔ）及び第１乃至第４関節の仮の関節角（θｔｍｐ＝｛θｔｍｐ_１，θｔｍｐ_２，θｔｍｐ_３，θｔｍｐ_４｝^Ｔ）が設定される。図３に示したＲＯＭ３０３には、第１乃至４関節における種々の関節角の組み合わせに対応する順運動学の解（例えば、マニピュレータ座標系におけるマニピュレータ機構１０３の手首位置）が記述されたマップが格納されている。ＣＰＵ３０１は、ステップＳ１２において設定された目標手首位置でこのマップを参照して最も近い手首位置を仮の手首位置に定める。この仮の手首位置に対応するマニピュレータ機構１０３の関節角の組み合わせが仮の関節角に設定される。

ステップ１５では、ＣＰＵ３０３は、設定された仮の手首位置が目標手首位置に充分近いか否かを判断する。具体的には、仮の手首位置と目標手首位置との距離が予め設定される閾値より小さい場合、仮の手首位置が目標手首位置に近いと判断される。仮の手首位置と目標手首位置との距離が閾値より大きい場合には、ステップＳ１６に進む。

ステップＳ１６では、ＣＰＵ３０１は、ステップＳ１３で選択されたヤコビ行列の各成分の値を計算する。このヤコビ行列におけるマニピュレータ機構１０３の第１乃至第４関節に関する成分（θ１〜θ４を含む成分）は、マニピュレータ機構１０３の関節角を変数として定義されることから、ステップＳ１４で設定された仮の関節角が代入される。第１のヤコビ行列における移動台車１０２の直進成分（ｗｘを含む成分）は、下記数式３のように設定され、第２のヤコビ行列における移動台車１０２の回転成分（γを含む成分）は、下記数式４のように設定される。

ステップＳ１７では、ＣＰＵ３０３は、目標手首位置と仮の手首位置との差ｅ＝｛ｅ_ｘ，ｅ_ｙ，ｅ_ｚ｝^Ｔを演算する。ステップＳ１８では、ステップ１３で選択されたヤコビ行列Ｊｓ或いはＪγを使用して、以下に示すように、第１乃至第４関節２１１〜２１４の関節角及び移動台車１０２の移動量或いは回転量の修正量を計算する。ステップＳ１３で第１のヤコビ行列が選択された場合、移動台車１０２の移動量及び第１関節から第４関節の関節角の修正量ΔΘｓ＝｛Δｗ_ｘ，ΔＪ_１，ΔＪ_２，ΔＪ_３，ΔＪ_４｝^Ｔは、下記数式５から算出される。

ここで、行列ｋｓは、修正ゲインを含む対角行列であって下記数式６のように予め設定される。この対角行列ｋｓの対角成分ｋｗｘ，ｋ_Ｊ１，ｋ_Ｊ２，ｋ_Ｊ３及びｋ_Ｊ４は、任意に設定されてよい。

また、ステップＳ１３で第２のヤコビ行列が選択される場合、移動台車１０２の回転量及び第１関節から第４関節の関節角の修正量ΔΘγ＝｛Δγ，ΔＪ_１，ΔＪ_２，ΔＪ_３，ΔＪ_４｝^Ｔは、下記数式７から算出される。

ここで、行列ｋγは、修正ゲインを含む対角行列であって下記数式８のように予め設定される。この対角行列ｋγの対角成分ｋγ，ｋ_Ｊ１，ｋ_Ｊ２，ｋ_Ｊ３及びｋ_Ｊ４は、任意に設定されてよい。

ステップＳ１９では、ＣＰＵ３０３は、算出された関節角の修正量ΔＪ１，ΔＪ２，ΔＪ３，ΔＪ４に応じて仮の関節角θｔｍｐ_ｉを下記数式９に従って修正する。算出された関節角θ´ｔｍｐ_ｉ（ｉ＝１，２，３，４）が仮の関節角に定められる。
θ´tmp_ｉ=θtmp_ｉ＋ΔＪ_ｉ (ｉ＝１，２，３，４) 数式９
ステップＳ２０では、仮の手首位置が修正される。ＣＰＵ３０３は、数式９で定められた新しい仮の関節角を使用して順運動学を解く。即ち、修正された仮の関節角から仮の手首位置が算出される。さらに、数式５から算出された移動量の修正量Δｗ_ｓ又は数式７から算出された回転量の修正量Δγに従って移動台車１０２の移動量又は回転量が定められる。仮の手首位置が決定されると、ステップＳ１５に戻り、ステップＳ１６からステップＳ２０で修正された仮の手首位置と目標手首位置との距離が閾値より小さいか否かが判断される。仮の手首位置が目標手首位置に充分近いと判断されると、ステップＳ２１に進む。

但し、ステップＳ１６からステップＳ２０を所定回数繰り返して仮の手首位置を修正した後にもステップＳ１５において仮の手首位置と目標手首位置との距離が閾値よりも大きくなる場合、図１１に示される処理を終了する。

ステップＳ２１では、ステップＳ１５で定められた仮の手首位置に対応する仮の関節角が第１乃至第４関節の目標関節角に定められる。また、この時の移動量の修正量Δｗ_ｘ又は回転量の修正量Δγに応じて移動台車１０２の目標移動距離又は目標回転角が定められる。

ステップＳ２２では、ＣＰＵ３０１は、マニピュレータ機構１０３の第５乃至第７関節２１５〜２１７の目標関節角度を解析的に算出する。まず、第１乃至第４関節２１１〜２１４の目標関節角から第４関節に設定される図２に示した座標系Σ４（手首座標系）の姿勢が算出される。次に、手首座標系Σ４と把持機構座標系Σｈとの相対姿勢より５軸６軸７軸の目標関節角度が算出される。第５乃至第７関節２１５〜２１７の設置姿勢がロール・ピッチ・ヨー角表現、又は、オイラー角表現に対応している場合、回転行列からロール・ピッチ・ヨー角への変換式又は回転行列からオイラー角への変換式を利用して第５乃至第７関節２１５〜２１７の関節角を定めることができる。

ステップＳ２２で第５乃至第７関節２１５〜２１７の関節角が算出されると、ある演算点における移動台車１０２の位置及び姿勢、並びにマニピュレータ機構１０３の位置及び姿勢が算出されたことになる。上述したように、図１１に示された手順を複数回経てドア開け操作する際の移動台車１０２の軌道及びマニピュレータ機構１０３の軌道（移動ロボット１００の制御軌道）が算出される。

但し、図２に示したマニピュレータ機構１０３が手首位置より先端に位置する３つの関節２１５，２１６及び２１７の各々の回転軸が１点で交差せずに形成される場合、ステップＳ１２に示される手首位置を算出する工程、及びステップＳ２２に示される関節２１５，２１６及び２１７の関節角度を算出する工程は、非常に複雑になる。この場合、図１１に示されるＳ１２及びＳ２２が削除され、ステップＳ１３からステップＳ２１に示される工程が第１乃至第７関節２１１〜２１７に対して実行される。この場合、第１及び第２のヤコビ行列は、夫々下記数式１０及び数式１１に変更される。

上述したように、ステップＳ０３では、移動ロボット１００の複数の制御軌道が算出される。ステップＳ０４では、これら複数の移動台車１０２及びマニピュレータ機構１０３の軌道から最適な制御軌道が下記数式１２に示される評価関数を利用して選定される。

ｄ_ｗｘ：ドア操作中の移動台車１０２の直進移動量の絶対値を標準化した値
ｄ_γ：ドア操作中の移動台車１０２の回転量の絶対値を標準化した値
ｄ_θ：ドア操作中のマニピュレータ機構１０３の関節角の変化量の絶対値を標準化した値
ｓ_ｗｘ：ドア操作中の移動台車１０２の移動方向を変更する回数を標準化した値
ｓ_γ：ドア操作中の移動台車１０２の回転方向を変更する回数を標準化した値
ｎ_ｗｘ：移動台車１０２を移動から回転へ切り替える回数を標準化した値
ｎ_γ：移動台車１０２を回転から移動へ切り替える回数を標準化した値
α，β，γ，δ，ε，ζ，η：重み（任意の実数）
ここに記述される標準化とは、平均が０、標準偏差が１になるようにすることを意味する。上記した各量は、単位が異なる、或いは、値のばらつき方が異なる値を１つの評価関数に含めるために標準化される。

評価関数では、（ア）移動台車１０２の直進方向の移動量、（イ）移動台車１０２の回転方向の移動量、（ウ）マニピュレータ機構１０３の関節角度の変化量、（エ）移動台車１０２の移動方向の変更回数、（オ）移動台車１０２の回転方向の変更回数、（カ）移動台車１０２を移動から回転へ切り替える回数、（キ）移動台車１０２を回転から移動へ切り替える回数、が評価指標に設定される。

評価関数は、移動台車１０２の移動誤差及び移動台車１０２及びマニピュレータ機構１０３を駆動するエネルギーを低減し、移動台車１０２の軌道をコマンドに変換する処理を容易にするように構成される。移動台車１０２の移動誤差は、床面及び移動輪間の摩擦等により発生される。即ち、台車１０２の移動距離及び回転角度が大きくなるに伴って実際に変更された位置及び姿勢と目標位置及び姿勢との相違が大きくなる。このため、評価指数（ア）（イ）（エ）（オ）を評価関数に含めることにより、移動台車の移動誤差が比較的小さい移動ロボット１００の制御軌道が選定される。

移動ロボット１００の消費エネルギーは、移動台車１０２及びマニピュレータ機構１０３の総移動量及び加減速の回数等に依存する。このため、評価指数（ア）（イ）（ウ）（エ）（オ）を評価関数に含めることにより、移動台車１０２及びマニピュレータ機構１０３を駆動するエネルギーが比較的小さい移動ロボット１００の制御軌道が選定される。

移動台車１０２の軌道をコマンドに変換する処理は、移動台車１０２の移動方向の変更回数、回転方向の変更回数、移動から回転への切り替え回数、及び回転から移動への切り替え回数を低減するように設定すれば容易になる。空中を移動するマニピュレータ機構１０３と異なり、床面３０と接触しながら移動する移動台車１０２は、小刻みに移動方向を変更する或いは加減速すると、目標軌道への追従精度が低減される。そのため、ある程度永い時間で移動する１つの軌道に沿って移動するように制御されることが好ましい。評価指数（エ）（オ）（カ）（キ）を評価関数に含めることによって、コマンド変換が容易、且つ、移動台車１０２の移動誤差が比較的小さい移動ロボット１００の制御軌道が選定される。

重みα〜ηは、移動ロボット１００及び作業の特性に応じて適宜変更することができる。例えば、移動台車１０２の回転時の誤差が非常に大きい場合は、重みβを大きくすることで、台車１０２の回転量が比較的小さい移動台車１０２の軌道が選定されるように設定することができる。

以上のように、本発明の実施の形態に係る移動ロボット１００においては、移動台車１０２及びマニピュレータ機構１０３の軌道が同時に定められ、把持した対象物を定められた軌道に沿って操作することができる。また、移動台車１０２は、１つのコマンドに対して移動又は姿勢のいずれか一方を変更するように制御され、移動台車１０２の軌道を制御コマンドに容易に変更できる。

１０…冷蔵庫、１１…冷蔵庫本体、１２，１４，１６…ドア、１３，１５，１７…ドアノブ、１８…ヒンジ取り付け位置、３０…床面、１００…移動ロボット、１０１…移動ロボット本体、１０２…移動台車、１０３…マニピュレータ機構、１０４…把持機構、２０１〜２０７…リンク部、２１１〜２１９…関節、３０１…ＣＰＵ、３０２…ＲＡＭ、３０３…ＲＯＭ、３０４…インターフェース、３０５…入力表示パネル、３０６…無線装置、３０７…マニピュレータ機構駆動部、３０８…把持機構駆動部、３０９…移動機構駆動部

Claims (6)

1. 移動ロボット本体と、
   前記移動ロボット本体を前後方向に移動可能且つ走行面に垂直な軸まわりに回転可能に載置する、ノンホロノミックな拘束を有する移動台車と、
   前記移動ロボット本体に設けられ、各々が関節角を有する６以上の関節を備えるマニピュレータと、
   前記マニピュレータの先端部に設けられ、操作する物品軌道を予め定められる物品を把持する把持機構と、
   を具備する移動マニピュレータと、
   前記物品軌道に従って前記移動台車の台車位置及び姿勢並びに前記関節角を含む前記移動マニピュレータの制御軌道を生成する軌道生成部と、
   を具備する移動マニピュレータの軌道生成システムであって、
   前記軌道生成部は、前記物品軌道に設定される複数の演算点毎に、操作開始時における前記把持機構の初期位置及び姿勢と操作終了時における前記把持機構の最終位置及び姿勢とから前記把持機構の位置及び姿勢を次々に定め、前記演算点毎に定められた前記把持機構の位置及び姿勢から把持機構軌跡を定め、前記把持機構軌跡に従って、前記移動台車の前後方向の移動量及び前記走行面に垂直な軸まわりの回転量のいずれか一方と前記関節角の変化量とを前記演算点毎に算出することにより、前記移動台車の台車位置及び姿勢並びに前記関節角を定め、
   前記軌道生成部は、操作開始時における前記移動台車の初期台車位置及び姿勢、前記操作開始時における前記把持機構の初期位置及び姿勢、操作終了時における前記把持機構の最終位置及び姿勢を含む初期設定について、前記移動台車の初期台車位置及び姿勢、前記把持機構の初期位置及び姿勢、並びに前記把持機構の最終位置及び姿勢の夫々を１以上の任意の数だけ設定することが可能であり、ここにおいて、前記移動台車の初期台車位置及び姿勢、前記把持機構の初期位置及び姿勢、並びに前記把持機構の最終位置及び姿勢のうちの少なくとも１つは複数個設定され、
   前記初期設定から選択された前記初期台車位置及び姿勢、前記把持機構の前記初期位置及び姿勢、並びに前記把持機構の最終位置及び姿勢を夫々１つ選定して生成される複数の組み合わせの各々に対して前記制御軌道を生成することを特徴とする移動マニピュレータの軌道生成システム。
2. 前記軌道生成部は、前記制御軌道を評価するための評価指標で記述される評価関数を有し、前記複数の組み合わせの各々に対して生成された制御軌道を評価指標で指標化して前記評価関数から実際の前記移動マニピュレータ軌道を選定することを特徴とする請求項１に記載の移動マニピュレータ機構の軌道生成システム。
3. 前記評価指標は、前記移動台車が移動する移動量及び回転する回転量、前記関節角の変化量、前記移動台車が移動方向を変更する移動方向変更回数、前記移動台車が回転方向を変更する回転方向変更回数の少なくとも１つを含むことを特徴とする請求項２に記載の移動マニピュレータの軌道生成システム。
4. 移動ロボット本体と、
   前記移動ロボット本体を前後方向に移動可能且つ走行面に垂直な軸まわりに回転可能に載置する、ノンホロノミックな拘束を有する移動台車と、
   前記移動ロボット本体に設けられ、各々が関節角を有する６以上の関節を備えるマニピュレータと、
   前記マニピュレータの先端部に設けられ、操作する物品軌道を予め定められる物品を把持する把持機構と、
   を具備する移動マニピュレータと、
   前記物品軌道に従って前記移動台車の台車位置及び姿勢並びに前記関節角を含む前記移動マニピュレータの制御軌道を生成する軌道生成部と、
   を具備する移動マニピュレータの軌道生成システムであって、
   前記軌道生成部は、前記物品軌道に設定される複数の演算点毎に、操作開始時における前記把持機構の初期位置及び姿勢と操作終了時における前記把持機構の最終位置及び姿勢とから前記把持機構の位置及び姿勢を次々に定め、前記演算点毎に定められた前記把持機構の位置及び姿勢から把持機構軌跡を定め、前記把持機構軌跡に従って、前記移動台車の前後方向の移動量及び前記走行面に垂直な軸まわりの回転量のいずれか一方と前記関節角の変化量とを前記演算点毎に算出することにより、前記移動台車の台車位置及び姿勢並びに前記関節角を定め、
   前記軌道生成部は、前記各演算点において前記移動台車の位置及び姿勢並びに前記関節角を定めるための第１のヤコビ行列及び第２のヤコビ行列を保持し、
   ここにおいて、前記第１のヤコビ行列は、前記関節角に関連する成分及び前記移動台車の前後方向の位置に関連する成分から構成され、前記第２のヤコビ行列は、前記関節角に関連する成分及び前記移動台車の姿勢に関連する成分から構成される、ことを特徴とする移動マニピュレータの軌道生成システム。
5. 前記軌道生成部は、前記演算点において前記移動台車の位置が変更される場合には、前記第１のヤコビ行列を使用し、前記演算点において前記移動台車の姿勢が変更される場合には、前記第２のヤコビ行列を使用することを特徴とする請求項４に記載の移動マニピュレータの軌道生成システム。
6. 前記軌道生成部は、前記各演算点において前記第１及び第２のヤコビ行列のいずれか一方を選択して使用することを特徴とする請求項４に記載の移動マニピュレータの軌道生成システム。

Images