JP5776486B2 - ロボット制御装置、その制御方法及びプログラム - Google Patents

Description

本発明は、ロボットの動作を制御して操作対象物を効率的に操作するロボット制御装置、その制御方法及びプログラムに関するものである。

近年、マニピュレータや移動装置を備え、人間の生活環境内で自律移動し、ユーザの指示に従って操作対象物を操作できる様々なサービスロボットなどが開発されている。
例えば、複数のマニピュレータを有する複腕ロボットを動作させ、複数の部品の組み付け作業を行うためのプログラムを生成するプログラム作成方法が知られている（特許文献１参照）。

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

しかしながら、上記特許文献１に示すプログラムを用いてロボットを制御した場合、例えば、家具や壁などの障害物によりロボットの動作範囲が限定されている環境内では、その環境に応じて、操作対象物を効率良く操作することについて、十分考慮されているとは言えず、そのような操作は困難と思われる。

本発明は、このような問題点を解決するためになされたものであり、ロボットの動作範囲が限定されている環境内においても、操作対象物を効率良く操作できるロボット制御装置、その制御方法及びプログラムを提供することを主たる目的とする。

上記目的を達成するための本発明の一態様は、複数の関節を含むロボットアームを備えるロボットを制御して操作対象物を操作するロボット制御装置であって、前記ロボットに所望の動作をさせるための動作情報を入力する入力手段と、前記ロボットの周囲の環境情報を検出する環境情報検出手段と、前記ロボットの状態量を検出する状態検出手段と、前記入力手段により入力された動作情報と、前記環境検出手段により検出された環境情報と、に基づいて、前記ロボットアームの各関節が可動できる第１関節可動範囲を算出する関節可動範囲算出手段と、前記可動範囲算出手段により算出された第１関節可動範囲の制約下において、前記ロボットアームに関するジャコビアン行列の特異値と、前記入力手段により入力された動作情報と、をパラメータとして含む評価関数を、最小化させるための前記ロボットアームの各関節角度を算出し、該算出した各関節角度に基づいて前記ロボットの位置を算出する位置関節角度算出手段と、前記状態検出手段により検出されたロボットの状態量と、前記位置関節角度算出手段により算出された前記ロボットアームの各関節角度及びロボットの位置に基づいて、前記ロボットを制御する制御手段と、を備える、ことを特徴とするロボット制御装置である。
この一態様において、前記ロボットアームに関するジャコビ行列は、第１特異値と、該第１特異値よりも大きな第２特異値と、を有しており、前記位置関節角度算出手段は、前記入力手段により入力された動作情報が所定方向に操作対象物を移動させる情報の場合において、前記第１特異値と前記動作情報とを含む第１評価関数を最小化するための前記ロボットアームの各関節角度を算出し、前記入力手段により入力された動作情報が操作対象物に対して所定力を付加する情報の場合において、前記第２特異値と前記動作情報とを含む第２評価関数を最小化するための前記ロボットアームの各関節角度を算出してもよい。
この一態様において、前記入力手段により入力された動作情報が所定方向に操作対象物を移動させつつ所定力を加える情報である場合において、前記位置関節角度算出手段は、前記操作対象物を所定方向へ移動させることが、前記操作対象物に所定力を加えることよりも重要であるとき、前記可動範囲算出手段により算出された前記第１関節可動範囲、及び前記重要度を示す所定係数と前記第１特異値とを含む関数で表わされる第２関節可動範囲の制約下で、前記第１評価関数を最小化させるための前記ロボットアームの各関節角度を算出し、前記操作対象物に所定力を加えることが前記操作対象物を所定方向へ移動させることよりも重要であるとき、前記可動範囲算出手段により算出された前記第１関節可動範囲、及び前記重要度を示す所定係数と前記第２特異値とを含む関数で表わされる第２関節可動範囲の制約下で、前記第２評価関数を最小化させるための前記ロボットアームの各関節角度を算出してもよい。
この一態様において、前記関節可動範囲算出手段は、前記入力手段により入力された動作情報と、前記環境情報検出手段により検出された前記ロボット周囲の障害物の位置情報と、に基づいて、前記ロボットの動作範囲を算出し、該算出した動作範囲と、予め設定された前記関節機構の制約による関節の可動範囲と、に基づいて、前記第１関節可動範囲を算出してもよい。
他方、上記目的を達成するための本発明の一態様は、複数の関節を含むロボットアームを備えるロボットを制御して操作対象物を操作するロボット制御装置の制御方法であって、前記ロボットに所望の動作をさせるための動作情報を入力するステップと、前記ロボットの周囲の環境情報を検出するステップと、前記ロボットの状態量を検出するステップと、前記入力された動作情報と、前記検出された環境情報と、に基づいて、前記ロボットアームの各関節が可動できる第１関節可動範囲を算出するステップと、前記算出された第１関節可動範囲の制約下において、前記ロボットアームに関するジャコビアン行列の特異値と、前記入力された動作情報と、をパラメータとして含む評価関数を、最小化させるための前記ロボットアームの各関節角度を算出し、該算出した各関節角度に基づいて前記ロボットの位置を算出するステップと、前記検出されたロボットの状態量と、前記算出されたロボットアームの各関節角度及びロボットの位置に基づいて、前記ロボットを制御するステップと、を含む、ことを特徴とするロボット制御装置の制御方法であってもよい。
さらに、上記目的を達成するための本発明の一態様は、複数の関節を含むロボットアームを備えるロボットを制御して操作対象物を操作するロボット制御装置のプログラムであって、前記ロボットに所望の動作をさせるための動作情報と、前記ロボットの周囲の環境情報と、に基づいて、前記ロボットアームの各関節が可動できる第１関節可動範囲を算出する処理と、前記算出された第１関節可動範囲の制約下において、前記ロボットアームに関するジャコビアン行列の特異値と、前記入力された動作情報と、をパラメータとして含む評価関数を、最小化させるための前記ロボットアームの各関節角度を算出し、該算出した各関節角度に基づいて前記ロボットの位置を算出する処理と、前記ロボットの状態量と、前記算出されたロボットアームの各関節角度及びロボットの位置に基づいて、前記ロボットを制御する処理と、を含む、ことを特徴とするロボット制御装置のプログラムであってもよい。

