JP6044511B2

JP6044511B2 - ロボットの制御方法及びロボットシステム

Info

Publication number: JP6044511B2
Application number: JP2013229372A
Authority: JP
Inventors: 高橋　太郎; 太郎高橋
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2013-11-05
Filing date: 2013-11-05
Publication date: 2016-12-14
Anticipated expiration: 2033-11-05
Also published as: JP2015089584A

Description

本発明は多関節ロボットの制御方法及びロボットシステムに関する。

近年、手先で物体を把持して移動させるロボットの開発が行われている。このようなロボットが作業を行う場合、ロボットのリンクや関節が外部の物体と接触する場合が有る。このような場合、接触による外力（接触力）の影響を回避するための接触力緩和動作を行う必要が有る。

特許文献１には、接触力緩和動作を行うためロボットのインピーダンス制御装置が開示されている。このロボットのインピーダンス制御装置は、接触力から接触による関節トルクを算出し、関節部の接触力緩和動作における逃げ量を算出する。そして、逃げ量を、ロボットの手先の位置及び姿勢を制御するための関節角度に加算する。その結果、ロボットの関節が指定された逃げ量だけ駆動され、接触力緩和動作が行われる。

図６は、特許文献１に開示されたロボットのインピーダンス制御装置の構成を示すブロック図である。図６において、３０１はアーム、３０２はアーム手先部に作用する外力を計測する力センサ、３０３はアーム３０１等に作用する接触外力の大きさと作用位置を計測する、１個〜複数個設けられた接触センサである。３０４はアーム３０１を駆動するモータ、３０５はモータ３０４の回転角を検出する回転角検出計、３０６は手先作用力情報をアーム３０１の手先位置変位に変換する手先変位算出部、３０７はアーム部作用接触力を関節外乱トルクに変換する接触外力／関節トルク変換部、３０８は関節外乱トルクをアーム３０１の関節角度変位に変換する関節変位算出部、３０９は無負荷時のアーム３０１の手先位置を設定する手先釣り合い位置設定部、３１０は加算装置、３１１はアーム３０１の手先位置から関節角度を算出する逆運動学計算部、３１２は加算装置、３１３は角度指令に基づきモータ３０４を制御するサーボコントローラである。

次いで、図６に示すロボットのインピーダンス制御装置について説明する。先ず、手元に取付けられた外力計測用センサ（力センサ）３０２によって、手先作用力が計測される。手先作用力は手先変位算出部３０６に入力され、環境との接触を伴うマニピュレータの力制御用のインピーダンス制御（又はコンプライアンス制御）に伴う、機械インピーダンス（仮想慣性、仮想粘性、仮想剛性）として設定された手先部仮想インピーダンスの値（Ｊｅ、ｄｅ、ｋｅ）を用いて、手先変位量に変換される。

この手先変位量は、加算装置３１０によって手先釣り合い位置設定部３０９の手先釣り合い位置（接触前の無負荷時のアーム３０１の手先釣り合い位置）と加算され、手先位置の指令値が算出される。そして算出された手先位置指令は、逆運動学計算部３１１で各関節の角度指令に変換される。

アーム表面に作用する接触外力の大きさと、作用位置を計測する接触センサ３０３によって計測されたアームの接触外力は、接触外力／関節トルク変換部３０７においてヤコビアン行列等により関節トルクに変換され、関節変位算出部３０８に入力する。関節変位算出部３０８は設定された関節部仮想インピーダンスの値（Ｊｊ、ｄｊ、ｋｊ）から各関節の変位量（変位角を出力）を算出する。

各関節の変位量は加算装置３１２で、逆運動学計算部３１１からの各関節の角度指令と加算され、サーボコントローラ３１３に入力する。サーボコントローラ３１３では、入力した角度指令と回転角検出計３０５により検出した角度情報との差を比較してモータ３０４を制御する。なお、アーム表面に作用する接触外力を計測する接触センサ３０３が、接触外力の大きさのみしか計測できない圧力センサの場合は、関節トルク変換部３０７は接触外力の大きさと、接触センサ３０３の検出位置の代表点の情報から関節部外乱トルクを算出する。

この例では、関節部に加わるトルクの計測手段として、アーム表面に取付けた接触センサ３０３（高感度圧力センサ）を用いたので、従来のトルクセンサによる計測の場合のアームの慣性力の影響、微小な力の計測不能といった問題は改善された。また、センサについては接触センサの他、関節軸に取付けたトルクセンサ、感圧センサ等の各種センサを複合することで検出精度を上げるようにして、関節部トルクの計測を行うことも可能である。但し、関節軸トルクセンサを用いる場合は、先のようなアーム自身の動作による慣性力の影響や、手先作用力の関節部への影響をそれぞれ補正する必要がある。又、各種センサを複合して関節部トルクを計測する場合には、各種センサの干渉部を補正する必要がある。

以上、特許文献１によれば、各センサからの情報を、各関節に加わるトルクと手先に加わる力に分離し、各々に設定された仮想インピーダンスからアームの逃げ量を算出することにより、アームがどの部分で環境に接触しても適切な回避動作を実現できる。

特許文献２には、接触力緩和動作を行うためのマニピュレータ制御方法が開示されている。この方法では、接触力による関節トルクを推定し、推定結果から関節部の接触力緩和動作における逃げ量を算出する。そして、ロボットの手先の位置・姿勢を制御するための関節角度に加算する。その結果、ロボットの関節が指定された逃げ量だけ駆動され、接触力緩和動作が行われる。

