JP3883544B2 - ロボット制御装置およびロボットの制御方法 - Google Patents

Description

本発明は、ロボット制御装置およびロボットの制御方法に関する。

多リンク構造を有する多軸ロボットは、各軸を駆動させるアクチュエータ（例えば、電動モータ）と、各アクチュエータの回転角を検出する角度センサ（例えば、ロータリエンコーダ）とを含む位置フィードバック制御系を備えている。位置フィードバック制御系には、ＰＩＤ（比例、積分、微分）制御が頻繁に用いられている。また、従来からアクチュエータの角度を制御して、間接的に各リンクの回転角を制御するセミクローズドループ制御が用いられている。

しかし、アクチュエータのトルクをリンクへ伝達するトルク伝達機構は、一般に剛体ではなく、或るバネ特性を有する減速機に近似される。よって、多リンク機構を有する多軸ロボットにおいては、アクチュエータやリンクが機械的な共振特性を有し、互いに振動するという問題が生じていた。

この問題に対処するために、リンクに角度センサや加速度センサを設け、これにより、リンクの角度や加速度を直接計測し、振動抑制のために状態フィードバック制御系を構成していた（特許文献1参照）。
特開昭６３−２５１８０８号公報 ロボット制御工学入門、ｐｐ.238−239、コロナ社、1989 電気学会論文誌Ｄ、Ｖｏｌ.123−2、ｐｐ.149−155、2003

しかし、角度センサを取り付けるためには、リンクの軸の外側に突出部を設ける等、複雑な構造が必要になる。よって、リンクの形状が制限されてしまう。また、既存の多軸ロボットには角度センサを追加することが不可能になる。

加速度センサは、既存の多軸ロボットの各リンク上に追加することができる（非特許文献１参照）。しかし、一般に、加速度センサの信号は、Ｓ／Ｎ（Signal/Noise）比が悪く、重力の影響を受けやすい。

また、リンクの加速度をフィードバックする効果は、慣性モーメントを変化させるだけである（非特許文献２参照）。よって、リンクの振動を抑制するためには、リンクの加速度の他、リンクの角速度や角度のフィードバックが不可欠である。しかし、通常、安価に入手できる加速度センサは並進成分しか計測できないので、或るリンク単独の角加速度を求めるためには、座標変換をするために複雑な演算を必要とする。例えば、非特許文献１のＡ１９ｂ式のような演算を必要しており、このケースでは、第２リンク単独の角加速度を求めるために、第１リンク単独の角速度が必要になってしまっている。さらに、リンクの角加速度からリンクの角速度および角度を求めるためには、角加速度をそれぞれ１回積分および２回積分する必要がある。角加速度を２回積分するとドリフトの影響を受けやすくなるので、リンク角度を正確に求めることができなくなってしまう。

本発明は、このような点に鑑みてなされたものであり、その目的は、座標変換に複雑な演算を必要とすることなく、かつ、リンクの動作を正確に制御するロボット制御装置およびロボットの制御方法を提供することである。

本発明に係る実施形態に従ったロボット制御装置は、基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ₁で伝達する第１のトルク伝達機構と、前記第１のアクチュエータの回転角θ_M1を検出する第１の角度センサと、前記基体に対する前記第１のリンクの角速度ω_L1を検出する第１の角速度センサと、ω_L1の積分値のうち第１の周波数以上の高周波数成分およびθ_M1＊Ｎ₁のうち前記第１の周波数以下の低周波数成分を用いて前記第１のリンクの角度を算出する演算部とを備えている。

本発明に係る実施形態に従ったロボットの制御方法は、基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ₁で伝達する第１のトルク伝達機構と、前記第１のアクチュエータに設けられた第１の角度センサと、前記第１のリンクに設けられた第１の角速度センサと、前記第１の角度センサおよび前記第１の角速度センサの検出結果を演算する演算部とを備えたロボットの制御方法であって、
前記第１のアクチュエータが前記第１のリンクを駆動するステップと、前記第１の角度センサが前記第１のアクチュエータの回転角θ_M1を検出するステップと、前記第１の角速度センサが前記基体に対する前記第１のリンクの角速度ω_L1を検出するステップと、前記演算部が、ω_L1の積分値のうち第１の周波数以上の高周波成分およびθ_M1＊Ｎ₁のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクの角度を算出するステップとを具備する。

