ロボット及びロボットのノイズ除去方法
【課題】慣性センサーを用いたフィードバック系において、発振してしないロボットを提供する。
【解決手段】アーム１０１と、アームを回動させるモーター１０３とトルク伝達機構１０２とを含むアーム連結部１０４と、アーム１０１の端部のアーム連結部１０４に連結された基体１０５と、モーター１０３の回動角度を検出しモーター１０３の回動角度情報を出力する角度センサー１０６と、アーム１０１に作用する慣性力の情報を出力する慣性センサー１０７と、アーム１０１の回動動作を制御する制御部１０８と、角度センサー１０６の出力と慣性センサー１０７の出力とから慣性センサー１０７のノイズ周波数を検出するノイズ検出部１０９と、ノイズ検出部１０９の情報からフィルター１１１の特性を決めるフィルター定数決定部１１０と、フィルター定数決定部１１０に基づき慣性センサー１０７のノイズを除去するフィルター１１１とを備える。
【解決手段】アーム１０１と、アームを回動させるモーター１０３とトルク伝達機構１０２とを含むアーム連結部１０４と、アーム１０１の端部のアーム連結部１０４に連結された基体１０５と、モーター１０３の回動角度を検出しモーター１０３の回動角度情報を出力する角度センサー１０６と、アーム１０１に作用する慣性力の情報を出力する慣性センサー１０７と、アーム１０１の回動動作を制御する制御部１０８と、角度センサー１０６の出力と慣性センサー１０７の出力とから慣性センサー１０７のノイズ周波数を検出するノイズ検出部１０９と、ノイズ検出部１０９の情報からフィルター１１１の特性を決めるフィルター定数決定部１１０と、フィルター定数決定部１１０に基づき慣性センサー１０７のノイズを除去するフィルター１１１とを備える。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボット及びロボットのノイズ除去方法にかかわり、特に、ロボットの制御方法に関する。
【背景技術】
【０００２】
ロボットのアームを動作させて停止するとき、アームが振動する。アームが振動すると位置精度良く作業できないので、振動を停止するまで待機することがある。そして、アームの速度を早くするとアームの停止時の振動が大きくなるので、振動が停止するまでの時間が長くなる。そこで、例えば、特許文献１にロボットに慣性センサーを用いることで残留振動を減少させる方法が開示されている。この残留振動はモーターとアーム先端との間に大きな弾性があるために生じる。エンコーダーに代表される位置センサーだけでもモーターの回転情報は取得できるが、慣性センサーを取り付けることでアーム先端の情報はより正確に取得できる。この情報を制御部にフィードバックすることで残留振動を減少させていた。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第３８８３５４４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
慣性センサーを使用するにはノイズを考慮する必要がある。特に振動式のジャイロセンサーでは機械的振動が加わると非常に大きな離調周波数ノイズが発生する。慣性センサーの出力を用いてアームを制御するフィードバック系を構築するときフィードバック系が発振してロボットが動作しなくなることがある。そこで、慣性センサーを含んだフィードバック系の発振を防ぐロボットが求められていた。
【課題を解決するための手段】
【０００５】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００６】
［適用例１］本適用例にかかるロボットは、アームと、前記アームと接続し前記アームを回動または並進させる駆動源を有するアーム連結部と、前記アーム連結部と接続する基体と、前記駆動源の回動角度を検出する角度センサーと、前記アームに取り付けられ前記アームに作用する慣性力を検出する慣性センサーと、前記慣性センサーの出力からノイズを除去するノイズフィルターと、前記アームの回動動作を制御する制御部と、前記角度センサーの出力と前記慣性センサーの出力とから前記慣性センサーのノイズ周波数を検出するノイズ検出部と、前記ノイズフィルターがノイズを除去するノイズフィルター特性を、前記ノイズ周波数を用いて決めるフィルター定数決定部と、を備えることを特徴とする。
【０００７】
本適用例によれば、基体にアーム連結部を介してアームが接続されている。アーム連結部は駆動源を有し、駆動源がアームを回動または並進させる。制御部は駆動源を制御することによりアームの回動動作を制御する。駆動源には角度センサーが設置され、角度センサーが駆動源の回転角度を検出する。制御装置は駆動源の回転角度の情報を元にアームの回動状態を認識する。
【０００８】
アームには慣性センサーが取り付けられている。そして、制御装置は慣性センサーの出力を用いてアームの回動状況を認識する。ノイズ検出部は角度センサーの出力と慣性センサーの出力とを用いて慣性センサーに固有なノイズにおけるノイズ周波数を検出する。フィルター定数決定部はノイズ周波数を用いてノイズフィルター特性を決定する。このノイズフィルター特性は慣性センサーに固有な特性に対応して決定される。慣性センサーにはノイズフィルターが接続され、ノイズフィルター特性に従ってノイズフィルターは慣性センサーの出力からノイズを除去する。従って、制御部はノイズが軽減された慣性センサーの出力を用いてアームを制御することができる。その結果、制御部は、慣性センサーを含んだフィードバック系の発振を防ぐことができる。
【０００９】
［適用例２］上記適用例に記載のロボットでは、前記ノイズ検出部は前記角度センサーが出力する信号の周波数分布と、前記慣性センサーが出力する信号の周波数分布とを用いて前記慣性センサーが出力する信号のノイズ周波数を検出することが好ましい。
【００１０】
本適用例によれば、角度センサーが出力する信号の周波数分布と慣性センサーが出力する信号の周波数分布とを用いている。従って、慣性センサーが検出する信号の振動周波数分布からノイズ成分の信号の周波数分布を分離できる。その結果、ノイズ周波数を精度よく特定できる。
【００１１】
［適用例３］上記適用例に記載のロボットでは、前記ノイズフィルターは帯域除去フィルターであることが好ましい。
【００１２】
本適用例によれば、除去すべき周波数以外の帯域では位相の変化を小さくできる。そして、慣性センサーが出力する信号からノイズ成分の周波数の帯域の信号を除去することができる。
【００１３】
［適用例４］上記適用例に記載のロボットでは、前記フィルター定数決定部は前記ノイズ周波数とテーブルとを参照してフィルター定数を決定することが好ましい。
【００１４】
本適用例によれば、複雑な計算を要することなくフィルター定数を決定できるので、計算にかかる負荷を少なくすることができる。
【００１５】
［適用例５］上記適用例に記載のロボットでは、前記ノイズフィルターは前記制御部にソフトウェアで実装されていることが好ましい。
【００１６】
本適用例によれば、ノイズフィルターを構成する電子部品等は必要なく、ハンダ付け等の工数も削減できる。従って、生産性良くロボットを製造することができる。
【００１７】
［適用例６］本適用例にかかるロボットは、アームと、前記アームと接続され前記アームを回動または並進させる駆動源を有するアーム連結部とが交互に連結されたアーム体と、前記アーム体と接続する基体と、前記駆動源の回動角度を検出する複数の角度センサーと、前記アームに作用する慣性力を検出する複数の慣性センサーと、前記慣性センサーの出力からノイズを除去する複数のノイズフィルターと、前記アームの回動動作を制御する制御部と、複数の前記角度センサーの出力と複数の前記慣性センサーの出力とから複数の前記慣性センサーのノイズ周波数を検出するノイズ検出部と、前記ノイズフィルターがノイズを除去するノイズフィルター特性を、前記ノイズ周波数を用いて決めるフィルター定数決定部と、を備えることを特徴とする。
【００１８】
本適用例によれば、ロボットは、アームとアーム連結部とが交互に連結されたアーム体を備えている。これにより、アームは自在に動くことができる。そして、アームは複数設置され、アームに作用する慣性力を検出する慣性センサーが複数設置されている。アーム連結部は複数設置され、アーム体には角度センサーが複数設置されている。
【００１９】
ノイズ検出部は複数の角度センサーの出力と複数の慣性センサーの出力とから慣性センサーのノイズ周波数を検出する。そして、フィルター定数決定部がノイズ周波数を用いて各慣性センサーと対応するノイズフィルター特性を決める。従って、ノイズフィルターは各慣性センサーのノイズを除去することができる。その結果、アームとアーム連結部とが交互に連結されたアーム体を備えたロボットにおいても、制御部は慣性センサーを含んだフィードバック系の発振を防ぐことができる。
【００２０】
［適用例７］上記適用例に記載のロボットでは、前記ノイズ検出部は前記角度センサーが出力する信号の周波数分布と、前記慣性センサーが出力する信号の周波数分布と、を用いて前記慣性センサーが出力する信号のノイズ周波数を検出することが好ましい。
【００２１】
本適用例によれば、角度センサーが出力する信号の周波数分布と慣性センサーが出力する信号の周波数分布とを用いている。従って、慣性センサーが検出する信号の振動周波数分布からノイズ成分の信号の周波数分布を分離できる。その結果、ノイズ周波数を精度よく特定できる。
【００２２】
［適用例８］上記適用例に記載のロボットでは、前記ノイズフィルターは帯域除去フィルターであることが好ましい。
【００２３】
本適用例によれば、除去すべき周波数以外の帯域では位相の変化が小さくなる。従って、慣性センサーが出力する信号からノイズ成分の周波数の帯域の信号を除去することができる。
【００２４】
［適用例９］上記適用例に記載のロボットでは、前記フィルター定数決定部は前記ノイズ周波数とテーブルとを参照してフィルター定数を決定することが好ましい。
【００２５】
本適用例によれば、複雑な計算を要することなくフィルター定数を決定できるので、計算にかかる負荷を少なくすることができる。
【００２６】
［適用例１０］上記適用例に記載のロボットでは、前記ノイズフィルターは前記制御部にソフトウェアで実装されていることが好ましい。
【００２７】
本適用例によれば、ノイズフィルターを構成する電子部品等は必要なく、ハンダ付け等の工数も削減できる。従って、生産性良くロボットを製造することができる。
【００２８】
［適用例１１］本適用例にかかるロボットのノイズ除去方法は、駆動源の回動角度を検出する駆動源角度検出工程と、慣性センサーを用いてアームに作用する慣性力を検出するアーム慣性力検出工程と、前記駆動源角度検出工程にて検出した前記回動角度の信号と前記アーム慣性力検出工程にて検出した前記慣性力の信号とを用いて前記慣性センサーのノイズ周波数を検出するノイズ成分検出工程と、前記検出した情報からノイズを除去するノイズフィルター特性を決定するフィルター定数決定工程と、前記ノイズフィルター特性に基づいたノイズフィルターでノイズを除去するノイズ除去工程と、を有することを特徴とする。
【００２９】
本適用例によれば、駆動源角度検出工程にて駆動源の回動角度を検出している。そして、アーム慣性力検出工程にて慣性センサーを用いてアームに作用する慣性力を検出している。続いて、ノイズ成分検出工程では駆動源角度検出工程にて検出した回動角度の信号とアーム慣性力検出工程にて検出した慣性力の信号とを用いて慣性センサーのノイズ周波数を検出している。次に、フィルター定数決定工程では検出した情報からノイズを除去する特性であるノイズフィルター特性を決定している。ノイズ除去工程ではノイズフィルター特性に基づいたノイズフィルターでノイズを除去している。
【００３０】
従って、ノイズフィルター特性は慣性センサーに固有のノイズの特性となるように設定することができる為、ノイズ除去工程では精度良くノイズを除去することができる。その結果、慣性センサーを含んだフィードバック系の発振を防ぐことができる。
【図面の簡単な説明】
【００３１】
【図１】実施形態１にかかるロボットの概略構成を示すブロック図。
【図２】センサー出力信号からノイズを除去する工程を示すフローチャート。
【図３】センサー出力信号からノイズを除去する工程を説明するための図。
【図４】センサー出力信号からノイズを除去する工程を説明するための図。
【図５】実施形態２にかかる概略構成を示すブロック図。
【図６】センサー出力信号からノイズを除去する工程を説明するための図。
【発明を実施するための形態】
【００３２】
以下、本発明の実施形態について、図面を参照して説明する。なお、以下の各図においては、各層や各部材を認識可能な程度の大きさにするため、各層や各部材の尺度を実際とは異ならせしめている。
【００３３】
（実施形態１）
本実施形態では、ロボットとロボットが行う特徴的なアームの制御方法について、図１〜図４に従って説明する。図１は、実施形態１にかかるロボットの概略構成を示すブロック図である。
【００３４】
図１に示すようにロボット１００は基体１０５を備えている。基体１０５の内部にはアーム連結部１０４が設置されている。アーム連結部１０４は角度センサー１０６、駆動源としてのモーター１０３、トルク伝達機構１０２がこの順に同軸上に配置されている。モーター１０３の出力軸の一端は角度センサー１０６と接続されている。角度センサー１０６はエンコーダーを備えモーター１０３の回動角度を検出する。モーター１０３の出力軸の他端はトルク伝達機構１０２の入力軸と接続されている。トルク伝達機構１０２は所定の減速比に設定された減速機である。従って、トルク伝達機構１０２の出力軸の回動角度はモーター１０３の回動角度を減速比で除算した値となる。
【００３５】
トルク伝達機構１０２の出力軸はアーム１０１の一端と接続されている。これにより、モーター１０３が回転することによりアーム１０１が回転する。アーム１０１の他端にはロボットハンド１１２が設置されている。そして、ロボットハンド１１２に慣性センサー１０７が設置されている。慣性センサー１０７は、振動式のジャイロセンサーである。そして、慣性センサー１０７はアーム１０１が回転するときの角速度を検出する。
【００３６】
ロボット１００は制御装置１１３を備えている。制御装置１１３はプロセッサーとして各種の演算処理を行うＣＰＵ（中央演算処理装置）と各種情報を記憶するメモリーとを有する。さらに、制御装置１１３は入出力インターフェイス及びデータバスを介してＣＰＵに接続されている。制御装置１１３が備える主な機能部として、制御部１０８、ノイズ検出部１０９、フィルター定数決定部１１０、ノイズフィルターとしてのフィルター１１１を有する。これらの機能はソフトウェアに組み込まれており、メモリーに記憶されている。そして、ＣＰＵはソフトウェアに従って各機能を実施する。
【００３７】
制御部１０８はアーム１０１の回動動作やロボットハンド１１２の動作を制御する機能を備える。ノイズ検出部１０９は角度センサー１０６が出力するモーター１０３の回動角度の信号と慣性センサー１０７が出力するアーム１０１の回動角速度の信号とを入力する。そして、慣性センサー１０７の出力信号に含まれるノイズ周波数を検出する機能を備える。尚、ノイズ周波数は離調周波数とも称す。
【００３８】
フィルター定数決定部１１０はノイズ周波数と定数テーブルとを参照してフィルター１１１の定数を設定する機能を備える。フィルター１１１は慣性センサー１０７が出力するアーム１０１の回動角速度の信号を入力する。そして、設定された定数に基きフィルター１１１は回動角速度の信号からノイズを除去する機能を備えている。
【００３９】
次に、信号の流れを説明する。まず、制御部１０８がモーター１０３に駆動信号を出力する。これにより、モーター１０３が作動して、アーム１０１が回動する。角度センサー１０６はモーター１０３の回転角度を示すモーター角度信号を制御部１０８とノイズ検出部１０９とに出力する。慣性センサー１０７はアーム１０１の回動角速度の信号であるアーム角速度信号をノイズ検出部１０９とフィルター１１１とに出力する。
【００４０】
ノイズ検出部１０９はモーター角度信号とアーム角速度信号とを入力しノイズ周波数を算出する。そして、算出したノイズ周波数の情報をノイズ検出部１０９はフィルター定数決定部１１０へ出力する。フィルター定数決定部１１０はノイズ周波数の情報からフィルター１１１の定数を演算する。そして、フィルター定数決定部１１０はフィルター１１１の定数を設定する。フィルター１１１は慣性センサー１０７からアーム角速度信号を入力する。そして、設定された定数を用いてアーム角速度信号からノイズを除去して制御部１０８に出力する。
【００４１】
図２は、センサー出力信号からノイズを除去する工程を示すフローチャートである。図２において、ステップＳ１は、アーム駆動工程に相当する。この工程は、制御部１０８がモーター１０３を駆動してアーム１０１を回動させる工程である。ステップＳ１は他のステップと並行して行われる。ステップＳ２は、駆動源角度検出工程に相当する。この工程は、角度センサー１０６がモーター１０３の角度を検出してモーター角度信号を出力する工程である。次にステップＳ４に移行する。ステップＳ３は、アーム慣性力検出工程に相当する。この工程は、慣性センサー１０７がアーム１０１の角速度を検出して出力する工程である。次にステップＳ４に移行する。ステップＳ２とステップＳ３とは並行して行われる。
【００４２】
ステップＳ４は、フィルター定数判断工程に相当し、フィルター１１１の定数を変更するか否かを判断する工程である。定数を変更するときとき、ステップＳ５に移行する。定数を変更しないとき、ステップＳ７に移行する。ステップＳ５は、ノイズ成分検出工程に相当する。この工程は、ノイズ検出部１０９が慣性センサー１０７に固有のノイズ周波数を算出する工程である。次にステップＳ６に移行する。ステップＳ６は、フィルター定数決定工程に相当する。この工程は、ノイズ周波数を参照してフィルター定数決定部１１０がフィルター１１１の定数を設定する工程である。次にステップＳ７に移行する。ステップＳ７は、ノイズ除去工程に相当する。この工程は、フィルター１１１が慣性センサー１０７からアーム角速度信号を入力しノイズを除去して制御部１０８に出力する工程である。以上で、センサー出力信号からノイズを除去する工程を終了する。
【００４３】
図３及び図４はセンサー出力信号からノイズを除去する工程を説明するための図である。次に、図３及び図４を用いて、図２に示したステップと対応させて、センサー出力信号からノイズを除去するする方法を詳細に説明する。ステップＳ１〜ステップＳ３は公知の技術を用いた工程であり、説明を省略する。ステップＳ４のフィルター定数判断工程ではフィルター１１１の定数が設定されているかの判断を行う。フィルター１１１の定数はフィルター１１１を交換した後に１度設定すればよく、再度交換するまでは設定しなおす必要はない。環境の変化により定数を変更する必要があるときには再度設定しても良い。
【００４４】
図３は、ステップＳ５のノイズ成分検出工程に対応するグラフである。図３において横軸は周波数を示し、右側が左側より高い周波数となっている。図３（ａ）及び図３（ｂ）において縦軸は強度を示し、上側が下側より高い強度となっている。図３（ｃ）において縦軸は強度比を示し、上側が下側より高い比率となっている。ノイズ検出部１０９は角度センサー１０６が出力するモーター角度信号を入力する。そして、ノイズ検出部１０９はモーター角度信号を減速比にて除算する。モーター角度信号は時間をパラメーターとして変化する信号である。ノイズ検出部１０９はモーター角度信号を時間にて微分する。これにより、アーム１０１の角速度信号が得られる。次に、ノイズ検出部１０９は除算したモーター角度信号をフーリエ変換することにより、図３（ａ）に示す周波数分布としての第１角速度スペクトル１１４を算出する。
【００４５】
ノイズ検出部１０９は慣性センサー１０７が出力するアーム角速度信号を入力する。アーム角速度信号は時間をパラメーターとして変化する信号である。ノイズ検出部１０９は慣性センサー１０７のアーム角速度信号をフーリエ変換することにより、図３（ｂ）に示す周波数分布としての第２角速度スペクトル１１５を算出する。第２角速度スペクトル１１５には強度がピークとなる周波数の場所が２ヶ所形成される。１つはアーム１０１の回動による回動速度周波数１１５ａである。もう１つは慣性センサー１０７に固有のノイズ周波数１１５ｂである。
【００４６】
続いて、ノイズ検出部１０９は第２角速度スペクトル１１５を第１角速度スペクトル１１４にて除算する。その結果、図３（ｃ）に示す第３角速度スペクトル１１６が算出される。第３角速度スペクトル１１６において、回動速度周波数１１５ａのピークは小さくなり、ノイズ周波数１１５ｂのピークのみが残る。これにより、ノイズ周波数１１５ｂを検出することができる。
【００４７】
図４はステップＳ６のフィルター定数決定工程に対応する図である。図４（ａ）はフィルター１１１を示すブロック図である。尚、このブロック図は次の数式のように表すことができる。
【００４８】
ａ：フィルター定数としての第１定数４０２。
ｂ：フィルター定数としての第２定数４０３。
ｃ：フィルター定数としての第３定数４０４。
ｄ：フィルター定数としての第４定数４０５。
ｅ：フィルター定数としての第５定数４０６。
Ｚ-1：遅延素子４０１。
【００４９】
【数１】
【００５０】
第１定数４０２〜第５定数４０６によってフィルター１１１がノイズを除去する特性であるノイズフィルター特性が決定される。図４（ｂ）はフィルター定数を設定するときに参照するテーブルとしての定数テーブル１１７の例を示す。図４（ｂ）に示すように、定数テーブル１１７はノイズ周波数１１５ｂに対応して第１定数４０２〜第５定数４０６が設定されている。カットオフ周波数の幅は特に限定されないが、本実施形態では、例えば、定数テーブル１１７にカットオフ周波数の幅が５０Ｈｚの例を示した。カットオフ周波数の幅は除去帯域とも称す。定数テーブル１１７はこの例に限定されず事前に実験して求めるのが好ましい。フィルター定数決定部１１０はノイズ周波数１１５ｂと定数テーブル１１７とを参照して第１定数４０２〜第５定数４０６を決定する。そして、フィルター定数決定部１１０はフィルター１１１のパラメーターに第１定数４０２〜第５定数４０６を設定する。
【００５１】
図４（ｃ）はフィルター１１１のノイズフィルター特性を示すグラフである。縦軸は利得を示し上側が下側より高い利得となっている。横軸は周波数を示し、右側が左側より高い周波数となっている。フィルター特性線１１８はフィルター１１１が信号を通過させる周波数特性を示している。図４（ｃ）に示すように、フィルター特性線１１８はノイズ周波数１１５ｂを中心とする除去帯域１１８ａ内の周波数の信号を減衰させる特性となっている。そして、ノイズ周波数１１５ｂを中心とする除去帯域１１８ａより低周波の信号と高周波の信号とを通過させる帯域除去フィルターとなっている。
【００５２】
ステップＳ７のノイズ除去工程ではフィルター１１１にアーム角速度信号が入力される。フィルター１１１には既に第１定数４０２〜第５定数４０６が設定されている。これにより、フィルター１１１はアーム角速度信号からノイズ周波数１１５ｂのノイズを除去し、制御部１０８に出力する。以上で、センサー出力信号からノイズを除去する工程を終了する。
【００５３】
以上述べたように、本実施形態にかかるロボット１００によれば、以下の効果を得ることができる。
（１）本実施形態によれば、ノイズ検出部１０９は角度センサー１０６の出力と慣性センサー１０７の出力とを用いて慣性センサー１０７に固有なノイズにおけるノイズ周波数１１５ｂを検出する。フィルター定数決定部１１０はノイズ周波数１１５ｂを用いてノイズフィルター特性を決定する。このノイズフィルター特性は慣性センサー１０７に固有な特性と対応している。慣性センサー１０７にはフィルター１１１が接続され、ノイズフィルター特性に従ってフィルター１１１は慣性センサー１０７の出力からノイズを除去する。従って、制御部１０８はノイズが減衰した慣性センサー１０７の出力を用いてアーム１０１を制御することができる。その結果、制御部１０８は、慣性センサー１０７を含んだフィードバック系の発振を防ぐことができる。
【００５４】
（２）本実施形態によれば、角度センサー１０６の出力の第１角速度スペクトル１１４と慣性センサー１０７の出力の第２角速度スペクトル１１５とを用いている。従って、慣性センサー１０７が検出する信号の振動周波数分布からノイズ周波数１１５ｂの信号を分離できる。その結果、ノイズ周波数１１５ｂを精度よく特定できる。
【００５５】
（３）本実施形態によれば、フィルター１１１のフィルター特性線１１８は帯域除去フィルターとなっている。従って、除去すべき周波数以外の帯域では位相の変化を小さくできる。さらに、慣性センサーが出力する信号からノイズ成分の周波数の帯域の信号を除去することができる。
【００５６】
（４）本実施形態によれば、フィルター定数決定部１１０はノイズ周波数１１５ｂから定数テーブル１１７を参照してフィルター定数を決定している。従って、複雑な計算を要することなくフィルター定数を決定できるので、計算にかかる負荷を少なくすることができる。
【００５７】
（５）本実施形態によれば、フィルター１１１は制御装置１１３にソフトウェアで実装されている。従って、フィルター１１１を構成する電子部品等は必要なく、ハンダ付け等の工数も削減でき、生産性良く製造可能なロボット１００を提供することができる。
【００５８】
（６）本実施形態によれば、ステップＳ２の駆動源角度検出工程にてモーター１０３の回動角度を検出している。そして、ステップＳ３のアーム慣性力検出工程にて慣性センサー１０７を用いてアーム１０１の角速度を検出している。続いて、ステップＳ５のノイズ成分検出工程では駆動源角度検出工程にて検出した回動角度の信号とアーム慣性力検出工程にて検出した慣性力の信号とを用いて慣性センサー１０７のノイズ周波数１１５ｂを検出している。次に、ステップＳ６のフィルター定数決定工程では検出した情報からノイズを除去する特性であるノイズフィルター特性を決定している。ステップＳ７のノイズ除去工程ではノイズフィルター特性に基づいたフィルター１１１でノイズを除去している。
【００５９】
従って、ノイズフィルター特性は慣性センサー１０７に固有のノイズの特性となっている為、ノイズ除去工程では精度良くノイズを除去することができる。その結果、慣性センサー１０７を含んだフィードバック系の発振を防ぐことができる。
【００６０】
（７）本実施形態によれば、制御装置１１３が慣性センサー１０７のノイズ周波数１１５ｂを自動で検出している。そのノイズを除去するフィルター１１１の第１定数４０２〜第５定数４０６をフィルター定数決定部１１０が自動で設定して、フィルター１１１が慣性センサー１０７のノイズを除去している。これによって慣性センサー１０７を含むフィードバック系を安定にできる。そして、操作者が操作することなくフィルター定数が設定されるので、生産性良くロボット１００を操作することができる。
【００６１】
（実施形態２）
次に、ロボットとロボットが行う特徴的なノイズの除去方法について、図５及び図６を用いて説明する。本実施形態が実施形態１と異なるところは、アームの数が１個から２個へ増えた点にある。尚、実施形態１と同じ点については説明を省略する。
【００６２】
すなわち、本実施形態では、図５の実施形態２にかかる概略構成を示すブロック図に示したようにアームの個数が２個になっている。ロボット５００は基体５０９を備えている。基体５０９の中には第１角度センサー５１０、駆動源としての第１モーター５０４、第１トルク伝達機構５０３がこの順に同軸上に重ねて配置されている。第１トルク伝達機構５０３と第１モーター５０４とで第１アーム連結部５０５を構成している。
【００６３】
第１トルク伝達機構５０３の出力軸には第１アーム５０１が接続され、第１モーター５０４を駆動することにより第１アーム５０１が回動する。尚、基体５０９、第１角度センサー５１０、第１アーム連結部５０５、第１アーム５０１は、実施形態１の基体１０５、角度センサー１０６、アーム連結部１０４、アーム１０１と同様な形態であり説明を省略する。
【００６４】
第１アーム５０１において、第１アーム連結部５０５と反対側には第２角度センサー５１１、駆動源としての第２モーター５０７、第２トルク伝達機構５０６がこの順に同軸上に重ねて配置されている。第２トルク伝達機構５０６と第２モーター５０７とで第２アーム連結部５０８を構成している。第１トルク伝達機構５０３は第１減速比の減速比で減速する減速機であり、第２トルク伝達機構５０６は第２減速比の減速比で減速する減速機である。尚、第２角度センサー５１１、第２アーム連結部５０８は、第１角度センサー５１０、第１アーム連結部５０５と同様な形態であり説明を省略する。第１アーム５０１において、第２角度センサー５１１の近くには第１慣性センサー５１２が設置されている。
【００６５】
第２トルク伝達機構５０６の出力軸には第２アーム５０２が接続され、第２モーター５０７を駆動することにより第２アーム５０２が回動する。そして、第１アーム５０１、第２アーム５０２、第１アーム連結部５０５、第２アーム連結部５０８によりアーム体５２１が構成されている。第２アーム５０２において第２アーム連結部５０８の反対側の端にはロボットハンド５２２が設置されている。そして、ロボットハンド５２２に第２慣性センサー５１３が設置されている。第１慣性センサー５１２及び第２慣性センサー５１３は、振動式のジャイロセンサーである。そして、第１慣性センサー５１２は第１アーム５０１が回転するときの角速度を検出し、第２慣性センサー５１３は第２アーム５０２の基体５０９に対する絶対的な角速度を検出する。
【００６６】
ロボット５００は制御装置５２３を備えている。制御装置５２３は実施形態１の制御装置１１３と同様にＣＰＵ（中央演算処理装置）、メモリー、入出力インターフェイス及びデータバスを備え、同様な機能を備えている。制御装置５２３が備える主な機能部として、制御部５１４、第１ノイズ検出部５１５、第１フィルター定数決定部５１７、ノイズフィルターとしての第１フィルター５１９を有する。制御部５１４、第１ノイズ検出部５１５、第１フィルター定数決定部５１７、第１フィルター５１９はそれぞれ実施形態１の制御部１０８、ノイズ検出部１０９、フィルター定数決定部１１０、フィルター１１１と同様な機能を備えている。
【００６７】
さらに、制御装置５２３が備える主な機能部として、第２ノイズ検出部５１６、第２フィルター定数決定部５１８、ノイズフィルターとしての第２フィルター５２０を有する。第２フィルター定数決定部５１８、第２フィルター５２０はそれぞれ実施形態１のフィルター定数決定部１１０、フィルター１１１と同様な機能を備えている。第２ノイズ検出部５１６は実施形態１のノイズ検出部１０９と類似な機能を備えている。第２ノイズ検出部５１６がノイズ検出部１０９と異なる点は第１角度センサー５１０と第２角度センサー５１１との２つの角度センサーの信号を入力する点にある。これらの機能はソフトウェアに組み込まれており、メモリーに記憶されている。そして、ＣＰＵはソフトウェアに従って各機能を実施する。
【００６８】
センサー出力信号からノイズを除去する一連の工程においてステップＳ１のアーム駆動工程〜ステップＳ４のフィルター定数判断工程は実施形態１と同じであり、説明を省略する。ステップＳ５において第１ノイズ検出部５１５が第１慣性センサー５１２のノイズ周波数を検出し、第２ノイズ検出部５１６が第２慣性センサー５１３のノイズ周波数を検出する。ステップＳ５〜ステップＳ７において第１ノイズ検出部５１５、第１フィルター定数決定部５１７、第１フィルター５１９が実施する内容は実施形態１のノイズ検出部１０９、フィルター定数決定部１１０、フィルター１１１が実施する内容と同様であり説明を省略する。そして、第２ノイズ検出部５１６が実施する内容について説明する。
【００６９】
図６は、センサー出力信号からノイズを除去する工程を説明するための図であり、ステップＳ５のノイズ成分検出工程に対応するグラフである。図６において横軸は周波数を示し、右側が左側より高い周波数となっている。図６（ａ）及び図６（ｂ）において縦軸は強度を示し、上側が下側より高い強度となっている。図６（ｃ）において縦軸は強度比を示し、上側が下側より高い比率となっている。
【００７０】
第２ノイズ検出部５１６は第１角度センサー５１０が出力する第１モーター角度信号、第２角度センサー５１１が出力する第２モーター角度信号、第２慣性センサー５１３が出力する第２アーム角速度信号とを入力する。そして、第２ノイズ検出部５１６は第１モーター角度信号を第１減速比にて除算する。次に、第２ノイズ検出部５１６は第２モーター角度信号を第２減速比にて除算する。続いて、第２ノイズ検出部５１６は除算した第１モーター角度信号と除算した第２モーター角度信号とを連結した連結モーター信号を形成する。
【００７１】
モーター角度信号は時間をパラメーターとして変化する信号である。第２ノイズ検出部５１６は連結モーター信号を時間にて微分する。これにより、第１アーム５０１及び第２アーム５０２の角速度信号を連結した連結角速度信号が得られる。次に、第２ノイズ検出部５１６は連結角速度信号をフーリエ変換することにより、図６（ａ）に示す周波数分布としての第４角速度スペクトル５２４を算出する。
【００７２】
第２ノイズ検出部５１６は第２慣性センサー５１３が出力する第２アーム角速度信号を入力する。第２アーム角速度信号は時間をパラメーターとして変化する信号である。第２ノイズ検出部５１６は第２アーム角速度信号をフーリエ変換することにより、図６（ｂ）に示す周波数分布としての第５角速度スペクトル５２５を算出する。第５角速度スペクトル５２５には強度がピークとなる周波数の場所が３ヶ所形成される。１つは第１アーム５０１の回動による第１回動速度周波数５２５ａである。１つは第２アーム５０２の回動による第２回動速度周波数５２５ｂである。もう１つは第２慣性センサー５１３に固有のノイズ周波数５２５ｃである。
【００７３】
続いて、第２ノイズ検出部５１６は第５角速度スペクトル５２５を第４角速度スペクトル５２４にて除算する。その結果、図６（ｃ）に示す第６角速度スペクトル５２６が算出される。第６角速度スペクトル５２６において、第１回動速度周波数５２５ａ及び第２回動速度周波数５２５ｂのピークは小さくなり、ノイズ周波数５２５ｃのピークのみが残る。これにより、ノイズ周波数５２５ｃを検出することができる。
【００７４】
ステップＳ６のフィルター定数決定工程では第２ノイズ検出部５１６から第２フィルター定数決定部５１８へノイズ周波数５２５ｃが出力される。第２フィルター定数決定部５１８はノイズ周波数５２５ｃを打ち消すフィルター定数をあらかじめ用意してあるテーブルに従って決定する。そして、第２フィルター定数決定部５１８は決定した定数を第２フィルター５２０に設定する。これにより、ステップＳ７のノイズ除去工程では、第２アーム角速度信号からノイズが除去される。以上で、センサー出力信号からノイズを除去する工程を終了する。
【００７５】
以上述べたように、本実施形態にかかるロボット５００によれば、以下の効果を得ることができる。
（１）本実施形態によれば、第２ノイズ検出部５１６が第２慣性センサー５１３のノイズ周波数を自動で検出している。そして、第２フィルター定数決定部５１８がノイズを除去する第２フィルター５２０の定数を自動で設定し、第２フィルター５２０が第２慣性センサー５１３のノイズを除去する。これによって制御装置５２３は第２慣性センサー５１３を含むフィードバック系を安定に駆動することができる。
【００７６】
（２）本実施形態によれば、ロボット５００は、アームとアーム連結部とが交互に連結されたアーム体を備えている。これにより、第１アーム５０１及び第２アーム５０２は自在に動くことができる。そして、第２ノイズ検出部５１６は第１角度センサー５１０及び第２角度センサー５１１の出力と第２慣性センサー５１３との出力とから第２慣性センサー５１３のノイズ周波数５２５ｃを検出している。そして、第２フィルター定数決定部５１８がノイズ周波数５２５ｃを用いて第２慣性センサー５１３に対応するノイズフィルター特性を決めている。従って、第２フィルター５２０は第２ノイズ検出部５１６のノイズを除去することができる。その結果、アームとアーム連結部とが交互に連結されたアーム体を備えたロボット５００においても、制御部５１４は第２慣性センサー５１３を含んだフィードバック系の発振を防ぐことができる。
【００７７】
尚、本実施形態は上述した実施形態に限定されるものではなく、種々の変更や改良を加えることも可能である。変形例を以下に述べる。
（変形例１）
前記第１の実施形態では、ロボット１００は水平多関節ロボット形式のロボットであったが、ロボットの形式はこれに限らない。垂直多関節ロボット、直交ロボット、パラレルリンクロボット等各種形態のロボットに上記の方法を用いることができる。この場合にも上記の方法と同様な方法を用いて慣性センサーのノイズを除去することができる。
【００７８】
直交ロボットのときには慣性センサーが直動するので、加速度センサーを用いる。このときにも回転モーターの回転角度やリニアモーターの移動距離のデータと加速度センサーの出力とを用いて加速度センサーに固有のノイズを検出することができる。そして、フィルターを用いて加速度センサーのノイズを除去することができる。
【００７９】
（変形例２）
前記第１の実施形態では、定数テーブル１１７を用いて第１定数４０２〜第５定数４０６を決定した。これに限らず、第１定数４０２〜第５定数４０６を算出する数式を用いても良い。これにより、さらに細かく第１定数４０２〜第５定数４０６を設定することができる。
【００８０】
（変形例３）
前記第１の実施形態では、フィルター１１１は帯域除去フィルターであったが、ローパスフィルターでも良い。慣性センサー１０７のノイズが除去可能であれば良い。そして、ローパスフィルターの方が帯域除去フィルターより定数の個数が少ないので、メモリーに記憶するデータ量を少なくすることができる。
【００８１】
（変形例４）
前記第１の実施形態では、角度センサー１０６と慣性センサー１０７とを用いてアーム１０１の角度を検出した。フィルター１１１の定数を設定した後は、慣性センサー１０７だけを用いてアーム１０１の角度を検出しても良い。簡便にアーム１０１の角度を検出することができる。尚、変形例１〜４の内容は実施形態２にも適用することができる。
【００８２】
（変形例５）
前記第２の実施形態では、アームの個数が２個のロボットであるが、アームの個数は３個以上でも良い。このとき、アームはさらに複雑な動作を行うことができる。
【００８３】
（変形例６）
前記第２の実施形態では、減速比にて除算した第１モーター角度信号と除算した第２モーター角度信号とを連結した連結モーター信号を形成した。そして、第２ノイズ検出部５１６は連結モーター信号を用いて第４角速度スペクトル５２４を算出した。これに限らず、まず、減速比にて除算した第１モーター角度信号から第１アーム角速度スペクトルを算出する。次に、減速比にて除算した第２モーター角度信号から第２アーム角速度スペクトルを算出する。そして、第１アーム角速度スペクトルと第２アーム角速度スペクトルとを用いて第４角速度スペクトル５２４を算出しても良い。計算時間の早い方法を選択しても良い。
【符号の説明】
【００８４】
１００，５００…ロボット、１０１…アーム、１０３…駆動源としてのモーター、１０４…アーム連結部、１０５，５０９…基体、１０６…角度センサー、１０７…慣性センサー、１０８，５１４…制御部、１０９…ノイズ検出部、１１０…フィルター定数決定部、１１１…ノイズフィルターとしてのフィルター、１１４…周波数分布としての第１角速度スペクトル、１１５…周波数分布としての第２角速度スペクトル、１１５ｂ，５２５ｃ…ノイズ周波数、１１７…テーブルとしての定数テーブル、４０２…フィルター定数としての第１定数、４０３…フィルター定数としての第２定数、４０４…フィルター定数としての第３定数、４０５…フィルター定数としての第４定数、４０６…フィルター定数としての第５定数、５０１…第１アーム、５０２…第２アーム、５０４…駆動源としての第１モーター、５０５…第１アーム連結部、５０７…駆動源としての第２モーター、５０８…第２アーム連結部、５１０…第１角度センサー、５１１…第２角度センサー、５１２…第１慣性センサー、５１３…第２慣性センサー、５１５…第１ノイズ検出部、５１６…第２ノイズ検出部、５１７…第１フィルター定数決定部、５１８…第２フィルター定数決定部、５１９…ノイズフィルターとしての第１フィルター、５２０…ノイズフィルターとしての第２フィルター、５２１…アーム体、５２４…周波数分布としての第４角速度スペクトル、５２５…周波数分布としての第５角速度スペクトル。
【技術分野】
【０００１】
本発明は、ロボット及びロボットのノイズ除去方法にかかわり、特に、ロボットの制御方法に関する。
【背景技術】
【０００２】
ロボットのアームを動作させて停止するとき、アームが振動する。アームが振動すると位置精度良く作業できないので、振動を停止するまで待機することがある。そして、アームの速度を早くするとアームの停止時の振動が大きくなるので、振動が停止するまでの時間が長くなる。そこで、例えば、特許文献１にロボットに慣性センサーを用いることで残留振動を減少させる方法が開示されている。この残留振動はモーターとアーム先端との間に大きな弾性があるために生じる。エンコーダーに代表される位置センサーだけでもモーターの回転情報は取得できるが、慣性センサーを取り付けることでアーム先端の情報はより正確に取得できる。この情報を制御部にフィードバックすることで残留振動を減少させていた。
【先行技術文献】
【特許文献】
【０００３】
【特許文献１】特許第３８８３５４４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００４】
慣性センサーを使用するにはノイズを考慮する必要がある。特に振動式のジャイロセンサーでは機械的振動が加わると非常に大きな離調周波数ノイズが発生する。慣性センサーの出力を用いてアームを制御するフィードバック系を構築するときフィードバック系が発振してロボットが動作しなくなることがある。そこで、慣性センサーを含んだフィードバック系の発振を防ぐロボットが求められていた。
【課題を解決するための手段】
【０００５】
本発明は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【０００６】
［適用例１］本適用例にかかるロボットは、アームと、前記アームと接続し前記アームを回動または並進させる駆動源を有するアーム連結部と、前記アーム連結部と接続する基体と、前記駆動源の回動角度を検出する角度センサーと、前記アームに取り付けられ前記アームに作用する慣性力を検出する慣性センサーと、前記慣性センサーの出力からノイズを除去するノイズフィルターと、前記アームの回動動作を制御する制御部と、前記角度センサーの出力と前記慣性センサーの出力とから前記慣性センサーのノイズ周波数を検出するノイズ検出部と、前記ノイズフィルターがノイズを除去するノイズフィルター特性を、前記ノイズ周波数を用いて決めるフィルター定数決定部と、を備えることを特徴とする。
【０００７】
本適用例によれば、基体にアーム連結部を介してアームが接続されている。アーム連結部は駆動源を有し、駆動源がアームを回動または並進させる。制御部は駆動源を制御することによりアームの回動動作を制御する。駆動源には角度センサーが設置され、角度センサーが駆動源の回転角度を検出する。制御装置は駆動源の回転角度の情報を元にアームの回動状態を認識する。
【０００８】
アームには慣性センサーが取り付けられている。そして、制御装置は慣性センサーの出力を用いてアームの回動状況を認識する。ノイズ検出部は角度センサーの出力と慣性センサーの出力とを用いて慣性センサーに固有なノイズにおけるノイズ周波数を検出する。フィルター定数決定部はノイズ周波数を用いてノイズフィルター特性を決定する。このノイズフィルター特性は慣性センサーに固有な特性に対応して決定される。慣性センサーにはノイズフィルターが接続され、ノイズフィルター特性に従ってノイズフィルターは慣性センサーの出力からノイズを除去する。従って、制御部はノイズが軽減された慣性センサーの出力を用いてアームを制御することができる。その結果、制御部は、慣性センサーを含んだフィードバック系の発振を防ぐことができる。
【０００９】
［適用例２］上記適用例に記載のロボットでは、前記ノイズ検出部は前記角度センサーが出力する信号の周波数分布と、前記慣性センサーが出力する信号の周波数分布とを用いて前記慣性センサーが出力する信号のノイズ周波数を検出することが好ましい。
【００１０】
本適用例によれば、角度センサーが出力する信号の周波数分布と慣性センサーが出力する信号の周波数分布とを用いている。従って、慣性センサーが検出する信号の振動周波数分布からノイズ成分の信号の周波数分布を分離できる。その結果、ノイズ周波数を精度よく特定できる。
【００１１】
［適用例３］上記適用例に記載のロボットでは、前記ノイズフィルターは帯域除去フィルターであることが好ましい。
【００１２】
本適用例によれば、除去すべき周波数以外の帯域では位相の変化を小さくできる。そして、慣性センサーが出力する信号からノイズ成分の周波数の帯域の信号を除去することができる。
【００１３】
［適用例４］上記適用例に記載のロボットでは、前記フィルター定数決定部は前記ノイズ周波数とテーブルとを参照してフィルター定数を決定することが好ましい。
【００１４】
本適用例によれば、複雑な計算を要することなくフィルター定数を決定できるので、計算にかかる負荷を少なくすることができる。
【００１５】
［適用例５］上記適用例に記載のロボットでは、前記ノイズフィルターは前記制御部にソフトウェアで実装されていることが好ましい。
【００１６】
本適用例によれば、ノイズフィルターを構成する電子部品等は必要なく、ハンダ付け等の工数も削減できる。従って、生産性良くロボットを製造することができる。
【００１７】
［適用例６］本適用例にかかるロボットは、アームと、前記アームと接続され前記アームを回動または並進させる駆動源を有するアーム連結部とが交互に連結されたアーム体と、前記アーム体と接続する基体と、前記駆動源の回動角度を検出する複数の角度センサーと、前記アームに作用する慣性力を検出する複数の慣性センサーと、前記慣性センサーの出力からノイズを除去する複数のノイズフィルターと、前記アームの回動動作を制御する制御部と、複数の前記角度センサーの出力と複数の前記慣性センサーの出力とから複数の前記慣性センサーのノイズ周波数を検出するノイズ検出部と、前記ノイズフィルターがノイズを除去するノイズフィルター特性を、前記ノイズ周波数を用いて決めるフィルター定数決定部と、を備えることを特徴とする。
【００１８】
本適用例によれば、ロボットは、アームとアーム連結部とが交互に連結されたアーム体を備えている。これにより、アームは自在に動くことができる。そして、アームは複数設置され、アームに作用する慣性力を検出する慣性センサーが複数設置されている。アーム連結部は複数設置され、アーム体には角度センサーが複数設置されている。
【００１９】
ノイズ検出部は複数の角度センサーの出力と複数の慣性センサーの出力とから慣性センサーのノイズ周波数を検出する。そして、フィルター定数決定部がノイズ周波数を用いて各慣性センサーと対応するノイズフィルター特性を決める。従って、ノイズフィルターは各慣性センサーのノイズを除去することができる。その結果、アームとアーム連結部とが交互に連結されたアーム体を備えたロボットにおいても、制御部は慣性センサーを含んだフィードバック系の発振を防ぐことができる。
【００２０】
［適用例７］上記適用例に記載のロボットでは、前記ノイズ検出部は前記角度センサーが出力する信号の周波数分布と、前記慣性センサーが出力する信号の周波数分布と、を用いて前記慣性センサーが出力する信号のノイズ周波数を検出することが好ましい。
【００２１】
本適用例によれば、角度センサーが出力する信号の周波数分布と慣性センサーが出力する信号の周波数分布とを用いている。従って、慣性センサーが検出する信号の振動周波数分布からノイズ成分の信号の周波数分布を分離できる。その結果、ノイズ周波数を精度よく特定できる。
【００２２】
［適用例８］上記適用例に記載のロボットでは、前記ノイズフィルターは帯域除去フィルターであることが好ましい。
【００２３】
本適用例によれば、除去すべき周波数以外の帯域では位相の変化が小さくなる。従って、慣性センサーが出力する信号からノイズ成分の周波数の帯域の信号を除去することができる。
【００２４】
［適用例９］上記適用例に記載のロボットでは、前記フィルター定数決定部は前記ノイズ周波数とテーブルとを参照してフィルター定数を決定することが好ましい。
【００２５】
本適用例によれば、複雑な計算を要することなくフィルター定数を決定できるので、計算にかかる負荷を少なくすることができる。
【００２６】
［適用例１０］上記適用例に記載のロボットでは、前記ノイズフィルターは前記制御部にソフトウェアで実装されていることが好ましい。
【００２７】
本適用例によれば、ノイズフィルターを構成する電子部品等は必要なく、ハンダ付け等の工数も削減できる。従って、生産性良くロボットを製造することができる。
【００２８】
［適用例１１］本適用例にかかるロボットのノイズ除去方法は、駆動源の回動角度を検出する駆動源角度検出工程と、慣性センサーを用いてアームに作用する慣性力を検出するアーム慣性力検出工程と、前記駆動源角度検出工程にて検出した前記回動角度の信号と前記アーム慣性力検出工程にて検出した前記慣性力の信号とを用いて前記慣性センサーのノイズ周波数を検出するノイズ成分検出工程と、前記検出した情報からノイズを除去するノイズフィルター特性を決定するフィルター定数決定工程と、前記ノイズフィルター特性に基づいたノイズフィルターでノイズを除去するノイズ除去工程と、を有することを特徴とする。
【００２９】
本適用例によれば、駆動源角度検出工程にて駆動源の回動角度を検出している。そして、アーム慣性力検出工程にて慣性センサーを用いてアームに作用する慣性力を検出している。続いて、ノイズ成分検出工程では駆動源角度検出工程にて検出した回動角度の信号とアーム慣性力検出工程にて検出した慣性力の信号とを用いて慣性センサーのノイズ周波数を検出している。次に、フィルター定数決定工程では検出した情報からノイズを除去する特性であるノイズフィルター特性を決定している。ノイズ除去工程ではノイズフィルター特性に基づいたノイズフィルターでノイズを除去している。
【００３０】
従って、ノイズフィルター特性は慣性センサーに固有のノイズの特性となるように設定することができる為、ノイズ除去工程では精度良くノイズを除去することができる。その結果、慣性センサーを含んだフィードバック系の発振を防ぐことができる。
【図面の簡単な説明】
【００３１】
【図１】実施形態１にかかるロボットの概略構成を示すブロック図。
【図２】センサー出力信号からノイズを除去する工程を示すフローチャート。
【図３】センサー出力信号からノイズを除去する工程を説明するための図。
【図４】センサー出力信号からノイズを除去する工程を説明するための図。
【図５】実施形態２にかかる概略構成を示すブロック図。
【図６】センサー出力信号からノイズを除去する工程を説明するための図。
【発明を実施するための形態】
【００３２】
以下、本発明の実施形態について、図面を参照して説明する。なお、以下の各図においては、各層や各部材を認識可能な程度の大きさにするため、各層や各部材の尺度を実際とは異ならせしめている。
【００３３】
（実施形態１）
本実施形態では、ロボットとロボットが行う特徴的なアームの制御方法について、図１〜図４に従って説明する。図１は、実施形態１にかかるロボットの概略構成を示すブロック図である。
【００３４】
図１に示すようにロボット１００は基体１０５を備えている。基体１０５の内部にはアーム連結部１０４が設置されている。アーム連結部１０４は角度センサー１０６、駆動源としてのモーター１０３、トルク伝達機構１０２がこの順に同軸上に配置されている。モーター１０３の出力軸の一端は角度センサー１０６と接続されている。角度センサー１０６はエンコーダーを備えモーター１０３の回動角度を検出する。モーター１０３の出力軸の他端はトルク伝達機構１０２の入力軸と接続されている。トルク伝達機構１０２は所定の減速比に設定された減速機である。従って、トルク伝達機構１０２の出力軸の回動角度はモーター１０３の回動角度を減速比で除算した値となる。
【００３５】
トルク伝達機構１０２の出力軸はアーム１０１の一端と接続されている。これにより、モーター１０３が回転することによりアーム１０１が回転する。アーム１０１の他端にはロボットハンド１１２が設置されている。そして、ロボットハンド１１２に慣性センサー１０７が設置されている。慣性センサー１０７は、振動式のジャイロセンサーである。そして、慣性センサー１０７はアーム１０１が回転するときの角速度を検出する。
【００３６】
ロボット１００は制御装置１１３を備えている。制御装置１１３はプロセッサーとして各種の演算処理を行うＣＰＵ（中央演算処理装置）と各種情報を記憶するメモリーとを有する。さらに、制御装置１１３は入出力インターフェイス及びデータバスを介してＣＰＵに接続されている。制御装置１１３が備える主な機能部として、制御部１０８、ノイズ検出部１０９、フィルター定数決定部１１０、ノイズフィルターとしてのフィルター１１１を有する。これらの機能はソフトウェアに組み込まれており、メモリーに記憶されている。そして、ＣＰＵはソフトウェアに従って各機能を実施する。
【００３７】
制御部１０８はアーム１０１の回動動作やロボットハンド１１２の動作を制御する機能を備える。ノイズ検出部１０９は角度センサー１０６が出力するモーター１０３の回動角度の信号と慣性センサー１０７が出力するアーム１０１の回動角速度の信号とを入力する。そして、慣性センサー１０７の出力信号に含まれるノイズ周波数を検出する機能を備える。尚、ノイズ周波数は離調周波数とも称す。
【００３８】
フィルター定数決定部１１０はノイズ周波数と定数テーブルとを参照してフィルター１１１の定数を設定する機能を備える。フィルター１１１は慣性センサー１０７が出力するアーム１０１の回動角速度の信号を入力する。そして、設定された定数に基きフィルター１１１は回動角速度の信号からノイズを除去する機能を備えている。
【００３９】
次に、信号の流れを説明する。まず、制御部１０８がモーター１０３に駆動信号を出力する。これにより、モーター１０３が作動して、アーム１０１が回動する。角度センサー１０６はモーター１０３の回転角度を示すモーター角度信号を制御部１０８とノイズ検出部１０９とに出力する。慣性センサー１０７はアーム１０１の回動角速度の信号であるアーム角速度信号をノイズ検出部１０９とフィルター１１１とに出力する。
【００４０】
ノイズ検出部１０９はモーター角度信号とアーム角速度信号とを入力しノイズ周波数を算出する。そして、算出したノイズ周波数の情報をノイズ検出部１０９はフィルター定数決定部１１０へ出力する。フィルター定数決定部１１０はノイズ周波数の情報からフィルター１１１の定数を演算する。そして、フィルター定数決定部１１０はフィルター１１１の定数を設定する。フィルター１１１は慣性センサー１０７からアーム角速度信号を入力する。そして、設定された定数を用いてアーム角速度信号からノイズを除去して制御部１０８に出力する。
【００４１】
図２は、センサー出力信号からノイズを除去する工程を示すフローチャートである。図２において、ステップＳ１は、アーム駆動工程に相当する。この工程は、制御部１０８がモーター１０３を駆動してアーム１０１を回動させる工程である。ステップＳ１は他のステップと並行して行われる。ステップＳ２は、駆動源角度検出工程に相当する。この工程は、角度センサー１０６がモーター１０３の角度を検出してモーター角度信号を出力する工程である。次にステップＳ４に移行する。ステップＳ３は、アーム慣性力検出工程に相当する。この工程は、慣性センサー１０７がアーム１０１の角速度を検出して出力する工程である。次にステップＳ４に移行する。ステップＳ２とステップＳ３とは並行して行われる。
【００４２】
ステップＳ４は、フィルター定数判断工程に相当し、フィルター１１１の定数を変更するか否かを判断する工程である。定数を変更するときとき、ステップＳ５に移行する。定数を変更しないとき、ステップＳ７に移行する。ステップＳ５は、ノイズ成分検出工程に相当する。この工程は、ノイズ検出部１０９が慣性センサー１０７に固有のノイズ周波数を算出する工程である。次にステップＳ６に移行する。ステップＳ６は、フィルター定数決定工程に相当する。この工程は、ノイズ周波数を参照してフィルター定数決定部１１０がフィルター１１１の定数を設定する工程である。次にステップＳ７に移行する。ステップＳ７は、ノイズ除去工程に相当する。この工程は、フィルター１１１が慣性センサー１０７からアーム角速度信号を入力しノイズを除去して制御部１０８に出力する工程である。以上で、センサー出力信号からノイズを除去する工程を終了する。
【００４３】
図３及び図４はセンサー出力信号からノイズを除去する工程を説明するための図である。次に、図３及び図４を用いて、図２に示したステップと対応させて、センサー出力信号からノイズを除去するする方法を詳細に説明する。ステップＳ１〜ステップＳ３は公知の技術を用いた工程であり、説明を省略する。ステップＳ４のフィルター定数判断工程ではフィルター１１１の定数が設定されているかの判断を行う。フィルター１１１の定数はフィルター１１１を交換した後に１度設定すればよく、再度交換するまでは設定しなおす必要はない。環境の変化により定数を変更する必要があるときには再度設定しても良い。
【００４４】
図３は、ステップＳ５のノイズ成分検出工程に対応するグラフである。図３において横軸は周波数を示し、右側が左側より高い周波数となっている。図３（ａ）及び図３（ｂ）において縦軸は強度を示し、上側が下側より高い強度となっている。図３（ｃ）において縦軸は強度比を示し、上側が下側より高い比率となっている。ノイズ検出部１０９は角度センサー１０６が出力するモーター角度信号を入力する。そして、ノイズ検出部１０９はモーター角度信号を減速比にて除算する。モーター角度信号は時間をパラメーターとして変化する信号である。ノイズ検出部１０９はモーター角度信号を時間にて微分する。これにより、アーム１０１の角速度信号が得られる。次に、ノイズ検出部１０９は除算したモーター角度信号をフーリエ変換することにより、図３（ａ）に示す周波数分布としての第１角速度スペクトル１１４を算出する。
【００４５】
ノイズ検出部１０９は慣性センサー１０７が出力するアーム角速度信号を入力する。アーム角速度信号は時間をパラメーターとして変化する信号である。ノイズ検出部１０９は慣性センサー１０７のアーム角速度信号をフーリエ変換することにより、図３（ｂ）に示す周波数分布としての第２角速度スペクトル１１５を算出する。第２角速度スペクトル１１５には強度がピークとなる周波数の場所が２ヶ所形成される。１つはアーム１０１の回動による回動速度周波数１１５ａである。もう１つは慣性センサー１０７に固有のノイズ周波数１１５ｂである。
【００４６】
続いて、ノイズ検出部１０９は第２角速度スペクトル１１５を第１角速度スペクトル１１４にて除算する。その結果、図３（ｃ）に示す第３角速度スペクトル１１６が算出される。第３角速度スペクトル１１６において、回動速度周波数１１５ａのピークは小さくなり、ノイズ周波数１１５ｂのピークのみが残る。これにより、ノイズ周波数１１５ｂを検出することができる。
【００４７】
図４はステップＳ６のフィルター定数決定工程に対応する図である。図４（ａ）はフィルター１１１を示すブロック図である。尚、このブロック図は次の数式のように表すことができる。
【００４８】
ａ：フィルター定数としての第１定数４０２。
ｂ：フィルター定数としての第２定数４０３。
ｃ：フィルター定数としての第３定数４０４。
ｄ：フィルター定数としての第４定数４０５。
ｅ：フィルター定数としての第５定数４０６。
Ｚ-1：遅延素子４０１。
【００４９】
【数１】
【００５０】
第１定数４０２〜第５定数４０６によってフィルター１１１がノイズを除去する特性であるノイズフィルター特性が決定される。図４（ｂ）はフィルター定数を設定するときに参照するテーブルとしての定数テーブル１１７の例を示す。図４（ｂ）に示すように、定数テーブル１１７はノイズ周波数１１５ｂに対応して第１定数４０２〜第５定数４０６が設定されている。カットオフ周波数の幅は特に限定されないが、本実施形態では、例えば、定数テーブル１１７にカットオフ周波数の幅が５０Ｈｚの例を示した。カットオフ周波数の幅は除去帯域とも称す。定数テーブル１１７はこの例に限定されず事前に実験して求めるのが好ましい。フィルター定数決定部１１０はノイズ周波数１１５ｂと定数テーブル１１７とを参照して第１定数４０２〜第５定数４０６を決定する。そして、フィルター定数決定部１１０はフィルター１１１のパラメーターに第１定数４０２〜第５定数４０６を設定する。
【００５１】
図４（ｃ）はフィルター１１１のノイズフィルター特性を示すグラフである。縦軸は利得を示し上側が下側より高い利得となっている。横軸は周波数を示し、右側が左側より高い周波数となっている。フィルター特性線１１８はフィルター１１１が信号を通過させる周波数特性を示している。図４（ｃ）に示すように、フィルター特性線１１８はノイズ周波数１１５ｂを中心とする除去帯域１１８ａ内の周波数の信号を減衰させる特性となっている。そして、ノイズ周波数１１５ｂを中心とする除去帯域１１８ａより低周波の信号と高周波の信号とを通過させる帯域除去フィルターとなっている。
【００５２】
ステップＳ７のノイズ除去工程ではフィルター１１１にアーム角速度信号が入力される。フィルター１１１には既に第１定数４０２〜第５定数４０６が設定されている。これにより、フィルター１１１はアーム角速度信号からノイズ周波数１１５ｂのノイズを除去し、制御部１０８に出力する。以上で、センサー出力信号からノイズを除去する工程を終了する。
【００５３】
以上述べたように、本実施形態にかかるロボット１００によれば、以下の効果を得ることができる。
（１）本実施形態によれば、ノイズ検出部１０９は角度センサー１０６の出力と慣性センサー１０７の出力とを用いて慣性センサー１０７に固有なノイズにおけるノイズ周波数１１５ｂを検出する。フィルター定数決定部１１０はノイズ周波数１１５ｂを用いてノイズフィルター特性を決定する。このノイズフィルター特性は慣性センサー１０７に固有な特性と対応している。慣性センサー１０７にはフィルター１１１が接続され、ノイズフィルター特性に従ってフィルター１１１は慣性センサー１０７の出力からノイズを除去する。従って、制御部１０８はノイズが減衰した慣性センサー１０７の出力を用いてアーム１０１を制御することができる。その結果、制御部１０８は、慣性センサー１０７を含んだフィードバック系の発振を防ぐことができる。
【００５４】
（２）本実施形態によれば、角度センサー１０６の出力の第１角速度スペクトル１１４と慣性センサー１０７の出力の第２角速度スペクトル１１５とを用いている。従って、慣性センサー１０７が検出する信号の振動周波数分布からノイズ周波数１１５ｂの信号を分離できる。その結果、ノイズ周波数１１５ｂを精度よく特定できる。
【００５５】
（３）本実施形態によれば、フィルター１１１のフィルター特性線１１８は帯域除去フィルターとなっている。従って、除去すべき周波数以外の帯域では位相の変化を小さくできる。さらに、慣性センサーが出力する信号からノイズ成分の周波数の帯域の信号を除去することができる。
【００５６】
（４）本実施形態によれば、フィルター定数決定部１１０はノイズ周波数１１５ｂから定数テーブル１１７を参照してフィルター定数を決定している。従って、複雑な計算を要することなくフィルター定数を決定できるので、計算にかかる負荷を少なくすることができる。
【００５７】
（５）本実施形態によれば、フィルター１１１は制御装置１１３にソフトウェアで実装されている。従って、フィルター１１１を構成する電子部品等は必要なく、ハンダ付け等の工数も削減でき、生産性良く製造可能なロボット１００を提供することができる。
【００５８】
（６）本実施形態によれば、ステップＳ２の駆動源角度検出工程にてモーター１０３の回動角度を検出している。そして、ステップＳ３のアーム慣性力検出工程にて慣性センサー１０７を用いてアーム１０１の角速度を検出している。続いて、ステップＳ５のノイズ成分検出工程では駆動源角度検出工程にて検出した回動角度の信号とアーム慣性力検出工程にて検出した慣性力の信号とを用いて慣性センサー１０７のノイズ周波数１１５ｂを検出している。次に、ステップＳ６のフィルター定数決定工程では検出した情報からノイズを除去する特性であるノイズフィルター特性を決定している。ステップＳ７のノイズ除去工程ではノイズフィルター特性に基づいたフィルター１１１でノイズを除去している。
【００５９】
従って、ノイズフィルター特性は慣性センサー１０７に固有のノイズの特性となっている為、ノイズ除去工程では精度良くノイズを除去することができる。その結果、慣性センサー１０７を含んだフィードバック系の発振を防ぐことができる。
【００６０】
（７）本実施形態によれば、制御装置１１３が慣性センサー１０７のノイズ周波数１１５ｂを自動で検出している。そのノイズを除去するフィルター１１１の第１定数４０２〜第５定数４０６をフィルター定数決定部１１０が自動で設定して、フィルター１１１が慣性センサー１０７のノイズを除去している。これによって慣性センサー１０７を含むフィードバック系を安定にできる。そして、操作者が操作することなくフィルター定数が設定されるので、生産性良くロボット１００を操作することができる。
【００６１】
（実施形態２）
次に、ロボットとロボットが行う特徴的なノイズの除去方法について、図５及び図６を用いて説明する。本実施形態が実施形態１と異なるところは、アームの数が１個から２個へ増えた点にある。尚、実施形態１と同じ点については説明を省略する。
【００６２】
すなわち、本実施形態では、図５の実施形態２にかかる概略構成を示すブロック図に示したようにアームの個数が２個になっている。ロボット５００は基体５０９を備えている。基体５０９の中には第１角度センサー５１０、駆動源としての第１モーター５０４、第１トルク伝達機構５０３がこの順に同軸上に重ねて配置されている。第１トルク伝達機構５０３と第１モーター５０４とで第１アーム連結部５０５を構成している。
【００６３】
第１トルク伝達機構５０３の出力軸には第１アーム５０１が接続され、第１モーター５０４を駆動することにより第１アーム５０１が回動する。尚、基体５０９、第１角度センサー５１０、第１アーム連結部５０５、第１アーム５０１は、実施形態１の基体１０５、角度センサー１０６、アーム連結部１０４、アーム１０１と同様な形態であり説明を省略する。
【００６４】
第１アーム５０１において、第１アーム連結部５０５と反対側には第２角度センサー５１１、駆動源としての第２モーター５０７、第２トルク伝達機構５０６がこの順に同軸上に重ねて配置されている。第２トルク伝達機構５０６と第２モーター５０７とで第２アーム連結部５０８を構成している。第１トルク伝達機構５０３は第１減速比の減速比で減速する減速機であり、第２トルク伝達機構５０６は第２減速比の減速比で減速する減速機である。尚、第２角度センサー５１１、第２アーム連結部５０８は、第１角度センサー５１０、第１アーム連結部５０５と同様な形態であり説明を省略する。第１アーム５０１において、第２角度センサー５１１の近くには第１慣性センサー５１２が設置されている。
【００６５】
第２トルク伝達機構５０６の出力軸には第２アーム５０２が接続され、第２モーター５０７を駆動することにより第２アーム５０２が回動する。そして、第１アーム５０１、第２アーム５０２、第１アーム連結部５０５、第２アーム連結部５０８によりアーム体５２１が構成されている。第２アーム５０２において第２アーム連結部５０８の反対側の端にはロボットハンド５２２が設置されている。そして、ロボットハンド５２２に第２慣性センサー５１３が設置されている。第１慣性センサー５１２及び第２慣性センサー５１３は、振動式のジャイロセンサーである。そして、第１慣性センサー５１２は第１アーム５０１が回転するときの角速度を検出し、第２慣性センサー５１３は第２アーム５０２の基体５０９に対する絶対的な角速度を検出する。
【００６６】
ロボット５００は制御装置５２３を備えている。制御装置５２３は実施形態１の制御装置１１３と同様にＣＰＵ（中央演算処理装置）、メモリー、入出力インターフェイス及びデータバスを備え、同様な機能を備えている。制御装置５２３が備える主な機能部として、制御部５１４、第１ノイズ検出部５１５、第１フィルター定数決定部５１７、ノイズフィルターとしての第１フィルター５１９を有する。制御部５１４、第１ノイズ検出部５１５、第１フィルター定数決定部５１７、第１フィルター５１９はそれぞれ実施形態１の制御部１０８、ノイズ検出部１０９、フィルター定数決定部１１０、フィルター１１１と同様な機能を備えている。
【００６７】
さらに、制御装置５２３が備える主な機能部として、第２ノイズ検出部５１６、第２フィルター定数決定部５１８、ノイズフィルターとしての第２フィルター５２０を有する。第２フィルター定数決定部５１８、第２フィルター５２０はそれぞれ実施形態１のフィルター定数決定部１１０、フィルター１１１と同様な機能を備えている。第２ノイズ検出部５１６は実施形態１のノイズ検出部１０９と類似な機能を備えている。第２ノイズ検出部５１６がノイズ検出部１０９と異なる点は第１角度センサー５１０と第２角度センサー５１１との２つの角度センサーの信号を入力する点にある。これらの機能はソフトウェアに組み込まれており、メモリーに記憶されている。そして、ＣＰＵはソフトウェアに従って各機能を実施する。
【００６８】
センサー出力信号からノイズを除去する一連の工程においてステップＳ１のアーム駆動工程〜ステップＳ４のフィルター定数判断工程は実施形態１と同じであり、説明を省略する。ステップＳ５において第１ノイズ検出部５１５が第１慣性センサー５１２のノイズ周波数を検出し、第２ノイズ検出部５１６が第２慣性センサー５１３のノイズ周波数を検出する。ステップＳ５〜ステップＳ７において第１ノイズ検出部５１５、第１フィルター定数決定部５１７、第１フィルター５１９が実施する内容は実施形態１のノイズ検出部１０９、フィルター定数決定部１１０、フィルター１１１が実施する内容と同様であり説明を省略する。そして、第２ノイズ検出部５１６が実施する内容について説明する。
【００６９】
図６は、センサー出力信号からノイズを除去する工程を説明するための図であり、ステップＳ５のノイズ成分検出工程に対応するグラフである。図６において横軸は周波数を示し、右側が左側より高い周波数となっている。図６（ａ）及び図６（ｂ）において縦軸は強度を示し、上側が下側より高い強度となっている。図６（ｃ）において縦軸は強度比を示し、上側が下側より高い比率となっている。
【００７０】
第２ノイズ検出部５１６は第１角度センサー５１０が出力する第１モーター角度信号、第２角度センサー５１１が出力する第２モーター角度信号、第２慣性センサー５１３が出力する第２アーム角速度信号とを入力する。そして、第２ノイズ検出部５１６は第１モーター角度信号を第１減速比にて除算する。次に、第２ノイズ検出部５１６は第２モーター角度信号を第２減速比にて除算する。続いて、第２ノイズ検出部５１６は除算した第１モーター角度信号と除算した第２モーター角度信号とを連結した連結モーター信号を形成する。
【００７１】
モーター角度信号は時間をパラメーターとして変化する信号である。第２ノイズ検出部５１６は連結モーター信号を時間にて微分する。これにより、第１アーム５０１及び第２アーム５０２の角速度信号を連結した連結角速度信号が得られる。次に、第２ノイズ検出部５１６は連結角速度信号をフーリエ変換することにより、図６（ａ）に示す周波数分布としての第４角速度スペクトル５２４を算出する。
【００７２】
第２ノイズ検出部５１６は第２慣性センサー５１３が出力する第２アーム角速度信号を入力する。第２アーム角速度信号は時間をパラメーターとして変化する信号である。第２ノイズ検出部５１６は第２アーム角速度信号をフーリエ変換することにより、図６（ｂ）に示す周波数分布としての第５角速度スペクトル５２５を算出する。第５角速度スペクトル５２５には強度がピークとなる周波数の場所が３ヶ所形成される。１つは第１アーム５０１の回動による第１回動速度周波数５２５ａである。１つは第２アーム５０２の回動による第２回動速度周波数５２５ｂである。もう１つは第２慣性センサー５１３に固有のノイズ周波数５２５ｃである。
【００７３】
続いて、第２ノイズ検出部５１６は第５角速度スペクトル５２５を第４角速度スペクトル５２４にて除算する。その結果、図６（ｃ）に示す第６角速度スペクトル５２６が算出される。第６角速度スペクトル５２６において、第１回動速度周波数５２５ａ及び第２回動速度周波数５２５ｂのピークは小さくなり、ノイズ周波数５２５ｃのピークのみが残る。これにより、ノイズ周波数５２５ｃを検出することができる。
【００７４】
ステップＳ６のフィルター定数決定工程では第２ノイズ検出部５１６から第２フィルター定数決定部５１８へノイズ周波数５２５ｃが出力される。第２フィルター定数決定部５１８はノイズ周波数５２５ｃを打ち消すフィルター定数をあらかじめ用意してあるテーブルに従って決定する。そして、第２フィルター定数決定部５１８は決定した定数を第２フィルター５２０に設定する。これにより、ステップＳ７のノイズ除去工程では、第２アーム角速度信号からノイズが除去される。以上で、センサー出力信号からノイズを除去する工程を終了する。
【００７５】
以上述べたように、本実施形態にかかるロボット５００によれば、以下の効果を得ることができる。
（１）本実施形態によれば、第２ノイズ検出部５１６が第２慣性センサー５１３のノイズ周波数を自動で検出している。そして、第２フィルター定数決定部５１８がノイズを除去する第２フィルター５２０の定数を自動で設定し、第２フィルター５２０が第２慣性センサー５１３のノイズを除去する。これによって制御装置５２３は第２慣性センサー５１３を含むフィードバック系を安定に駆動することができる。
【００７６】
（２）本実施形態によれば、ロボット５００は、アームとアーム連結部とが交互に連結されたアーム体を備えている。これにより、第１アーム５０１及び第２アーム５０２は自在に動くことができる。そして、第２ノイズ検出部５１６は第１角度センサー５１０及び第２角度センサー５１１の出力と第２慣性センサー５１３との出力とから第２慣性センサー５１３のノイズ周波数５２５ｃを検出している。そして、第２フィルター定数決定部５１８がノイズ周波数５２５ｃを用いて第２慣性センサー５１３に対応するノイズフィルター特性を決めている。従って、第２フィルター５２０は第２ノイズ検出部５１６のノイズを除去することができる。その結果、アームとアーム連結部とが交互に連結されたアーム体を備えたロボット５００においても、制御部５１４は第２慣性センサー５１３を含んだフィードバック系の発振を防ぐことができる。
【００７７】
尚、本実施形態は上述した実施形態に限定されるものではなく、種々の変更や改良を加えることも可能である。変形例を以下に述べる。
（変形例１）
前記第１の実施形態では、ロボット１００は水平多関節ロボット形式のロボットであったが、ロボットの形式はこれに限らない。垂直多関節ロボット、直交ロボット、パラレルリンクロボット等各種形態のロボットに上記の方法を用いることができる。この場合にも上記の方法と同様な方法を用いて慣性センサーのノイズを除去することができる。
【００７８】
直交ロボットのときには慣性センサーが直動するので、加速度センサーを用いる。このときにも回転モーターの回転角度やリニアモーターの移動距離のデータと加速度センサーの出力とを用いて加速度センサーに固有のノイズを検出することができる。そして、フィルターを用いて加速度センサーのノイズを除去することができる。
【００７９】
（変形例２）
前記第１の実施形態では、定数テーブル１１７を用いて第１定数４０２〜第５定数４０６を決定した。これに限らず、第１定数４０２〜第５定数４０６を算出する数式を用いても良い。これにより、さらに細かく第１定数４０２〜第５定数４０６を設定することができる。
【００８０】
（変形例３）
前記第１の実施形態では、フィルター１１１は帯域除去フィルターであったが、ローパスフィルターでも良い。慣性センサー１０７のノイズが除去可能であれば良い。そして、ローパスフィルターの方が帯域除去フィルターより定数の個数が少ないので、メモリーに記憶するデータ量を少なくすることができる。
【００８１】
（変形例４）
前記第１の実施形態では、角度センサー１０６と慣性センサー１０７とを用いてアーム１０１の角度を検出した。フィルター１１１の定数を設定した後は、慣性センサー１０７だけを用いてアーム１０１の角度を検出しても良い。簡便にアーム１０１の角度を検出することができる。尚、変形例１〜４の内容は実施形態２にも適用することができる。
【００８２】
（変形例５）
前記第２の実施形態では、アームの個数が２個のロボットであるが、アームの個数は３個以上でも良い。このとき、アームはさらに複雑な動作を行うことができる。
【００８３】
（変形例６）
前記第２の実施形態では、減速比にて除算した第１モーター角度信号と除算した第２モーター角度信号とを連結した連結モーター信号を形成した。そして、第２ノイズ検出部５１６は連結モーター信号を用いて第４角速度スペクトル５２４を算出した。これに限らず、まず、減速比にて除算した第１モーター角度信号から第１アーム角速度スペクトルを算出する。次に、減速比にて除算した第２モーター角度信号から第２アーム角速度スペクトルを算出する。そして、第１アーム角速度スペクトルと第２アーム角速度スペクトルとを用いて第４角速度スペクトル５２４を算出しても良い。計算時間の早い方法を選択しても良い。
【符号の説明】
【００８４】
１００，５００…ロボット、１０１…アーム、１０３…駆動源としてのモーター、１０４…アーム連結部、１０５，５０９…基体、１０６…角度センサー、１０７…慣性センサー、１０８，５１４…制御部、１０９…ノイズ検出部、１１０…フィルター定数決定部、１１１…ノイズフィルターとしてのフィルター、１１４…周波数分布としての第１角速度スペクトル、１１５…周波数分布としての第２角速度スペクトル、１１５ｂ，５２５ｃ…ノイズ周波数、１１７…テーブルとしての定数テーブル、４０２…フィルター定数としての第１定数、４０３…フィルター定数としての第２定数、４０４…フィルター定数としての第３定数、４０５…フィルター定数としての第４定数、４０６…フィルター定数としての第５定数、５０１…第１アーム、５０２…第２アーム、５０４…駆動源としての第１モーター、５０５…第１アーム連結部、５０７…駆動源としての第２モーター、５０８…第２アーム連結部、５１０…第１角度センサー、５１１…第２角度センサー、５１２…第１慣性センサー、５１３…第２慣性センサー、５１５…第１ノイズ検出部、５１６…第２ノイズ検出部、５１７…第１フィルター定数決定部、５１８…第２フィルター定数決定部、５１９…ノイズフィルターとしての第１フィルター、５２０…ノイズフィルターとしての第２フィルター、５２１…アーム体、５２４…周波数分布としての第４角速度スペクトル、５２５…周波数分布としての第５角速度スペクトル。
【特許請求の範囲】
【請求項１】
アームと、前記アームと接続し前記アームを回動または並進させる駆動源を有するアーム連結部と、
前記アーム連結部と接続する基体と、
前記駆動源の回動角度を検出する角度センサーと、
前記アームに取り付けられ前記アームに作用する慣性力を検出する慣性センサーと、
前記慣性センサーの出力からノイズを除去するノイズフィルターと、
前記アームの回動動作を制御する制御部と、
前記角度センサーの出力と前記慣性センサーの出力とから前記慣性センサーのノイズ周波数を検出するノイズ検出部と、
前記ノイズフィルターがノイズを除去するノイズフィルター特性を、前記ノイズ周波数を用いて決めるフィルター定数決定部と、を備えることを特徴とするロボット。
【請求項２】
前記ノイズ検出部は前記角度センサーが出力する信号の周波数分布と、前記慣性センサーが出力する信号の周波数分布とを用いて前記慣性センサーが出力する信号のノイズ周波数を検出することを特徴とする請求項１に記載のロボット。
【請求項３】
前記ノイズフィルターは帯域除去フィルターであることを特徴とする、請求項２に記載のロボット。
【請求項４】
前記フィルター定数決定部は前記ノイズ周波数とテーブルとを参照してフィルター定数を決定することを特徴とする、請求項３に記載のロボット。
【請求項５】
前記ノイズフィルターは前記制御部にソフトウェアで実装されていることを特徴とする、請求項４に記載のロボット。
【請求項６】
アームと、前記アームと接続され前記アームを回動または並進させる駆動源を有するアーム連結部とが交互に連結されたアーム体と、
前記アーム体と接続する基体と、
前記駆動源の回動角度を検出する複数の角度センサーと、
前記アームに作用する慣性力を検出する複数の慣性センサーと、
前記慣性センサーの出力からノイズを除去する複数のノイズフィルターと、
前記アームの回動動作を制御する制御部と、
複数の前記角度センサーの出力と複数の前記慣性センサーの出力とから複数の前記慣性センサーのノイズ周波数を検出するノイズ検出部と、
前記ノイズフィルターがノイズを除去するノイズフィルター特性を、前記ノイズ周波数を用いて決めるフィルター定数決定部と、を備えることを特徴とするロボット。
【請求項７】
前記ノイズ検出部は前記角度センサーが出力する信号の周波数分布と、前記慣性センサーが出力する信号の周波数分布と、を用いて前記慣性センサーが出力する信号のノイズ周波数を検出することを特徴とする請求項６に記載のロボット。
【請求項８】
前記ノイズフィルターは帯域除去フィルターであることを特徴とする、請求項７に記載のロボット。
【請求項９】
前記フィルター定数決定部は前記ノイズ周波数とテーブルとを参照してフィルター定数を決定することを特徴とする、請求項８に記載のロボット。
【請求項１０】
前記ノイズフィルターは前記制御部にソフトウェアで実装されていることを特徴とする、請求項９に記載のロボット。
【請求項１１】
駆動源の回動角度を検出する駆動源角度検出工程と、
慣性センサーを用いてアームに作用する慣性力を検出するアーム慣性力検出工程と、
前記駆動源角度検出工程にて検出した前記回動角度の信号と前記アーム慣性力検出工程にて検出した前記慣性力の信号とを用いて前記慣性センサーのノイズ周波数を検出するノイズ成分検出工程と、
前記検出した情報からノイズを除去するノイズフィルター特性を決定するフィルター定数決定工程と、
前記ノイズフィルター特性に基づいたノイズフィルターでノイズを除去するノイズ除去工程と、を有することを特徴とするロボットのノイズ除去方法。
【請求項１】
アームと、前記アームと接続し前記アームを回動または並進させる駆動源を有するアーム連結部と、
前記アーム連結部と接続する基体と、
前記駆動源の回動角度を検出する角度センサーと、
前記アームに取り付けられ前記アームに作用する慣性力を検出する慣性センサーと、
前記慣性センサーの出力からノイズを除去するノイズフィルターと、
前記アームの回動動作を制御する制御部と、
前記角度センサーの出力と前記慣性センサーの出力とから前記慣性センサーのノイズ周波数を検出するノイズ検出部と、
前記ノイズフィルターがノイズを除去するノイズフィルター特性を、前記ノイズ周波数を用いて決めるフィルター定数決定部と、を備えることを特徴とするロボット。
【請求項２】
前記ノイズ検出部は前記角度センサーが出力する信号の周波数分布と、前記慣性センサーが出力する信号の周波数分布とを用いて前記慣性センサーが出力する信号のノイズ周波数を検出することを特徴とする請求項１に記載のロボット。
【請求項３】
前記ノイズフィルターは帯域除去フィルターであることを特徴とする、請求項２に記載のロボット。
【請求項４】
前記フィルター定数決定部は前記ノイズ周波数とテーブルとを参照してフィルター定数を決定することを特徴とする、請求項３に記載のロボット。
【請求項５】
前記ノイズフィルターは前記制御部にソフトウェアで実装されていることを特徴とする、請求項４に記載のロボット。
【請求項６】
アームと、前記アームと接続され前記アームを回動または並進させる駆動源を有するアーム連結部とが交互に連結されたアーム体と、
前記アーム体と接続する基体と、
前記駆動源の回動角度を検出する複数の角度センサーと、
前記アームに作用する慣性力を検出する複数の慣性センサーと、
前記慣性センサーの出力からノイズを除去する複数のノイズフィルターと、
前記アームの回動動作を制御する制御部と、
複数の前記角度センサーの出力と複数の前記慣性センサーの出力とから複数の前記慣性センサーのノイズ周波数を検出するノイズ検出部と、
前記ノイズフィルターがノイズを除去するノイズフィルター特性を、前記ノイズ周波数を用いて決めるフィルター定数決定部と、を備えることを特徴とするロボット。
【請求項７】
前記ノイズ検出部は前記角度センサーが出力する信号の周波数分布と、前記慣性センサーが出力する信号の周波数分布と、を用いて前記慣性センサーが出力する信号のノイズ周波数を検出することを特徴とする請求項６に記載のロボット。
【請求項８】
前記ノイズフィルターは帯域除去フィルターであることを特徴とする、請求項７に記載のロボット。
【請求項９】
前記フィルター定数決定部は前記ノイズ周波数とテーブルとを参照してフィルター定数を決定することを特徴とする、請求項８に記載のロボット。
【請求項１０】
前記ノイズフィルターは前記制御部にソフトウェアで実装されていることを特徴とする、請求項９に記載のロボット。
【請求項１１】
駆動源の回動角度を検出する駆動源角度検出工程と、
慣性センサーを用いてアームに作用する慣性力を検出するアーム慣性力検出工程と、
前記駆動源角度検出工程にて検出した前記回動角度の信号と前記アーム慣性力検出工程にて検出した前記慣性力の信号とを用いて前記慣性センサーのノイズ周波数を検出するノイズ成分検出工程と、
前記検出した情報からノイズを除去するノイズフィルター特性を決定するフィルター定数決定工程と、
前記ノイズフィルター特性に基づいたノイズフィルターでノイズを除去するノイズ除去工程と、を有することを特徴とするロボットのノイズ除去方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図２】
【図３】
【図４】
【図５】
【図６】
【公開番号】特開２０１２−１３９８０７（Ｐ２０１２−１３９８０７Ａ）
【公開日】平成２４年７月２６日（２０１２．７．２６）
【国際特許分類】
【出願番号】特願２０１１−９７９（Ｐ２０１１−９７９）
【出願日】平成２３年１月６日（２０１１．１．６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
【公開日】平成２４年７月２６日（２０１２．７．２６）
【国際特許分類】
【出願日】平成２３年１月６日（２０１１．１．６）
【出願人】（０００００２３６９）セイコーエプソン株式会社 (51,324)
【Ｆターム（参考）】
[ Back to top ]