JP4148189B2 - 柔軟関節ロボットアームの制御装置 - Google Patents

Description

本発明はロボットアームの制御装置に関し、特に、外乱に対して高周波振動を抑制するために柔軟関節を有するロボットアームの制御装置に関する。

モータによって各関節軸を作動させている多軸式のロボットアームにおいては外乱力の影響を小さくするために、電動機とロボットアームとの間にバネ要素を配置したロボットアームが知られている。こうしたロボットアームにおいて軸干渉を低減し、指令への追従性を向上させる制御方法として特許文献１〜３に記載された技術が知られている。

特許文献１の技術は、多軸から受ける干渉力を干渉トルクとして求め、これを相殺するモデル補正トルクを算出してフィードフォワード指令値としてフィードバック制御器へと入力している。

特許文献２の技術は、負荷位置、モータ位置を高次微分可能な関数として表し、この高次微分可能な関数からモータの位置、速度、トルク指令を算出して、フィードフォワード指令として与えるものである。

特許文献３の技術は、ロボットアームの衝突検出を行うものであって、モータの位置、速度、加速度情報から衝突がないと仮定した場合の各モータが発生すべきトルクを逆動力学演算によって求め、実際に発生したトルクとの差が大きい場合に衝突発生を判定するものである。
特開平１０−３２９０６３号公報 特開２００２−９１５７０号公報 特開２００２−２８３２７６号公報

例えば、特許文献１の技術では、モータ及びバネ機構の運動方程式、および２リンク系からの干渉トルクを考慮したモデルを用いて、モデルにおけるモータ位置、モータ速度、バネねじり角、バネねじり角速度を計算し、実際のモータ位置、モータ速度と別に推定されるバネねじり角、バネねじり角速度との差のフィードバック制御の形式で計算されている。しかし、この制御のためには、モデルのモータ位置、モータ速度を陽に計算する必要があるが、モデルと実機でパラメータ値が異なる場合、全ての指令値にその誤差が蓄積され、修正する手段がないため、安定した制御を行うことが難しい。

さらに、３軸以上のリンク機構に拡張する場合に、全体リンク系の運動方程式を考慮した演算を行う必要があると考えられるが、その手法が明らかでなく、モデルが複雑なものとなり、前述した誤差がさらに増幅するおそれがある。

また、特許文献２、特許文献３の技術においても、同様にモデルの内部状態量と実際の状態とに差が生じた場合に、これを修正する手段がない点は共通し、同様の問題が発生する。

そこで本発明は、モデルパラメータ誤差や外乱に対する制御性を向上させた柔軟受動関節を有するロボットアームの制御装置を提供することを課題とする。

上記課題を解決するため、本発明にかかる柔軟関節ロボットアームの制御装置は、モータ出力軸とリンク回転軸との間に弾性要素・減衰要素系を配置した柔軟関節要素を多自由度直列に配置した柔軟関節ロボットアームの制御装置において、各関節のモータ出力軸とリンク回転軸の回転状態を検出する回転状態検出手段と、各関節の目標角度軌道を求める目標設定部と、目標角度軌道からリンク系の逆動力学計算によりリンク回転軸位置での目標駆動トルクを計算し、回転状態検出手段の出力を基に弾性要素・減衰要素系の発生トルクを推定して、弾性要素・減衰要素系の運動方程式を解くことにより目標駆動トルクを実現するために必要なモータ駆動トルクを求めるフィードフォワード制御部と、目標リンク角度と実リンク角度の差からモータへ付与する駆動トルクを求めるフィードバック制御部と、フィードフォワード制御部の目標トルクとフィードバック制御部の目標トルクを加算してモータへの指令トルクとするトルク指令部と、を備えていることを特徴とする。

アームに対する所望の運動指令は、各関節の目標角度軌道の組み合わせとして設定される。つまり、関節の角度、角速度、角加速度が目標値となる。この目標値を実現するため、必要となるトルクは、ニュートン法、オイラー法を用いて各リンクの重心における軌道計算と、力・モーメントの釣り合いから関節（リンク）に加えられるトルク（リンク要求トルク）を計算する逆動力学計算を解くことで求まる。一方、リンク要求トルクを実現するために必要なモータ駆動トルクは、弾性要素・減衰要素系の運動方程式を解くことにより求められるが、ここで、弾性要素・減衰要素系で発生するトルクを推定し、運動方程式を解くことで、弾性要素・減衰要素系における部材の位置等を正確に求めることなくモータ駆動トルクを求めることを可能とした。また、外乱やモデルパラメータに基づく誤差を修正するためにフィードバック制御部を備える。