本発明に係る他の実施形態に従ったロボットの制御方法は、基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ_１で伝達する第１のトルク伝達機構と、前記第１のアクチュエータに設けられた第１の角度センサと、前記第１の角度センサの検出結果を演算する演算部とを備えたロボットの制御方法であって、
前記第１のリンクに前記第１の角速度センサを配設するステップと、前記第１のアクチュエータが前記第１のリンクを駆動するステップと、前記第１の角度センサが前記第１のアクチュエータの回転角θ_Ｍ１を検出するステップと、前記第１の角速度センサが前記基体に対する前記第１のリンクの角速度ω_Ｌ１を検出するステップと、前記演算部が、ω_Ｌ１の積分値のうち第１の周波数以上の高周波成分およびθ_Ｍ１＊Ｎ_１のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクの角度を算出するステップと、前記演算部によって算出された前記第１のリンクの角度に基づいて前記ロボットの運動制御を調整するステップと、前記ロボットの運動制御の調整後、前記第１の角速度センサを前記第１のリンクから取り外すステップとを具備する。

本発明によるロボット制御装置およびロボットの制御方法は、座標変換に複雑な演算を不用とし、かつ、リンクの動作を正確に制御することができる。

以下、図面を参照して、本発明に係る実施形態を説明する。以下の実施形態は本発明を制限するものではない。

以下の実施形態は、多リンク構造を有する多軸ロボットのリンクに対してレートジャイロ等の角速度センサを備える。これによって、複雑な演算を必要とせず、かつ、その動作を正確に制御することができる。

（第1の実施形態）
図１は、本発明に係る第1の実施形態に従った２軸ロボット１００（以下、ロボット１００という）の断面図である。ロボット１００は、架台１と、リンク３、８と、モータ４、９と、減速機（トルク伝達機構）５、１１と、角度センサ６、１０と、角速度センサ１２、１３とを備えている。

架台１の上部にリンク３の一端が取り付けられており、リンク３の他端にはリンク８が取り付けられている。リンク３は、軸２を中心として架台１に対して相対的に回転運動することができ、リンク８は、軸７を中心としてリンク３に対して相対的に回転運動することができる。

モータ４は減速機５を介してリンク３を駆動し、モータ９は減速機１１を介してリンク８を駆動する。減速機５および１１の減速比をそれぞれＮ₁およびＮ₂とする。角度センサ６は、モータ４の頂部に設けられ、軸２の回りのモータ回転角θ_M1を測定する。角度センサ１０は、モータ９の頂部に設けられ、軸７の回りのモータ回転角θ_M2を測定する。

角速度センサ１２はリンク３に設けられ、角速度センサ１３はリンク８に設けられている。角速度センサ１２は架台１に対するリンク３の角速度ω_L1を検出し、角速度センサ１３はリンク３に対するリンク８の角速度ω_L2を検出する。角速度センサ１２および１３は、例えば、飛翔体の空中姿勢安定化や移動車の誘導制御に用いられるレートジャイロである。本実施形態において、角速度センサ１２および１３は、それぞれリンク３および８の先端に配置されている。しかし、リンク３および８の角速度はリンク３および８のそれぞれにおいて一定であるので、角速度センサ１２および１３はリンク３および８のそれぞれの任意の位置に配置してよい。また、角速度ω_L1および角速度ω_L2はリンク３および８の回転面内の角速度成分である。

図２は、リンク３および８のそれぞれの角度θ_A1およびθ_A2を算出する過程を示すフロー図である。ステップＳ１０およびＳ６０以外の演算処理は、ロボット１００内の任意の位置に配置されたＣＰＵ１５が実行してよく、または、ロボット１００の外部に設けられたＣＰＵ１５が実行してもよい。

まず、角速度センサ１２および１３が、角速度ω₁およびω₂を検出する（Ｓ１０）。次に、リンク３および８のそれぞれの単独の角速度ｄθ_A1／ｄｔおよびｄθ_A2／ｄｔを算出する。角速度ω₁およびω₂は、それぞれ架台１に対するリンク３および８の角速度であるので、角速度ｄθ_A1／ｄｔおよびｄθ_A2／ｄｔは、式１のように表すことができる。