本発明によれば、ロボットの動作範囲が限定されている環境内においても、操作対象物を効率良く操作できるロボット制御装置、その制御方法及びプログラムを提供することができる。

本発明の一実施の形態に係るロボット制御装置の概略的なシステム構成を示すブロック図である。 本発明の一実施の形態に係るロボットの動作を説明するための摸式図である。 本発明の一実施の形態に係るシミュレーションにおいてロボット制御装置の制御装置の位置関節角度演算器が用いる第１評価関数を示す図である。 本発明の一実施の形態に係るシミュレーションにおいて、ロボット制御装置の制御装置の位置関節角度演算器が用いる第１評価関数を最小化した場合のロボットの位置及び姿勢を示す図である。 本発明の一実施の形態に係るシミュレーションにおいてロボット制御装置の制御装置の位置関節角度演算器が用いる第２評価関数を示す図である。 本発明の一実施の形態に係るシミュレーションにおいて、ロボット制御装置の制御装置の位置関節角度演算器が用いる第２評価関数を最小化した場合のロボットの位置及び姿勢を示す図である。

以下、図面を参照して本発明の実施の形態について説明する。図１は、本発明の一実施の形態に係るロボット制御装置の概略的なシステム構成を示すブロック図である。本実施の形態に係るロボット制御装置１００は、例えば、ロボット１３０の動作範囲が家具や壁などで限定された環境内において、操作対象物を所望の方向に所望の速度で移動させ、操作対象物に対して所望の力を付加するような制御を効率的に実行できるものである。

ロボット制御装置１００は、入力装置１１０と、入力装置１１０に入力された入力情報に基づいて、制御対象であるロボット１３０の動作を制御する制御装置１２０と、を備えている。

入力装置１１０は、入力手段の一具体例であり、例えば、ユーザがロボット１３０に実施させたい所望の動作を示す動作指令を入力し、その所望の動作の特徴を示す動作情報を算出し、出力する機能を有している。入力装置１１０は、指令入力器１１１と、指令演算器１１２と、を有している。

指令入力器１１１は、例えば、音声入力装置、タッチパネルなどのユーザ指示が入力できる任意の装置で構成されており、ユーザが入力した動作指令を電気信号に変換し、指令演算器１１２に対して出力する。

指令演算器１１２は、指令入力器１１１からの電気信号に基づいて、ユーザがロボット１３０に実施させたい所望の動作の特徴を示す動作情報（例えば、操作対象物に対して所定力を付加する動作、操作対象物を所定速度で所定方向へ移動させる動作、などの情報）を算出し、制御装置１２０に対して出力する。

制御装置１２０は、例えば、入力装置１１０からの動作情報と、ロボット１３０からの環境情報（ロボット１３０周囲にある障害物の位置などの情報）と、に基づいて、ロボット１３０の各関節（肩関節、肘関節、手首関節、股関節、膝関節、足首関節など）の可動範囲（以下、関節可動範囲と称す）と、周囲の障害物などによる環境的な制約の中で、ユーザが指示する所望動作に最も近い動作を、ロボット１３０に動作させるためのモータ電流を演算し、ロボット１３０に対して出力する。

制御装置１２０は、例えば、ＡＳＩＣ（Application Specific Integrated Circuit）やプログラマブル電子システムなどとして、実現することができる。制御装置１２０は、関節可動範囲演算器１２１と、位置関節角度演算器１２２と、制御器１２３と、を有している。

関節可動範囲演算器１２１は、関節可動範囲演算手段の一具体例であり、入力装置１１０の指令演算器１１２から出力される動作情報と、ロボット１３０の環境検出器１３１から出力される環境情報と、に基づいてロボット１３０の各関節の関節可動範囲を演算し、位置関節角度演算器１２２に対して出力する。

位置関節角度演算器１２２は、位置関節角度演算手段の一具体例であり、関節可動範囲演算器１２１からの関節可動範囲と、入力装置１１０の指令演算器１１２からの動作情報と、に基づいて、ユーザがロボット１３０に実施させたい所望動作を実施するのに最適な、ロボット１３０の位置およびロボット各関節の関節角度を演算し、位置関節角度演算値として制御器１２３に対して出力する。

制御器１２３は、制御手段の一具体例であり、位置関節角度演算器１２２からの位置関節角度演算値と、ロボット１３０の状態検出器１３４からの状態検出値（ロボット１３０の各関節の関節角度など）と、に基づいて、ロボット１３０に所望動作を実施させるためのモータ電流を、ロボット１３０に対して出力する。