このフィードフォワード制御部は、数値微分をカットオフ周波数以上のゲインを抑えて位相進みを小さくした不完全微分フィルタを用いて行うとよい。

測定値から直接数値微分を行った場合、高周波ノイズが発生する可能性が高いが、このようなローパス特性を有する不完全微分フィルタによれば、高周波ノイズの発生が抑制される。

フィードバック制御部は、弾性要素・減衰要素系の共振周波数近辺の周波数領域での位相を進ませ、かつ、定常領域でのゲインを比較的低く設定する位相進み遅れフィルタをさらに備えているとよい。

共振周波数近辺の周波数領域で位相を進ませることで、ゲインが上がり、発振に対するゲイン余裕自体は小さくなるが、位相を進ませたことで位相余裕の改善効果が大きくなる。さらに、低周波領域ではゲインを高くして、目標追従性の向上を図る。

本発明によれば、モデルの内部状態量を陽に計算することなく、リンク内で発生するトルクを推定して必要なモータ駆動トルクをフィードフォワード制御を行うため、所望のリンク系運動を精度良く求めることが可能となる。さらに、フィードバック制御を加えることで、モデルパラメータ誤差や外乱によるリンク運動軌跡の所望軌跡との偏差を修正し、制御性を向上させる。

フィードフォワード制御部における微分計算にローパス特性を有する不完全微分フィルタを用いることで、数値微分における高周波ノイズの発生を抑制して、高周波振動の発生を抑制することができる。

さらに、フィードバック制御部では、位相進み遅れフィルタを用いることで、目標追従性の向上と共振周波数領域での振動抑制を両立させる。

以下、添付図面を参照して本発明の好適な実施の形態について詳細に説明する。説明の理解を容易にするため、各図面において同一の構成要素に対しては可能な限り同一の参照番号を附し、重複する説明は省略する。

図１は、本発明に係る柔軟関節ロボットアームの制御装置により制御されるロボットアームの概略図である。このロボットアーム１００は、多関節型の多軸式で多自由度を有するロボットアーム１００であり、左腕部の構成を示している。躯体４には、上腕部３と下腕部２により、ハンド部１が接続されている。上腕部３、下腕部２の両者には腕の延長方向に対して３軸方向に駆動を行う関節部が搭載されている。図２は各関節の構成を模式的に示す図である。

各柔軟関節は、躯体側のリンク本体に固定されているモータ１０の出力軸と、手先側のリンク本体の入力軸との間にプーリー１１、１２とギヤボックス１３、ダンパ１４、ばね１５が接続されて構成されている。プーリー１１、１２とギヤボックス１３はモータ１０の出力を減速する減速要素であり、ダンパ１４、ばね１５は、外部からの衝突等の突発外乱の衝撃を吸収するための減衰要素、弾性要素としてそれぞれ機能する。ここで、ダンパ１４、ばね１５は並列に配置されて減衰要素・弾性要素系を構成し、これと各減速要素１１〜１３とは直列に接続されている。

図３は、このロボットアーム１００の制御装置２００のブロック構成図である。この制御装置２００は、目標設定部２０１と、フィードフォワード制御部２０２と、フィードバック制御部２０３と、トルク指令部２０４と、ロボットアーム１００内に配置される回転状態検出手段２０５から構成される。