特許文献３には、接触力緩和動作時の運動を制御するマニピュレータの柔軟制御装置が開示されている。この装置は、外力（接触力）によるトルクを推定し、推定値が閾値以上の場合に、フィードバック補償演算の積分ゲインを小さくする。これにより、接触力緩和動作時のロボットの動作を安定化することができる。

特許文献４には、ロボットが障害物等に接触した場合に、柔軟な動作を行うことができるロボット制御装置が開示されている。

特開２００１−３８６６４号公報特開２００７−１３６５６４号公報特開２００７−１０５８２３号公報特開２００８−７３８３０号公報

しかし、発明者は、上述の技術には以下に示す問題点が有ることを見出した。一般に、多関節ロボットに接触力が加わった場合、多関節ロボットの各関節を駆動させて接触力緩和動作を行う。一方で、多関節ロボットが作業する上で、手先が所望の位置及び姿勢であることが重要である。つまり、接触力緩和動作により手先の位置及び姿勢の一方又は双方がずれてしまうと、その後の作業に支障を来すことになる。よって、多関節ロボットにおいて、手先の位置及び姿勢を保持しつつ接触力緩和動作を行うことが求められる。

これに対し、特許文献１〜４に記載の技術では、ロボットが外部の物体等に接触し、接触力緩和動作を行う際に、手先の位置及び姿勢を保持することを想定していない。そのため、接触力緩和動作を行うと手先の位置及び姿勢の一方又は双方がずれてしまう。

本発明の一態様であるロボットの制御方法は、可動部を備えるロボットであり、逆運動学演算により、前記ロボットの位置及び姿勢の両方又は一方を指定する情報から前記可動部の変位である第１の値を算出するロボットの制御方法であり、前記ロボットが出力している力とは異なる力である外力を検知した際には前記外力にならって変移させる制御を備えるロボットの制御方法であって、前記第１の値から前記可動部の変位を指定する指定値を算出する指定値算出ステップと、前記指定値に基づいて前記ロボットの前記可動部を駆動して、前記可動部に生じるトルク又は力である第２の値を計測又は推定する第２の値推定ステップと、逆動力学演算により、前記指定値に対応して前記可動部に生じるべきトルク又は力に対応する値である第３の値を算出する第３の値算出ステップと、前記第２の値から前記第３の値を減算して、外力により前記可動部に加わるトルク又は力を示す第４の値を算出する第４の値算出ステップと、前記第４の値に応じて、接触力緩和動作を行う際の前記可動部の変位速度を示す目標値を算出する目標値算出ステップと、前記目標値を零空間変換して補正目標値を算出する補正目標値算出ステップと、前記補正目標値から補正値を算出する補正値算出ステップと、記補正値に基づき前記第１の値を補正する補正ステップと、を備え、前記指定値算出ステップは、前記補正値が既に算出されている場合には、前記第１の値と前記補正値とに基づき前記指定値を算出するものである。
ものである。

上記のロボットの制御方法においては、前記目標値算出ステップは、標準姿勢を示す前記可動部の変位である第５の値と前記指定値との差分から、前記標準姿勢に復帰するための前記可動部の変位速度である第６の値を算出し、前記第４の値に応じて算出した接触力緩和動作を行う際の前記可動部の変位速度を示す値に、前記第６の値を加算して、前記目標値を算出することが望ましい。

上記のロボットの制御方法においては、前記目標値算出ステップは、前記可動部の変位を前記可動部の可動範囲に収めるための変位である第５の値と前記指定値との差分から、前記可動部の変位を前記可動範囲に収めるための前記可動部の変位速度である第６の値を算出し、前記第４の値に応じて算出した接触力緩和動作を行う際の前記可動部の変位速度を示す値に、前記第６の値を加算して、前記目標値を算出することが望ましい。
ことが望ましい。

上記のロボットの制御方法においては、前記可動部は、回転運動を行う回転関節であり、前記第１の値は、前記回転関節の回転運動の回転角度であり、前記第２の値は、前記回転関節に生じるトルクであり、前記第３の値は、前記指定値に対応して前記回転関節に生じるべきトルクであり、前記第４の値は、外力により前記回転関節に加わるトルクであり、前記可動部の変位速度は、前記回転関節の回転運動の角速度であることが望ましい。

上記のロボットの制御方法においては、前記可動部は、回転運動を行う回転関節であり、前記第１の値及び前記第５の値は、前記回転関節の回転運動の回転角度であり、前記第２の値は、前記回転関節に生じるトルクであり、前記第３の値は、前記指定値に対応して前記回転関節に生じるべきトルクであり、前記第４の値は、外力により前記回転関節に加わるトルクであり、前記可動部の変位速度は、前記回転関節の回転運動の角速度であることが望ましい。

上記のロボットの制御方法においては、前記可動部は、並進運動を行う直動関節であり、前記第１の値は、前記直動関節の並進運動の変位量であり、前記第２の値は、前記直動関節に生じる力であり、前記第３の値は、前記指定値に対応して前記直動関節に生じる力であり、前記第４の値は、外力により前記直動関節に加わる力であり、前記可動部の変位速度は、前記直動関節の並進運動の速度であることが望ましい。

上記のロボットの制御方法においては、前記可動部は、並進運動を行う直動関節であり、
前記第１の値及び前記第５の値は、前記直動関節の並進運動の変位量であり、前記第２の値は、前記直動関節に生じる力であり、前記第３の値は、前記指定値に対応して前記直動関節に生じる力であり、前記第４の値は、外力により前記直動関節に加わる力であり、前記可動部の変位速度は、前記直動関節の並進運動の速度であることが望ましい。