ロボット１３０は、操作対象物を操作可能なロボットアーム２０５を有し、制御装置１２０からのモータ電流に基づいて、ユーザがロボット１３０に実施させたい所望の動作を実施できるように構成された、例えば、移動型サーバントロボット、二足歩行ロボット、ヒューマノイド型ロボットなどである。

ロボット１３０は、環境検出器１３１と、関節モータ１３２と、ロボット機構１３３と、状態検出器１３４と、を有している。

環境検出器１３１は、環境情報検出手段の一具体例であり、ロボット１３０に搭載され、あるいは、ロボット１３０が動作する環境に設置された検出器（例えば、超音波センサ、ミリ波センサ、カメラ、赤外線センサなどの距離センサ）であり、ロボット１３０の周囲の障害物の位置などの情報である環境情報を検出し、有線又は無線を介して制御装置１２０の関節可動範囲演算器１２１に対して出力する。

関節モータ１３２は、ロボット機構１３３のロボットアーム２０５などに設けられた各関節に取付けられ、制御装置１２０の制御器１２３から出力されるモータ電流に従って各関節を回転駆動する。また、関節モータ１３２は、減速機構などを内蔵しており、駆動軸の駆動力をこの減速機構を介して関節の関節軸に伝達する。

ロボット機構１３３は、例えば、複数のリンクと、各リンクを回転可能に連結する複数の関節と、を有する機構であり、例えば、ロボットアーム２０５などが含まれる。

状態検出器１３４は、状態検出手段の一具体例であり、ロボット１３０に搭載され、ロボットの状態量（ロボットの位置、各関節などの可動部の状態量など）を検出し、状態検出値として制御装置１２０の制御器１２３に対して出力する。

例えば、状態検出器１３４は、角度センサを用いて各関節の関節角度を検出し、トルクセンサを用いて各関節の関節トルクを検出し、姿勢センサを用いてロボット姿勢を検出し、加速度センサや速度センサを用いてロボット１３０の位置、移動速度、移動加速度を検出する。

次に、本実施の形態に係るロボット制御装置の動作原理について、詳細に説明する。
図２は、本実施の形態に係るロボットの動作を説明するための摸式図の一例である。本実施の形態に係るロボット１３０は、例えば、胴体であるロボット体幹部２０１に肩関節を介して回転可能に連結された上腕部２０２と、上腕部２０２に肘関節を介して回転可能に連結された前腕部２０３と、前腕部２０３の先端に手首関節を介して回転可能に連結され、操作対象物を操作可能なロボットハンド２０４と、を有するロボットアーム２０５を備えており、ロボット体幹部２０１が破線で示す予め設定された動作範囲２０６内側（限定された環境内）で動作可能となるように構成されている。

まず、ロボット体幹部２０１の中心に対するロボットハンド２０４の相対的な位置は、下記（１）式及び（２）式で表される。

但し、上記（１）式及び（２）式において各記号は以下のように設定されている。
ｘ_Ｅ：ロボット体幹部２０１の中心に対するロボットハンド２０４のｘ方向相対位置[ｍ]、
ｙ_Ｅ：ロボット体幹部２０１の中心に対するロボットハンド２０４のｙ方向相対位置[ｍ]、
θ_１：第１関節角度（ロボット体幹部２０１と上腕部２０２とが成す関節角度）［ｒａｄ］、
θ_２：第２関節角度（上腕部２０２と前腕部２０３とが成す関節角度）［ｒａｄ］、
ｌ_１：第１リンク長さ（上腕部２０２の長さ）[ｍ]、
ｌ_２：第２リンク長さ（前腕部２０３の長さ）[ｍ]
上記（１）式及び（２）式を、ロボットアーム２０５に関するジャコビアン行列Ｊを用いて線形近似すると下記（３）式が求められる。

上記（３）式より下記（４）式が導出される。

ジャコビアン行列Ｊの特異値は上記（４）式を用いて、下記（５）式および（６）式として求められる。

但し、上記（５）式及び（６）式において、σ_１は第１特異値であり、σ_２は第２特異値である。なお、第１特異値σ_１および第２特異値σ_２は、ともに０または正の実数であり、第２特異値σ_２は第１特異値σ_１以上となっている。
特異ベクトルは上記（５）式および（６）式を用いて、下記（７）式および（８）式として求められる。

但し、上記（７）式および（８）式において、ｃ_１とｃ_２は任意の実数である。
ここで、例えば、上記（３）式に示すような、ロボットアーム２０５の位置および角度を変換するジャコビアン行列の、より小さな特異値に対応した特異ベクトルの方向により、大きな速度で大きな動作をすることができることが一般的に知られている。同様に、ロボットアームの力およびトルクを変換するジャコビアン行列の、より大きな特異値に対応した特異ベクトルの方向により、大きな力を発生することができることが一般的に知られている。（例えば、Changhyun Cho, Munsang Kim, and Jae-Bok Song, "Direct Control of a Passive Haptic Device Based on Passive Force Manipulability Ellipsoid Analysis", International Journal of Control, Automation, and Systems, Volume 2, No. 2, June 2004を参照）また、ロボットアームの力およびトルクを変換するジャコビアン行列は、位置と角度を変換するジャコビアン行列の転置行列であり、それぞれの特異値は逆数の関係にあることが知られている。ただし、上記文献とは変換する方向が逆であるため特異値の大小関係が逆となっている。