目標設定部２０１は、例えば、把持対象物の位置情報等の入力信号に基づいて所望の運動を実現するために必要とされる各リンク系（以下、ｉはリンク系および関節の番号を表している。）の入力部における目標リンク角度θ_ｄｉを設定する（目標角度軌道の設定）。回転状態検出手段２０５は、ロボットアーム１００の関節中の各モータ１０ｉの回転角度θ_ｍｉと、リンク入力軸１６ｉの実リンク角度θ_ｉとを測定し、出力している。フィードフォワード制御部２０２には、目標設定部２０１で設定されたθ_ｄｉと回転状態検出手段２０５で検出されたθ_ｍｉ、θ_ｉが入力され、フィードフォワード制御トルクτ_ｍを出力する。一方、フィードバック制御部２０３には、目標設定部２０１で設定されたθ_ｄｉと回転状態検出手段２０５で検出されたθ_ｉが入力され、フィードバック制御トルクτ_ｃを出力する。トルク指令部２０４は、フィードフォワード制御トルクτ_ｍとフィードバック制御トルクτ_ｃを加算してモータ１０ｉの駆動トルクτ_ｍｉを求め、ロボットアーム１００の各モータ１０ｉに求めた駆動トルクτ_ｍｉを実現するよう指示する。これにより、ロボットアーム１００に所望の運動を実現させる。

フィードフォワード制御部２０２とフィードバック制御部２０３における具体的な制御内容を以下に説明する。

まず、フィードフォワード制御部２０２における制御を説明する。図４は、フィードフォワード制御部２０２における制御の処理フローチャートであり、図５は、本制御において用いるリンク座標系の一例を示す図である。ここでは、図５に示されるように７自由度のリンク系を考える。

最初にニューイン・オイラー法を用いた逆動力学計算によって設定された目標軌道に応じて必要となる力とトルクの計算を行う。まず、初期値として目標の角度、加速度、角加速度を設定する（ステップＳ１）。ここでは、初期値として、

とする。ここで、ω_０は、座標系０の角度であり、その微分値は角速度である。また、ｐ０は座標系０に働く外力であり、ここでは重力加速度ｇに対応する。ｆ_８は、ハンド部１における外力であり、ｎ_８はハンド部１に外部から加わるトルクである。

次に、第１関節から第７関節まで順に関節の角度、角速度、外力、リンクの中心点に働く加速度をそれぞれ以下の式（１．１）〜（１．４）により求める（正順計算）。

ここで、各ａ_ｉ、ｌ_ｉ、ｑ_ｉ、Ａ_ｉはリンク間の座標変換に用いられる座標変換のための行列であり、以下のように設定される。

また、回転軸はすべてｚ軸なのでｑ_ｉはすべての関節について

となる。

次に、第７関節から第１関節まで順に以下の（２．１）式〜（２．３）式により各関節に加わる外力ｆ_ｉ、外から加わるトルクｎ_ｉとリンク入力トルクτ_ｉをそれぞれ計算する（ステップＳ３、逆順計算）。

次に、モータ出力軸トルクの計算を行う（ステップＳ４）。ここでは、図２に示されるようなモデルを用いてリンク系の運動方程式を以下のように表す。

ここで、ω_ｍ、θ_ｍ、τ_ｍ、Ｉ_ｍはそれぞれモータ１０の回転速度、回転角度、駆動トルク、慣性モーメントをそれぞれ表し、ω_ｉ、θ_ｉ、Ｉ_ｉは、リンク１６の回転速度、回転角度をはモータ１０の回転速度を表している。また、τ_ｆは、関節内の回転摩擦トルク（リンク回転軸位置換算。粘弾性トルクは除く。）を、Ｉ_ｐは、プーリー１１、１２とギヤボックス１３のギヤヘッドの慣性モーメントを、Ｉ_ｈは、ギヤボックス１３のハーモニックドライブの慣性モーメントを、Ｉ_ｌは、ダンパ１４とばね１５からなる減衰要素・弾性要素系の慣性モーメントをそれぞれ表し、Ｋは、ばね１５の弾性係数であり、Ｄは、ダンパ１４の粘性摩擦係数である。また、Ｇｐはプーリー１１、１２とギヤボックス１３のギヤヘッドに至る減速比であり、Ｇｈはギヤボックス１３のハーモニックドライブの減速比である。このため、Ｇは最終減速比を、Ｉ_Ｍは、関節内の駆動・減速系の慣性モーメントに相当する。（ここでは、関節番号を示す添字を省略している。）
（３）（４）式においては、弾性要素・減衰要素系の発生トルク

