JP2016155203A

JP2016155203A - ロボット制御装置

Info

Publication number: JP2016155203A
Application number: JP2015035781A
Authority: JP
Inventors: 将弘土井; Masahiro Doi
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2015-02-25
Filing date: 2015-02-25
Publication date: 2016-09-01
Anticipated expiration: 2035-02-25
Also published as: JP6500492B2

Abstract

【課題】従来のロボット制御装置では、ロボットの位置エネルギーを含むエネルギーを最小化することができなかった。
【解決手段】本発明のロボット制御装置は、アームの位置及び姿勢を示すアーム指令値に基づき目標速度ベクトルを算出する目標速度ベクトル計算部１０と、関節の現在の関節角度ベクトルからヤコビ行列を算出するヤコビ行列計算部１１と、関節を動かすモータのトルクを計測したトルクセンサ計測値τを取得し、トルクセンサ計測値τに基づき重み行列を算出し、当該重み行列とヤコビ行列を用いて、トルクセンサ計測値τと前記関節の角速度との積から算出されるエネルギーの時間変化率と、所定の消散パラメータと、の差分を最小化する関節の角速度を算出する角速度計算部１２と、角速度計算部１２にて算出された角速度を積分して関節に指令値として与える関節角度ベクトルを生成する積分器１３と、を有する。
【選択図】図１

Description

本発明は、ロボット制御装置に関し、例えば、関節の自由度がアームの先端の自由度よりも高い冗長ロボットを制御するロボット制御装置に関する。

近年、人とロボットの共存をキーワードに人にぶつかっても安全なロボットの実現が期待されており、関節トルクセンサや関節バネ機構を用いた柔軟関節により、ロボットが人に衝突しても柔らかく衝撃を受け流すことで、危害を軽減する技術が盛んに研究されている。こうした技術は安全なロボットを実現する上において重要であるが、衝突が生じてしまった後の対応となるため、十分に安全性を確保できるわけではない。安全性にとって最も重要なことは、衝突する前にロボットの持つ力学的なエネルギーを十分に抑えておくことである。

ロボットに何か作業を行わせる場合、特定の部位（例えば、ロボットアームの手先）の位置及び姿勢を目標に精度よく追従させることが重要になる。ロボットアームの安全性がどれだけ確保出来たとしても、ロボットアームの手先が目標位置からはずれてばかりで作業できないのでは、ロボットの本来の目的を達成することはできない。ここで、例えば、手先の位置及び姿勢に６自由度が設定されている場合、手先を目標に一致させるためにはロボットに最低６軸の自由度が必要になる。このとき、ロボットの全関節角は一意に決まるため、手先の位置及び姿勢を決めた時点でロボットの取り得る姿勢の選択肢は１つしかない。ところが、ロボットの関節が７自由度以上ある場合には冗長自由度が存在するため、手先の位置及び姿勢を目標に一致させつつ、ロボットは無数の姿勢を取ることが可能である。この無数の姿勢の中から、何らかの目的及び方法で姿勢及び関節角を決定するのが冗長逆運動学の問題である。

そこで、特許文献１には、冗長自由度を有するロボットにおける冗長逆運動学問題の解決方法の一例が開示されている。特許文献１では、冗長逆運動学とエネルギーという観点で、冗長逆運動学問題を解決する例が開示されている。より具体的には、冗長逆運動学計算で用いる疑似逆行列の重みに慣性行列を設定する事で運動エネルギーを最小化することで、手先の位置及び姿勢に設定された目標値を満たしつつ、ロボットアームの運動エネルギーを最小化する技術が開示されている。

特表２０１３−５２０３２７号公報

しかしながら、特許文献１に記載の技術では、運動エネルギーを最小化できるものの、位置エネルギーについては考慮されないという問題がある。

本発明は、上記事情に鑑みてなされたものであり、ロボットの全体エネルギーが消散するように関節角速度を決定することにより、手先の位置及び姿勢を満たしつつロボットの持つエネルギーを継続的に減少させることで、ロボットの安全性を高めることを目的とするものである。

本発明にかかるロボット制御装置の一態様は、関節の自由度がアームの先端の自由度よりも高い冗長ロボットを制御するロボット制御装置であって、前記アームの位置及び姿勢を示すアーム指令値に基づき目標速度ベクトルを算出する目標速度ベクトル計算部と、前記関節の現在の関節角度ベクトルからヤコビ行列を算出するヤコビ行列計算部と、前記関節を動かすモータのトルクを計測したトルクセンサ計測値を取得し、前記トルクセンサ計測値に基づき重み行列を算出し、当該重み行列と前記ヤコビ行列を用いて、前記トルクセンサ計測値と前記関節の角速度との積から算出されるエネルギーの時間変化率と、所定の消散パラメータと、の差分を最小化する前記関節の角速度を算出する角速度計算部と、前記角速度計算部にて算出された前記角速度を積分して前記関節に指令値として与える関節角度ベクトルを生成する積分器と、を有する。