上記ジャコビアン行列の関係を用いて、上記（５）式乃至（８）式は、ロボットハンド２０４が（７）式の示す方向により大きな速度で大きな動作をすることが可能であり、（８）式の示す方向により大きな力を発生することが可能であることを示していることが分かる。本願発明は、上述したようなロボットアーム２０５におけるジャコビアン行列の特異値の特性を、ロボット１３０の制御に効率的に利用したものである。以下、その制御方法について、具体例を挙げて説明する。

動作範囲２０６の中心を原点としたカーテシアン座標系におけるロボット体幹部２０１の中心位置を（ｘ_０、ｙ_０）と表すと、動作範囲２０６の中心に対するロボットハンド２０４の位置は、例えば、下記（９）式および（１０）式と表される。

但し、上記（９）式および（１０）式における各記号の意味は以下の通りであり、例えば、操作対象物の位置と一致するｘ_Ｅ０、ｙ_Ｅ０は、環境検出器１３１により検出される。ｘ_Ｅ、ｙ_Ｅは、上記（１）式及び（２）式で示される。
ｘ_０：ロボット体幹部２０１の中心のｘ座標[ｍ]、
ｙ_０：ロボット体幹部２０１の中心のｙ座標[ｍ]、
ｘ_Ｅ０：ロボットハンド２０４のｘ座標[ｍ]、
ｙ_Ｅ０：ロボットハンド２０４のｙ座標[ｍ]

次に、ロボット体幹部２０１が動作範囲２０６の内側のみを動作できる場合において、ロボットハンド２０４により操作対象物Ｓを所望の方向に最も効果的に動かすことができるロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度（第１関節角度θ_１及び第２関節角度θ_２）を導出する。
動作範囲２０６は、例えば、下記（１１）式のように表され、環境検出器１３１により検出された環境情報（障害物の位置情報など）に基づいて自動的に設定されてもよく、ユーザにより入力装置１１０を介して入力される入力情報に基づいて設定されてもよい。

上記（９）式乃至（１１）式を組み合わせることにより、例えば、ロボットアーム２０５の各関節の動作範囲の制約は下記（１２）式として表される。

但し、上記（１２）式における各記号の意味は以下の通りである。
Ω_ｊ：ロボットアーム２０５の各関節が可動できる第１関節可動範囲
Ω_ｊｐ：動作範囲２０６より制約されるロボットアーム２０５の各関節の可動範囲であり、動作範囲２０６に基づいて算出される。
Ω_ｍ：ロボットアーム２０５の各関節の機構により制約される可動範囲であり、例えば、ハードウェアにより予め設定される範囲である。
上記のように、ロボット１３０の動作範囲２０６、ロボットアーム２０５の関節機構などを総合的に考慮して、ロボットアーム２０５の関節可動範囲を最適に設定することができる。

ロボットハンド２０４により操作対象物Ｓを第１関節可動範囲Ω_ｊ内で所望の方向に最も効果的に移動させることができるロボット体幹部２０１（ロボット１３０）の位置およびロボットアーム２０５の各関節角度は、以下のように求めることができる。まず、入力装置１１０の指令演算器１１２から出力される動作情報ｒ_１と、ジャコビアン行列Ｊの第１特異値σ_１と、に基づいて下記（１３）式に示す第１評価関数を、上記（１２）式に示す第１関節可動範囲Ω_ｊの制約下で最小化する第１関節角度θ_１及び第２関節角度θ_２を算出する。そして、上記（９）式および（１０）式において、ロボットハンド２０４のｘ座標ｘ_Ｅ０およびｙ座標ｙ_Ｅ０を、操作対象物Ｓのｘ座標およびｙ座標として、ロボット体幹部２０１の中心のｘ座標ｘ_０およびｙ座標ｙ_０に関して解く。これにより、上記ロボットハンド２０４により操作対象物Ｓを所望の方向に最も効果的に動かすことができるロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を求めることができる。

但し、上記（１３）式において、動作情報ｒ_１は、例えば、操作対象物Ｓの所定方向への移動を表すベクトルのｙ成分とｘ成分との比である。また、ｒ_１は、入力装置１１０の指令入力器１１１から入力されたユーザの操作情報に基づいて、指令演算器１１２が算出する動作情報の一例である。また、上記第１評価関数Ｉ_１は、ロボットアーム２０５のジャコビアン行列Jの第１特異値σ_１と、動作情報ｒ_１と、をパラメータとして含んでいる。

同様に、ロボットハンド２０４により操作対象物Ｓに対して所望の方向に最も効果的に力を加えることができるロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度は、以下のようにして求めることができる。まず、入力装置１１０の指令演算器１１２から出力される動作情報ｒ_２と、ジャコビアン行列Ｊの第２特異値σ_２と、に基づいて下記（１４）式の第２評価関数を上記（１２）式の第１関節可動範囲Ω_ｊに対して最小化する第１関節角度θ_１及び第２関節角度θ_２を算出する。そして、上記（９）式および（１０）式において、ロボットハンド２０４のｘ座標ｘ_Ｅ０およびｙ座標ｙ_Ｅ０を、操作対象物Ｓのｘ座標およびｙ座標として、ロボット体幹部２０１の中心のｘ座標ｘ_０およびｙ座標ｙ_０に関して解く。これにより、上記ロボットハンド２０４により操作対象物Ｓに対して所望の方向に最も効果的に力を加えることができるロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を求めることができる。