上記のロボットの制御方法においては、前記目標値算出ステップは、前記目標値を、外力により前記可動部に加わるトルク又は力に基づいたマス・ダンパモデルを用いて算出することが望ましい。

上記のロボットの制御方法においては、前記第２の値は、前記可動部のトルク又力をセンサにより計測した値であることが望ましい。

本発明の一態様であるロボットシステムは、複数の可動部を有するロボットと、前記ロボットの接触力緩和動作を制御するロボット制御装置と、を備え、前記ロボット制御装置は、可動部を備えるロボットであり、逆運動学演算により、前記ロボットの位置及び姿勢の両方又は一方を指定する情報から前記可動部の変位である第１の値を算出するロボットの制御方法であり、前記ロボットが出力している力とは異なる力である外力を検知した際には前記外力にならって変移させる制御を備えるロボットの制御方法であって、前記第１の値から前記可動部の変位を指定する指定値を算出する指定値算出ステップと、前記指定値に基づいて前記ロボットの前記可動部を駆動して、前記可動部に生じるトルク又は力である第２の値を計測又は推定する第２の値推定ステップと、逆動力学演算により、前記指定値に対応して前記可動部に生じるべきトルク又は力に対応する値である第３の値を算出する第３の値算出ステップと、前記第２の値から前記第３の値を減算して、外力により前記可動部に加わるトルク又は力を示す第４の値を算出する第４の値算出ステップと、前記第４の値に応じて、接触力緩和動作を行う際の前記可動部の変位速度を示す目標値を算出する目標値算出ステップと、前記目標値を零空間変換して補正目標値を算出する補正目標値算出ステップと、前記補正目標値から補正値を算出する補正値算出ステップと、記補正値に基づき前記第１の値を補正する補正ステップと、を備え、前記指定値算出ステップは、前記補正値が既に算出されている場合には、前記第１の値と前記補正値とに基づき前記指定値を算出するものである。

上記のロボットシステムにおいては、前記ロボット制御装置は、標準姿勢を示す前記可動部の変位である第５の値と前記指定値との差分から、前記標準姿勢に復帰するための前記可動部の変位速度である第６の値を算出し、前記第４の値に応じて算出した接触力緩和動作を行う際の前記可動部の変位速度を示す値に、前記第６の値を加算して、前記目標値を算出することが望ましい。

上記のロボットシステムにおいては、前記ロボット制御装置は、前記可動部の変位を前記可動部の可動範囲に収めるための変位である第５の値と前記指定値との差分から、前記可動部の変位を前記可動範囲に収めるための前記可動部の変位速度である第６の値を算出し、前記第４の値に応じて算出した接触力緩和動作を行う際の前記可動部の変位速度を示す値に、前記第６の値を加算して、前記目標値を算出することが望ましい。

上記のロボットシステムにおいては、前記可動部は、回転運動を行う回転関節であり、前記第１の値は、前記回転関節の回転運動の回転角度であり、前記第２の値は、前記回転関節に生じるトルクであり、前記第３の値は、前記指定値に対応して前記回転関節に生じるべきトルクであり、前記第４の値は、外力により前記回転関節に加わるトルクであり、前記可動部の変位速度は、前記回転関節の回転運動の角速度であることが望ましい。

上記のロボットシステムにおいては、前記可動部は、回転運動を行う回転関節であり、前記第１の値及び前記第５の値は、前記回転関節の回転運動の回転角度であり、前記第２の値は、前記回転関節に生じるトルクであり、前記第３の値は、前記指定値に対応して前記回転関節に生じるべきトルクであり、前記第４の値は、外力により前記回転関節に加わるトルクであり、前記可動部の変位速度は、前記回転関節の回転運動の角速度であることが望ましい。

上記のロボットシステムにおいては、前記可動部は、並進運動を行う直動関節であり、前記第１の値は、前記直動関節の並進運動の変位量であり、前記第２の値は、前記直動関節に生じる力であり、前記第３の値は、前記指定値に対応して前記直動関節に生じる力であり、前記第４の値は、外力により前記直動関節に加わる力であり、前記可動部の変位速度は、前記直動関節の並進運動の速度であることが望ましい。

上記のロボットシステムにおいては、前記可動部は、並進運動を行う直動関節であり、前記第１の値及び前記第５の値は、前記直動関節の並進運動の変位量であり、前記第２の値は、前記直動関節に生じる力であり、前記第３の値は、前記指定値に対応して前記直動関節に生じる力であり、前記第４の値は、外力により前記直動関節に加わる力であり、前記可動部の変位速度は、前記直動関節の並進運動の速度であることが望ましい。

上記のロボットシステムにおいては、前記目標値は、外力により前記可動部に加わるトルク又は力に基づいたマス・ダンパモデルを用いて算出することが望ましい。

上記のロボットシステムにおいては、前記第２の値は、前記可動部のトルク又力をセンサにより計測した値であることが望ましい。

本発明によれば、ロボットにおいて手先の位置及び姿勢を保持しつつ接触力緩和動作を行うことができる。

実施の形態１にかかるロボットシステム１０００の構成を模式的に示す図である。実施の形態１にかかるロボット制御装置１００の構成を模式的に示すブロック図である。実施の形態１にかかるロボットの接触力緩和動作を示す制御ブロック図である。実施の形態２にかかるロボット制御装置２００の構成を模式的に示すブロック図である。実施の形態２にかかるロボットの接触力緩和動作を示す制御ブロック図である。特許文献１に開示されたロボットのインピーダンス制御装置の構成を示すブロック図である。