本発明にかかるロボット制御装置では、トルクセンサ計測値と関節の角速度との積から算出されるエネルギーの時間変化率と、所定の消散パラメータと、の差分を最小化する関節の角速度を算出する。これにより、本発明にかかるロボット制御装置では、ロボットの運動エネルギーと位置エネルギーを含むロボットの全体のエネルギーを最小化する。

本発明にかかるロボット制御装置によれば、安全生の高いロボットを提供することができる。

実施の形態１にかかるロボット制御装置における処理部を示すブロック線図である。実施の形態１にかかるロボット制御装置の処理フローを示すフローチャートである。

以下、図面を参照して本発明の実施の形態について説明する。説明の明確化のため、以下の記載及び図面は、適宜、省略、及び簡略化がなされている。各図面において、同一の要素には同一の符号が付されており、必要に応じて重複説明は省略されている。

まず、以下の説明では、ロボット制御装置の一部を構成するハードウェアにおいて各演算が行われる例を用いる。しかし、ロボット制御装置の処理は、主にロボット制御装置内に設けられる演算部（例えば、マイクロコントローラ）とメモリとを用いて行うこともできる。この場合、演算部はロボットを制御する制御プログラムをメモリとの間でデータを送受信しながら実行し、計算結果をロボットのアームへの指令値として出力する。

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

実施の形態１にかかるロボット制御装置における関節角度ベクトルの算出方法について説明する。そこで、まず始めに、一般的な逆運動学問題について説明する。逆運動学問題とは、ロボットのアームの先端（以下、手先と称す）の位置及び姿勢等を表すアーム指令値ベクトルをｒ、アームの関節の角度を表す関節角度ベクトルをθとし、アーム指令値ベクトルｒと関節角度ベクトルθとが式（１）の関係を満たす場合に、式（１）の逆問題である式（２）の解を求める問題である。なお、アーム指令値ベクトルｒはｍ次元のベクトルであり、関節角度ベクトルθはｎ次元のベクトルである。また、ｍ及びｎは整数であるものとする。

すなわち、逆運動学問題とは、手先の位置及び姿勢を満たす関節角を求める問題と言い換えることができる。７軸マニピュレータの手先の位置及び姿勢を制御する場合、ｍ＝６、ｎ＝７となる。このように、ｍ＜ｎの関係が成り立つロボットは冗長マニピュレータもしくは冗長ロボットと呼ばれる。冗長ロボットではアーム指令値ベクトルｒに対する関節角度ベクトルθが一意に決まらないため、何らかの指標を用いてこれを解くことになる。また、このような問題を冗長逆運動学問題と称す。実施の形態１にかかるロボット制御装置は、この冗長逆運動学問題を扱うものであり、ｍ＜ｎの関係が成り立つロボットの制御を行う。

式（２）は非線形性が強く通常は解析的に解くことが困難である。そのため、冗長逆運動学問題では、式（２）の両辺を微分した式（３）を用いて関節角速度を求める。そして、算出した関節角速度を積分することで数値的に式（２）を解く。なお、式（３）において記号の上にあるドットは微分階数を示すものである。以下の説明においても、記号の上のドットは微分階数を示すものとする。また、文章中では、１階微分の項を「ｒ・」或いは「θ・」のように記号の後に「・」を示す形態で表現する

ここで、式（３）におけるＪは、式（４）の関係を満たすヤコビ行列である。また、ヤコビ行列Ｊは、ｍ×ｎ次元のベクトルである。

以上のように、式（２）の逆運動学問題は、式（３）を角速度について解くという問題に帰着される。ｍ＝ｎかつ、特異点等の問題が無ければ式（５）に示すようにヤコビ行列の逆行列を求めることができるため、式（２）の逆運動学問題を簡単に解くことができる。

しかしながら、ｍ＜ｎの関係を有する冗長逆運動学問題では、目的に応じて何らかの指標を用いて式（３）を一意に解く必要がある。そこで、以下で、冗長逆運動学問題において式（３）を解く方法について説明する。

ここでは、まず冗長逆運動学問題において一般的に用いられている疑似逆行列の導出について説明し、その後に実施の形態１にかかるロボット制御装置において適用される重み付き疑似逆行列を導入した式（３）の解決方法について説明する。