但し、上記（１４）式において、動作情報ｒ_２は、例えば、操作対象物Ｓに加える所定力を表すベクトルのｙ成分とｘ成分の比である。また、ｒ_２は、入力装置１１０の指令入力器１１１から入力されたユーザの操作情報に基づいて、指令演算器１１２が算出する動作情報の一例である。上記第２評価関数は、ロボットアーム２０５のジャコビアン行列Jの第２特異値σ_２と、動作情報ｒ_２と、をパラメータとして含んでいる。

また、操作対象物Ｓを所定方向へ移動させることが、操作対象物Ｓに所定力を付加することよりもｄ_１倍（ｄ_１≧１）重要である場合、上記（１２）式に示す第１関節可動範囲Ω_ｊの制約条件に、下記（１５）式に示す第２関節可動範囲Ω_ｒ１の制約条件を加え、上記（１３）式に示す第１評価関数を最小化することにより、ロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を求める。なお、ｄ_１は、例えば、操作対象物Ｓを所定方向へ移動させることの重要度を示す所定係数であり、ユーザが入力装置１１０を介して関節可動範囲演算器１２１に任意に設定できる。また、第２関節可動範囲Ω_ｒ１は、上記（４）式及び（５）式により導出され、所定係数ｄ_１を含む関数となっている。

同様に、操作対象物Ｓに所定力を付加することが、操作対象物Ｓを所定方向へ移動させることよりもｄ_２倍（ｄ_２≧１）重要である場合、上記（１２）式に示す第１関節可動範囲Ω_ｊの制約条件に下記（１６）式に示す第２関節可動範囲Ω_ｒ２の制約条件を加え、上記（１４）式に示す第２評価関数を最小化することにより、ロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を求める。なお、なお、ｄ_２は、例えば、操作対象物Ｓに所定力を付加することの重要度を示す所定係数であり、ユーザが入力装置１１０を介して関節可動範囲演算器１２１に任意に設定できる。また、第２関節可動範囲Ω_ｒ２は、上記（４）式及び（６）式より導出され、所定係数ｄ_２を含む関数となっている。
以上のように、第１関節可動範囲の制約に加えて、第２関節可動範囲の制約を加えることで、ロボット１３０の操作内容の重要度に応じた最適なロボットアーム２０５の関節可動範囲を設定することができる。

次に、上述したロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を求める演算処理について、より具体的に説明する。

（Ａ）図２に示すような動作範囲２０６の内側を移動できるロボット１３０が、ロボットアーム２０５の先端に設けられたロボットハンド２０４を用いて、操作対象物Ｓを所定方向へ移動させる作業を行う場合を想定する。このようなロボットハンド２０４を用いて操作対象物Ｓを移動させる作業として、例えば、家具と壁の間で掃除機（操作対象物）を移動させて掃除を行う作業などが該当する。

この場合、関節可動範囲演算器１２１は、上記（１２）式を用いて第１関節可動範囲Ω_ｊを算出する。また、位置関節角度演算器１２２は、関節可動範囲演算器１２１により算出された第１関節可動範囲Ω_ｊの制約条件の下で、ジャコビアン行列Ｊの第１特異値σ_１と、入力装置１１０の指令演算器１１２からの動作情報ｒ_１と、に基づいて上記第１評価関数（１３）式を最小化するように、第１関節角度θ_１および第２関節角度θ_２を算出する。そして、位置関節角度演算器１２２は、上記（９）式および（１０）式において、ロボットハンド２０４のｘ座標ｘ_Ｅ０およびｙ座標ｙ_Ｅ０を、操作対象物Ｓのｘ座標およびｙ座標として、ロボット体幹部２０１の中心のｘ座標ｘ_０およびｙ座標ｙ_０に関して解く。このようにして、操作対象物Ｓを所望の方向に最も効果的に移動させることができるロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を算出することができる。

（Ｂ）図２に示すような動作範囲２０６の内側を移動できるロボット１３０が、ロボットアーム２０５の先端に設けられたロボットハンド２０４を用いて、所定方向に操作対象物Ｓに対して所定力を付加する作業を行う場合を想定する。このようなロボットハンド２０４を用いて操作対象物Ｓに対して所定力を付加する作業として、例えば、風で動く扉をその位置に保持する作業などが該当する。

この場合、関節可動範囲演算器１２１は、上記（１２）式を用いて第１関節可動範囲Ω_ｊを算出する。また、位置関節角度演算器１２２は、関節可動範囲演算器１２１により算出された第１関節可動範囲Ω_ｊの制約条件の下で、ジャコビアン行列Ｊの第２特異値σ_２と、入力装置１１０の指令演算器１１２からの動作情報ｒ_２と、に基づいて上記第２評価関数（１４）式を最小化するように第１関節角度θ_１および第２関節角度θ_２を算出する。そして、位置関節角度演算器１２２は、上記（９）式および（１０）式において、ロボットハンド２０４のｘ座標ｘ_Ｅ０およびｙ座標ｙ_Ｅ０を、操作対象物Ｓのｘ座標およびｙ座標として、ロボット体幹部２０１の中心のｘ座標ｘ_０およびｙ座標ｙ_０に関して解く。このようにして、操作対象物Ｓを所望の方向に最も効果的に力を加えることができるロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を算出することができる。