実施の形態１
実施の形態１にかかるロボットの接触力緩和動作について説明する。本実施の形態では、多関節ロボットの何処が他の物体と接触して接触部に接触力が加わった場合、多関節ロボットが手先の位置及び姿勢を保持しつつ、接触力による影響を緩和する接触力緩和動作を行う。

本実施の形態にかかるロボットの接触力緩和動作は、冗長性を有するロボットに適用される。例えば、ロボットの手先に把持した物体の位置及び姿勢は６自由度を有する。したがって、本実施の形態にかかるロボットの接触力緩和動作は、典型的には７軸以上のロボットに適用することができる。但し、ロボットの手先に把持した物体の位置及び姿勢のうち、保持すべき自由度が６よりも小さい場合には、６軸以下のロボットに適用できる場合が存在することを排除するものではない。

本実施の形態では、ロボットの手先の位置及び姿勢がずれないように、ロボットの手先で保持された物体等の位置及び姿勢を維持しつつ、接触力緩和動作を行う。以下で説明する接触力緩和動作は、ロボットの各関節について行われるものである。以下では、例えば７軸ロボットにおいて、それぞれの関節における接触力緩和動作及びその動作を制御する制御装置の構成について、図面を参照して説明する。

図１は、実施の形態１にかかるロボットシステム１０００の構成を模式的に示す図である。ロボットシステム１０００は、ロボット１０１、トルクセンサ１０２及びロボット制御装置１００を有する。

ロボット１０１は、多関節のロボットである。図１では、一例として、７関節のロボットを模式的に図示している。関節Ｊ１、Ｊ３、Ｊ５及びＪ７は、図１紙面に平行な方向を軸として回転する関節である。関節Ｊ２、Ｊ４及びＪ６は、図１紙面に垂直な方向を軸として回転する関節である。ＨＤは、物体の把持部である。

トルクセンサ１０２は、ロボット１０１が動作している際の各関節のトルクを測定し、ベクトルで示されるトルクτを出力する。以下の説明では、ベクトル及び行列については、太字で表す。

ロボット制御装置１００は、制御信号ＣＯＮを与えることで、ロボット１０１の動作制御を行う。また、ロボット制御装置１００は、ロボット１０１が他の物体と接触して接触部に接触力が加わった場合に、手先の位置及び姿勢を保持しつつ接触力による影響を緩和する接触力緩和動作をロボット１０１に行わせる。但し、本実施の形態にかかる接触力緩和動作は、必ずしも手先の位置及び姿勢の両方を制御することに限定されるものではない。すなわち、本実施の形態にかかる接触力緩和動作は、手先の位置及び姿勢の一方のみを保持する動作を含む。また、本実施の形態にかかる接触力緩和動作は、手先の位置及び姿勢を示す座標方向成分のうち、１成分のみを保持する、又は２以上の成分を保持する動作を含む。２以上の成分を保持する動作を行う場合、保持する成分の組み合わせは、手先の位置を示す成分の組み合わせ、手先の姿勢を示す成分の組み合わせ、及び、手先の位置を示す成分と手先の姿勢を示す成分との組み合わせのいずれであってもよい。

図２は、実施の形態１にかかるロボット制御装置１００の構成を模式的に示すブロック図である。ロボット制御装置１００は、関節角度算出部１、関節角度制御部２、トルク算出部４、角速度算出部５、零空間変換部６及びゲイン付与部７を有する。なお、図２の関節駆動部３は、ロボット１０１に含まれるものとする。ロボット制御装置１００は、ロボットの計画手先位置Ｘ_ｐｌａｎと実測トルクＴ_ａｃｔとに基づいて、ロボットの各関節を駆動する関節駆動部３へ駆動信号Ｓｄｒを出力する。これにより、ロボット制御装置１００は、ロボット１０１の各関節の接触力緩和動作を制御する。実測トルクＴ_ａｃｔは、関節のトルクセンサ１０２により、各関節のトルクを実測した値である。

図３は、実施の形態１にかかるロボットの接触力緩和動作を示す制御ブロック図である。本実施の形態における接触力緩和動作は、関節角度算出ステップＳ１、加算ステップＳ２、関節角度制御ステップＳ３、関節駆動ステップＳ４、トルク算出ステップＳ５、減算ステップＳ６、角速度算出ステップＳ７、零空間変換ステップＳ８及びゲイン付与ステップＳ９からなる。

以下、図２及び図３を参照して、ロボット制御装置１００の構成及び接触力緩和動作について説明する。

一般に、ロボット１０１の手先座標ベクトルＰと、関節角度ベクトルｑとの間には、以下の式（１）が成立する。

式（１）において、なお、手先座標ベクトルＰ及び関節角度ベクトルｑの上に付した黒点は、１階の時間微分を示す。Ｊはヤコビアン行列である。以下、式（１）の成立を前提として説明する。

関節角度算出ステップＳ１
まず、関節角度算出部１は、ロボット１０１の計画手先位置Ｘ_ｐｌａｎを受けとる。ロボット１０１の計画手先位置Ｘ_ｐｌａｎは、ロボットの動作制御に供する軌道計画により決定される、手先の位置及び姿勢を示す情報であり、例えば、３次元直交座標や３次元極座標などの形式で表される。関節角度算出部１は、計画手先位置Ｘ_ｐｌａｎを用いて、例えばニュートン法などの数値的な繰り返し演算を行う逆運動学演算により、ロボット１０１の関節の回転運動の角度（関節角度）ｑ_ｐｌａｎを算出する。数値的な繰り返し演算には、以下の式（２）を用いて行うことができる。

