(57)【要約】【課題】キャリブレーションを行う手間をかけることなく補正を行うことで、角速度センサーの検出値を正確に得ることが可能な角速度センサーの補正方法を提供する。【解決手段】リンク３の一連の動きにおいて、角度センサー６がアクチュエーター４の回転角を検出する回転角検出工程と、回転角検出工程の後に演算部１５が回転角を微分して基準の角速度を算出する基準の角速度演算工程と、回転角検出工程と同じ工程で、角速度センサー１２が基体１に対するリンクの角速度を検出するリンクの角速度検出工程と、基準の角速度演算工程の後に、リンクの角速度が基準の角速度に近づくように補正するリンクの角速度補正工程と、を有する。【選択図】図１
2011058820.tif 000002
【特許請求の範囲】
【請求項１】
基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターの回転角を検出する角度センサーと、前記基体に対する前記リンクの角速度を検出する角速度センサーと、前記回転角に基づいて基準の角速度を算出する演算部と、を有するロボットにおける角速度センサーの補正方法であって、
前記リンクが駆動を開始してから停止するまでの一連の動きにおいて、
前記角度センサーが前記アクチュエーターの回転角を検出する回転角検出工程と、
前記回転角検出工程の後に前記演算部が前記回転角を微分して前記基準の角速度を算出する基準の角速度演算工程と、
前記回転角検出工程と同じ工程で、前記角速度センサーが前記基体に対する前記リンクの角速度を検出するリンクの角速度検出工程と、
前記基準の角速度演算工程の後に、前記リンクの角速度が前記基準の角度に近づくように補正するリンクの角速度補正工程と、を有することを特徴とする角速度センサーの補正方法。
【請求項２】
前記リンクの角速度補正工程は、前記リンクの角速度に含まれる前記アクチュエーターからの入力成分とは無関係な出力成分であるバイアスを補正するバイアス補正工程と、前記リンクの角速度に含まれる温度特性を有するスケールファクタを補正するスケールファクタ補正工程と、を有し、
前記リンクの角速度補正工程において、前記バイアス補正工程と前記スケールファクタ補正工程とのうち少なくとも一方が行われることを特徴とする請求項１に記載の角速度センサーの補正方法。
【請求項３】
前記リンクが駆動を開始してから停止するまでの一連の動きには、加速運動区間、等速運動区間、減速運動区間の３つの運動区間を１サイクルとしたサイクル区間が少なくとも１サイクル以上含まれていることを特徴とする請求項１または２に記載の角速度センサーの補正方法。
【請求項４】
前記基準の角速度演算工程と前記リンクの角速度補正工程との間には、所定のサイクル数の前記サイクル区間において前記リンクの角速度と前記基準の角速度との差を算出する工程を有することを特徴とする請求項３に記載の角速度センサーの補正方法。
【請求項５】
前記リンクが前記加速運動区間を経過した後に、前記バイアス補正工程と前記スケールファクタ補正工程とが行われることを特徴とする請求項３または４に記載の角速度センサーの補正方法。
【請求項６】
前記リンクが前記等速運動区間を経過した後に、前記バイアス補正工程と前記スケールファクタ補正工程とが行われることを特徴とする請求項３〜５のいずれか１項に記載の角速度センサーの補正方法。
【請求項７】
前記リンクが前記減速運動区間を経過した後に、前記バイアス補正工程と前記スケールファクタ補正工程とが行われることを特徴とする請求項３〜６のいずれか１項に記載の角速度センサーの補正方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、角速度センサーの補正方法に関するものである。
【背景技術】
【０００２】
従来、加速度センサーや角速度センサー（例えばレートジャイロ）などの慣性センサーを用いて、多関節構造を有するロボットアーム（アーム）の振動を抑制して高精度な位置決めを行う技術が開発されている。
【０００３】
例えば、特許文献１では、２次モード以上の高周波成分を除去し１次モード周波数成分及び直流成分を通過させるローパスフィルタを用いている。すなわち、ローパスフィルタにより２次モード以上の加速度信号をカットし、２次モード以上の振動が励起されてアームが振動しまうことを抑制してアームの正確な位置決めを可能にしている。
また、特許文献２では、ワークの掴み部（チャック部）近傍に慣性センサーを設けてチャック部の動作（振れ状態）を検出している。そして、慣性センサー及び各アームの関節となる駆動体に接続された制御回路に指令信号が入力されることにより、各アームの駆動を制御してチャック部の振れの制振を行っている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開平１−１７３１１６号公報
【特許文献２】特開平７−９３７４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
アームの駆動中にアームを駆動するアクチュエーター（例えばモーター）を停止させた場合、アームにはわずかながら振動（揺らぎ）が発生する。この揺らぎを抑制する方法としては、例えばアクチュエーターの制御回路に対してアームの揺らぎを打ち消すような駆動信号を付与する方法がある。この方法を採用するためには、慣性センサーの検出値が正確であることが要求される。
【０００６】
しかしながら、特許文献１及び２では、慣性センサーとは別個の制御手段（例えばローパスフィルタや制御回路）によってアームやチャック部の振動を制御しているため、慣性センサー自体の検出値のバラツキを補正することはできない。一般に、慣性センサーにはアクチュエーターからの入力成分と無関係な出力成分（バイアス）や温度特性を有するスケールファクタが存在するため、時間とともに静止時のゼロ点（基準点）が変化する。特に、慣性センサーの検出値を積分して必要な値を求める場合（例えば角速度センサーの検出値を積分して角度を求める場合や加速度センサーの検出値を積分して速度または位置を求める場合）、必要な値にバイアスやスケールファクタによる誤差が含まれてしまうという問題がある。
【０００７】
通常、慣性センサーが工場から出荷される時には、慣性センサーの読み（検出値）が基準点からずれないようにキャリブレーションが行われる。例えば角速度センサーでは、工場出荷時に、角速度センサーの検出値にバイアス及びスケールファクタによる誤差が含まれないようキャリブレーションされる。しかしながら、バイアスは角速度センサーの電源をＯＮ／ＯＦＦする度に変化し、スケールファクタは角速度センサーが使用される雰囲気温度の影響を受けて変化してしまう。このため、角速度センサーの電源を入れる度にキャリブレーションを行う必要があり手間がかかる。
【０００８】
本発明はこのような事情に鑑みてなされたものであって、キャリブレーションを行う手間をかけることなく補正を行うことで、角速度センサーの検出値を正確に得ることが可能な角速度センサーの補正方法を提供することを目的とする。
【課題を解決するための手段】
【０００９】
上記の課題を解決するため、本発明の角速度センサーの補正方法は、基体に対して回転運動可能なリンクと、前記リンクを駆動するアクチュエーターと、前記アクチュエーターの回転角を検出する角度センサーと、前記基体に対する前記リンクの角速度を検出する角速度センサーと、前記回転角に基づいて基準の角速度を算出する演算部と、を有するロボットにおける角速度センサーの補正方法であって、前記リンクが駆動を開始してから停止するまでの一連の動きにおいて、前記角度センサーが前記アクチュエーターの回転角を検出する回転角検出工程と、前記回転角検出工程の後に前記演算部が前記回転角を微分して前記基準の角速度を算出する基準の角速度演算工程と、前記回転角検出工程と同じ工程で、前記角速度センサーが前記基体に対する前記リンクの角速度を検出するリンクの角速度検出工程と、前記基準の角速度演算工程の後に、前記リンクの角速度が前記基準の角度に近づくように補正するリンクの角速度補正工程と、を有することを特徴とする。
【００１０】
本発明の角速度センサーの補正方法によれば、リンクの駆動中において、角速度センサーがリンクの角速度を検出する工程と、角度センサーがアクチュエーターの回転角を検出する工程とが同一の工程で行われる。そして、アクチュエーターの回転角を微分して基準の角速度を算出する工程の後、つまり、リンクの角速度、基準の角速度が求められた後に、リンクの角速度が基準の角速度に近づくように補正が行われる。すなわち、本発明では、特許文献１及び２のように慣性センサーと別個の制御手段によってアームの振動を制御する方法とは異なり、リンクの駆動中において角速度センサー自体の検出値のバラツキが補正される。したがって、キャリブレーションを行う手間をかけることなく補正を行うことで、角速度センサーの検出値を正確に得ることが可能となる。
【００１１】
また、この補正方法においては、前記リンクの角速度補正工程は、前記リンクの角速度に含まれる前記アクチュエーターからの入力成分とは無関係な出力成分であるバイアスを補正するバイアス補正工程と、前記リンクの角速度に含まれる温度特性を有するスケールファクタを補正するスケールファクタ補正工程と、を有し、前記リンクの角速度補正工程において、前記バイアス補正工程と前記スケールファクタ補正工程とのうち少なくとも一方が行われてもよい。
【００１２】
この補正方法によれば、リンクの角速度補正工程において、バイアス補正工程とスケールファクタ補正工程とのうち少なくとも一方が行われる。このため、角速度センサーに存在するバイアスのランダムな時間変動が小さくなり、バイアス安定性が向上する。これにより、角速度センサーの検出値から必要な値（例えばリンクの角速度を積分して角度）を求める場合、必要な値にバイアスやスケールファクタによる誤差が含まれてしまうことが抑制できる。また、リンクの駆動中において角速度センサーに存在するバイアスの補正とスケールファクタの補正との補正が行われるので、角速度センサーの電源を入れる度にキャリブレーションを行う必要がない。したがって、キャリブレーションを行う手間をかけることなくバイアスの補正及びスケールファクタ補正を行うことで、角速度センサーの検出値を正確に得ることが可能となる。
【００１３】
また、この補正方法においては、前記リンクが駆動を開始してから停止するまでの一連の動きには、加速運動区間、等速運動区間、減速運動区間の３つの運動区間を１サイクルとしたサイクル区間が少なくとも１サイクル以上含まれていてもよい。
【００１４】
この補正方法によれば、補正を行うタイミングを、リンクの駆動中における各運動区間の中から適宜選択することができる。例えば、加速運動区間、等速運動区間、減速運動区間の３つの運動区間の中の少なくとも１つの運動区間を選択して補正を行ってもよいし、所定サイクルのサイクル区間で補正を行うこともできる。したがって、補正を行うタイミングの自由度を高くすることができる。
【００１５】
また、この補正方法においては、前記基準の角速度演算工程と前記バイアス補正工程との間に、所定のサイクル数の前記サイクル区間において前記リンクの角速度と前記基準の角速度との差を算出する工程を有していてもよい。
【００１６】
この補正方法によれば、リンクの角速度、基準の角速度が求められた後に、所定のサイクル区間におけるリンクの角速度と基準の角速度との差が求められるので、補正を行うタイミングを必要に応じてずらすことができる。例えば、リンクの角速度と基準の角速度との差が所定の範囲内であれば補正を行わないこととし、これに対して、その差が所定の範囲外であれば補正を行うこととすることができる。したがって、角速度センサーの検出値の高精度化を適宜選択的に行うことができる。
【００１７】
また、この補正方法においては、前記リンクが前記加速運動区間を経過した後に、前記バイアス補正工程と前記スケールファクタ補正工程とが行われてもよい。
【００１８】
この補正方法によれば、１サイクルのサイクル区間における加速運動区間の中で検出されたリンクの角速度と、アクチュエーターの回転角を微分して算出された基準の角速度と、に基づいてバイアス補正とスケールファクタ補正とが行われる。すると、これらの補正は加速運動区間の後の等速運動区間や減速運動区間に反映される。したがって、１サイクルまたは複数サイクルのサイクル区間経過後に一括して補正が行われるよりも、角速度センサーの検出値の高精度化を格段にスムーズに行うことができる。
【００１９】
また、この補正方法においては、前記リンクが前記等速運動区間を経過した後に、前記バイアス補正工程と前記スケールファクタ補正工程とが行われてもよい。
【００２０】
この補正方法によれば、１サイクルのサイクル区間における等速運動区間の中で検出されたリンクの角速度と、アクチュエーターの回転角を微分して算出された基準の角速度と、に基づいてバイアス補正とスケールファクタ補正とが行われる。すると、これらの補正は等速運動区間の後の減速運動区間、さらにその後の加速運動区間に反映される。したがって、１サイクルまたは複数サイクルのサイクル区間経過後に一括して補正が行われるよりも、角速度センサーの検出値の高精度化を格段にスムーズに行うことができる。
【００２１】
また、この補正方法においては、前記リンクが前記減速運動区間を経過した後に、前記バイアス補正工程と前記スケールファクタ補正工程とが行われてもよい。
【００２２】
この補正方法によれば、１サイクルのサイクル区間における減速運動区間の中で検出されたリンクの角速度と、アクチュエーターの回転角を微分して算出された基準の角速度と、に基づいてバイアス補正及びスケールファクタ補正が行われる。すると、これらの補正は減速運動区間の後の加速運動区間や等速運動区間に反映される。したがって、１サイクルまたは複数サイクルのサイクル区間経過後に一括して補正が行われるよりも、角速度センサーの検出値の高精度化を格段にスムーズに行うことができる。
【図面の簡単な説明】
【００２３】
【図１】本発明に係る角速度センサーが設けられたロボットを模式的に示す図である。
【図２】本発明に係る角速度センサーの補正方法のフローチャート図である。
【図３】本発明に係る角速度センサーの検出値と基準の角速度とを示すグラフである。
【発明を実施するための形態】
【００２４】
以下、図面を参照して、本発明の実施の形態について説明する。かかる実施の形態は、本発明の一態様を示すものであり、この発明を限定するものではなく、本発明の技術的思想の範囲内で任意に変更可能である。また、以下の図面においては、各構成をわかりやすくするために、実際の構造と各構造における縮尺や数等が異なっている。
【００２５】
図１は、本発明の一実施形態における角速度センサー１２，１３が設けられたロボット１００の模式断面図である。図１に示すように、ロボット１００は、架台（基体）１と、リンク３，８と、モーター（アクチュエーター）４，９と、減速機５，１１と、角度センサー６，１０と、角速度センサー１２，１３と、演算部１５と、を備えて構成されている。
【００２６】
架台１の上部にはリンク３の一端が取り付けられており、リンク３の他端にはリンク８の一端が取り付けられている。リンク３は、軸２を中心として架台１に対して相対的に回転運動可能になっている。リンク８は、軸７を中心としてリンク３に対して相対的に回転運動可能になっている。
【００２７】
モーター４は、減速機５を介してリンク３を駆動するものである。モーター９は、減速機１１を介してリンク８を駆動するものである。
【００２８】
角度センサー６は、モーター４の頂部に設けられている。角度センサー６は、軸２の回りにおけるモーター４の回転角Θ１を検出する機能を有する。角度センサー１０は、モーター９の頂部に設けられている。角度センサー１０は、軸７の回りにおけるモーター９の回転角Θ２を検出する機能を有する。
【００２９】
角速度センサー１２は、リンク３の他端に設けられている。角速度センサー１２は、架台１に対するリンク３の角速度ｇ１を検出する機能を有する。角速度センサー１３は、リンク８の他端に設けられている。角速度センサー１３は、リンク３に対するリンク８の角速度ｇ２を検出する機能を有する。なお、架台１に対するリンク３の角速度ｇ１は、リンク３の回転面内の角速度成分である。また、リンク３に対するリンク８の角速度ｇ２は、リンク８の回転面内の角速度成分である。
【００３０】
演算部１５は、架台１の近傍に設けられている。演算部１５は、角度センサー６，１０でそれぞれ検出されたモーター４，９の回転角Θ１，Θ２を微分して、モーター４，９における基準の角速度ｒ１，ｒ２を算出する機能を有する。また、演算部１５は、角度センサー６，１０で検出された回転角Θ１，Θ２、角速度センサー１２，１３で検出されたリンク３,８の角速度ｇ１，ｇ２を記憶する記憶部としての機能をも有している。
【００３１】
なお、本実施形態においては、リンク、モーター、減速機、角度センサー、角速度センサーがそれぞれ２つ設けられているが、これに限らない。例えば、リンク、モーター、減速機、角度センサー、角速度センサーがそれぞれ１つずつ設けられていてもよく、あるいは３つ以上など任意の数が設けられていてもよい。
【００３２】
また、角速度センサー１２，１３は、それぞれリンク３，８の先端に配置されているが、これに限らない。例えば、角速度センサー１２，１３は、それぞれリンク３，８の任意の位置に配置されていてもよい。また、演算部１５は、架台１の近傍に配置されているが、これに限らない。例えば、演算部１５は、角度センサー６，１０の内部など任意の位置に配置されていてもよい。
【００３３】
また、演算部１５は回転角Θ１，Θ２を微分して基準の角速度ｒ１，ｒ２を算出する機能を有するとともに、回転角Θ１，Θ２、リンクの角速度ｇ１，ｇ２を記憶する機能をも有しているが、これに限らない。例えば、演算部１５とは別個に、回転角Θ１，Θ２、リンクの角速度ｇ１，ｇ２を記憶する記憶部が設けられていてもよい。
【００３４】
（角速度センサーの補正方法）
次に、本実施形態における角速度センサー１２，１３の補正方法を説明する。図２は、リンクが駆動を開始してから停止するまでの一連の動きにおいて、角速度センサーの検出値ｇ（角速度センサーで検出されたリンクの角速度）を補正する過程を示すフローチャート図である。なお、ステップＳ２及びＳ５の記憶処理、ステップＳ６〜Ｓ８の演算処理は、ロボット１００の架台１近傍に配置された演算部１５が実行してもよく、または、ロボット１００の外部に設けられたＣＰＵ（図示略）が実行してもよい。
【００３５】
先ず、角速度センサー１２によって架台１に対するリンク３の角速度ｇ１が検出される。また、角速度センサー１３によってリンク３に対するリンク８の角速度ｇ２が検出される（リンクの角速度検出工程（Ｓ１））。これら角速度センサー１２，１３で検出されたリンク３，８の角速度ｇ１，ｇ２は演算部１５に入力され記憶される（Ｓ２）。
【００３６】
一方、リンクの角速度検出工程（Ｓ１）と同じ工程で、角度センサー６によって軸２の回りにおけるモーター４の回転角Θ１が検出される。また、角度センサー１０によって軸７の回りにおけるモーター９の回転角Θ２が検出される（回転角検出工程（Ｓ３））。そして、これら角度センサー６，１０で検出されたモーター４，９の回転角Θ１，Θ２は演算部１５に入力される。すると、演算部１５によって、角度センサー６，１０で検出されたモーター４，９の回転角Θ１，Θ２がそれぞれ微分されて基準の角速度ｒ１，ｒ２が算出される（基準の角速度演算工程（Ｓ４））。これら基準の角速度ｒ１，ｒ２は、演算部１５で記憶される（Ｓ５）。
【００３７】
ところで、リンクの駆動中にリンクを減速機を介して駆動するモーターを停止させた場合、リンクにはわずかながら振動（揺らぎ）が発生する。この揺らぎを抑制する方法としては、例えばモーターの制御回路に対してリンクの揺らぎを打ち消すような駆動信号を付与する方法がある。この方法を採用するためには、角速度センサーの検出値が正確であることが要求される。
【００３８】
一般に、角速度センサーにはモーターからの入力成分と無関係な出力成分（バイアス）が存在する。このため、時間とともに静止時のゼロ点（基準点）が変化する。また、角速度センサーには温度特性を有するスケールファクタも存在する。このスケールファクタは、角速度センサーを使用する雰囲気の温度の影響を受けて変動する。
【００３９】
ここで、角速度センサーの検出値ｇ（角速度センサー１２，１３で検出されたリンクの角速度ｇ１，ｇ２）について、基準の角速度ｒ（回転角Θ１，Θ２を微分して求められた基準の角速度ｒ１，ｒ２）と、バイアスＢと、スケールファクタＳＦと、の関係で表すと、以下の式（１）が成立する。
【００４０】
ｇ＝ＳＦ・ｒ＋Ｂ  ……（１）
【００４１】
式（１）より、角速度センサーの検出値（リンクの角速度）ｇと基準の角速度ｒとの間には、バイアスＢと、スケールファクタＳＦとにより差が生じることが確認される。以下、バイアスＢと、スケールファクタＳＦとにより生じた角速度センサーの検出値ｇと基準の角速度ｒとの差を小さくするための工程について説明する。
【００４２】
ステップＳ２及びＳ５を経た後、つまり、演算部１５において角速度ｇ１，ｇ２及び基準の角速度ｒ１，ｒ２が記憶された後、演算部１５によって、角速度センサー１２，１３で検出されたリンク３，８の角速度ｇ１，ｇ２と基準の角速度ｒ１，ｒ２との差（ｇ１−ｒ１），（ｇ２−ｒ２）が算出される（Ｓ６）。
【００４３】
次に、演算部１５によって、所定のサイクル区間（例えば、加速運動区間Ｌ１、等速運動区間Ｌ２、減速運動区間Ｌ３を経た１サイクル区間）におけるリンクの角速度ｇ１，ｇ２と基準の角速度ｒ１，ｒ２との差（ｇ１−ｒ１），（ｇ２−ｒ２）が所定の範囲内（±α以内）に入るか否かが演算される（Ｓ７）。なお、αの値は角速度センサー１２，１３の線形性能を表す指標から決定することができる（例えばα＝０．５）。
【００４４】
リンクの角速度ｇ１，ｇ２と基準の角速度ｒ１，ｒ２との差（ｇ１−ｒ１），（ｇ２−ｒ２）が所定の範囲内に入っているときは、ステップＳ６に戻る。一方、リンクの角速度ｇ１，ｇ２と基準の角速度ｒ１，ｒ２との差（ｇ１−ｒ１），（ｇ２−ｒ２）が所定の範囲内に入っていないときは、次のステップ７に進む。すなわち、リンクの角速度と基準の角速度との差が所定の範囲内であれば補正を行わないこととし、これに対して、その差が所定の範囲外であれば補正を行うこととすることができる。
【００４５】
次に、演算部１５によって、角速度センサーが検出した複数のリンクの角速度のデータプロットＰ（図３参照）について最小二乗法が適用される（Ｓ８）。これにより、複数のリンクの角速度のデータプロットＰが近似される（図３に示すＨ１）。なお、角速度センサーが検出した複数のデータプロットＰを近似する方法は最小二乗法に限らず、例えばカルマンフィルタ法を用いることもできる。
【００４６】
次に、演算部１５によって、所定のサイクル区間においてリンクの角速度に存在するバイアス、スケールファクタが推定される（Ｓ９）。
【００４７】
そして、演算部１５によって、第１のバイアスと第２のバイアスとの間の差が小さくなるように補正するバイアス補正と、第１のスケールファクタと第２のスケールファクタとの間の差が小さくなるように補正するスケールファクタ補正と、のうち少なくとも一方の補正が行われる（Ｓ１０）。以下、図３を用いてステップＳ９及びＳ１０の工程を説明する。
【００４８】
一般に、リンク３，８が駆動を開始してから停止するまでの一連の動きには、リンク３，８が駆動を開始してから一定の速度になるまでの加速運動区間、リンク３，８が一定の速度で駆動し続ける等速運動区間、リンク３，８が減速して停止するまでの減速運動区間の３つの運動区間が含まれる。
【００４９】
図３は、リンク３が駆動を開始してから停止するまでの一連の動きにおいて、角速度センサー１２が検出したリンク３の角速度ｇ１と、角度センサー６で検出されたモーター４の回転角Θ１が微分されて求められた基準の角速度ｒ１とを示したグラフである。図３は、横軸に時間ｔ、縦軸に角速度ｇ１を示したグラフである。
【００５０】
図３において、符号Ｌ１は加速運動区間（時間範囲０＜ｔ＜ｔ１）、符号Ｌ２は等速運動区間（時間範囲ｔ１＜ｔ＜ｔ２）、符号Ｌ３は減速運動区間（時間範囲ｔ２＜ｔ＜ｔｅ）である。また、符号Ｐは、角速度センサー１２が検出した複数のデータプロット（リンク３の角速度ｇ１）である。また、符号Ｈ１は、リンク３が駆動を開始してから停止するまでの一連の動きにおいて、角速度センサー１２が検出したリンク３の角速度ｇ１を示したグラフである。一方、符号Ｈ２は、角度センサー６で検出されたモーター４の回転角Θ１が微分されて求められた基準の角速度ｒ１を示したグラフである。
【００５１】
なお、本図では、これら３つの運動区間を１サイクルとしたサイクル区間が１サイクル含まれている例を示している。また、角速度センサー１２が検出したリンク３の角速度ｇ１を一例として挙げているが、角速度センサー１３が検出したリンク８の角速度ｇ２についても同様に適用可能である。また、角度センサー６で検出されたモーター４の回転角Θ１が微分されて求められた基準の角速度ｒ１を一例として挙げているが、角度センサー１０で検出されたモーター９の回転角Θ２が微分されて求められた基準の角速度ｒ２についても同様に適用可能である。
【００５２】
図３に示すように、加速運動区間Ｌ１において複数の角速度のデータプロットＰについて最小二乗法が適用されると、時間と角速度との関係の軌跡は原点を通らない右上がりの直線となる。加速運動区間Ｌ１におけるリンクの角速度ｇ１１は、傾きをａ１（正の整数）、切片をｂ１（正の整数）とすると、以下の式（２）で表される。
【００５３】
ｇ１１＝ａ１・ｔ＋ｂ１  ……（２）
【００５４】
また、等速運動区間Ｌ２において複数の角速度のデータプロットＰについて最小二乗法が適用されると、時間と角速度との関係の軌跡は原点を通らない右上がりの直線となる。等速運動区間Ｌ２におけるリンクの角速度ｇ１２は、傾きをａ２（正の整数）、切片をｂ２（正の整数）とすると、以下の式（３）で表される。
【００５５】
ｇ１２＝ａ２・ｔ＋ｂ２  ……（３）
【００５６】
また、減速運動区間Ｌ３において複数の角速度のデータプロットＰについて最小二乗法が適用されると、時間と角速度との関係の軌跡は原点を通らない右下がりの直線となる。減速運動区間Ｌ３におけるリンクの角速度ｇ１３は、傾きをａ３（負の整数）、切片をｂ３（正の整数）とすると、以下の式（４）で表される。
【００５７】
ｇ１３＝ａ３・ｔ＋ｂ３  ……（４）
【００５８】
これらの式（１）〜（４）により、バイアスＢと、スケールファクタＳＦとを推定することができる。具体的には、式（１）と式（２）の関係より、加速運動区間Ｌ１では、傾きａ１をスケールファクタＳＦと、切片ｂ１をバイアスＢと推定することができる。また、式（１）と式（３）の関係より、等速運動区間Ｌ２では、傾きａ２をスケールファクタＳＦと、切片ｂ２をバイアスＢと推定することができる。また、式（１）と式（４）の関係より、減速運動区間Ｌ３では、傾きａ３をスケールファクタＳＦと、切片ｂ３をバイアスＢと推定することができる。
【００５９】
したがって、式（１）〜（４）により、所定のサイクル区間においてリンクの角速度に存在するスケールファクタＳＦとバイアスＢとが推定されることになる（Ｓ９）。
【００６０】
一方、基準の角速度を見ると、加速運動区間Ｌ１においては、時間と基準の角速度との関係の軌跡は原点を通る右上がりの直線となる。加速運動区間Ｌ１における基準の角速度ｒ１１は、傾きをｃ１（正の整数）、切片をｄ１（ｄ１＝０）とすると、以下の式（５）で表される。
【００６１】
ｒ１１＝ｃ１・ｔ  ……（５）
【００６２】
また、等速運動区間Ｌ２においては、時間と基準の角速度との関係の軌跡は横軸に平行な直線となる。等速運動区間Ｌ２における基準の角速度ｒ１２は、傾きをｃ２（ｃ２＝０）、切片をｄ２（正の整数）とすると、以下の式（６）で表される。
【００６３】
ｒ１２＝ｄ２  ……（６）
【００６４】
また、減速運動区間Ｌ３においては、時間と基準の角速度との関係の軌跡は原点を通らない右下がりの直線となる。減速運動区間Ｌ３における基準の角速度ｒ１３は、傾きをｃ３（負の整数）、切片をｄ３（正の整数）とすると、以下の式（７）で表される。
【００６５】
ｒ１３＝ｃ３・ｔ＋ｄ３  ……（７）
【００６６】
式（２）と式（５）の関係から、式（２）に対して傾きａ１とｃ１との比（ｃ１／ａ１）を乗じることにより、リンクの角速度におけるスケールファクタＳＦに替えて基準の角速度における基準値ｃ１とすることができる。また、式（２）に対して切片ｂ１とｄ１（ｄ１＝０）との比（ｄ１／ｂ１＝０）を乗じることにより、リンクの角速度におけるバイアスＢに替えて基準の角速度における基準値ｄ１（ｄ１＝０）とすることができる。
【００６７】
また、式（３）と式（６）の関係から、式（３）に対して傾きａ２とｃ２（ｃ２＝０）との比（ｃ２／ａ２＝０）を乗じることにより、リンクの角速度におけるスケールファクタＳＦに替えて基準の角速度における基準値ｃ２（ｃ２＝０）とすることができる。また、式（３）に対して切片ｂ２とｄ２との比（ｄ２／ｂ２）を乗じることにより、リンクの角速度におけるバイアスＢに替えて基準の角速度における基準値ｄ２とすることができる。
【００６８】
また、式（４）と式（７）の関係から、式（４）に対して傾きａ３とｃ３との比（ｃ３／ａ３）を乗じることにより、リンクの角速度におけるスケールファクタＳＦに替えて基準の角速度における基準値ｃ３とすることができる。また、式（４）に対して切片ｂ３とｄ３との比（ｄ３／ｂ３）を乗じることにより、リンクの角速度におけるバイアスＢに替えて基準の角速度における基準値ｄ３とすることができる。
【００６９】
したがって、式（２）〜（７）により、リンクの角速度におけるバイアスＢと基準の角速度における基準値との間の差が小さくなるように補正するバイアス補正と、角速度におけるスケールファクタＳＦと基準の角速度における基準値との間の差が小さくなるように補正するスケールファクタ補正とが行われることになる（Ｓ１０）。以上の工程により、補正されたバイアス及びスケールファクタは演算部１５において補正前のデータと書き換えられる。そして、ステップＳ１の角速度検出工程及びステップＳ３の回転角検出工程に戻り、Ｓ１〜Ｓ１０の工程が繰りかえされ、バイアスＢとスケールファクタＳＦとの補正が行われる。
【００７０】
本発明の角速度センサーの補正方法によれば、リンク３，８の駆動中において、角速度センサー１２，１３がリンクの角速度ｇ１，ｇ２を検出する工程と、角度センサー６，１０がモーターの回転角Θ１，Θ２を検出する工程とが同一の工程で行われる。そして、モーターの回転角Θ１，Θ２を微分して基準の角速度ｒ１，ｒ２を算出する工程の後、つまり、リンクの角速度ｇ１，ｇ２、基準の角速度ｒ１，ｒ２が求められた後に、リンクの角速度ｇ１，ｇ２が基準の角速度ｒ１，ｒ２に近づくように補正が行われる。すなわち、本発明では、特許文献１及び２のように慣性センサーと別個の制御手段によってアームの振動を制御する方法とは異なり、リンク３，８の駆動中において角速度センサー１２，１３自体の検出値のバラツキが補正される。したがって、キャリブレーションを行う手間をかけることなく補正を行うことで、角速度センサー１２，１３の検出値を正確に得ることが可能となる。
【００７１】
また、この補正方法によれば、リンクの角速度補正工程において、バイアス補正工程とスケールファクタ補正工程とのうち少なくとも一方が行われる。このため、角速度センサー１２，１３に存在するバイアスＢのランダムな時間変動が小さくなり、バイアス安定性が向上する。これにより、角速度センサー１２，１３の検出値から必要な値（例えばリンクの角速度を積分して角度）を求める場合、必要な値にバイアスＢやスケールファクタＳＦによる誤差が含まれてしまうことが抑制できる。また、リンク３，８の駆動中において、角速度センサー１２，１３に存在するバイアスＢの補正とスケールファクタＳＦの補正とが行われるので、角速度センサー１２，１３の電源を入れる度にキャリブレーションを行う必要がない。したがって、キャリブレーションを行う手間をかけることなくバイアス補正及びスケールファクタ補正を行うことで、角速度センサー１２，１３の検出値を正確に得ることが可能となる。
【００７２】
また、この補正方法によれば、角速度センサー１２，１３の補正を行うタイミングを、リンク３，８の駆動中における各運動区間の中から適宜選択することができる。例えば、加速運動区間Ｌ１、等速運動区間Ｌ２、減速運動区間Ｌ３の３つの運動区間の中の少なくとも１つの運動区間を選択して補正を行ってもよいし、所定サイクルのサイクル区間で補正を行うこともできる。したがって、角速度センサー１２，１３の補正を行うタイミングの自由度を高くすることができる。
【００７３】
また、この補正方法によれば、リンク３，８の角速度ｇ１，ｇ２、基準の角速度ｒ１，ｒ２が求められた後に、所定のサイクル区間における角速度ｇ１，ｇ２と基準の角速度ｒ１，ｒ２との差（ｇ１−ｒ１），（ｇ２−ｒ２）が求められるので、角速度センサー１２，１３の補正を行うタイミングを必要に応じてずらすことができる。例えば、角速度ｇ１，ｇ２と基準の角速度ｒ１，ｒ２との差（ｇ１−ｒ１），（ｇ２−ｒ２）が所定の範囲内であれば補正を行わないこととし、これに対して、その差が所定の範囲外であれば補正を行うこととすることができる。したがって、角速度センサー１２，１３の検出値の高精度化を適宜選択的に行うことができる。
【００７４】
なお、本実施形態の角速度センサーの補正方法によれば、リンク３が駆動を開始してから停止するまでの一連の動きにおいて、加速運動区間Ｌ１、等速運動区間Ｌ２、減速運動区間Ｌ３の運動区間を１サイクルとしたサイクル区間が１サイクル含まれている例を挙げて説明したが、これに限らない。例えば、これら３つの運動区間を１サイクルとしたサイクル区間が複数サイクル含まれていてもよい。
【００７５】
また、この補正方法においては、リンク３，８が加速運動区間Ｌ１を経過した後に、バイアス補正とスケールファクタ補正とが行われてもよい。これにより、１サイクルのサイクル区間における加速運動区間Ｌ１の中で検出されたリンクの角速度ｇ１，ｇ２と、モーターの回転角Θ１，Θ２を微分して算出された基準の角速度ｒ１，ｒ２と、に基づいてバイアス補正とスケールファクタ補正とが行われる。すると、この補正は加速運動区間Ｌ１の後の等速運動区間Ｌ２や減速運動区間Ｌ３に反映される。
【００７６】
また、この補正方法においては、リンク３，８が等速運動区間Ｌ２を経過した後に、バイアス補正とスケールファクタ補正とが行われてもよい。これにより、１サイクルのサイクル区間における等速運動区間Ｌ２の中で検出されたリンクの角速度ｇ１，ｇ２と、モーターの回転角Θ１，Θ２を微分して算出された基準の角速度ｒ１，ｒ２と、に基づいてバイアス補正とスケールファクタ補正とが行われる。すると、この補正は等速運動区間Ｌ２の後の減速運動区間Ｌ３、さらにその後の加速運動区間Ｌ１に反映される。
【００７７】
また、この補正方法においては、リンク３，８が減速運動区間Ｌ３を経過した後に、バイアス補正とスケールファクタ補正とが行われてもよい。これにより、１サイクルのサイクル区間における減速運動区間Ｌ３の中で検出された角速度ｇ１，ｇ２と、モーターの回転角Θ１，Θ２を微分して算出された基準の角速度ｒ１，ｒ２と、に基づいてバイアス補正及びスケールファクタ補正が行われる。すると、これらの補正は減速運動区間Ｌ３の後の加速運動区間Ｌ１や等速運動区間Ｌ２に反映される。
【００７８】
このように、各運動区間を経過した後にバイアス補正とスケールファクタ補正との補正を行うことで、１サイクルまたは複数サイクルのサイクル区間経過後に一括して補正が行われるよりも、角速度センサー１２，１３の検出値の高精度化を格段にスムーズに行うことができる。
【符号の説明】
【００７９】
１…架台（基体）、３，８…リンク、４，９…モーター（アクチュエーター）、６，１０…角度センサー、１２，１３…角速度センサー、１００…ロボット、ｇ…角速度、ｒ…基準の角速度、Ｂ…バイアス、Ｌ１…加速運動区間、Ｌ２…等速運動区間、Ｌ３…減速運動区間、ＳＦ…スケールファクタ、Θ１，Θ２…回転角
【図１】
2011058820.tif 000003
【図２】
2011058820.tif 000004
【図３】
2011058820.tif 000005