一般的な疑似逆行列に基づく冗長逆運動学問題は、式（６）に示す問題を出発点として考える。すなわち、一般的な疑似逆行列に基づく冗長逆運動学問題では、式（３）を満たしつつ関節角速度を最小にする解を求める。ここで、関節角速度ベクトルθ・に付されているＴは、転置行列を意味する。

一般的な疑似逆行列に基づく冗長逆運動学問題では、式（６）をラグランジュ乗数λを用いて式（７）に示すラグランジュ関数Ｌに置き換える。

そして、ラグランジュ関数の停留条件である式（８）と式（３）を用いてラグランジュ乗数を消去すると式（９）として式（３）を解くことができる。ここで、Ｊ^＃は一般的に疑似逆行列と呼ばれる行列である。

次に、式（６）を式（１０）のように重み付きの評価関数に置き換える。ここで、重み行列Ｗはｎ×ｎ次元のベクトルであり、一般的に対角行列で設定される。重み行列Ｗは、各関節の動きやすさを表すものであり、重みを大きくするほど関節が動きにくくなる。

そして、式（６）から式（７）への置き換えと同様に、式（１０）をラグランジュ乗数λを用いて式（１１）に示すラグランジュ関数Ｌに置き換える。

続いて、ラグランジュ関数の停留条件である式（１２）と式（３）を用いてラグランジュ乗数を消去すると式（１３）として式（３）を解くことができる。式（１３）は、重み付きの疑似逆行列である。

式（１３）により、アーム指令値ベクトルｒに基づき１つの関節角度ベクトルθを算出することができる。しかしながら、式（１３）では、ロボットのエネルギーを最小化することはできない。そこで、実施の形態１にかかるロボット制御装置では、式（６）或いは式（１０）に換えて、式（１４）に示す評価関数を用いる。ここで、τは関節トルクセンサによって計測されたトルクセンサ計測値であり、ｎ次元のベクトルである。αは、エネルギーの消散量を決める所定の消散パラメータである。Ｒは、ｎ×ｎ次元のベクトルであり、解を安定させるための微少な対角行列である。また、Ｒは、Ｒ＝εＩ、０＜ε＜＜１となるものである。

ここで、式（１４）の意味ついて説明する。式（１４）の左辺第１項中のτが含まれる項は、各関節のトルクセンサ計測値と各関節の角速度との積を足し合わせたものであり、式（１５）の関係を有する。つまり、式（１４）の左辺第１項中のτが含まれる項は、アームの運動エネルギーと位置エネルギーを含むエネルギーの時間変化量（例えば、エネルギーの時間微分）を意味している。

そして、式（１４）の左辺第１項は、式（１６）に示す部分を最小にすることを意味する。つまり、式（１４）の左辺第１項は、エネルギーの時間変化率をできる限り消散パラメータαに一致させることを目的としている。言い換えれば、エネルギーが、設定された消散パラメータαに従って減少していくことを意味している。

ここで、消散パラメータαについて説明する。消散パラメータαは、エネルギーの減衰量を決定するが、これは実機のスペックやロボットに行わせるタスクに応じて決定すればよい。消散パラメータαを大きくするとエネルギーは早く減衰するが、その分、関節には大きな各加速度やトルクが発生する可能性がある。逆に、消散パラメータαを小さくするとエネルギーの減衰が穏やかになるため、速い動作を伴う作業を行わせる場合にはエネルギーの減衰が十分に進まない可能性がある。数行先までのアーム指令動作が分かっているような場合には、制御周期毎に消散パラメータαをリアルタイムに変更しながら逆運動学を計算することも可能である。

続いて、式（１４）を用いた関節角度ベクトルθの算出について説明する。式（１５）に対しても、式（６）から式（７）への置き換えと同様のラグランジュ関数への置き換えを行う。そこで、式（１５）をラグランジュ関数Ｌに置き換えた式を式（１７）に示す。

続いて、ラグランジュ関数の停留条件と式（３）を用いてラグランジュ乗数を消去する。このラグランジュ乗数λの計算手順を式（１８）から式（２１）に示す。まず、式（１８）では、式（１７）の両辺を角速度ベクトルθ・で微分する。

続いて、式（１８）の右辺第１項の括弧内の項を重み行列Ｗとして、式を整理して式（１９）とする。式（１９）では、ラグランジュ関数の停留条件が適用されている。

続いて、式（１９）の両辺にＪＷ^−１を乗算して式（２０）を導出する。また、式（１９）から式（２０）への変形では、式（３）の関係を適用する。

続いて、式（２０）で導出したλを式（１９）の左辺第３項のλに代入して式（２１）を導出する。