次に、角速度ｄθ_A1／ｄｔおよびｄθ_A2／ｄｔを積分する（Ｓ３０）。さらに、これらの積分結果をハイパスフィルタＦ_Hに通過させる（Ｓ４０）。これにより、角度θ_A1およびθ_A2のうちカットオフ周波数１／Ｔ_F以上の高周波数成分が得られる。ここで、角度θ_A1およびθ_A2の低周波数成分を除去する理由は、リンク３、８の角速度ｄθ_A1／ｄｔ、ｄθ_A2／ｄｔの積分ではドリフトの影響によりリンク角度θ_A1、θ_A2を正確に求めることができないので、このドリフトの影響を除去するためである。

一方、ロボット１００が停止あるいは極低周波帯域（図４の左側部分）で動作している場合には、減速機５、１１はほとんど捩れないので式２が成り立つ。

そこで、角度センサ６および１０が、モータ角度θ_M1およびθ_M2を検出する（Ｓ６０）。次に、減速比ｎ１およびｎ２（ｎ１、ｎ２＜１）をそれぞれモータ角度θ_M1およびθ_M2に乗算し、θ_M1＊ｎ１およびθ_M2＊ｎ２を算出する（Ｓ７０）。次に、θ_M1＊ｎ１およびθ_M2＊ｎ２をローパスフィルタＦ_Ｌに通過させる（Ｓ８０）。これにより、角度θ_A1およびθ_A2のうちカットオフ周波数１／Ｔ_Ｆ以下の低周波数成分が得られる。ここで、角度θ_A1およびθ_A2の高周波数成分を除去する理由は、高周波数成分では減速機５、１１が捩れるため式２が成立しなくなるからである。

ハイパスフィルタＦ_Hのフィルタ係数をＨ_Fとし、ローパスフィルタＦ_Lのフィルタ係数をＬ_Fとすると、Ｈ_F＝１−Ｌ_Fの関係を有する。例えば、フィルタ係数Ｈ_F、Ｌ_Fは、式３のように表すことができる。

尚、１／Ｔ_Fは、各フィルタのカットオフ角周波数を示しており、例えば、Ｔ_F＝１〜１０程度である。

このように、Ｈ_F＝１−Ｌ_Fの関係を有するハイパスフィルタＦ_HおよびローパスフィルタＦ_Lを用いることによって、ステップＳ１０〜Ｓ４０により得られた角度θ_A1およびθ_A2の高周波数成分と、ステップＳ６０〜Ｓ８０により得られた角度θ_A1およびθ_A2の低周波数成分とを加算することができる（Ｓ１００）。この演算によって、架台１に対するリンク３の角度θ_A1およびリンク３に対するリンク８の角度θ_A2を算出することができる。

さらに、ステップＳ２０において得られた角速度ｄθ_A1／ｄｔ、ｄθ_A2／ｄｔを１回近似微分することによって、リンク３、８の角加速度ｄ²θ_A1／ｄｔ²、ｄ²θ_A2／ｄｔ²を算出することができる（Ｓ５０）。また、ステップＳ６０において得られたモータ角度θ_M1、θ_M2を１回近似微分することによって、モータ６、１０のモータ角速度ｄθ_M1／ｄｔ、ｄθ_M2／ｄｔを算出することができる（Ｓ９０）
以上の方法により、本実施の形態は２軸ロボット１００の状態変数を式４のように求めることができる。

本実施形態によれば、モータ角度からリンク角度やリンク角速度などの状態変数を推定する状態オブザーバを構成する必要がない。よって、従来よりも正確でロバストな状態フィードバック制御が可能になる。

本実施形態は、従来のような複雑な座標変換演算を必要とせず、式１のように簡単な座標変換演算によって、各リンク３、８の単独の角速度ｄθ_A1／ｄｔ、ｄθ_A2／ｄｔを得ることができる。

本実施形態は、１回積分または１回微分によって式４に示すモータ角度θ_M1およびθ_M2、モータ角速度ｄθ_M1／ｄｔおよびｄθ_M2／ｄｔ、リンク角度θ_A1およびθ_A2、リンク角速度ｄθ_A1／ｄｔおよびｄθ_A2／ｄｔ、リンク角加速度ｄ²θ_A1／ｄｔ²、ｄ²θ_A2／ｄｔ²を求めることができる。即ち、本実施形態は、従来のように２回積分または２回微分を必要としないため、式４に示す状態変数が従来より正確である。

（第２の実施形態）
第２の実施形態は、第１の実施形態におけるリンク３および８間の干渉力補償をさらに考慮して状態フィードバック制御を行う。本実施形態は、第１の実施形態と同様の構成でよいが、ＣＰＵは干渉力補償のための演算を行なう点で異なる。ロボット１００は、関節剛性を考慮すると、次の式５のモデルで表すことができる。