式（２）において、ｑ_ｋはロボット１０１のｋ回目の繰り返し演算後の関節の角度、Ｊ^＋はヤコビアン行列の疑似逆行列、ｘ_ｋ−１はｋ回目の繰り返し演算後の番目の関節の座標を示す。繰り返し演算で得られた結果は、関節角度ｑ_ｐｌａｎとして出力される。なお、関節角度ｑ_ｐｌａｎを、第１の値とも称する。ｋは、繰り返し演算の回数を示すパラメータである。ロボット１０１の関節の角度ｑ_ｐｌａｎは、ロボット１０１に接触力が作用しないときに、ロボット１０１の手先を計画手先位置Ｘ_ｐｌａｎで指定される位置及び姿勢に保持するために、関節に設定される関節角度を示している。

加算ステップＳ２
関節角度制御部２は、ロボット１０１の関節角度ｑ_ｐｌａｎと、ゲイン付与部７からの補正角度Δｑ_ｎｕｌｌと、を加算し、指定角度ｑ_ｒｅｆを算出する。なお、補正角度Δｑ_ｎｕｌｌを、補正値とも称する。指定角度ｑ_ｒｅｆを、指定値とも称する。

関節角度制御ステップＳ３
関節角度制御部２は、指定角度ｑ_ｒｅｆに基づいて、駆動信号Ｓｄｒを生成する。

関節駆動ステップＳ４
関節駆動部３は、駆動信号Ｓｄｒに基づいて、関節を駆動する。同時に、関節駆動部３は、トルクセンサにより節に生じた実測トルクＴ_ａｃｔを測定し、測定結果を出力する。なお、実測トルクＴ_ａｃｔを、第２の値とも称する。

トルク算出ステップＳ５
トルク算出部４は、ロボット１０１の関節角度ｑ_ｐｌａｎと、ゲイン付与部７からの補正角度Δｑ_ｎｕｌｌと、を加算し、指定角度ｑ_ｒｅｆを算出する。トルク算出部４は、指定角度ｑ_ｒｅｆを用いた逆動力学演算により、関節を指定角度ｑ_ｒｅｆだけ駆動させるのに必要なトルクＴ_ｃａｌを算出し、算出結果を出力する。なお、トルクＴ_ｃａｌを、第３の値とも称する。

減算ステップＳ６
角速度算出部５は、実測トルクＴ_ａｃｔからトルクＴｃａｌを減算して、外力によるトルクτを算出する。なお、トルクτを、第４の値とも称する。

角速度算出ステップＳ７
角速度算出部５は、外力によるトルクτを用いてマス・ダンパモデルにより演算を行い、関節に加わる外力の影響を回避するための目標角速度ω_{ａｖｏｉｄ}を算出する。なお、目標角速度ω_{ａｖｏｉｄ}を、目標値とも称する。

零空間変換ステップＳ８
零空間変換部６は、各関節の目標角速度ω_{ａｖｏｉｄ}が包含されるベクトル空間を、変換後の各関節の補正角速度ω_ｎｕｌｌが構成するベクトル空間が零空間となるように変換する。これにより、ロボット１０１の各関節の補正角速度を指定しても、出力として得られるロボット１０１の手先の位置及び姿勢を保持することができる。なお、補正角速度ω_ｎｕｌｌを、補正目標値とも称する。

ゲイン付与ステップＳ９
ゲイン付与部７は、補正角速度ω_ｎｕｌｌにゲインΔｔを乗じ、関節に加わる外力の影響を回避するための補正角度Δｑ_ｎｕｌｌを算出する。上述のように、各関節の補正角速度ω_ｎｕｌｌは、零空間を構成する。各関節の補正角度Δｑ_ｎｕｌｌが加算されることで、関節角度制御部２は、出力として得られるロボット１０１の手先の位置及び姿勢が保持できるように、駆動信号Ｓｄｒを生成することができる。

一般に、角度のみでロボットの姿勢制御を行うと、移動先の角度のみを指定するため、移動先の座標以外を指定することができない。この場合、例えば、移動先への移動速度を細かく制御して柔軟なロボットの動作を実現することが難しい。また、移動先でロボットの一部が外部の物体と接触し、接触点を一定の力で押し込む場合、座標による姿勢制御では座標のみしか指定できないので、押し付け力を制御することが困難である。また、異なる運動のタスクを重ね合わせる場合、角度を与えるのみでは重ね合わせたタスクの運動を制御することがそもそも困難である。

これに対し、本実施の形態では、角速度を用いて外力による影響を緩和する接触力緩和動作を行っている。そのため、ロボットの回避動作を外力の大きさに応じた回避速度で行うことができる。その結果、不必要な高速で動作したり、回避速度が不足してしまうといった事態を防止することができる。また、移動先でロボット１０１の一部が外部の物体と接触し、接触点を一定の力で押し込む場合、移動先での角速度を指定すれば押し付け力を容易に制御することができる。また、異なる運動のタスクを重ね合わせる場合、各速度を用いれば、重ね合わせたタスクの運動を制御することできる。よって、角度を用いた制御に比べ、より複雑かつ柔軟なロボットの運動を実現できる。