が共通するから、両式からこれを消去すると、（２）式で求めたリンク回転軸トルクτ_ｉを実現するためのモータ駆動トルクは以下の（５）式により求めることができる。

ここで、（５）式中のτ_ｍ上に付している波線は、推定値であることを表す。（５）式から明らかなように、減衰要素・弾性要素系において発生する粘弾性力や弾性力、摩擦力等により発生するトルクが陽に明らかでなくても、その合力は、θｍ、θｉそれぞれの２階微分値、すなわち、モータ１０の角加速度とリンクの角加速度から推定できる。ここで、θｍ、θｉは、回転状態検出手段２０５によって測定しているため、これを２階微分すれば求めることができる。

しかし、単純な数値微分（＝差分）は高周波ノイズを発生させるおそれがある。そこで、式（６）に示す不完全微分フィルタ（カットオフ周波数ｆ_ｃ）を２回かけることで２階微分値を求める。

図６は、この不完全微分フィルタの周波数応答を、単純微分sと比較したグラフである。カットオフ周波数ｆ_ｃ以上でゲインが頭打ちとなると同時に位相進みが小さくなることが分かる。なお、カットオフ周波数は、２階微分値の推定値が振動的に変化しないよう、目標運動軌道の周波数帯域を考慮して低めに設定するとよい。

θｍ、θｉそれぞれの２階微分値の推定値をそれぞれ上に波線を表記して２階微分値自体と区別して表すものとすると、その伝達関数表現は（６）式の不完全微分フィルタを用いて、

で表される。このとき、（５）式は以下のように変形され、フィードフォワード制御トルクを求めることができる。

上述したようにこれは、弾性要素・減衰要素系における発生トルクを推定して必要なモータ駆動トルクを求めることに等しい。

実際の計算においては、（６）式を離散化した漸化式表現とし、計算するとプログラムとしての実装が容易になる。例えば、制御周期を３msとし、（６）式の離散化式が次式で表せるとする。

ここで、モータの回転角度θｍ（ｚ）から、その微分値、２階微分値の推定値はそれぞれ不完全微分フィルタを１回または２回乗じることで得られるから、それぞれ

と表されるので、以下の漸化式で各変数を計算できる。

リンクの回転角についても同様に、

で計算することができる。以上で得られたθ_ｍ、θ_ｉの２階微分値の推定値を式（８）に代入してフィードフォワード制御トルクτ_ｍの推定値を求める。

一方、フィードバック制御部２０３においては、目標リンク角度θ_ｄｉと実リンク角度θ_ｉとの差（θ_ｄｉ−θ_ｉ）に応じたフィードバック制御トルクを求める。

本実施形態では、位相余裕を改善し、振動しにくくするため、位相進み遅れフィルタによる位相進み遅れ補償を行っている。機械系の共振により１Hz〜10Hz近辺の振動が発生しやすいことが確認されていることから、この帯域で位相を進ませるような式（９）に示される特性を有するフィルタを用いてフィードバックを行うこととする。

このフィルタの周波数特性の一例を図７に示す。図８は、通常のPDフィードバックの周波数特性を示している。一般に位相を進ませると、ゲインが上がり、発振に対するゲイン余裕は小さくなる。本実施形態では、共振周波数領域では位相を進ませ、低周波領域ではゲインをあげるとともに、高周波領域では位相を遅らせてゲインを下げているため、振動の発生を抑制して、目標追従性と振動抑制を両立することができる。

フィードバック制御トルクτ_ｃは、（９）式のフィルタを使って、

で求めることができる。さらに、積分フィードバックを追加する場合、フィードバック制御トルクτ_ｃは、

となる。

フィードバック制御トルクについてもプログラムとして実装する場合は（９）式のフィルタを離散化するとよい。不完全微分フィルタの場合と同様に制御周期を3msecとして（９）式を離散化すると、例えば、

で表せる。フィードバック制御トルクは、このフィルタを使って、

と表されるので、これを漸化式表現して以下の式により求めることができる。

一方、積分フィードバック項を追加する場合には、上の式は、

として表せる。

トルク指令部２０４は、フィードフォワード制御トルクτ_ｍとフィードバック制御トルクτ_ｃを加算してモータ１０ｉの駆動トルクτ_ｍｉを求める。