ここで、θ_M＝[θ_M1，θ_M2]^Tはモータ角度、θ_A＝[θ_A1，θ_A2]^Tはリンク角度、ＭＡ（θＡ）∈Ｒ^2×2はリンク慣性行列、ｃ_A（ｄθ_A／ｄｔ，θ_A）∈Ｒ^2×1はコリオリ・遠心力ベクトル、Ｍ_M＝diag（ｍ_M1，ｍ_M2）はモータロータと減速機高速段慣性を加えたもの、ｍ_M1およびｍ_M2はそれぞれモータ６および１０のモータロータと減速機高速段慣性を加えたもの、Ｄ_Ｍ＝diag（ｄ_M1，ｄ_M2）およびＤ_A＝diag（ｄ_A1，ｄ_A2）はモータおよびリンク粘性摩擦係数、ｄ_M1およびｄ_M2はそれぞれモータ６および１０の粘性摩擦係数、ｄ_A1およびｄ_A2はそれぞれリンク３および８の粘性摩擦係数、Ｋ_H＝diag（ｋ_H1，ｋ_H2）およびＤ_H＝diag（ｄ_H1，ｄ_H2）はバネ定数および減衰係数、Ｎ_H＝diag（ｎ_H1，ｎ_H2）は減速比（ｎ_H1，ｎ_H2≦１）、ｆ_M＝[ｆ_M1，ｆ_M2]^Tはクーロン動摩擦、ｆ_M1およびｆ_M2はそれぞれモータ６および１０のクーロン動摩擦、τ＝[τ₁，τ₂]^Tはモータトルク指令、τ_１およびτ_２はそれぞれモータ６および１０へのモータトルク指令を示す。

α、β、γを基本パラメータとすると、リンク慣性行列は式６のように表すことができる。

また、コリオリ・遠心力ベクトルは式７のように表すことができる。

次に、ロボット１００のリンク３および８の関節が機械共振周波数から乖離した低周波帯域において剛体であると仮定すると、θ_M＝Ｎ_Hθ_Aが成り立つ。これを式５に代入すると、式８が導出される。尚、リンク３および８の関節が機械共振周波数から乖離した低周波帯域とは、図４に示す周波数応答特性の５ｒａｄ／ｓ以下の周波数領域である。

ここでＭ（θ_A）とＤは、剛体モデルとして、それぞれ式９のような慣性行列および式１０のような粘性摩擦係数行列で表すことができる。

式８のＭ（θ_A）やｆ_Mなどに用いられる各パラメータは、２軸ロボットのランダムなモータ入力（トルク指令τ）に基づく試験動作データから最小２乗法で同定可能である。この試験動作データは、第１の実施形態において測定または算出された式４に示す状態変数ｄθ_M1／ｄｔ、ｄθ_M2／ｄｔ、θ_A1、θ_A2、ｄθ_A1／ｄｔ、ｄθ_A2／ｄｔ、ｄ²θ_A1／ｄｔ²、ｄ²θ_A2／ｄｔ²である。このように同定されたパラメータβ，γ，ｆ_Mを用いると、式５の２軸ロボットモデルの非線形補償と非干渉化が同時に可能になる。

例えば、新しいトルク指令ｕ＝[ｕ₁，ｕ₂]^Tを定義すると、式１１および式１２が成り立つ。

式１１および式１２を式（５）へ代入すると、式１３および式１４が成立する。

式１３および式１４は新しいトルク指令ｕについて線形であり、かつ、各行列は対角化されているので、軸２および軸７の動作は非干渉化される。即ち、リンク３および８は、互いに干渉することなく、独立したリンクとして演算することができる。

これにより、新しいトルク指令ｕからモータ角速度ｄθ_M／ｄｔまでの伝達関数Ｇ_M(ｓ)を定義すると、伝達関数Ｇ_M(ｓ)は、式１５および式１６のように表すことができる。

式１６は、１リンクのパラメータ同定方法によって、上記演算で非干渉化された各リンクを独立して同定することができる。