続いて、式（２１）を整理して式（２２）を導出する。ここで、Ｊ^＃は一般的に疑似逆行列と呼ばれる行列である。また、式（２２）に示すように重み行列Ｗは、トルクセンサ計測値τから生成した行列と、補正行列Ｒから構成される。トルクセンサ計測値は０になり得ることもあるため、その際に重み行列Ｗの逆行列が存在するように、正則性を保つ役割を補正行列Ｒが担っていることが分かる。従って、補正行列Ｒは数値的な安定性を保証できる範囲内でできる限り小さい対角行列に設定することが望ましい。

実施の形態１にかかるロボット制御装置では、式（１４）の評価関数を用いて関節の角速度ベクトルを算出し、当該角速度ベクトルを積分することで関節の関節角度ベクトルθを得る。このように、式（１４）の評価関数を用いて関節角度ベクトルθを算出することで、アームの手先をアーム指令値により示される目標に一致させながら、ロボットの運動エネルギーと位置エネルギーとを含むエネルギーを漸近的に最小にすることができる。

また、手先の位置及び姿勢を拘束条件としてエネルギーを最小化する解を非線形の最適化問題として求めることは計算上は可能であるが、ロボットの動作のサイクル毎に行う逆運動学の計算中に非線形の最適化問題を解くことは演算部における計算負荷が高く現実的ではない問題もある。しかしながら、実施の形態１にかかるロボット制御装置では、非線形問題である式（２）の解を、式（２）を微分した式（３）を用いて算出した関節角速度を積分することで数値的に解く。これにより、実施の形態１にかかるロボット制御装置では、演算コストを削減して実装を容易にすることができる。

上述したように、実施の形態１にかかるロボット制御装置においては、式（２２）を用いて算出した角速度ベクトルθ・を積分して関節角度ベクトルθを求める。そのため、上述した関節角度ベクトルθの算出を継続して行うと出力される関節角度ベクトルθに積分誤差が蓄積する問題が生じる。そこで、実施の形態１にかかるロボット制御装置においては、上述した逆運動学問題の計算方法をクローズループ型の逆運動学問題に適用する。クローズループ型逆運動学問題では、手先の位置を順運動学で計算し、計算された手先位置をフィードバックすることで積分誤差の蓄積を防止する。

そこで、まず、図１に実施の形態１にかかるロボット制御装置における処理部を示すブロック線図を示す。なお、図１に示したブロック線図では、アーム指令値ベクトルｒ_ｄに基づく関節角度ベクトルθの算出手順に関連するブロックのみを示した。

図１に示すように、実施の形態１にかかるロボット制御装置は、目標速度ベクトル計算部１０、ヤコビ行列計算部１１、角速度計算部１２、積分器１３、順運動学計算部１４を有する。

目標速度ベクトル計算部１０は、アームの位置及び姿勢を示すアーム指令値ｒ_ｄに基づき目標速度ベクトルを算出する。ヤコビ行列計算部１１は、関節の現在の関節角度ベクトルθからヤコビ行列Ｊを算出する。角速度計算部１２は、関節を動かすモータのトルクを計測したトルクセンサ計測値τを取得し、トルクセンサ計測値τに基づき重み行列Ｗを算出し、重み行列Ｗとヤコビ行列Ｊを用いて、トルクセンサ計測値τと関節の角速度θ・との積から算出されるエネルギーの時間変化率と、所定の消散パラメータと、の差分を最小化する関節の角速度（例えば、角速度ベクトルθ・）を算出する。積分器１３は、角速度計算部１２にて算出された角速度を積分して関節に指令値として与える関節角度ベクトルθを生成する。順運動学計算部１４は、順運動学問題の計算を行うことで関節の現在の関節角度ベクトルθから手先の位置を示すアーム指令値ｒを計算する。

また、目標速度ベクトル計算部１０は、微分器２１、加算器２２、２４、係数行列乗算器２３を有する。微分器２１は、アーム指令値ｒ_ｄを微分して微分アーム指令値ｒ_ｄ・を出力する。加算器２２は、アーム指令値ｒ_ｄから順運動学計算部１４が出力したアーム指令値ｒとの差分を出力する。係数行列乗算器２３は、予め設定した係数Ｋｐを加算器２２が出力した差分値に乗算した値を出力する。加算器２４は、係数行列乗算器２３が出力した値と微分アーム指令値ｒ_ｄ・との和を変数ｐ・として出力する。

ここで、目標速度ベクトル計算部１０において、係数Ｋｐはフィードバックゲインを設定する値であり、正の値であるとする。また、係数Ｋｐを０とした場合が上記式（２２）をそのまま適用できる状態である。