以上より、本実施の形態によれば、手先位置及び姿勢を保持しつつ、ロボットの運動状態を制御しつつ、接触力緩和動作を行うことができる。これにより、より高精度かつ柔軟なロボットの姿勢制御を行うことができる。

実施の形態２
実施の形態２にかかるロボットの接触力緩和動作について説明する。図４は、実施の形態２にかかるロボット制御装置２００の構成を模式的に示すブロック図である。ロボット制御装置２００は、実施の形態１にかかるロボット制御装置１００に、復帰角速度算出部８を追加し、更にロボット制御装置１００の角速度算出部５を角速度算出部９に置換した構成を有する。ロボット制御装置２００のその他の構成は、ロボット制御装置１００と同様であるので、説明を省略する。ロボット制御装置２００は、接触力緩和動作を行った際に、例えばロボット１０１が不安定な姿勢等にならないように、所定の姿勢への復帰動作を更に行うことができる。

図５は、実施の形態２にかかるロボットの接触力緩和動作を示す制御ブロック図である。本実施の形態における接触力緩和動作は、図３の実施の形態１にかかるロボットの接触力緩和動作の角速度算出ステップＳ７を削除し、かつステップＳ２１〜Ｓ２３を追加したものである。図５のステップＳ１〜Ｓ６、Ｓ８及びＳ９は、図３と同様であるので、説明を省略する。

復帰角速度算出ステップＳ２１
復帰角速度算出部８は、標準姿勢を示す関節角度ｑ_ｉｎｉｔと指定角度ｑ_ｒｅｆとが入力される。復帰角速度算出部８は、標準姿勢を示す関節角度ｑ_ｉｎｉｔと指定角度ｑ_ｒｅｆとの間の差分から、標準姿勢へ復帰するために必要な復帰角速度ω_ｉｎｉｔを算出する。なお、標準姿勢を示す関節角度ｑ_ｉｎｉｔを、第５の値とも称する。復帰角速度ω_ｉｎｉｔを、第６の値とも称する。

角速度算出ステップＳ２２
角速度算出部９は、実測トルクＴ_ａｃｔからトルクＴ_ｃａｌを減算して、外力によるトルクτを算出する。角速度算出部５は、外力によるトルクτを用いてマス・ダンパモデルにより演算を行い、関節に加わる外力の影響を回避するための角速度ω_{ａｖｏｉｄ１}を算出する。ここで、角速度ω_{ａｖｏｉｄ１}は、実施の形態１における目標角速度ω_{ａｖｏｉｄ}に相当する。

角速度算出ステップＳ２３
角速度算出部９は、角速度ω_{ａｖｏｉｄ１}と復帰角速度ω_ｉｎｉｔとを加算し、目標角速度ω_{ａｖｏｉｄ}を出力する。

以上より、本実施の形態において零空間変換部６に入力される目標角速度ω_{ａｖｏｉｄ}は、関節に加わる外力の影響を回避するための角速度ω_{ａｖｏｉｄ１}だけでなく、復帰角速度ω_ｉｎｉｔが反映されている。これにより、接触力緩和動作を行っても、ロボット１０１が所定の位置及び姿勢に復帰させることが可能となる。

その他の実施の形態
なお、本発明は上記実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。例えば、例えば、上述の実施の形態にかかる接触力緩和動作は、冗長性を有するならば、７軸以外の数の多関節ロボットに適用できることは言うまでもない。

ロボット制御装置１００は、コンピュータなどのハードウェア資源で実現することも可能である。この場合、実施の形態１にかかる接触力緩和動作の手順を規定するプログラムを、ロボット制御装置１００に実行させることで、同様に行うことが可能である。

上述の実施の形態２では、標準姿勢への復帰について説明したが、これは一例に過ぎない。例えば、標準姿勢に代えて、関節の可動範囲を考慮してもよい。この場合、実施の形態２の関節角度ｑ_ｉｎｉｔを、関節の可動範囲を示す関節角度に置き換えることで実現することができる。これにより、ロボットを構成する各関節を、可動範囲内に収まるように運動させることが可能である。

上述の実施の形態では、関節として回転関節のみを想定し、関節角度の回転角度、角速度及びトルクに着目したが、これは例示に過ぎない。多関節ロボットは、並進運動（伸縮）が可能な直動関節を有する構成をとることも可能である。この場合、直動関節の運動制御においては、回転関節の関節角度に代えて直動関節の並進運動の変位量、回転関節の角速度に代えて直動関節の並進運動の変位速度、回転関節のトルクに代えて直動関節の並進運動にかかる力に着目することで、上記の方法を用いて同様の接触力緩和動作を実現することができる。

上述の通り、回転関節及び直動関節を有する多関節ロボットの接触力緩和動作を行うこと可能である。以下、回転関節及び直動関節を可動部と称し、回転関節の関節角度及び直動関節の並進運動の変位量を可動部の変位と称する。また、回転関節の角速度及び直動関節の並進運動の速度を可動部の変位速度と称する。