以上述べた手続きにより、式５の記述に必要な物理パラメータは全て同定できる。尚、図３は軸２または軸７のいずれか１軸分の伝達関数Ｇ_M(ｓ)を同定するためのブロック線図であり、図４は同定された周波数応答特性を示すグラフである。尚、図４のＣ₁は、ｕ₁およびｕ₂をそれぞれｄθ_M1／ｄｔおよびｄθ_M2／ｄｔに近づけたときの曲線であり、Ｃ₂は、ｕ₁およびｕ₂をそれぞれｄθ_M1／ｄｔおよびｄθ_M2／ｄｔに近づけ、ｋ_H1およびｋ_H2を∞に近づけたときの曲線であり、Ｃ₃は、ｕ₁およびｕ₂をそれぞれｎ_H1・ｄθ_A1／ｄｔおよびｎ_H2・ｄθ_A2／ｄｔに近づけたときの曲線である。

上述のように、リンク３および８は、それぞれ独立したリンクとして扱うことができる。よって、次に、防振制御のための状態フィードバック制御定数を算出するために、線形１軸分の状態方程式を導く。

を定義しておき、

と変形すると、式１９ような状態方程式が得られる。

これの具体例として、式２０のように、定常偏差を０にするための積分制御を付けた全状態フィードバック制御を適用することができる。

式２０における制御定数ｋ_i，ｆ_1i，ｆ_2i，ｆ_3i，ｆ_4iは、例えば、振動抑制に有効な折返し法などによって算出することができる。しかし、本実施形態では、状態オブザーバを併用する必要がなく、ロバストな防振制御系を構成することができる。

本実施形態によれば、式４に示す状態変数と、式１１、式１２、式２０を組み合わせることによって、リンク３および８の非線形干渉力を取り除きつつ、各軸２および７の低剛性に起因する振動を抑制することができる。さらに、本実施形態は、第１の実施形態と同様の効果をも有する。

（第３の実施形態）
第３の実施形態は、リンク３または８に与えられている外部からの力（以下、外力という）を考慮して状態フィードバック制御を行う。本実施形態は、第１の実施形態と同様の構成を有しているが、外力を考慮しつつフィードバック制御を行う。

本実施形態では、リンク８は力覚センサ（図示せず）を備えており、リンク８の先端に印加される力が力覚センサによって計測される。例えば、ロボット１００が或る対象物に対して所定の力を加えながら作業する場合に、本実施形態が適用される。この場合、リンク８の先端には定常的な力が掛かるので、リンク角速度ｄθ_A／ｄｔが０になっても、関節は捩れた状態を維持する。よって、式２は成り立たない。そこで、本実施形態では、この関節の捩れ角を補正して正しいリンク角度を推定する。

リンク８の先端に取り付けられた力覚センサによって計測された力ベクトルをｆとすると、関節トルクベクトルτは、式２１のように表すことができる。

ここでＪ^Tは、次の式２２のようなヤコビ行列（２行２列）の転置である。

このヤコビ行列は、通常計測不可能なリンク角度θ_Aの関数である。しかし、力センサの精度誤差と関節捩れ量が非常に小さいことを考慮すると、ヤコビ行列は、計測可能なモータ角度θ_M1およびθ_M2から求めても問題ない。モータ角度θ_M1およびθ_M2からヤコビ行列を求めると、式２３のように表すことができる。

よって、既に同定されている関節のバネ定数Ｋ_H（式５内の対角行列を用いることによって、捩れ角Δθは式２４のように表すことができる。

リンク８の先端に定常的な力が加わっており、尚且つ、リンク角速度ｄθ_A／ｄｔが０である場合には、式２５のようにリンク角θ_Aを求めることができる。

本実施形態によれば、リンクの先端に定常的な力が掛かっている場合であっても、リンク間の関節の捩れ角を補正して正しいリンク角度を推定することができる。また、本実施形態は、第１および第２の実施形態と組み合せることによって、第１および第２の実施形態の効果をも有する。

（第４の実施形態）
第４の実施形態は、第３の実施形態においてリンク８に掛かる定常的な力が重力である場合の実施形態である。図１に示すロボット１００において、リンク３および８は水平面内で旋回するものであるが、リンク３および８を垂直面内で旋回するように構成すると、リンク３および８は、重力の影響を受ける。よって、定常状態においてもリンク３および８の関節は捩れた状態となる。

この場合、式１１に重力加速度ｇで表される重力項ｇ_A(θ_A)を加える。これにより、トルクτは式２６のように表すことができる。また、重力項ｇ_A(θ_A)は式２７のように表すことができる。

