(57)【要約】      （修正有）【課題】  座標変換に複雑な演算を実行することなく、かつ、リンクの動作を正確に制御するロボット制御装置およびロボットの制御方法を提供する。【解決手段】ロボット制御装置は、基体１に対して回転運動可能な第１のリンク３と、第１のリンク３を駆動する第１のアクチュエータ４と、第１のアクチュエータ４のトルクを第１のリンク３に減速比Ｎ1で伝達する第１のトルク伝達機構５と、第１のアクチュエータ４の回転角θM1を検出する第１の角度センサ６と、基体１に対する第１のリンク３の角速度ωL1を検出する第１の角速度センサ１２と、ωL1の積分値の高周波成分およびθM1＊Ｎ1の低周波成分を用いて第１のリンク３の角度θA1を算出する演算部１５とを備えている。【選択図】  図１
2005242794.tif 000002
【特許請求の範囲】
【請求項１】
基体に対して回転運動可能な第１のリンクと、
前記第１のリンクを駆動する第１のアクチュエータと、
前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ1で伝達する第１のトルク伝達機構と、
前記第１のアクチュエータの回転角θM1を検出する第１の角度センサと、
前記基体に対する前記第１のリンクの角速度ωL1を検出する第１の角速度センサと、
ωL1の積分値のうち第１の周波数以上の高周波数成分およびθM1＊Ｎ1のうち前記第１の周波数以下の低周波数成分を用いて前記第１のリンクの角度を算出する演算部とを備えたロボット制御装置。
【請求項２】
前記第１のリンクに対して回転運動可能な第２のリンクと、
前記第２のリンクを駆動する第２のアクチュエータと、
前記第２のアクチュエータのトルクを前記第２のリンクに減速比Ｎ2で伝達する第２のトルク伝達機構と、
前記第２のアクチュエータの回転角θM2を検出する第２の角度センサと、
前記基体に対する前記第２のリンクの角速度ωL2を検出する第２の角速度センサとをさらに備え、
前記演算部は、ωL2−ωL1の積分値のうち第１の周波数以上の高周波数成分およびθM2＊Ｎ2のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクに対する前記第２のリンクの角度をさらに算出することを特徴とする請求項１に記載のロボット制御装置。
【請求項３】
前記演算部は、ωL1の積分値の高周波成分およびθM1＊Ｎ1の低周波成分との和を前記基体に対する前記第１のリンクの角度とし、ωL2−ωL1の積分値の高周波数成分およびθM2＊Ｎ2の低周波成分との和を前記第１のリンクに対する前記第２のリンクの角度とすることを特徴とする請求項２に記載のロボット制御装置。
【請求項４】
前記演算部は、前記回転角θM1およびθM2を微分して該第１および第２のアクチュエータの角速度を算出し、前記角速度ωL1およびωL2を微分して該第１および第２のリンクの角加速度を算出することを特徴とする請求項２に記載のロボット制御装置。
【請求項５】
θM1＊Ｎ1およびθM2＊Ｎ2のそれぞれの低周波成分を通過させるローパスフィルタと、
ωL1の積分値およびωL2−ωL1の積分値のそれぞれの高周波数成分を通過させるハイパスフィルタとをさらに備え、
前記ローパスフィルタおよび前記ハイパスフィルタのそれぞれのフィルタ係数の和は、ほぼ１であることを特徴とする請求項１または請求項２に記載のロボット制御装置。
【請求項６】
前記第１の角速度センサおよび前記第２の角速度センサは、それぞれ着脱可能に前記第１のリンクおよび前記第２のリンクに配設されていることを特徴とする請求項１または請求項２に記載のロボット制御装置。
【請求項７】
前記演算部は、前記第１のリンクと前記第２のリンクとの間の干渉力、または、前記第１のリンクおよび前記第２のリンクに与えられる外部からの力に基づく該第１のリンクの角度および該第２のリンクの捩れ角を算出することを特徴とする請求項１または請求項２に記載のロボット制御装置。
【請求項８】
基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ1で伝達する第１のトルク伝達機構と、前記第１のアクチュエータに設けられた第１の角度センサと、前記第１のリンクに設けられた第１の角速度センサと、前記第１の角度センサおよび前記第１の角速度センサの検出結果を演算する演算部とを備えたロボットの制御方法であって、
前記第１のアクチュエータが前記第１のリンクを駆動するステップと、
前記第１の角度センサが前記第１のアクチュエータの回転角θM1を検出するステップと、
前記第１の角速度センサが前記基体に対する前記第１のリンクの角速度ωL1を検出するステップと、
前記演算部が、ωL1の積分値のうち第１の周波数以上の高周波成分およびθM1＊Ｎ1のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクの角度を算出するステップとを具備するロボットの制御方法。
【請求項９】
前記第１のリンクに対して回転運動可能な第２のリンクと、前記第２のリンクを駆動する第２のアクチュエータと、前記第２のアクチュエータのトルクを前記第２のリンクに減速比Ｎ2で伝達する第２のトルク伝達機構と、前記第２のアクチュエータに設けられた第２の角度センサと、前記第２のリンクに設けられた第２の角速度センサとをさらに備え、前記演算部は前記第２の角度センサおよび前記第２の角速度センサの検出結果を演算するロボットの制御方法であって、
前記第２のアクチュエータが前記第２のリンクを駆動するステップと、
前記第２の角度センサが前記第２のアクチュエータの回転角θM2を検出するステップと、
前記第２の角速度センサが前記基体に対する前記第２のリンクの角速度ωL2を検出するステップと、
前記演算部が、ωL2−ωL1の積分値のうち第１の周波数以上の高周波数成分およびθM2＊Ｎ2のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクに対する前記第２のリンクの角度を算出するステップとをさらに具備することを特徴とする請求項８に記載のロボットの制御方法。
【請求項１０】
基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ1で伝達する第１のトルク伝達機構と、前記第１のアクチュエータに設けられた第１の角度センサと、前記第１の角度センサの検出結果を演算する演算部とを備えたロボットの制御方法であって、
前記第１のリンクに前記第１の角速度センサを配設するステップと、
前記第１のアクチュエータが前記第１のリンクを駆動するステップと、
前記第１の角度センサが前記第１のアクチュエータの回転角θM1を検出するステップと、
前記第１の角速度センサが前記基体に対する前記第１のリンクの角速度ωL1を検出するステップと、
前記演算部が、ωL1の積分値のうち第１の周波数以上の高周波成分およびθM1＊Ｎ1のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクの角度を算出するステップと、
前記演算部によって算出された前記第１のリンクの角度に基づいて前記ロボットの運動制御を調整するステップと、
前記ロボットの運動制御の調整後、前記第１および第２の角速度センサを前記第１および第２のリンクから取り外すステップとを具備するロボットの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット制御装置およびロボットの制御方法に関する。
【背景技術】
【０００２】
多リンク構造を有する多軸ロボットは、各軸を駆動させるアクチュエータ（例えば、電動モータ）と、各アクチュエータの回転角を検出する角度センサ（例えば、ロータリエンコーダ）とを含む位置フィードバック制御系を備えている。位置フィードバック制御系には、ＰＩＤ（比例、積分、微分）制御が頻繁に用いられている。また、従来からアクチュエータの角度を制御して、間接的に各リンクの回転角を制御するセミクローズドループ制御が用いられている。
【０００３】
しかし、アクチュエータのトルクをリンクへ伝達するトルク伝達機構は、一般に剛体ではなく、或るバネ特性を有する減速機に近似される。よって、多リンク機構を有する多軸ロボットにおいては、アクチュエータやリンクが機械的な共振特性を有し、互いに振動するという問題が生じていた。
【０００４】
この問題に対処するために、リンクに角度センサや加速度センサを設け、これにより、リンクの角度や加速度を直接計測し、振動抑制のために状態フィードバック制御系を構成していた（特許文献1参照）。
【特許文献１】特開昭６３−２５１８０８号公報
【非特許文献１】ロボット制御工学入門、ｐｐ.238−239、コロナ社、1989
【非特許文献２】電気学会論文誌Ｄ、Ｖｏｌ.123−2、ｐｐ.149−155、2003
【発明の開示】
【発明が解決しようとする課題】
【０００５】
しかし、角度センサを取り付けるためには、リンクの軸の外側に突出部を設ける等、複雑な構造が必要になる。よって、リンクの形状が制限されてしまう。また、既存の多軸ロボットには角度センサを追加することが不可能になる。
【０００６】
加速度センサは、既存の多軸ロボットの各リンク上に追加することができる（非特許文献１参照）。しかし、一般に、加速度センサの信号は、Ｓ／Ｎ（Signal/Noise）比が悪く、重力の影響を受けやすい。
【０００７】
また、リンクの加速度をフィードバックする効果は、慣性モーメントを変化させるだけである（非特許文献２参照）。よって、リンクの振動を抑制するためには、リンクの加速度の他、リンクの角速度や角度のフィードバックが不可欠である。しかし、通常、安価に入手できる加速度センサは並進成分しか計測できないので、或るリンク単独の角加速度を求めるためには、座標変換をするために複雑な演算を必要とする。例えば、非特許文献１のＡ１９ｂ式のような演算を必要しており、このケースでは、第２リンク単独の角加速度を求めるために、第１リンク単独の角速度が必要になってしまっている。さらに、リンクの角加速度からリンクの角速度および角度を求めるためには、角加速度をそれぞれ１回積分および２回積分する必要がある。角加速度を２回積分するとドリフトの影響を受けやすくなるので、リンク角度を正確に求めることができなくなってしまう。
【０００８】
本発明は、このような点に鑑みてなされたものであり、その目的は、座標変換に複雑な演算を必要とすることなく、かつ、リンクの動作を正確に制御するロボット制御装置およびロボットの制御方法を提供することである。
【課題を解決するための手段】
【０００９】
本発明に係る実施形態に従ったロボット制御装置は、基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ1で伝達する第１のトルク伝達機構と、前記第１のアクチュエータの回転角θM1を検出する第１の角度センサと、前記基体に対する前記第１のリンクの角速度ωL1を検出する第１の角速度センサと、ωL1の積分値のうち第１の周波数以上の高周波数成分およびθM1＊Ｎ1のうち前記第１の周波数以下の低周波数成分を用いて前記第１のリンクの角度を算出する演算部とを備えている。
【００１０】
本発明に係る実施形態に従ったロボットの制御方法は、基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ1で伝達する第１のトルク伝達機構と、前記第１のアクチュエータに設けられた第１の角度センサと、前記第１のリンクに設けられた第１の角速度センサと、前記第１の角度センサおよび前記第１の角速度センサの検出結果を演算する演算部とを備えたロボットの制御方法であって、
前記第１のアクチュエータが前記第１のリンクを駆動するステップと、前記第１の角度センサが前記第１のアクチュエータの回転角θM1を検出するステップと、前記第１の角速度センサが前記基体に対する前記第１のリンクの角速度ωL1を検出するステップと、前記演算部が、ωL1の積分値のうち第１の周波数以上の高周波成分およびθM1＊Ｎ1のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクの角度を算出するステップとを具備する。
【００１１】
本発明に係る他の実施形態に従ったロボットの制御方法は、基体に対して回転運動可能な第１のリンクと、前記第１のリンクを駆動する第１のアクチュエータと、前記第１のアクチュエータのトルクを前記第１のリンクに減速比Ｎ1で伝達する第１のトルク伝達機構と、前記第１のアクチュエータに設けられた第１の角度センサと、前記第１の角度センサの検出結果を演算する演算部とを備えたロボットの制御方法であって、
前記第１のリンクに前記第１の角速度センサを配設するステップと、前記第１のアクチュエータが前記第１のリンクを駆動するステップと、前記第１の角度センサが前記第１のアクチュエータの回転角θM1を検出するステップと、前記第１の角速度センサが前記基体に対する前記第１のリンクの角速度ωL1を検出するステップと、前記演算部が、ωL1の積分値のうち第１の周波数以上の高周波成分およびθM1＊Ｎ1のうち前記第１の周波数以下の低周波成分を用いて前記第１のリンクの角度を算出するステップと、前記演算部によって算出された前記第１のリンクの角度に基づいて前記ロボットの運動制御を調整するステップと、前記ロボットの運動制御の調整後、前記第１および第２の角速度センサを前記第１および第２のリンクから取り外すステップとを具備する。
【発明の効果】
【００１２】
本発明によるロボット制御装置およびロボットの制御方法は、座標変換に複雑な演算を不用とし、かつ、リンクの動作を正確に制御することができる。
【発明を実施するための最良の形態】
【００１３】
以下、図面を参照して、本発明に係る実施形態を説明する。以下の実施形態は本発明を制限するものではない。
【００１４】
以下の実施形態は、多リンク構造を有する多軸ロボットのリンクに対してレートジャイロ等の角速度センサを備える。これによって、複雑な演算を必要とせず、かつ、その動作を正確に制御することができる。
【００１５】
（第1の実施形態）
図１は、本発明に係る第1の実施形態に従った２軸ロボット１００（以下、ロボット１００という）の断面図である。ロボット１００は、架台１と、リンク３、８と、モータ４、９と、減速機（トルク伝達機構）５、１１と、角度センサ６、１０と、角速度センサ１２、１３とを備えている。
【００１６】
架台１の上部にリンク３の一端が取り付けられており、リンク３の他端にはリンク８が取り付けられている。リンク３は、軸２を中心として架台１に対して相対的に回転運動することができ、リンク８は、軸７を中心としてリンク３に対して相対的に回転運動することができる。
【００１７】
モータ４は減速機５を介してリンク３を駆動し、モータ９は減速機１１を介してリンク８を駆動する。減速機５および１１の減速比をそれぞれＮ1およびＮ2とする。角度センサ６は、モータ４の頂部に設けられ、軸２の回りのモータ回転角θM1を測定する。角度センサ１０は、モータ９の頂部に設けられ、軸７の回りのモータ回転角θM2を測定する。
【００１８】
角速度センサ１２はリンク３に設けられ、角速度センサ１３はリンク８に設けられている。角速度センサ１２は架台１に対するリンク３の角速度ωL1を検出し、角速度センサ１３はリンク３に対するリンク８の角速度ωL2を検出する。角速度センサ１２および１３は、例えば、飛翔体の空中姿勢安定化や移動車の誘導制御に用いられるレートジャイロである。本実施形態において、角速度センサ１２および１３は、それぞれリンク３および８の先端に配置されている。しかし、リンク３および８の角速度はリンク３および８のそれぞれにおいて一定であるので、角速度センサ１２および１３はリンク３および８のそれぞれの任意の位置に配置してよい。また、角速度ωL1および角速度ωL2はリンク３および８の回転面内の角速度成分である。
【００１９】
図２は、リンク３および８のそれぞれの角度θA1およびθA2を算出する過程を示すフロー図である。ステップＳ１０およびＳ６０以外の演算処理は、ロボット１００内の任意の位置に配置されたＣＰＵ１５が実行してよく、または、ロボット１００の外部に設けられたＣＰＵ１５が実行してもよい。
【００２０】
まず、角速度センサ１２および１３が、角速度ω1およびω2を検出する（Ｓ１０）。次に、リンク３および８のそれぞれの単独の角速度ｄθA1／ｄｔおよびｄθA2／ｄｔを算出する。角速度ω1およびω2は、それぞれ架台１に対するリンク３および８の角速度であるので、角速度ｄθA1／ｄｔおよびｄθA2／ｄｔは、式１のように表すことができる。
【数１】
2005242794.tif 000003
【００２１】
次に、角速度ｄθA1／ｄｔおよびｄθA2／ｄｔを積分する（Ｓ３０）。さらに、これらの積分結果をハイパスフィルタＦHに通過させる（Ｓ４０）。これにより、角度θA1およびθA2のうちカットオフ周波数１／ＴF以上の高周波数成分が得られる。ここで、角度θA1およびθA2の低周波数成分を除去する理由は、リンク３、８の角速度ｄθA1／ｄｔ、ｄθA2／ｄｔの積分ではドリフトの影響によりリンク角度θA1、θA2を正確に求めることができないので、このドリフトの影響を除去するためである。
【００２２】
一方、ロボット１００が停止あるいは極低周波帯域（図８の左側部分）で動作している場合には、減速機５、１１はほとんど捩れないので式２が成り立つ。
【数２】
2005242794.tif 000004
【００２３】
そこで、角度センサ６および１０が、モータ角度θM1およびθM2を検出する（Ｓ６０）。次に、減速比ｎ１およびｎ２（ｎ１、ｎ２＜１）をそれぞれモータ角度θM1およびθM2に乗算し、θM1＊ｎ１およびθM2＊ｎ２を算出する（Ｓ７０）。次に、θM1＊ｎ１およびθM2＊ｎ２をローパスフィルタＦＬに通過させる（Ｓ８０）。これにより、角度θA1およびθA2のうちカットオフ周波数１／ＴＦ以下の低周波数成分が得られる。ここで、角度θA1およびθA2の高周波数成分を除去する理由は、高周波数成分では減速機５、１１が捩れるため式２が成立しなくなるからである。
【００２４】
ハイパスフィルタＦHのフィルタ係数をＨFとし、ローパスフィルタＦLのフィルタ係数をＬFとすると、ＨF＝１−ＬFの関係を有する。例えば、フィルタ係数ＨF、ＬFは、式３のように表すことができる。
【数３】
2005242794.tif 000005
【００２５】
尚、１／ＴFは、各フィルタのカットオフ角周波数を示しており、例えば、ＴF＝１〜１０程度である。
【００２６】
このように、ＨF＝１−ＬFの関係を有するハイパスフィルタＦHおよびローパスフィルタＦLを用いることによって、ステップＳ１０〜Ｓ４０により得られた角度θA1およびθA2の高周波数成分と、ステップＳ６０〜Ｓ８０により得られた角度θA1およびθA2の低周波数成分とを加算することができる（Ｓ１００）。この演算によって、架台１に対するリンク３の角度θA1およびリンク３に対するリンク８の角度θA2を算出することができる。
【００２７】
さらに、ステップＳ２０において得られた角速度ｄθA1／ｄｔ、ｄθA2／ｄｔを１回近似微分することによって、リンク３、８の角加速度ｄ2θA1／ｄｔ2、ｄ2θA2／ｄｔ2を算出することができる（Ｓ５０）。また、ステップＳ６０において得られたモータ角度θM1、θM2を１回近似微分することによって、モータ６、１０のモータ角速度ｄθM1／ｄｔ、ｄθM2／ｄｔを算出することができる（Ｓ９０）
以上の方法により、本実施の形態は２軸ロボット１００の状態変数を式４のように求めることができる。
【数４】
2005242794.tif 000006
【００２８】
本実施形態によれば、モータ角度からリンク角度やリンク角速度などの状態変数を推定する状態オブザーバを構成する必要がない。よって、従来よりも正確でロバストな状態フィードバック制御が可能になる。
【００２９】
本実施形態は、従来のような複雑な座標変換演算を必要とせず、式１のように簡単な座標変換演算によって、各リンク３、８の単独の角速度ｄθA1／ｄｔ、ｄθA2／ｄｔを得ることができる。
【００３０】
本実施形態は、１回積分または１回微分によって式４に示すモータ角度θM1およびθM2、モータ角速度ｄθM1／ｄｔおよびｄθM2／ｄｔ、リンク角度θA1およびθA2、リンク角速度ｄθA1／ｄｔおよびｄθA2／ｄｔ、リンク角加速度ｄ2θA1／ｄｔ2、ｄ2θA2／ｄｔ2を求めることができる。即ち、本実施形態は、従来のように２回積分または２回微分を必要としないため、式４に示す状態変数が従来より正確である。
【００３１】
（第２の実施形態）
第２の実施形態は、第１の実施形態におけるリンク３および８間の干渉力補償をさらに考慮して状態フィードバック制御を行う。本実施形態は、第１の実施形態と同様の構成でよいが、ＣＰＵは干渉力補償のための演算を行なう点で異なる。ロボット１００は、関節剛性を考慮すると、次の式５のモデルで表すことができる。
【数５】
2005242794.tif 000007
【００３２】
ここで、θ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はそれぞれモータ６および１０のクーロン動摩擦、τ＝[τ1，τ2]Tはモータトルク指令、τ１およびτ２はそれぞれモータ６および１０へのモータトルク指令を示す。
【００３３】
α、β、γを基本パラメータとすると、リンク慣性行列は式６のように表すことができる。
【数６】
2005242794.tif 000008
【００３４】
また、コリオリ・遠心力ベクトルは式７のように表すことができる。
【数７】
2005242794.tif 000009
【００３５】
次に、ロボット１００のリンク３および８の関節が機械共振周波数から乖離した低周波帯域において剛体であると仮定すると、θM＝ＮHθAが成り立つ。これを式５に代入すると、式８が導出される。尚、リンク３および８の関節が機械共振周波数から乖離した低周波帯域とは、図４に示す周波数応答特性の５ｒａｄ／ｓ以下の周波数領域である。
【数８】
2005242794.tif 000010
【００３６】
ここでＭ（θA）とＤは、剛体モデルとして、それぞれ式９のような慣性行列および式１０のような粘性摩擦係数行列で表すことができる。
【数９】
2005242794.tif 000011
【数１０】
2005242794.tif 000012
【００３７】
式８のＭ（θA）やｆMなどに用いられる各パラメータは、２軸ロボットのランダムなモータ入力（トルク指令τ）に基づく試験動作データから最小２乗法で同定可能である。この試験動作データは、第１の実施形態において測定または算出された式４に示す状態変数ｄθM1／ｄｔ、ｄθM2／ｄｔ、θA1、θA2、ｄθA1／ｄｔ、ｄθA2／ｄｔ、ｄ2θA1／ｄｔ2、ｄ2θA2／ｄｔ2である。このように同定されたパラメータβ，γ，ｆMを用いると、式５の２軸ロボットモデルの非線形補償と非干渉化が同時に可能になる。
【００３８】
例えば、新しいトルク指令ｕ＝[ｕ1，ｕ2]Tを定義すると、式１１および式１２が成り立つ。
【数１１】
2005242794.tif 000013
【数１２】
2005242794.tif 000014
【００３９】
式１１および式１２を式（５）へ代入すると、式１３および式１４が成立する。
【数１３】
2005242794.tif 000015
【数１４】
2005242794.tif 000016
【００４０】
式１３および式１４は新しいトルク指令ｕについて線形であり、かつ、各行列は対角化されているので、軸２および軸７の動作は非干渉化される。即ち、リンク３および８は、互いに干渉することなく、独立したリンクとして演算することができる。
【００４１】
これにより、新しいトルク指令ｕからモータ角速度ｄθM／ｄｔまでの伝達関数ＧM(ｓ)を定義すると、伝達関数ＧM(ｓ)は、式１５および式１６のように表すことができる。
【数１５】
2005242794.tif 000017
【数１６】
2005242794.tif 000018
【００４２】
式１６は、１リンクのパラメータ同定方法によって、上記演算で非干渉化された各リンクを独立して同定することができる。
【００４３】
以上述べた手続きにより、式５の記述に必要な物理パラメータは全て同定できる。尚、図３は軸２または軸７のいずれか１軸分の伝達関数ＧM(ｓ)を同定するためのブロック線図であり、図４は同定された周波数応答特性を示すグラフである。尚、図４のＣ1は、ｕ1およびｕ2をそれぞれｄθM1／ｄｔおよびｄθM2／ｄｔに近づけたときの曲線であり、Ｃ2は、ｕ1およびｕ2をそれぞれｄθM1／ｄｔおよびｄθM2／ｄｔに近づけ、ｋH1およびｋH2を∞に近づけたときの曲線であり、Ｃ3は、ｕ1およびｕ2をそれぞれｎH1・ｄθA1／ｄｔおよびｎH2・ｄθA2／ｄｔに近づけたときの曲線である。
【００４４】
上述のように、リンク３および８は、それぞれ独立したリンクとして扱うことができる。よって、次に、防振制御のための状態フィードバック制御定数を算出するために、線形１軸分の状態方程式を導く。
【数１７】
2005242794.tif 000019
【００４５】
を定義しておき、
【数１８】
2005242794.tif 000020
【００４６】
と変形すると、式１９ような状態方程式が得られる。
【数１９】
2005242794.tif 000021
【００４７】
これの具体例として、式２０のように、定常偏差を０にするための積分制御を付けた全状態フィードバック制御を適用することができる。
【数２０】
2005242794.tif 000022
【００４８】
式２０における制御定数ｋi，ｆ1i，ｆ2i，ｆ3i，ｆ4iは、例えば、振動抑制に有効な折返し法などによって算出することができる。しかし、本実施形態では、状態オブザーバを併用する必要がなく、ロバストな防振制御系を構成することができる。
【００４９】
本実施形態によれば、式４に示す状態変数と、式１１、式１２、式２０を組み合わせることによって、リンク３および８の非線形干渉力を取り除きつつ、各軸２および７の低剛性に起因する振動を抑制することができる。さらに、本実施形態は、第１の実施形態と同様の効果をも有する。
【００５０】
（第３の実施形態）
第３の実施形態は、リンク３または８に与えられている外部からの力（以下、外力という）を考慮して状態フィードバック制御を行う。本実施形態は、第１の実施形態と同様の構成を有しているが、外力を考慮しつつフィードバック制御を行う。
【００５１】
本実施形態では、リンク８は力覚センサ（図示せず）を備えており、リンク８の先端に印加される力が力覚センサによって計測される。例えば、ロボット１００が或る対象物に対して所定の力を加えながら作業する場合に、本実施形態が適用される。この場合、リンク８の先端には定常的な力が掛かるので、リンク角速度ｄθA／ｄｔが０になっても、関節は捩れた状態を維持する。よって、式２は成り立たない。そこで、本実施形態では、この関節の捩れ角を補正して正しいリンク角度を推定する。
【００５２】
リンク８の先端に取り付けられた力覚センサによって計測された力ベクトルをｆとすると、関節トルクベクトルτは、式２１のように表すことができる。
【数２１】
2005242794.tif 000023
【００５３】
ここでＪTは、次の式２２のようなヤコビ行列（２行２列）の転置である。
【数２２】
2005242794.tif 000024
【００５４】
このヤコビ行列は、通常計測不可能なリンク角度θAの関数である。しかし、力センサの精度誤差と関節捩れ量が非常に小さいことを考慮すると、ヤコビ行列は、計測可能なモータ角度θM1およびθM2から求めても問題ない。モータ角度θM1およびθM2からヤコビ行列を求めると、式２３のように表すことができる。
【数２３】
2005242794.tif 000025
【００５５】
よって、既に同定されている関節のバネ定数ＫH（式５内の対角行列を用いることによって、捩れ角Δθは式２４のように表すことができる。
【数２４】
2005242794.tif 000026
【００５６】
リンク８の先端に定常的な力が加わっており、尚且つ、リンク角速度ｄθA／ｄｔが０である場合には、式２５のようにリンク角θAを求めることができる。
【数２５】
2005242794.tif 000027
【００５７】
本実施形態によれば、リンクの先端に定常的な力が掛かっている場合であっても、リンク間の関節の捩れ角を補正して正しいリンク角度を推定することができる。また、本実施形態は、第１および第２の実施形態と組み合せることによって、第１および第２の実施形態の効果をも有する。
【００５８】
（第４の実施形態）
第４の実施形態は、第３の実施形態においてリンク８に掛かる定常的な力が重力である場合の実施形態である。図１に示すロボット１００において、リンク３および８は水平面内で旋回するものであるが、リンク３および８を垂直面内で旋回するように構成すると、リンク３および８は、重力の影響を受ける。よって、定常状態においてもリンク３および８の関節は捩れた状態となる。
【００５９】
この場合、式１１に重力加速度ｇで表される重力項ｇA(θA)を加える。これにより、トルクτは式２６のように表すことができる。また、重力項ｇA(θA)は式２７のように表すことができる。
【数２６】
2005242794.tif 000028
【数２７】
2005242794.tif 000029
【００６０】
次に、ｇA(θA)内のパラメータｒ1，ｒ2を同定する。ここで、式２７は式２３と同様な理由で、計測可能なモータ角度θM1およびθM2から記述しても問題ない。これによって、リンク３および８の各関節に掛かる重力トルクを算出することができる。さらに、式２１〜式２５をあてはめることによって、定常的な捩れ角Δθを補正することができる。
【００６１】
本実施形態は、リンクに定常的に重力が掛かっている場合であっても、リンク間の関節の捩れ角を補正して正しいリンク角度を推定することができる。また、本実施形態は、第１〜第３の実施形態と組み合せることによって、第１〜第３の実施形態の効果をも有する。
【００６２】
第１〜第４の実施形態において、２軸ロボット１００を用いたが、３軸以上の多軸ロボットであっても同様に制御することができる。
【００６３】
第１〜第４の実施形態において、角速度センサ（例えば、レートジャイロ）は、総てのリンクに常時配置してよい。しかし、コストを削減するため、あるいは、リンクの取付位置に制約がある場合には、角速度センサは選択的に一部のリンクのみに配置してもよい。例えば、角速度センサは、防振制御性能を重視するリンクだけに配置してもよい（変形例１）。また、ロボットのモデルを同定するときにだけ角速度センサを装着し、ロボットモデルの同定後、そのモデルに従ったロボットを製造または使用するときには、角速度センサはロボットに装着しなくてもよい（変形例２）。さらに、一旦、リンクに角速度センサを配置してロボットの状態推定オブザーバを角速度センサの信号に適合させ、その後、角速度センサをリンクから取り外してもよい（変形例３）。
【００６４】
これらの変形例１〜３を実現するためには、角速度センサ１２および１３は、リンク３および８に着脱可能に配設すればよい。例えば、角速度センサ１２および１３は、ネジ止め機構または嵌め合い機構によりリンク３および８に配設する。また、リンク３および８には、角速度センサ１２および１３のために信号線用のコネクタを設けておくことが好ましい。
【００６５】
尚、レートジャイロなどの角速度センサは、飛翔体の空中姿勢安定化や移動車の誘導制御に使われてきた。近年のＭＥＭＳ（Micro Electro Mechanical System）技術の進歩により、小型、軽量、高精度、安価なレートジャイロが入手可能になっている。
【図面の簡単な説明】
【００６６】
【図１】本発明に係る第１の実施形態に従ったロボット１００の断面図。
【図２】角度θA1およびθA2を算出する過程を示すフロー図。
【図３】伝達関数ＧM(ｓ)を同定するための制御線図。
【図４】同定された周波数応答特性を示すグラフ。
【符号の説明】
【００６７】
１００  ロボット
１  架台
２、７  軸
３、８  リンク
４、９  モータ
５、１１  減速機（トルク伝達機構）
６、１０  角度センサ
１２、１３  角速度センサ
【図１】
2005242794.tif 000030
【図２】
2005242794.tif 000031
【図３】
2005242794.tif 000032
【図４】
2005242794.tif 000033