（Ｃ）図２に示すような動作範囲２０６の内側を移動できるロボット１３０が、ロボットアーム２０５の先端に設けられたロボットハンド２０４を用いて、所定方向に操作対象物Ｓを移動させつつ、操作対象物Ｓに所定力を付加する作業を行う場合であって、所定方向へ移動させることが所定力を付加することよりもｄ_１倍重要となる作業を行う場合を想定する。このような操作対象物Ｓを移動させつつ力も加えなければならない作業としては、例えば、事故車やがれきの中から人を救出する作業などが該当する。

この場合、関節可動範囲演算器１２１は、上記（１２）式を用いて第１関節可動範囲Ω_ｊおよび上記（１５）式を用いて第２関節可動範囲Ω_ｒ１を算出する。また、位置関節角度演算器１２２は、関節可動範囲演算器１２１により算出された第１関節可動範囲Ω_ｊおよび第２関節可動範囲Ω_ｒ１の制約条件の下で、ジャコビアン行列Ｊの第１特異値σ_１と、入力装置１１０の指令演算器１１２からの動作情報ｒ_１と、に基づいて上記第１評価関数（１３）式を最小化するように第１関節角度θ_１および第２関節角度θ_２を算出する。そして、位置関節角度演算器１２２は、上記（９）式および（１０）式において、ロボットハンド２０４のｘ座標ｘ_Ｅ０およびｙ座標ｙ_Ｅ０を、操作対象物Ｓのｘ座標およびｙ座標として、ロボット体幹部２０１の中心のｘ座標ｘ_０およびｙ座標ｙ_０に関して解く。このようにして、操作対象物Ｓを所望の方向に最も効果的に移動させ力を加えることができるロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を算出することができる。

（Ｄ）図２に示すような動作範囲２０６の内側を移動できるロボット１３０が、ロボットアーム２０５の先端に設けられたロボットハンド２０４を用いて、所定方向に操作対象物Ｓを移動させつつ、操作対象物Ｓに所定力を付加する作業を行う場合であって、所定力を付加することが所定方向へ移動させることよりもｄ_２倍重要となる作業を行う場合を想定する。

この場合、関節可動範囲演算器１２１は、上記（１２）式を用いて第１関節可動範囲Ω_ｊおよび上記（１６）式を用いて第２関節可動範囲Ω_ｒ２を算出する。また、位置関節角度演算器１２２は、関節可動範囲演算器１２１により算出された第１関節可動囲Ω_ｊおよび第２関節可動範囲Ω_ｒ２の制約条件の下で、ジャコビアン行列Ｊの第２特異値σ_２と、入力装置１１０の指令演算器１１２からの動作情報ｒ_２と、に基づいて上記第２評価関数（１４）式を最小化するように第１関節角度θ_１および第２関節角度θ_２を算出する。そして、位置関節角度演算器１２２は、上記（９）式および（１０）式において、ロボットハンド２０４のｘ座標ｘ_Ｅ０およびｙ座標ｙ_Ｅ０を、操作対象物Ｓのｘ座標およびｙ座標として、ロボット体幹部２０１の中心のｘ座標ｘ_０およびｙ座標ｙ_０に関して解く。このようにして、操作対象物Ｓを所望の方向に最も効果的に移動させ力を加えることができるロボット体幹部２０１の位置およびロボットアーム２０５の各関節角度を算出することができる。

上記（Ｃ）及び（Ｄ）のように、操作対象物Ｓを移動させつつ力も付加しなければならない作業に対しては、上記（１２）式に示す第１関節可動範囲Ω_ｊの制約条件だけでなく、上記（１５）式に示す第２関節可動範囲Ω_ｒ１または（１６）式に示す第２関節可動範囲Ω_ｒ２の制約条件を加味した関節可動範囲を用いることにより、ロボット１３０は作業に最も適した姿勢および位置で作業することができる。
上述したように、ロボット１３０の動作範囲２０６、ロボットアーム２０５の関節機構など加味して関節可動範囲を最適に設定し、ロボットアーム２０５のジャコビアン行列の特異値による特性を反映した評価関数を用いてロボットアーム２０５の各関節角度を算出することで、ロボット１３０の動作範囲が限定されている環境においても、操作対象物２０５を効率良く操作できる。

次に、本実施の形態に係るロボット制御装置を用いて行ったシミュレーション結果について説明する。本シミュレーションにおいて、各パラメータには、例えば、下記数値が設定されている。なお、下記設定された数値は、例えば、家事手伝いをするモービルサーバントロボットの現実的な数値の一例である。

ｌ_１＝０．５[ｍ]、ｌ_２＝０．５[ｍ]、ｒ_１＝ｔａｎ（π／４）、ｒ_２＝ｔａｎ（π／４）、ｘ_Ｅ０＝０．１[ｍ]、ｙ_Ｅ０＝１．２[ｍ]、θ_１ｍｉｎ＝０［ｒａｄ］、θ_１ｍａｘ＝２π［ｒａｄ］、θ_２ｍｉｎ＝０［ｒａｄ］、θ_２ｍａｘ＝３π／４［ｒａｄ］

さらに、本シミュレーションにおいて、ロボット１３０は半径１[ｍ]の円で示す動作範囲２０６の内側を自由に移動することができ、その円の中心を原点としたカーテシアン座標系においてｘ方向に０．１[ｍ]、ｙ方向に１．２[ｍ]の位置にある操作対象物Ｓを操作する作業を実施する場合を模擬している。