次に、ｇ_A(θ_A)内のパラメータｒ₁，ｒ₂を同定する。ここで、式２７は式２３と同様な理由で、計測可能なモータ角度θ_M1およびθ_M2から記述しても問題ない。これによって、リンク３および８の各関節に掛かる重力トルクを算出することができる。さらに、式２１〜式２５をあてはめることによって、定常的な捩れ角Δθを補正することができる。

本実施形態は、リンクに定常的に重力が掛かっている場合であっても、リンク間の関節の捩れ角を補正して正しいリンク角度を推定することができる。また、本実施形態は、第１〜第３の実施形態と組み合せることによって、第１〜第３の実施形態の効果をも有する。

第１〜第４の実施形態において、２軸ロボット１００を用いたが、３軸以上の多軸ロボットであっても同様に制御することができる。

第１〜第４の実施形態において、角速度センサ（例えば、レートジャイロ）は、総てのリンクに常時配置してよい。しかし、コストを削減するため、あるいは、リンクの取付位置に制約がある場合には、角速度センサは選択的に一部のリンクのみに配置してもよい。例えば、角速度センサは、防振制御性能を重視するリンクだけに配置してもよい（変形例１）。また、ロボットのモデルを同定するときにだけ角速度センサを装着し、ロボットモデルの同定後、そのモデルに従ったロボットを製造または使用するときには、角速度センサはロボットに装着しなくてもよい（変形例２）。さらに、一旦、リンクに角速度センサを配置してロボットの状態推定オブザーバを角速度センサの信号に適合させ、その後、角速度センサをリンクから取り外してもよい（変形例３）。

これらの変形例１〜３を実現するためには、角速度センサ１２および１３は、リンク３および８に着脱可能に配設すればよい。例えば、角速度センサ１２および１３は、ネジ止め機構または嵌め合い機構によりリンク３および８に配設する。また、リンク３および８には、角速度センサ１２および１３のために信号線用のコネクタを設けておくことが好ましい。

尚、レートジャイロなどの角速度センサは、飛翔体の空中姿勢安定化や移動車の誘導制御に使われてきた。近年のＭＥＭＳ（Micro Electro Mechanical System）技術の進歩により、小型、軽量、高精度、安価なレートジャイロが入手可能になっている。

本発明に係る第１の実施形態に従ったロボット１００の断面図。 角度θ_A1およびθ_A2を算出する過程を示すフロー図。 伝達関数Ｇ_M(ｓ)を同定するための制御線図。 同定された周波数応答特性を示すグラフ。

符号の説明

１００ ロボット
１ 架台
２、７ 軸
３、８ リンク
４、９ モータ
５、１１ 減速機（トルク伝達機構）
６、１０ 角度センサ
１２、１３ 角速度センサ

Claims (10)

1. 基体に対して回転運動可能な第１のリンクと、
   前記第１のリンクを駆動する第１のアクチュエータと、
   前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ₁で伝達する第１のトルク伝達機構と、
   前記第１のアクチュエータの回転角θ_M1を検出する第１の角度センサと、
   前記基体に対する前記第１のリンクの角速度ω_L1を検出する第１の角速度センサと、
   ω_L1の積分値のうち第１の周波数以上の高周波数成分およびθ_M1＊Ｎ₁のうち前記第１の周波数以下の低周波数成分を用いて前記第１のリンクの角度を算出する演算部とを備えたロボット制御装置。
2. 前記第１のリンクに対して回転運動可能な第２のリンクと、
   前記第２のリンクを駆動する第２のアクチュエータと、
   前記第２のアクチュエータのトルクを前記第２のリンクに減速比Ｎ₂で伝達する第２のトルク伝達機構と、
   前記第２のアクチュエータの回転角θ_M2を検出する第２の角度センサと、
   前記基体に対する前記第２のリンクの角速度ω_L2を検出する第２の角速度センサとをさらに備え、
   前記演算部は、ω_L2−ω_L1の積分値のうち第１の周波数以上の高周波数成分およびθ_M2＊Ｎ₂のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクに対する前記第２のリンクの角度をさらに算出することを特徴とする請求項１に記載のロボット制御装置。