すなわち、上述の第１の値及び前記第５の値は、回転関節の回転角度と、直動関節の並進運動の変位量と、を包含する。上述の第２の値は、回転関節に生じるトルクと、直動関節に生じる力と、を包含する。上述の第３の値は、指定値に対応して回転関節に生じるべきトルクと、指定値に対応して直動関節に生じる力と、を包含する。上述の第４の値は、外力により回転関節に加わるトルクと、外力により直動関節に加わる力と、を包含する。上述の第６の値は、標準姿勢に復帰するための回転関節の角速度と、標準姿勢に復帰するための直動関節の並進運動の変位速度と、を包含する。更に、上述の第６の値は、回転関節の回転角度を可動範囲に収めるための回転関節の角速度と、直動関節の並進運動の変位を可動範囲に収めるための直動関節の並進運動の変位速度と、を包含する。上述の指定値は、回転関節の指定角度と、直動関節の指定変位と、を包含する。上述の補正値は、回転関節の補正角度と、直動関節の補正変位と、を包含する。述の目標値は、回転関節の目標角速度と、直動関節の並進運動の目標変位速度と、を包含する。述の補正目標値は、回転関節の補正角速度と、直動関節の並進運動の補正変位速度と、を包含する。

上述の実施の形態では、トルクセンサでトルクを算出する例について説明したが、これは例示に過ぎない。例えば、トルクの計測に代えて、関節の動作時に測定される、関節の運動を示す他の物理量（例えば、関節を駆動するモータの電流値）から、関節のトルクを推定してもよい。

１関節角度算出部
２関節角度制御部
３関節駆動部
４トルク算出部
５角速度算出部
６零空間変換部
７ゲイン付与部
８復帰角速度算出部
９角速度算出部
１００、２００ロボット制御装置
１０１ロボット
１０２トルクセンサ
２００ロボット制御装置
３０１アーム
３０２力センサ
３０３接触センサ
３０４モータ
３０５回転角検出計
３０６手先変位算出部
３０７関節トルク変換部
３０８関節変位算出部
３０９位置設定部
３１０加算装置
３１１逆運動学計算部
３１２加算装置
３１３サーボコントローラ
１０００ロボットシステム

Claims