図３は、本シミュレーションにおいてロボット制御装置の制御装置の位置関節角度演算器が用いる第１評価関数を示す図である。なお、図３において、細線のメッシュは関節可動範囲Ω_ｊを示し、太線のメッシュは関節可動範囲外の領域を示し、大きな点は第１評価関数Ｉ_１を最小化する第１関節角度θ_１と第２関節角度θ_２を示している。

図３に示すように、操作対象物Ｓをπ／４［ｒａｄ］の方向へ移動させるのに最適な姿勢は、第１関節角度θ_１が０［ｒａｄ］であり、かつ第２関節角度θ_２が略２．１２［ｒａｄ］であることを示している。

図４は、本シミュレーションにおいて、ロボット制御装置の制御装置の位置関節角度演算器が用いる第１評価関数を最小化した場合のロボットの位置及び姿勢を示す図である。なお、図４において、破線は操作対象物Ｓの所望の移動方向を示している。図４に示しように、ロボット体幹部２０１はｘ方向に略−０．１４[ｍ]、ｙ方向に略０．７７[ｍ]の位置に移動し、ロボット１３０は操作対象物Ｓを所望の方向に移動させるのに適した姿勢となっていることが分かる。

図５は、本シミュレーションにおいてロボット制御装置の制御装置の位置関節角度演算器が用いる第２評価関数を示す図である。なお、図５において、細線のメッシュは関節可動範囲Ω_ｊを示し、太線のメッシュは関節可動範囲外の領域を示し、大きな点は第２評価関数Ｉ_２を最小化する第１関節角度θ_１と第２関節角度θ_２を示している。

図５に示すように、操作対象物Ｓに対してπ／４［ｒａｄ］の方向へ力を加えるのに最適な姿勢は、第１関節角度θ_１が略０．３８［ｒａｄ］であり、第２関節角度θ_２が略０．５７［ｒａｄ］であることを示している。

図６は、本シミュレーションにおいて、ロボット制御装置の制御装置の位置関節角度演算器が用いる第２評価関数を最小化した場合のロボットの位置及び姿勢を示す図である。図６において、破線は操作対象物Ｓに付加する力の所望方向を示している。ロボット体幹部２０１はｘ方向に略−０．６６[ｍ]、ｙ方向に略０．６１[ｍ]の位置に移動し、ロボットは操作対象物Ｓに対して力を加えるのに適した姿勢となっていることが分かる。

以上、本実施の形態に係るロボット制御装置１００において、ロボット１３０の動作情報、及び環境情報に基づいた第１関節可動範囲を算出し、算出された第１関節可動範囲の制約下において、ロボットアームのジャコビアン行列における特異値と動作情報とをパラメータとして含む評価関数を最小化させるためのロボットアームの各関節角度を算出し、算出した各関節角度に基づいてロボットの位置を算出する。
これにより、ロボット１３０の動作範囲２０６、ロボットアーム２０５の関節機構など加味してロボットアーム２０５の関節可動範囲を最適に設定でき、ロボットアーム２０５のジャコビアン行列の特異値による特性を反映した評価関数を用いて操作対象物を効率良く操作できるロボットアーム２０５の各関節角度を算出することができる。すなわち、ロボット１３０の動作範囲が限定されている環境においても、操作対象物Ｓを効率良く操作できる。

なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。
例えば、上記一実施の形態において、ロボットアーム２０５で平面的に操作対象物Ｓを操作する作業について説明したが、上記（１）式及び（２）式を３次元の動作を表す式に置き換えることにより、ロボット１３０の全身を使って３次元的に操作対象物Ｓを操作する作業にも適用することが可能である。

上記実施形態では、本発明をハードウェアの構成として説明したが、本発明は、これに限定されるものではない。本発明は、例えば、制御装置１２０が実行する処理を、ＣＰＵにコンピュータプログラムを実行させることにより実現することも可能である。

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

本発明は、例えば、動作範囲が家具や壁などで限定された空間内において、操作対象物を効率よく操作できる、介護ロボット、救助ロボット、家事手伝いロボットなど人間と共生するサービスロボット全般に広く適用できる。

１００ ロボット制御装置
１１０ 入力装置
１１１ 指令入力器
１１２ 指令演算器
１２０ 制御装置
１２１ 関節可動範囲演算器
１２２ 位置関節角度演算器
１２３ 制御器
１３０ ロボット
１３１ 環境検出器
１３２ 関節モータ
１３３ ロボット機構
１３４ 状態検出器
２０１ ロボット体幹部
２０２ 上腕部
２０３ 前腕部
２０４ ロボットハンド
２０５ ロボットアーム
２０６ 動作範囲

Claims (5)

1. 複数の関節を含むロボットアームを備えるロボットを制御して操作対象物を操作するロボット制御装置であって、
   前記ロボットに所望の動作をさせるための動作情報を入力する入力手段と、
   前記ロボットの周囲の環境情報を検出する環境情報検出手段と、
   前記ロボットの状態量を検出する状態検出手段と、
   前記入力手段により入力された動作情報と、前記環境検出手段により検出された環境情報と、に基づいて、前記ロボットアームの各関節が可動できる第１関節可動範囲を算出する関節可動範囲算出手段と、
   前記可動範囲算出手段により算出された第１関節可動範囲の制約下において、前記ロボットアームに関するジャコビアン行列の特異値と、前記入力手段により入力された動作情報と、をパラメータとして含む評価関数を、最小化させるための前記ロボットアームの各関節角度を算出し、該算出した各関節角度に基づいて前記ロボットの位置を算出する位置関節角度算出手段と、
   前記状態検出手段により検出されたロボットの状態量と、前記位置関節角度算出手段により算出された前記ロボットアームの各関節角度及びロボットの位置に基づいて、前記ロボットを制御する制御手段と、
   を備え、
   前記ロボットアームに関するジャコビ行列は、第１特異値と、該第１特異値よりも大きな第２特異値と、を有しており、
   前記位置関節角度算出手段は、
   前記入力手段により入力された動作情報が所定方向に操作対象物を移動させる情報の場合において、前記第１特異値と前記動作情報とを含む第１評価関数を最小化するための前記ロボットアームの各関節角度を算出し、
   前記入力手段により入力された動作情報が操作対象物に対して所定力を付加する情報の場合において、前記第２特異値と前記動作情報とを含む第２評価関数を最小化するための前記ロボットアームの各関節角度を算出する、ことを特徴とするロボット制御装置。