3. 前記演算部は、ω_L1の積分値の高周波成分およびθ_M1＊Ｎ₁の低周波成分との和を前記基体に対する前記第１のリンクの角度とし、ω_L2−ω_L1の積分値の高周波数成分およびθ_M2＊Ｎ₂の低周波成分との和を前記第１のリンクに対する前記第２のリンクの角度とすることを特徴とする請求項２に記載のロボット制御装置。
4. 前記演算部は、前記回転角θ_M1およびθ_M2を微分して該第１および第２のアクチュエータの角速度を算出し、前記角速度ω_L1およびω_L2を微分して該第１および第２のリンクの角加速度を算出することを特徴とする請求項２に記載のロボット制御装置。
5. θ_M1＊Ｎ₁およびθ_M2＊Ｎ₂のそれぞれの低周波成分を通過させるローパスフィルタと、
   ω_L1の積分値およびω_L2−ω_L1の積分値のそれぞれの高周波数成分を通過させるハイパスフィルタとをさらに備え、
   前記ローパスフィルタおよび前記ハイパスフィルタのそれぞれのフィルタ係数の和は、ほぼ１であることを特徴とする請求項１または請求項２に記載のロボット制御装置。
6. 前記第１の角速度センサおよび前記第２の角速度センサは、それぞれ着脱可能に前記第１のリンクおよび前記第２のリンクに配設されていることを特徴とする請求項１または請求項２に記載のロボット制御装置。
7. 前記演算部は、前記第１のリンクと前記第２のリンクとの間の干渉力、または、前記第１のリンクおよび前記第２のリンクに与えられる外部からの力に基づく該第１のリンクの角度および該第２のリンクの捩れ角を算出することを特徴とする請求項１または請求項２に記載のロボット制御装置。
8. 基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ₁で伝達する第１のトルク伝達機構と、前記第１のアクチュエータに設けられた第１の角度センサと、前記第１のリンクに設けられた第１の角速度センサと、前記第１の角度センサおよび前記第１の角速度センサの検出結果を演算する演算部とを備えたロボットの制御方法であって、
   前記第１のアクチュエータが前記第１のリンクを駆動するステップと、
   前記第１の角度センサが前記第１のアクチュエータの回転角θ_M1を検出するステップと、
   前記第１の角速度センサが前記基体に対する前記第１のリンクの角速度ω_L1を検出するステップと、
   前記演算部が、ω_L1の積分値のうち第１の周波数以上の高周波成分およびθ_M1＊Ｎ₁のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクの角度を算出するステップとを具備するロボットの制御方法。
9. 前記第１のリンクに対して回転運動可能な第２のリンクと、前記第２のリンクを駆動する第２のアクチュエータと、前記第２のアクチュエータのトルクを前記第２のリンクに減速比Ｎ₂で伝達する第２のトルク伝達機構と、前記第２のアクチュエータに設けられた第２の角度センサと、前記第２のリンクに設けられた第２の角速度センサとをさらに備え、前記演算部は前記第２の角度センサおよび前記第２の角速度センサの検出結果を演算するロボットの制御方法であって、
   前記第２のアクチュエータが前記第２のリンクを駆動するステップと、
   前記第２の角度センサが前記第２のアクチュエータの回転角θ_M2を検出するステップと、
   前記第２の角速度センサが前記基体に対する前記第２のリンクの角速度ω_L2を検出するステップと、
   前記演算部が、ω_L2−ω_L1の積分値のうち第１の周波数以上の高周波数成分およびθ_M2＊Ｎ₂のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクに対する前記第２のリンクの角度を算出するステップとをさらに具備することを特徴とする請求項８に記載のロボットの制御方法。
10. 基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ_１で伝達する第１のトルク伝達機構と、前記第１のアクチュエータに設けられた第１の角度センサと、前記第１の角度センサの検出結果を演算する演算部とを備えたロボットの制御方法であって、
    前記第１のリンクに前記第１の角速度センサを配設するステップと、
    前記第１のアクチュエータが前記第１のリンクを駆動するステップと、
    前記第１の角度センサが前記第１のアクチュエータの回転角θ_Ｍ１を検出するステップと、
    前記第１の角速度センサが前記基体に対する前記第１のリンクの角速度ω_Ｌ１を検出するステップと、
    前記演算部が、ω_Ｌ１の積分値のうち第１の周波数以上の高周波成分およびθ_Ｍ１＊Ｎ_１のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクの角度を算出するステップと、
    前記演算部によって算出された前記第１のリンクの角度に基づいて前記ロボットの運動制御を調整するステップと、
    前記ロボットの運動制御の調整後、前記第１の角速度センサを前記第１のリンクから取り外すステップとを具備するロボットの制御方法。

Images