可動部を備えるロボットであり、逆運動学演算により、前記ロボットの位置及び姿勢の両方又は一方を指定する情報から前記可動部の変位である第１の値を算出するロボットの制御方法であり、前記ロボットが出力している力とは異なる力である外力を検知した際には前記外力にならって変移させる制御を備えるロボットの制御方法であって、
前記第１の値から前記可動部の変位を指定する指定値を算出する指定値算出ステップと、
前記指定値に基づいて前記ロボットの前記可動部を駆動して、前記可動部に生じるトルク又は力である第２の値を計測又は推定する第２の値推定ステップと、
逆動力学演算により、前記指定値に対応して前記可動部に生じるべきトルク又は力に対応する値である第３の値を算出する第３の値算出ステップと、
前記第２の値から前記第３の値を減算して、外力により前記可動部に加わるトルク又は力を示す第４の値を算出する第４の値算出ステップと、
前記第４の値に応じて、接触力緩和動作を行う際の前記可動部の変位速度を示す目標値を算出する目標値算出ステップと、
前記目標値を零空間変換して補正目標値を算出する補正目標値算出ステップと、
前記補正目標値から補正値を算出する補正値算出ステップと、
前記補正値に基づき前記第１の値を補正する補正ステップと、を備え、
前記指定値算出ステップは、前記補正値が既に算出されている場合には、前記第１の値と前記補正値とに基づき前記指定値を算出する、
ロボットの制御方法。
前記目標値算出ステップは、
標準姿勢を示す前記可動部の変位である第５の値と前記指定値との差分から、前記標準姿勢に復帰するための前記可動部の変位速度である第６の値を算出し、
前記第４の値に応じて算出した接触力緩和動作を行う際の前記可動部の変位速度を示す値に、前記第６の値を加算して、前記目標値を算出する、
請求項１に記載のロボットの制御方法。
前記目標値算出ステップは、
前記可動部の変位を前記可動部の可動範囲に収めるための変位である第５の値と前記指定値との差分から、前記可動部の変位を前記可動範囲に収めるための前記可動部の変位速度である第６の値を算出し、
前記第４の値に応じて算出した接触力緩和動作を行う際の前記可動部の変位速度を示す値に、前記第６の値を加算して、前記目標値を算出する、
請求項１に記載のロボットの制御方法。
前記可動部は、回転運動を行う回転関節であり、
前記第１の値は、前記回転関節の回転運動の回転角度であり、
前記第２の値は、前記回転関節に生じるトルクであり、
前記第３の値は、前記指定値に対応して前記回転関節に生じるべきトルクであり、
前記第４の値は、外力により前記回転関節に加わるトルクであり、
前記可動部の変位速度は、前記回転関節の回転運動の角速度である、
請求項１乃至３のいずれか一項に記載のロボットの制御方法。
前記可動部は、回転運動を行う回転関節であり、
前記第１の値及び前記第５の値は、前記回転関節の回転運動の回転角度であり、
前記第２の値は、前記回転関節に生じるトルクであり、
前記第３の値は、前記指定値に対応して前記回転関節に生じるべきトルクであり、
前記第４の値は、外力により前記回転関節に加わるトルクであり、
前記可動部の変位速度は、前記回転関節の回転運動の角速度である、
請求項２又は３に記載のロボットの制御方法。
前記可動部は、並進運動を行う直動関節であり、
前記第１の値は、前記直動関節の並進運動の変位量であり、
前記第２の値は、前記直動関節に生じる力であり、
前記第３の値は、前記指定値に対応して前記直動関節に生じる力であり、
前記第４の値は、外力により前記直動関節に加わる力であり、
前記可動部の変位速度は、前記直動関節の並進運動の速度である、
請求項１乃至３のいずれか一項に記載のロボットの制御方法。
前記可動部は、並進運動を行う直動関節であり、
前記第１の値及び前記第５の値は、前記直動関節の並進運動の変位量であり、
前記第２の値は、前記直動関節に生じる力であり、
前記第３の値は、前記指定値に対応して前記直動関節に生じる力であり、
前記第４の値は、外力により前記直動関節に加わる力であり、
前記可動部の変位速度は、前記直動関節の並進運動の速度である、
請求項２又は３に記載のロボットの制御方法。
前記目標値算出ステップは、前記目標値を、外力により前記可動部に加わるトルク又は力に基づいたマス・ダンパモデルを用いて算出する、
請求項１乃至７のいずれか一項に記載のロボットの制御方法。
前記第２の値は、前記可動部のトルク又力をセンサにより計測した値である、
請求項１乃至８のいずれか一項に記載のロボットの制御方法。
複数の可動部を有するロボットと、
前記複数の可動部を有するロボットの接触力緩和動作を制御するロボット制御装置と、を備え、
前記ロボット制御装置は、
可動部を備えるロボットであり、逆運動学演算により、前記ロボットの位置及び姿勢の両方又は一方を指定する情報から前記可動部の変位である第１の値を算出するロボットの制御方法であり、前記ロボットが出力している力とは異なる力である外力を検知した際には前記外力にならって変移させる制御を備えるロボットの制御方法であって、
前記第１の値から前記可動部の変位を指定する指定値を算出する指定値算出ステップと、
前記指定値に基づいて前記ロボットの前記可動部を駆動して、前記可動部に生じるトルク又は力である第２の値を計測又は推定する第２の値推定ステップと、
逆動力学演算により、前記指定値に対応して前記可動部に生じるべきトルク又は力に対応する値である第３の値を算出する第３の値算出ステップと、
前記第２の値から前記第３の値を減算して、外力により前記可動部に加わるトルク又は力を示す第４の値を算出する第４の値算出ステップと、
前記第４の値に応じて、接触力緩和動作を行う際の前記可動部の変位速度を示す目標値を算出する目標値算出ステップと、
前記目標値を零空間変換して補正目標値を算出する補正目標値算出ステップと、
前記補正目標値から補正値を算出する補正値算出ステップと、
前記補正値に基づき前記第１の値を補正する補正ステップと、を備え、
前記指定値算出ステップは、前記補正値が既に算出されている場合には、前記第１の値と前記補正値とに基づき前記指定値を算出する、
ロボットシステム。
前記ロボット制御装置は、
標準姿勢を示す前記可動部の変位である第５の値と前記指定値との差分から、前記標準姿勢に復帰するための前記可動部の変位速度である第６の値を算出し、
前記第４の値に応じて算出した接触力緩和動作を行う際の前記可動部の変位速度を示す値に、前記第６の値を加算して、前記目標値を算出する、
請求項１０に記載のロボットシステム。
前記ロボット制御装置は、
前記可動部の変位を前記可動部の可動範囲に収めるための変位である第５の値と前記指定値との差分から、前記可動部の変位を前記可動範囲に収めるための前記可動部の変位速度である第６の値を算出し、
前記第４の値に応じて算出した接触力緩和動作を行う際の前記可動部の変位速度を示す値に、前記第６の値を加算して、前記目標値を算出する、
請求項１０に記載のロボットシステム。
前記可動部は、回転運動を行う回転関節であり、
前記第１の値は、前記回転関節の回転運動の回転角度であり、
前記第２の値は、前記回転関節に生じるトルクであり、
前記第３の値は、前記指定値に対応して前記回転関節に生じるべきトルクであり、
前記第４の値は、外力により前記回転関節に加わるトルクであり、
前記可動部の変位速度は、前記回転関節の回転運動の角速度である、
請求項１０乃至１２のいずれか一項に記載のロボットシステム。
前記可動部は、回転運動を行う回転関節であり、
前記第１の値及び前記第５の値は、前記回転関節の回転運動の回転角度であり、
前記第２の値は、前記回転関節に生じるトルクであり、
前記第３の値は、前記指定値に対応して前記回転関節に生じるべきトルクであり、
前記第４の値は、外力により前記回転関節に加わるトルクであり、
前記可動部の変位速度は、前記回転関節の回転運動の角速度である、
請求項１１又は１２に記載のロボットシステム。
前記可動部は、並進運動を行う直動関節であり、
前記第１の値は、前記直動関節の並進運動の変位量であり、
前記第２の値は、前記直動関節に生じる力であり、
前記第３の値は、前記指定値に対応して前記直動関節に生じる力であり、
前記第４の値は、外力により前記直動関節に加わる力であり、
前記可動部の変位速度は、前記直動関節の並進運動の速度である、
請求項１０乃至１２のいずれか一項に記載のロボットシステム。
前記可動部は、並進運動を行う直動関節であり、
前記第１の値及び前記第５の値は、前記直動関節の並進運動の変位量であり、
前記第２の値は、前記直動関節に生じる力であり、
前記第３の値は、前記指定値に対応して前記直動関節に生じる力であり、
前記第４の値は、外力により前記直動関節に加わる力であり、
前記可動部の変位速度は、前記直動関節の並進運動の速度である、
請求項１１又は１２に記載のロボットシステム。
前記目標値は、外力により前記可動部に加わるトルク又は力に基づいたマス・ダンパモデルを用いて算出する、
請求項１０乃至１６のいずれか一項に記載のロボットシステム。
前記第２の値は、前記可動部のトルク又力をセンサにより計測した値である、
請求項１０乃至１７のいずれか一項に記載のロボットシステム。