2. 請求項１記載のロボット制御装置であって、
   前記入力手段により入力された動作情報が所定方向に操作対象物を移動させつつ所定力を加える情報である場合において、
   前記位置関節角度算出手段は、
   前記操作対象物を所定方向へ移動させることが、前記操作対象物に所定力を加えることよりも重要であるとき、前記可動範囲算出手段により算出された前記第１関節可動範囲、及び前記重要度を示す所定係数と前記第１特異値とを含む関数で表わされる第２関節可動範囲の制約下で、前記第１評価関数を最小化させるための前記ロボットアームの各関節角度を算出し、
   前記操作対象物に所定力を加えることが前記操作対象物を所定方向へ移動させることよりも重要であるとき、前記可動範囲算出手段により算出された前記第１関節可動範囲、及び前記重要度を示す所定係数と前記第２特異値とを含む関数で表わされる第２関節可動範囲の制約下で、前記第２評価関数を最小化させるための前記ロボットアームの各関節角度を算出する、
   ことを特徴とするロボット制御装置。
3. 請求項１又は２記載のロボット制御装置であって、
   前記関節可動範囲算出手段は、前記入力手段により入力された動作情報と、前記環境情報検出手段により検出された前記ロボット周囲の障害物の位置情報と、に基づいて、前記ロボットの動作範囲を算出し、該算出した動作範囲と、予め設定された前記関節機構の制約による関節の可動範囲と、に基づいて、前記第１関節可動範囲を算出する、ことを特徴とするロボット制御装置。
4. 複数の関節を含むロボットアームを備えるロボットを制御して操作対象物を操作するロボット制御装置の制御方法であって、
   前記ロボットに所望の動作をさせるための動作情報を入力するステップと、
   前記ロボットの周囲の環境情報を検出するステップと、
   前記ロボットの状態量を検出するステップと、
   前記入力された動作情報と、前記検出された環境情報と、に基づいて、前記ロボットアームの各関節が可動できる第１関節可動範囲を算出するステップと、
   前記算出された第１関節可動範囲の制約下において、前記ロボットアームに関するジャコビアン行列の特異値と、前記入力された動作情報と、をパラメータとして含む評価関数を、最小化させるための前記ロボットアームの各関節角度を算出し、該算出した各関節角度に基づいて前記ロボットの位置を算出するステップと、
   前記検出されたロボットの状態量と、前記算出されたロボットアームの各関節角度及びロボットの位置に基づいて、前記ロボットを制御するステップと、
   を含み、
   前記ロボットアームに関するジャコビ行列は、第１特異値と、該第１特異値よりも大きな第２特異値と、を有しており、
   前記入力した動作情報が所定方向に操作対象物を移動させる情報の場合において、前記第１特異値と前記動作情報とを含む第１評価関数を最小化するための前記ロボットアームの各関節角度を算出し、
   前記入力した動作情報が操作対象物に対して所定力を付加する情報の場合において、前記第２特異値と前記動作情報とを含む第２評価関数を最小化するための前記ロボットアームの各関節角度を算出する、ことを特徴とするロボット制御装置の制御方法。
5. 複数の関節を含むロボットアームを備えるロボットを制御して操作対象物を操作するロボット制御装置のプログラムであって、
   前記ロボットに所望の動作をさせるための動作情報と、前記ロボットの周囲の環境情報と、に基づいて、前記ロボットアームの各関節が可動できる第１関節可動範囲を算出する処理と、
   前記算出された第１関節可動範囲の制約下において、前記ロボットアームに関するジャコビアン行列の特異値と、前記入力された動作情報と、をパラメータとして含む評価関数を、最小化させるための前記ロボットアームの各関節角度を算出し、該算出した各関節角度に基づいて前記ロボットの位置を算出する処理と、
   前記ロボットの状態量と、前記算出されたロボットアームの各関節角度及びロボットの位置に基づいて、前記ロボットを制御する処理と、
   をコンピュータに実行させ、
   前記ロボットアームに関するジャコビ行列は、第１特異値と、該第１特異値よりも大きな第２特異値と、を有しており、
   前記入力した動作情報が所定方向に操作対象物を移動させる情報の場合において、前記第１特異値と前記動作情報とを含む第１評価関数を最小化するための前記ロボットアームの各関節角度を算出し、
   前記入力した動作情報が操作対象物に対して所定力を付加する情報の場合において、前記第２特異値と前記動作情報とを含む第２評価関数を最小化するための前記ロボットアームの各関節角度を算出する、ことを特徴とするロボット制御装置のプログラム。