このようにして求めたフィードフォワード制御トルクτ_ｍとフィードバック制御トルクτ_ｃにより制御を行うことで、弾性要素・減衰要素による振動を抑制しつつ、所望のリンク運動軌跡への追従性を向上させたロボットアームの制御装置が得られる。

ここで、本発明の効果を確認するためシミュレーションを行ったので、その結果について報告する。図９はこのシミュレーション結果を示すものである。ここでは、３リンク系とし、ばねのストローク可能角度は１８°とした。図９（ａ）は各関節の目標角度、リンク角度、モータ角度の時間変化を示しており、図９（ｂ）は、フィードフォワードトルク（ＦＦ）、フィードバックトルク（ＦＢ）とその和（ＦＦ＋ＦＢ）を示している。いずれのリンクも目標に良く追従していることが確認された。なお、シミュレーション開始後２．５秒の段階で制御トルクが急激に変化しているが、これはバネがストロークリミッタにあたって衝撃的な力が発生したことによる。このような場合でも本発明によれば、振動を起こさず、目標に良く追従することが確認された。

本発明によれば、リンク系全体を考慮してフィードフォワード制御部を設計することにより、ここで述べた３リンク系に限らず、上述した７リンク系等の多リンク系に対しても適用が可能である。したがって、多自由度を有するロボットアームの制御に好適である。

本発明に係る柔軟関節ロボットアームの制御装置により制御されるロボットアームの概略図である。 各関節の構成を模式的に示す図である。 ロボットアーム１００の制御装置２００（本発明に係る柔軟関節ロボットアームの制御装置）のブロック構成図である。 フィードフォワード制御部２０２における制御処理のフローチャートである。 本制御において用いるリンク座標系の一例を示す図である。 本発明で用いられる不完全微分フィルタの周波数応答を、単純微分sと比較したグラフである。 本発明で用いられる位相進み遅れフィルタの周波数特性の一例を示すグラフである。 通常のPDフィードバックの周波数特性を示すグラフである。 本発明の効果を確認するためシミュレーション結果を示すグラフである。

符号の説明

１…ハンド部、２…下腕部、３…上腕部、４…躯体、１０…モータ、１１、１２…プーリー、１３…ギヤボックス、１４…ダンパ、１６…リンク、１００…ロボットアーム、２００…制御装置、２０１…目標設定部、２０２…フィードフォワード制御部、２０３…フィードバック制御部、２０４…トルク指令部、２０５…回転状態検出手段。

Claims (3)

1. モータ出力軸とリンク回転軸との間に弾性要素・減衰要素系を配置した柔軟関節要素を多自由度直列に配置した柔軟関節ロボットアームの制御装置において、
   各関節の前記モータ出力軸と前記リンク回転軸の回転状態を検出する回転状態検出手段と、
   各関節の目標角度軌道を求める目標設定部と、
   前記目標角度軌道からリンク系の逆動力学計算によりリンク回転軸位置での目標駆動トルクを計算し、前記弾性要素・減衰要素系の運動方程式から前記回転状態検出手段の出力を基に前記弾性要素・減衰要素系の発生トルクを推定して、目標駆動トルクを実現するために必要なモータ駆動トルクを求めるフィードフォワード制御部と、
   目標リンク角度と実リンク角度の差からモータへ付与する駆動トルクを求めるフィードバック制御部と、
   前記フィードフォワード制御部の目標トルクと前記フィードバック制御部の目標トルクを加算してモータへの指令トルクとするトルク指令部と、
   を備えていることを特徴とする柔軟関節ロボットアームの制御装置。
2. 前記フィードフォワード制御部は、数値微分をカットオフ周波数以上のゲインを抑えて位相進みを小さくした不完全微分フィルタを用いて行うことを特徴とする請求項１記載の柔軟関節ロボットアームの制御装置。
3. 前記フィードバック制御部は、前記弾性要素・減衰要素系の共振周波数近辺の周波数領域での位相を進ませ、かつ、定常領域でのゲインを比較的低く設定する位相進み遅れフィルタをさらに備えていることを特徴とする請求項１記載の柔軟関節ロボットアームの制御装置。

Images