クローズループ型逆運動学問題は、アーム指令値ｒｄとアーム指令値ｒとの誤差ｅ（例えば、式（２３））をゼロにするθを求める問題である。そのため、クローズループ型逆運動学問題を計算する実施の形態１にかかるロボット制御装置では、式（２２）中のｒ・に変えて変数ｐ・を用いる。変数ｐ・は、式（２４）によって表される。

続いて、式（２２）のｒ・を式（２４）の変数ｐ・に置き換えた式を式（２５）に示す。この式（２５）によって算出された角速度ベクトルθ・を積分することで関節角度ベクトルθが算出される。

ここで、式（３）、（２３）、（２４）、（２５）も用いれば式（２６）に示す関係を導き出すことができる。

つまり、クローズループ型逆運動学問題では、誤差ｅは０に漸近収束することがわかる。すなわち、実施の形態１にかかるロボット制御装置では、エネルギーを減衰させるように関節の角速度を算出するが、それはあくまで零空間内においてのみであり、手先の追従性は正しく満たされる。言い換えれば、実施の形態１にかかるロボット制御装置により制御されるロボットは、手先の追従に必要なエネルギーは発生するが、それ以外のエネルギーをできる限り減少させるように動作する。

最後に、実施の形態１にかかるロボット制御装置の動作フローを示すフローチャートを図２に示す。

図２に示すように、実施の形態１にかかるロボット制御装置は、まず、順運動学の計算を順運動学計算部１４にて行う（ステップＳ１）。続いて、実施の形態１にかかるロボット制御装置は、ヤコビ行列の計算をヤコビ行列計算部１１にて行う（ステップＳ２）。続いて、実施の形態１にかかるロボット制御装置は、目標速度ベクトルの計算を目標速度ベクトル計算部１０にて行う（ステップＳ３）。続いて、実施の形態１にかかるロボット制御装置は、角速度計算部１２にてトルクセンサ計測値τを受信する（ステップＳ４）。その後、角速度計算部１２において、重み行列Ｗの計算を行い（ステップＳ５）、当該重み行列Ｗとヤコビ行列Ｊを用いて重み付き疑似逆行列の計算を行う（ステップＳ６）。そして、角速度計算部１２は、ステップＳ５、Ｓ６にて算出した各値を用いて間接の角速度ベクトルθ・を計算する（ステップＳ７）。続いて、実施の形態１にかかるロボット制御装置は、角速度ベクトルθ・を積分して関節角度ベクトルθを算出する（ステップＳ８）。

上記説明より、実施の形態１にかかるロボット制御装置では、式（１４）から導出した式（２２）を用いて関節の角速度ベクトルθ・を算出することで、アームの手先をアーム指令値により示される目標に一致させながら、ロボットの運動エネルギーと位置エネルギーとを含むエネルギーを最小にすることができる。そして、実施の形態１にかかるロボット制御装置では、ロボットのエネルギーを最小化することで安全性の高いロボットを実現することができる。

また、実施の形態１にかかるロボット制御装置では、クローズループ型逆運動学問題に式（２２）の演算を適用することで、角速度ベクトルθ・の積分において生じる積分誤差の発生を防止することができる。

上記説明は、本発明者によってなされた発明を実施の形態に基づき具体的に説明したが、本発明は既に述べた実施の形態に限定されるものではなく、その要旨を逸脱しない範囲において種々の変更が可能であることはいうまでもない。

１０目標速度ベクトル計算部
１１ヤコビ行列計算部
１２角速度計算部
１３積分器
１４順運動学計算部
２１微分器
２２加算器
２３係数行列乗算器
２４加算器

Claims

関節の自由度がアームの先端の自由度よりも高い冗長ロボットを制御するロボット制御装置であって、
前記アームの位置及び姿勢を示すアーム指令値に基づき目標速度ベクトルを算出する目標速度ベクトル計算部と、
前記関節の現在の関節角度ベクトルからヤコビ行列を算出するヤコビ行列計算部と、
前記関節を動かすモータのトルクを計測したトルクセンサ計測値を取得し、前記トルクセンサ計測値に基づき重み行列を算出し、当該重み行列と前記ヤコビ行列を用いて、前記トルクセンサ計測値と前記関節の角速度との積から算出されるエネルギーの時間変化率と、所定の消散パラメータと、の差分を最小化する前記関節の角速度を算出する角速度計算部と、
前記角速度計算部にて算出された前記角速度を積分して前記関節に指令値として与える関節角度ベクトルを生成する積分器と、
を有するロボット制御装置。