(57)【要約】【課題】  モデルパラメータ誤差や外乱に対する制御性を向上させた柔軟受動関節を有するロボットアームの制御装置を提供する。【解決手段】  弾性要素・減衰要素系を有する柔軟関節を組み合わせた多関節ロボットアーム１００の制御装置２００は、各関節のモータ、リンク回転軸の回転状態θｉ、θｍｉを検出する回転状態検出手段２０５と、目標リンク角θｄｉを求める目標設定部２０１と、目標角度軌道からリンク系の逆動力学計算によりリンク回転軸の目標駆動トルクτｉを計算し、弾性要素・減衰要素系の運動方程式からθｉ、θｍｉを基に弾性要素・減衰要素系の発生トルクτｆを推定し、必要なモータ駆動トルクτｍを求めるフィードフォワード制御部２０２と、θｄｉとθｉの差からモータ駆動トルクτｃを求めるフィードバック制御部２０３と、τｍとτｃを加算してモータ指令トルクτｍｉとするトルク指令部２０４と、を備えている。【選択図】  図３
2005349555.tif 000002
【特許請求の範囲】
【請求項１】
モータ出力軸とリンク回転軸との間に弾性要素・減衰要素系を配置した柔軟関節要素を多自由度直列に配置した柔軟関節ロボットアームの制御装置において、
各関節の前記モータ出力軸と前記リンク回転軸の回転状態を検出する回転状態検出手段と、
各関節の目標角度軌道を求める目標設定部と、
前記目標角度軌道からリンク系の逆動力学計算によりリンク回転軸位置での目標駆動トルクを計算し、前記弾性要素・減衰要素系の運動方程式から前記回転状態検出手段の出力を基に前記弾性要素・減衰要素系の発生トルクを推定して、目標駆動トルクを実現するために必要なモータ駆動トルクを求めるフィードフォワード制御部と、
目標リンク角度と実リンク角度の差からモータへ付与する駆動トルクを求めるフィードバック制御部と、
前記フィードフォワード制御部の目標トルクと前記フィードバック制御部の目標トルクを加算してモータへの指令トルクとするトルク指令部と、
を備えていることを特徴とする柔軟関節ロボットアームの制御装置。
【請求項２】
前記フィードフォワード制御部は、数値微分をカットオフ周波数以上のゲインを抑えて位相進みを小さくした不完全微分フィルタを用いて行うことを特徴とする請求項１記載の柔軟関節ロボットアームの制御装置。
【請求項３】
前記フィードバック制御部は、前記弾性要素・減衰要素系の共振周波数近辺の周波数領域での位相を進ませ、かつ、定常領域でのゲインを比較的低く設定する位相進み遅れフィルタをさらに備えていることを特徴とする請求項１記載の柔軟関節ロボットアームの制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボットアームの制御装置に関し、特に、外乱に対して高周波振動を抑制するために柔軟関節を有するロボットアームの制御装置に関する。
【背景技術】
【０００２】
モータによって各関節軸を作動させている多軸式のロボットアームにおいては外乱力の影響を小さくするために、電動機とロボットアームとの間にバネ要素を配置したロボットアームが知られている。こうしたロボットアームにおいて軸干渉を低減し、指令への追従性を向上させる制御方法として特許文献１〜３に記載された技術が知られている。
【０００３】
特許文献１の技術は、多軸から受ける干渉力を干渉トルクとして求め、これを相殺するモデル補正トルクを算出してフィードフォワード指令値としてフィードバック制御器へと入力している。
【０００４】
特許文献２の技術は、負荷位置、モータ位置を高次微分可能な関数として表し、この高次微分可能な関数からモータの位置、速度、トルク指令を算出して、フィードフォワード指令として与えるものである。
【０００５】
特許文献３の技術は、ロボットアームの衝突検出を行うものであって、モータの位置、速度、加速度情報から衝突がないと仮定した場合の各モータが発生すべきトルクを逆動力学演算によって求め、実際に発生したトルクとの差が大きい場合に衝突発生を判定するものである。
【特許文献１】特開平１０−３２９０６３号公報
【特許文献２】特開２００２−９１５７０号公報
【特許文献３】特開２００２−２８３２７６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００６】
例えば、特許文献１の技術では、モータ及びバネ機構の運動方程式、および２リンク系からの干渉トルクを考慮したモデルを用いて、モデルにおけるモータ位置、モータ速度、バネねじり角、バネねじり角速度を計算し、実際のモータ位置、モータ速度と別に推定されるバネねじり角、バネねじり角速度との差のフィードバック制御の形式で計算されている。しかし、この制御のためには、モデルのモータ位置、モータ速度を陽に計算する必要があるが、モデルと実機でパラメータ値が異なる場合、全ての指令値にその誤差が蓄積され、修正する手段がないため、安定した制御を行うことが難しい。
【０００７】
さらに、３軸以上のリンク機構に拡張する場合に、全体リンク系の運動方程式を考慮した演算を行う必要があると考えられるが、その手法が明らかでなく、モデルが複雑なものとなり、前述した誤差がさらに増幅するおそれがある。
【０００８】
また、特許文献２、特許文献３の技術においても、同様にモデルの内部状態量と実際の状態とに差が生じた場合に、これを修正する手段がない点は共通し、同様の問題が発生する。
【０００９】
そこで本発明は、モデルパラメータ誤差や外乱に対する制御性を向上させた柔軟受動関節を有するロボットアームの制御装置を提供することを課題とする。
【課題を解決するための手段】
【００１０】
上記課題を解決するため、本発明にかかる柔軟関節ロボットアームの制御装置は、モータ出力軸とリンク回転軸との間に弾性要素・減衰要素系を配置した柔軟関節要素を多自由度直列に配置した柔軟関節ロボットアームの制御装置において、各関節のモータ出力軸とリンク回転軸の回転状態を検出する回転状態検出手段と、各関節の目標角度軌道を求める目標設定部と、目標角度軌道からリンク系の逆動力学計算によりリンク回転軸位置での目標駆動トルクを計算し、回転状態検出手段の出力を基に弾性要素・減衰要素系の発生トルクを推定して、弾性要素・減衰要素系の運動方程式を解くことにより目標駆動トルクを実現するために必要なモータ駆動トルクを求めるフィードフォワード制御部と、目標リンク角度と実リンク角度の差からモータへ付与する駆動トルクを求めるフィードバック制御部と、フィードフォワード制御部の目標トルクとフィードバック制御部の目標トルクを加算してモータへの指令トルクとするトルク指令部と、を備えていることを特徴とする。
【００１１】
アームに対する所望の運動指令は、各関節の目標角度軌道の組み合わせとして設定される。つまり、関節の角度、角速度、角加速度が目標値となる。この目標値を実現するため、必要となるトルクは、ニュートン法、オイラー法を用いて各リンクの重心における軌道計算と、力・モーメントの釣り合いから関節（リンク）に加えられるトルク（リンク要求トルク）を計算する逆動力学計算を解くことで求まる。一方、リンク要求トルクを実現するために必要なモータ駆動トルクは、弾性要素・減衰要素系の運動方程式を解くことにより求められるが、ここで、弾性要素・減衰要素系で発生するトルクを推定し、運動方程式を解くことで、弾性要素・減衰要素系における部材の位置等を正確に求めることなくモータ駆動トルクを求めることを可能とした。また、外乱やモデルパラメータに基づく誤差を修正するためにフィードバック制御部を備える。
【００１２】
このフィードフォワード制御部は、数値微分をカットオフ周波数以上のゲインを抑えて位相進みを小さくした不完全微分フィルタを用いて行うとよい。
【００１３】
測定値から直接数値微分を行った場合、高周波ノイズが発生する可能性が高いが、このようなローパス特性を有する不完全微分フィルタによれば、高周波ノイズの発生が抑制される。
【００１４】
フィードバック制御部は、弾性要素・減衰要素系の共振周波数近辺の周波数領域での位相を進ませ、かつ、定常領域でのゲインを比較的低く設定する位相進み遅れフィルタをさらに備えているとよい。
【００１５】
共振周波数近辺の周波数領域で位相を進ませることで、ゲインが上がり、発振に対するゲイン余裕自体は小さくなるが、位相を進ませたことで位相余裕の改善効果が大きくなる。さらに、低周波領域ではゲインを高くして、目標追従性の向上を図る。
【発明の効果】
【００１６】
本発明によれば、モデルの内部状態量を陽に計算することなく、リンク内で発生するトルクを推定して必要なモータ駆動トルクをフィードフォワード制御を行うため、所望のリンク系運動を精度良く求めることが可能となる。さらに、フィードバック制御を加えることで、モデルパラメータ誤差や外乱によるリンク運動軌跡の所望軌跡との偏差を修正し、制御性を向上させる。
【００１７】
フィードフォワード制御部における微分計算にローパス特性を有する不完全微分フィルタを用いることで、数値微分における高周波ノイズの発生を抑制して、高周波振動の発生を抑制することができる。
【００１８】
さらに、フィードバック制御部では、位相進み遅れフィルタを用いることで、目標追従性の向上と共振周波数領域での振動抑制を両立させる。
【発明を実施するための最良の形態】
【００１９】
以下、添付図面を参照して本発明の好適な実施の形態について詳細に説明する。説明の理解を容易にするため、各図面において同一の構成要素に対しては可能な限り同一の参照番号を附し、重複する説明は省略する。
【００２０】
図１は、本発明に係る柔軟関節ロボットアームの制御装置により制御されるロボットアームの概略図である。このロボットアーム１００は、多関節型の多軸式で多自由度を有するロボットアーム１００であり、左腕部の構成を示している。躯体４には、上腕部３と下腕部２により、ハンド部１が接続されている。上腕部３、下腕部２の両者には腕の延長方向に対して３軸方向に駆動を行う関節部が搭載されている。図２は各関節の構成を模式的に示す図である。
【００２１】
各柔軟関節は、躯体側のリンク本体に固定されているモータ１０の出力軸と、手先側のリンク本体の入力軸との間にプーリー１１、１２とギヤボックス１３、ダンパ１４、ばね１５が接続されて構成されている。プーリー１１、１２とギヤボックス１３はモータ１０の出力を減速する減速要素であり、ダンパ１４、ばね１５は、外部からの衝突等の突発外乱の衝撃を吸収するための減衰要素、弾性要素としてそれぞれ機能する。ここで、ダンパ１４、ばね１５は並列に配置されて減衰要素・弾性要素系を構成し、これと各減速要素１１〜１３とは直列に接続されている。
【００２２】
図３は、このロボットアーム１００の制御装置２００のブロック構成図である。この制御装置２００は、目標設定部２０１と、フィードフォワード制御部２０２と、フィードバック制御部２０３と、トルク指令部２０４と、ロボットアーム１００内に配置される回転状態検出手段２０５から構成される。
【００２３】
目標設定部２０１は、例えば、把持対象物の位置情報等の入力信号に基づいて所望の運動を実現するために必要とされる各リンク系（以下、ｉはリンク系および関節の番号を表している。）の入力部における目標リンク角度θｄｉを設定する（目標角度軌道の設定）。回転状態検出手段２０５は、ロボットアーム１００の関節中の各モータ１０ｉの回転角度θｍｉと、リンク入力軸１６ｉの実リンク角度θｉとを測定し、出力している。フィードフォワード制御部２０２には、目標設定部２０１で設定されたθｄｉと回転状態検出手段２０５で検出されたθｍｉ、θｉが入力され、フィードフォワード制御トルクτｍを出力する。一方、フィードバック制御部２０３には、目標設定部２０１で設定されたθｄｉと回転状態検出手段２０５で検出されたθｉが入力され、フィードバック制御トルクτｃを出力する。トルク指令部２０４は、フィードフォワード制御トルクτｍとフィードバック制御トルクτｃを加算してモータ１０ｉの駆動トルクτｍｉを求め、ロボットアーム１００の各モータ１０ｉに求めた駆動トルクτｍｉを実現するよう指示する。これにより、ロボットアーム１００に所望の運動を実現させる。
【００２４】
フィードフォワード制御部２０２とフィードバック制御部２０３における具体的な制御内容を以下に説明する。
【００２５】
まず、フィードフォワード制御部２０２における制御を説明する。図４は、フィードフォワード制御部２０２における制御の処理フローチャートであり、図５は、本制御において用いるリンク座標系の一例を示す図である。ここでは、図５に示されるように７自由度のリンク系を考える。
【００２６】
最初にニューイン・オイラー法を用いた逆動力学計算によって設定された目標軌道に応じて必要となる力とトルクの計算を行う。まず、初期値として目標の角度、加速度、角加速度を設定する（ステップＳ１）。ここでは、初期値として、
【００２７】
【数１】
2005349555.tif 000003
とする。ここで、ω０は、座標系０の角度であり、その微分値は角速度である。また、ｐ０は座標系０に働く外力であり、ここでは重力加速度ｇに対応する。ｆ８は、ハンド部１における外力であり、ｎ８はハンド部１に外部から加わるトルクである。
【００２８】
次に、第１関節から第７関節まで順に関節の角度、角速度、外力、リンクの中心点に働く加速度をそれぞれ以下の式（１．１）〜（１．４）により求める（正順計算）。
【００２９】
【数２】
2005349555.tif 000004
ここで、各ａｉ、ｌｉ、ｑｉ、Ａｉはリンク間の座標変換に用いられる座標変換のための行列であり、以下のように設定される。
【００３０】
【数３】
2005349555.tif 000005
【００３１】
【数４】
2005349555.tif 000006
【００３２】
【数５】
2005349555.tif 000007
【００３３】
【数６】
2005349555.tif 000008
【００３４】
【数７】
2005349555.tif 000009
【００３５】
【数８】
2005349555.tif 000010
【００３６】
【数９】
2005349555.tif 000011
【００３７】
【数１０】
2005349555.tif 000012
【００３８】
【数１１】
2005349555.tif 000013
また、回転軸はすべてｚ軸なのでｑｉはすべての関節について
【００３９】
【数１２】
2005349555.tif 000014
となる。
【００４０】
次に、第７関節から第１関節まで順に以下の（２．１）式〜（２．３）式により各関節に加わる外力ｆｉ、外から加わるトルクｎｉとリンク入力トルクτｉをそれぞれ計算する（ステップＳ３、逆順計算）。
【００４１】
【数１３】
2005349555.tif 000015
次に、モータ出力軸トルクの計算を行う（ステップＳ４）。ここでは、図２に示されるようなモデルを用いてリンク系の運動方程式を以下のように表す。
【００４２】
【数１４】
2005349555.tif 000016
【００４３】
【数１５】
2005349555.tif 000017
【００４４】
【数１６】
2005349555.tif 000018
【００４５】
【数１７】
2005349555.tif 000019
ここで、ωｍ、θｍ、τｍ、Ｉｍはそれぞれモータ１０の回転速度、回転角度、駆動トルク、慣性モーメントをそれぞれ表し、ωｉ、θｉ、Ｉｉは、リンク１６の回転速度、回転角度をはモータ１０の回転速度を表している。また、τｆは、関節内の回転摩擦トルク（リンク回転軸位置換算。粘弾性トルクは除く。）を、Ｉｐは、プーリー１１、１２とギヤボックス１３のギヤヘッドの慣性モーメントを、Ｉｈは、ギヤボックス１３のハーモニックドライブの慣性モーメントを、Ｉｌは、ダンパ１４とばね１５からなる減衰要素・弾性要素系の慣性モーメントをそれぞれ表し、Ｋは、ばね１５の弾性係数であり、Ｄは、ダンパ１４の粘性摩擦係数である。また、Ｇｐはプーリー１１、１２とギヤボックス１３のギヤヘッドに至る減速比であり、Ｇｈはギヤボックス１３のハーモニックドライブの減速比である。このため、Ｇは最終減速比を、ＩＭは、関節内の駆動・減速系の慣性モーメントに相当する。（ここでは、関節番号を示す添字を省略している。）
（３）（４）式においては、弾性要素・減衰要素系の発生トルク
【００４６】
【数１８】
2005349555.tif 000020
が共通するから、両式からこれを消去すると、（２）式で求めたリンク回転軸トルクτｉを実現するためのモータ駆動トルクは以下の（５）式により求めることができる。
【００４７】
【数１９】
2005349555.tif 000021
ここで、（５）式中のτｍ上に付している波線は、推定値であることを表す。（５）式から明らかなように、減衰要素・弾性要素系において発生する粘弾性力や弾性力、摩擦力等により発生するトルクが陽に明らかでなくても、その合力は、θｍ、θｉそれぞれの２階微分値、すなわち、モータ１０の角加速度とリンクの角加速度から推定できる。ここで、θｍ、θｉは、回転状態検出手段２０５によって測定しているため、これを２階微分すれば求めることができる。
【００４８】
しかし、単純な数値微分（＝差分）は高周波ノイズを発生させるおそれがある。そこで、式（６）に示す不完全微分フィルタ（カットオフ周波数ｆｃ）を２回かけることで２階微分値を求める。
【００４９】
【数２０】
2005349555.tif 000022
図６は、この不完全微分フィルタの周波数応答を、単純微分sと比較したグラフである。カットオフ周波数ｆｃ以上でゲインが頭打ちとなると同時に位相進みが小さくなることが分かる。なお、カットオフ周波数は、２階微分値の推定値が振動的に変化しないよう、目標運動軌道の周波数帯域を考慮して低めに設定するとよい。
【００５０】
θｍ、θｉそれぞれの２階微分値の推定値をそれぞれ上に波線を表記して２階微分値自体と区別して表すものとすると、その伝達関数表現は（６）式の不完全微分フィルタを用いて、
【００５１】
【数２１】
2005349555.tif 000023
で表される。このとき、（５）式は以下のように変形され、フィードフォワード制御トルクを求めることができる。
【００５２】
【数２２】
2005349555.tif 000024
上述したようにこれは、弾性要素・減衰要素系における発生トルクを推定して必要なモータ駆動トルクを求めることに等しい。
【００５３】
実際の計算においては、（６）式を離散化した漸化式表現とし、計算するとプログラムとしての実装が容易になる。例えば、制御周期を３msとし、（６）式の離散化式が次式で表せるとする。
【００５４】
【数２３】
2005349555.tif 000025
ここで、モータの回転角度θｍ（ｚ）から、その微分値、２階微分値の推定値はそれぞれ不完全微分フィルタを１回または２回乗じることで得られるから、それぞれ
【００５５】
【数２４】
2005349555.tif 000026
と表されるので、以下の漸化式で各変数を計算できる。
【００５６】
【数２５】
2005349555.tif 000027
リンクの回転角についても同様に、
【００５７】
【数２６】
2005349555.tif 000028
で計算することができる。以上で得られたθｍ、θｉの２階微分値の推定値を式（８）に代入してフィードフォワード制御トルクτｍの推定値を求める。
【００５８】
一方、フィードバック制御部２０３においては、目標リンク角度θｄｉと実リンク角度θｉとの差（θｄｉ−θｉ）に応じたフィードバック制御トルクを求める。
【００５９】
本実施形態では、位相余裕を改善し、振動しにくくするため、位相進み遅れフィルタによる位相進み遅れ補償を行っている。機械系の共振により１Hz〜10Hz近辺の振動が発生しやすいことが確認されていることから、この帯域で位相を進ませるような式（９）に示される特性を有するフィルタを用いてフィードバックを行うこととする。
【００６０】
【数２７】
2005349555.tif 000029
このフィルタの周波数特性の一例を図７に示す。図８は、通常のPDフィードバックの周波数特性を示している。一般に位相を進ませると、ゲインが上がり、発振に対するゲイン余裕は小さくなる。本実施形態では、共振周波数領域では位相を進ませ、低周波領域ではゲインをあげるとともに、高周波領域では位相を遅らせてゲインを下げているため、振動の発生を抑制して、目標追従性と振動抑制を両立することができる。
【００６１】
フィードバック制御トルクτｃは、（９）式のフィルタを使って、
【００６２】
【数２８】
2005349555.tif 000030
で求めることができる。さらに、積分フィードバックを追加する場合、フィードバック制御トルクτｃは、
【００６３】
【数２９】
2005349555.tif 000031
となる。
【００６４】
フィードバック制御トルクについてもプログラムとして実装する場合は（９）式のフィルタを離散化するとよい。不完全微分フィルタの場合と同様に制御周期を3msecとして（９）式を離散化すると、例えば、
【００６５】
【数３０】
2005349555.tif 000032
で表せる。フィードバック制御トルクは、このフィルタを使って、
【００６６】
【数３１】
2005349555.tif 000033
と表されるので、これを漸化式表現して以下の式により求めることができる。
【００６７】
【数３２】
2005349555.tif 000034
一方、積分フィードバック項を追加する場合には、上の式は、
【００６８】
【数３３】
2005349555.tif 000035
として表せる。
【００６９】
トルク指令部２０４は、フィードフォワード制御トルクτｍとフィードバック制御トルクτｃを加算してモータ１０ｉの駆動トルクτｍｉを求める。
【００７０】
このようにして求めたフィードフォワード制御トルクτｍとフィードバック制御トルクτｃにより制御を行うことで、弾性要素・減衰要素による振動を抑制しつつ、所望のリンク運動軌跡への追従性を向上させたロボットアームの制御装置が得られる。
【００７１】
ここで、本発明の効果を確認するためシミュレーションを行ったので、その結果について報告する。図９はこのシミュレーション結果を示すものである。ここでは、３リンク系とし、ばねのストローク可能角度は１８°とした。図９（ａ）は各関節の目標角度、リンク角度、モータ角度の時間変化を示しており、図９（ｂ）は、フィードフォワードトルク（ＦＦ）、フィードバックトルク（ＦＢ）とその和（ＦＦ＋ＦＢ）を示している。いずれのリンクも目標に良く追従していることが確認された。なお、シミュレーション開始後２．５秒の段階で制御トルクが急激に変化しているが、これはバネがストロークリミッタにあたって衝撃的な力が発生したことによる。このような場合でも本発明によれば、振動を起こさず、目標に良く追従することが確認された。
【００７２】
本発明によれば、リンク系全体を考慮してフィードフォワード制御部を設計することにより、ここで述べた３リンク系に限らず、上述した７リンク系等の多リンク系に対しても適用が可能である。したがって、多自由度を有するロボットアームの制御に好適である。
【図面の簡単な説明】
【００７３】
【図１】本発明に係る柔軟関節ロボットアームの制御装置により制御されるロボットアームの概略図である。
【図２】各関節の構成を模式的に示す図である。
【図３】ロボットアーム１００の制御装置２００（本発明に係る柔軟関節ロボットアームの制御装置）のブロック構成図である。
【図４】フィードフォワード制御部２０２における制御処理のフローチャートである。
【図５】本制御において用いるリンク座標系の一例を示す図である。
【図６】本発明で用いられる不完全微分フィルタの周波数応答を、単純微分sと比較したグラフである。
【図７】本発明で用いられる位相進み遅れフィルタの周波数特性の一例を示すグラフである。
【図８】通常のPDフィードバックの周波数特性を示すグラフである。
【図９】本発明の効果を確認するためシミュレーション結果を示すグラフである。
【符号の説明】
【００７４】
１…ハンド部、２…下腕部、３…上腕部、４…躯体、１０…モータ、１１、１２…プーリー、１３…ギヤボックス、１４…ダンパ、１６…リンク、１００…ロボットアーム、２００…制御装置、２０１…目標設定部、２０２…フィードフォワード制御部、２０３…フィードバック制御部、２０４…トルク指令部、２０５…回転状態検出手段。
【図１】
2005349555.tif 000036
【図２】
2005349555.tif 000037
【図３】
2005349555.tif 000038
【図４】
2005349555.tif 000039
【図５】
2005349555.tif 000040
【図６】
2005349555.tif 000041
【図７】
2005349555.tif 000042
【図８】
2005349555.tif 000043
【図９】
2005349555.tif 000044