(57)【特許請求の範囲】
【請求項１】
制御部によって制御されるロボットであって、
第１回転軸が設けられたアームと、
第２回転軸が設けられた動力部と、
前記第１回転軸と前記第２回転軸とを接続する接続部と、
前記第１回転軸の第１角速度を検出する第１角速度検出部と、
前記第２回転軸の第２角速度を検出する第２角速度検出部と、を備え、
障害物と前記アームとの衝突が検知された場合、前記第１角速度と前記第２角速度とに基づいて演算される捻れ角速度が所定の範囲内となるように、前記制御部によって前記動力部が制御されることを特徴とするロボット。
【請求項２】
請求項１に記載のロボットであって、
前記接続部は、前記第２回転軸の運動を減速する減速機を備えることを特徴とするロボット。
【請求項３】
請求項２に記載のロボットであって、
前記第１回転軸は、前記減速機を介して前記第２回転軸とＬ字に配置されていることを特徴とするロボット。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、回転動力源制御方法および回転動力源制御装置、並びにロボットに関する。
【背景技術】
【０００２】
従来、回転動力源となるモーターの回転軸の回転運動をアームに設けられた回転軸に伝達し、アームの動作をさせるロボットが知られている。また、モーターの回転運動によってアームを動作させるため、モーターと、アームとの間に、それぞれの回転軸を接続する接続部が設けられている。
【０００３】
これらロボットは、モーターの回転運動によってロボットのアームを動作させ、アームを所定の位置へ向かう運動をさせる際に、その位置へ向かう運動経路上に障害物が存在すると、障害物によってアームの運動が妨げられる。この状態で、モーターの回転運動が継続されると、モーターやアーム、また接続部（関節部）の破損につながる。
【０００４】
そこで、アームが障害物へ衝突したことを検知し、衝突の検知後直ちに、モーターの回転運動の制御を、アームの位置によるフィードバック制御から速度指令制御に切り替え、速度指令値をゼロとする。当該速度指令値と、観測（状態推定）されたモーターの回転運動の速度との差に応じて、モーターの速度制御を行い、速やかにアームを停止させる制御方法が知られている。
【０００５】
特許文献１には、アームを駆動させるモーターが減速機を介してアームと接続されている構造を有し、オブザーバ（状態推定器）を用いることによって、アームの衝突時にモーターが受ける推定外乱トルクを算出し、この推定外乱トルクから既知の外乱トルクを差し引くことにより外乱トルクの衝突成分を算出する。この算出した外乱トルクの衝突成分が予め設定された規定値を超えた時に、衝突の発生を検出したものと判断するようにしたロボットの衝突検出方法において、ロボットを衝突の発生がない状態で動作させ、このときの外乱トルクの衝突成分の最大値を算出し、この最大値に所定のマージン値を乗ずることにより前記規定値を自動的に設定する制御方法が開示されている。
【先行技術文献】
【特許文献】
【０００６】
【特許文献１】特開平１１―２５４３８０号公報
【発明の概要】
【発明が解決しようとする課題】
【０００７】
アームが障害物へ衝突した際の衝撃に対して最も弱い部位は、モーターの回転軸の回転を、アームの回転軸に伝達する接続部（関節部）を構成する減速機である。モーターの速度指令値を衝突の検知後、直ちにゼロとし、急激にアームの動作を停止させようとすると、接続部を構成する減速機に大きな力が加わり減速機の破損につながる。また、減速機の破損を防ぐために、モーターの回転軸トルクに一定の閾値を設けて急激にアームの動作を停止させない方法もある。
【０００８】
しかし、閾値を設けてモーター等の制御をおこなうと、閾値を小さくした場合、衝突の検知からアームの運動が停止するまでの時間が長くなり、衝突の検知からアームが停止するまでのアームの運動によって減速機などを破損させる力が生じる。また、閾値を大きくした場合、衝突の検知からアームの運動が停止するまでの時間が短くなるが、急激な制動によって減速機などを破損させる力が生じる。さらに、アームと衝突した被衝突物が、アームの運動方向と対向する方向に動作している場合、速度指令値がゼロであるとアームは被衝突物の動きに対向して動作することとなり、減速機に逆方向の捻れ力が加わり減速機を破損させる力が生じる。これらのことから、アームの衝突時に生じるこれらの力によって接続部を構成する減速機を破損させる課題があった。特許文献１では、前述の課題については触れられていない。
【０００９】
このことから、アームの衝突を検知した場合に、モーターと、アームと、接続部とを破損させることなく、アームの動作を停止させることができる回転動力源の制御方法および制御装置が要望されていた。
【課題を解決するための手段】
【００１０】
本発明は、上述した課題の少なくとも一部を解決するためになされたものであり、以下の形態または適用例として実現することが可能である。
【００１１】
（適用例１）本適用例に係る回転動力源制御装置は、動力部の回転動力が伝達され運動する第１回転軸と、動力部の回転動力を取り出す回転軸となる第２回転軸と、第１回転軸と、第２回転軸とを接続し、第２回転軸の運動を第１回転軸に伝達する接続部と、第１回転軸の第１角速度を検出する第１角速度検出部と、第２回転軸の第２角速度を検出する第２角速度検出部と、第１角速度と、第２角速度とに基づいて、第１回転軸と、第２回転軸との間に生じる捻れ角速度の演算をする捻れ角速度演算部と、捻れ角速度を所定の範囲内に制御する制御部と、を備えることを特徴とする。
【００１２】
このような回転動力源制御装置によれば、回転動力源となる動力部の動力を取り出す第２回転軸と、第２回転軸と接続部によって接続され、第２回転軸の運動が伝達される第１回転軸とを備える。また、第１回転軸の運動に伴う第１角速度を第１角速度検出部で検出し、第２回転軸の運動に伴う第２角速度を第２角速度検出部で検出することで、第１回転軸と、第２回転軸との接続部に生じる捻れ角速度の演算を捻れ角速度演算部でおこなうことができる。
【００１３】
このことによって、第１回転軸および第２回転軸の少なくとも一方に運動を阻害する力が加えられた時に、動力部の回転運動を制御部で制御することで第２回転軸の運動速度を制御し、第１回転軸と、第２回転軸との接続部に生じる捻れ角速度を所定の範囲内に制御することができる。従って、第１回転軸および第２回転軸の少なくとも一方の運動が阻害された場合に、接続部が破損する捻れ角速度を超えることを抑制し、第１回転軸と、第２回転軸との運動を停止させることができる。
【００１４】
（適用例２）上記適用例に係る回転動力源制御装置は、接続部に第２回転軸の運動を減速して第１回転軸に伝達をする減速機を備えることが好ましい。
【００１５】
このような回転動力源制御装置によれば、第２回転軸の運動を接続部に備えられた減速機によって第１回転軸へ減速して伝達することで、接続部で減速される比率に応じたトルクを第１回転軸で得ることができる。よって、第１回転軸と、第２回転軸との間に生じる捻れ角速度を所定の範囲内に制御する場合、減速しない場合と比較して広範に制御の目標値を設定することができる。従って、接続部が破損に至る捻れ角速度の許容値が緩和され、接続部の破損を抑制することができる。
【００１６】
（適用例３）本適用例に係る回転動力源制御方法は、動力部の回転動力によって運動する第１回転軸と、動力部の回転動力を取り出す第２回転軸と、第２回転軸の運動を第１回転軸に伝達する接続部とを備えた回転動力源制御装置の制御方法であって、第１回転軸の運動に伴う第１角速度を検出する第１角速度検出工程と、第２回転軸の運動に伴う第２角速度を検出する第２角速度検出工程と、第１回転軸と、第２回転軸との間に生じる捻れ角速度を演算する捻れ角速度演算工程と、捻れ角速度を所定の範囲内に動力部を制御する制御工程とを備え、捻れ角速度演算工程では、第１角速度と、第２角速度とに基づいて捻れ角速度の演算をおこない、制御工程では、回転軸速度指令工程からの速度指令に基づいて動力部の制御をおこなうことを特徴とする。
【００１７】
このような回転動力源制御方法によれば、回転動力源となる動力部の回転動力を取り出す第２回軸と、第２回転軸の運動が伝達され運動をする第１回転軸を備える。また、第１回転軸の運動に伴う第１角速度を第１角速度検出工程で検出し、第２回転軸の運動に伴う第２角速度を第２角速度検出工程で検出することで、第１回転軸と、第２回転軸との接続部に生じる捻れ角速度の演算を捻れ角速度演算工程でおこなうことができる。
【００１８】
このことによって、第１回転軸および第２回転軸の少なくても一方に運動を阻害する力が加えられた時、動力部の回転運動を制御工程で制御することで第２回転軸の運動速度を制御し、第１回転軸と、第２回転軸との接続部に生じる捻れ角速度を所定の範囲内に維持するように制御をすることができる。従って、第１回転軸および第２回転軸の少なくても一方の運動が阻害された場合に、接続部が破損する捻れ角速度を超えることを抑制し、第１回転軸と、第２回転軸との運動を停止させることができる。
【００１９】
（適用例４）本適用例に係るロボットは、第１回転軸の運動を動力源として運動するアーム部と、動力部の回転動力を取り出す回転軸となる第２回転軸と、第２回転軸の運動を第１回転軸に伝達する接続部とを備えたロボットであって、アーム部の運動に伴う第１角速度を検出する第１角速度検出工程と、第２回転軸の運動に伴う第２角速度を検出する第２角速度検出工程と、第１角速度と、第２角速度とに基づいて、第１回転軸と、第２回転軸との間に生じる捻れ角速度の演算をする捻れ角速度演算工程と、捻れ角速度が所定の範囲に動力部を制御する制御工程とを備え、第１角速度と、第２角速度とに基づいて、第１回転軸と、第２回転軸との捻れ角速度の演算を捻れ角速度演算工程によっておこない、捻れ角速度が所定の範囲を超えないように、制御工程に与えられる速度指令値に基づいて動力部の制御をおこなう上述の回転軸動力源制御装置を備えることを特徴とする。
【００２０】
このようなロボットによれば、アーム部の運動に伴う第１角速度を検出する第１角速度検出部と、アーム部に運動力を与える第１回転軸と、アーム部の運動の動力源となる動力部と、動力部から回転動力を取り出す回転軸となる第２回転軸と、第２回転軸の運動を第１回転軸に伝達する接続部と、第２回転軸の運動に伴う第２角速度を検出する第２角速度検出部を備える。このことによって、アーム部の第１角速度と、第２回転軸の第２角速度とに基づいて、第１回転軸と、第２回転軸との接続部に生じる捻れ角速度の演算を演算制御部でおこなうことができる。
【００２１】
したがって、アーム部、第１回転軸または第２回転軸の運動の少なくても一つに運動を阻害する力が与えられたときに、動力部を制御部で制御することで第２回転軸の回転速度を制御し、アーム部の運動を停止し、第１回転軸と、第２回転軸との接続部に生じる捻れ角速度を所定の範囲内に維持するように制御し接続部の破損を抑制することができる。
【図面の簡単な説明】
【００２２】
【図１】回転動力源制御方法を実行する回転源動力制御装置の概構成を示すブロック図。
【図２】回転動力源制御方法の各工程を示すフロー図。
【図３】回転動力源制御方法および回転動力源制御装置を用いて制御されるロボットを示す模式図。
【発明を実施するための形態】
【００２３】
以下、実施形態を図面に基づいて説明をする。なお、以下に示す各図においては、各構成要素を図面上で認識され得る程度の大きさとするため、各構成要素の寸法や比率を実際の構成要素とは適宜に異ならせて記載をしている。
【００２４】
（第１実施形態）
図１は、本実施形態の回転動力源制御装置１の概構成を示すブロック図である。回転動力源制御装置１は、駆動部７００と、第１角速度検出部２１０と、第２角速度検出部２２０と、演算制御部２０とを備えている。また、駆動部７００は、動力部７１０と、接続部７２０と、アーム部７３０とを備え、第２角速度検出部２２０は、角度センサー部２２１と、微分部２２２を備えている。
【００２５】
さらに、演算制御部２０は、回転軸速度指令部２５０と、捻れ角速度演算部３００と、捻れ角速度制御部４００と、捻れ角速度閾値設定部４５０と、回転軸速度制御部５００とが備えられている。
【００２６】
駆動部７００は、回転動力源となる動力部７１０に回転動力を取り出す第２回転軸７１１を有する。また、アーム部７３０は、当該アーム部７３０が備える第１回転軸７２１の回転によってアーム部７３０を運動させる。
【００２７】
接続部７２０は、第１回転軸７２１と、第２回転軸７１１とを減速機７５０を介して減速して接続する場合や、カップリングなどを介して等速で運動を伝達する場合がある。本実施形態では、減速機７５０を介して第２回転軸７１１の回転運動を減速して第１回転軸７２１へ伝達されている。
【００２８】
本実施形態の説明では、各数式を除き、角速度を表す符号を（数式１）に示す符号に置き換えて記載し説明する。
【００２９】
【数１】
0005849455.tif 000002
【００３０】
本実施形態の回転動力源制御装置１は、動力部７１０の回転動力を第２回転軸７２１によって取り出し、接続部７２０を介して、第１回転軸７２１へ伝達する。第１回転軸７２１の運動によってアーム部７３０が運動をする。アーム部７３０が運動をする際に、何らかの障害物と、アーム部７３０とが衝突をした場合、第１角速度検出部２１０と、第２角速度検出部２２０とで第１角速度ａと、第２角速度ｍを検出し、演算制御部２０で、第１回転軸７２１と、第２回転軸７１１との接続部７２０に生じる捻れ角速度ωの演算をする。演算された捻れ角速度ωを所定の範囲内に維持しつつ、アーム部７３０の運動を停止させるため、第２回転軸７２１の回転速度を制御する。
【００３１】
図２は、本実施形態の回転動力源制御装置１の制御方法の各工程を示すフロー図である。回転動力源制御方法は、衝突検知工程Ｓ１００と、第１角速度検出工程Ｓ２１０と、第２角速度検出工程Ｓ２２０と、回転軸速度指令工程Ｓ２５０とを備える。また、捻れ角速度演算工程Ｓ３００と、捻れ角速度制御工程Ｓ４００と、捻れ角速度閾値設定工程Ｓ４５０と、回転軸速度制御工程Ｓ５００と、駆動工程Ｓ７００と、アーム停止検知工程Ｓ８００とを備える。さらに、第２角速度検出工程Ｓ２２０は、回転軸角度検出工程Ｓ２２１と、回転軸角度微分工程Ｓ２２２とを備える。
【００３２】
本実施形態の回転動力源制御装置１の各工程を、各工程順に詳述する。
【００３３】
先ず、衝突検知工程Ｓ１００は、アーム部７３０が障害物などへ衝突したことを検知する。衝突を検知した場合は、衝突信号ＣＬを第１角速度検出工程Ｓ２１０と、第２角速度検出工程Ｓ２２０と、回転軸速度指令工程Ｓ２５０へ伝達する。
【００３４】
第１角速度検出工程Ｓ２１０は、前述の衝突検知工程Ｓ１００からの衝突信号ＣＬを受けて当該工程を開始する。第１角速度検出工程Ｓ２１０は、アーム部７３０の運動に伴う第１角速度ａを検出する。検出した第１角速度ａは、後述する捻れ角速度演算工程Ｓ３００に伝達される。
【００３５】
第２角速度検出工程Ｓ２２０は、前述の衝突検知工程Ｓ１００からの衝突信号ＣＬを受けて当該工程を開始する。第２角速度検出工程Ｓ２２０は、回転軸角度検出工程Ｓ２２１と、回転軸角度微分工程Ｓ２２２を備え、動力部７１０の回転軸となる第２回転軸７１１の角速度である第２角速度ｍを検出する。第２角速度検出工程Ｓ２２０による第２角速度ｍの検出は、第２回転軸７１１の角度を回転軸角度検出工程Ｓ２２１で検出し、検出された第２回転軸７１１の角度を回転軸角度微分工程Ｓ２２２によって微分することで第２回転軸７１１の角速度である第２角速度ｍを検出する。検出された第２角速度ｍは、後述する捻れ角速度演算工程Ｓ３００と、回転軸速度制御工程Ｓ５００に伝達される。
【００３６】
回転軸速度指令工程Ｓ２５０は、前述の衝突検知工程Ｓ１００からの衝突信号ＣＬを受けて当該工程を開始する。回転軸速度指令工程Ｓ２５０は、予め設定された速度指令値Ｖｃを衝突検知工程Ｓ１００からの衝突信号ＣＬを受信した時に、回転軸速度制御工程Ｓ５００へ速度指令値Ｖｃとして出力し伝達をする。
【００３７】
なお、前述の第１角速度検出工程Ｓ２１０と、第２角速度検出工程Ｓ２２０と、回転軸速度指令工程Ｓ２５０とは、衝突検知工程Ｓ１００からの衝突信号ＣＬを受けて略同時に各工程が実行される。
【００３８】
捻れ角速度演算工程Ｓ３００は、アーム部７３０の角速度ａと、第２回転軸の第２角速度ｍとに基づいて第１回転軸７２１と、第２回転軸７１１との間に設けられる接続部７２０（減速機７５０）に生じる捻れ角速度ωを演算する。演算された捻れ角速度ωは、捻れ角速度制御工程Ｓ４００に伝達される。なお、接続部７２０において第１回転軸７２１と、第２回転軸７１１とが減速機７５０を介して接続されている場合は、減速比Ｎを考慮して演算することとし、本実施形態では、減速して接続されている実施形態として、減速比Ｎを考慮して演算する場合を説明する。
【００３９】
ここで捻れ角速度ωは、減速比をＮ、第２回転軸７１１の角度をθｍ、アーム部７３０の角度をθａとした場合、次式（数２）によって演算される。
【００４０】
【数２】
0005849455.tif 000003
【００４１】
捻れ角速度制御工程Ｓ４００は、捻れ角速度演算工程Ｓ３００で演算された捻れ角速度ωと、捻れ角速度閾値設定工程Ｓ４５０で設定される捻れ角速度閾値±αとに基づき、設定される捻れ角速度閾値±αの範囲内に捻れ角速度ωが収まるように制御するため、捻れ角速度閾値±αと、捻れ角速度ωとの差にゲインをかけた速度制御信号Ｔｓを回転軸速度制御工程Ｓ５００へ出力する。
【００４２】
ここで、捻れ角速度閾値設定工程Ｓ４５０で設定される捻れ角速度閾値±αは、第１回転軸７２１と、第２回転軸７１１とが接続される接続部７２０を構成する減速機７５０、もしくは等速で接続される場合は、カップリングなどが座屈を起こさない捻れ角速度ωとなる範囲、すなわち即ち、捻れ角速度閾値±αを設定する。
【００４３】
捻れ角速度閾値±αは、接続部７２０（減速機７５０）の座屈トルクτＢと、アーム部７３０の慣性モーメントＩと、捻れ角速度ωとに基づき、次式（数３）から（数９）によって計算することができる。なお、座屈トルクτＢは接続部７２０（減速機７５０）の仕様によって定まり、当該仕様に基づき座屈トルクτＢの値を代入する。
【００４４】
【数３】
0005849455.tif 000004
【００４５】
【数４】
0005849455.tif 000005
【００４６】
前記（数３）および（数４）とすれば、（数３）は、次式（数５）として表すことができる。
【００４７】
【数５】
0005849455.tif 000006
【００４８】
さらに、制御周期をΔｔとすれば、次式（数６）として表すことができ、制御周期Δｔが短ければ次式（数７）として表すことができる。
【００４９】
【数６】
0005849455.tif 000007
【００５０】
【数７】
0005849455.tif 000008
【００５１】
前記（式６）および（式７）から座屈を起こさない捻れ角速度ωの範囲は次式（式８）で表され、捻れ角速度閾値±αは次式（式９）で表される。
【００５２】
【数８】
0005849455.tif 000009
【００５３】
【数９】
0005849455.tif 000010
【００５４】
上述の通り、接続部７２０（減速機７５０）が座屈を起こさない捻れ角速度ωの範囲は、−α＜捻れ角速度ω＜α（αは、捻れ角速度閾値を示す。）であり、当該捻れ角速度閾値±αを捻れ角速度閾値設定工程Ｓ４５０によって設定し、捻れ角速度制御工程Ｓ４００へ捻れ角速度閾値αを伝達する。
【００５５】
次に回転軸速度制御工程Ｓ５００は、回転軸速度指令工程Ｓ２５０から指令された速度指令値Ｖｃと、前述の捻れ角速度制御工程Ｓ４００から出力された速度制御信号Ｔｓと、第２角速度検出工程Ｓ２２０から出力された第２回転軸の第２角速度ｍとに基づき、動力部７１０の速度制御を行う。回転軸速度制御工程Ｓ５００は、速度指令値Ｖｃと、速度制御信号Ｔｓとの差とに基づき、第２角速度ｍが捻れ角速度閾値±αの範囲内に収まるように駆動工程Ｓ７００を制御する。
【００５６】
駆動工程Ｓ７００は、動力部７１０と、アーム部７３０と、回転動力源となる動力部７１０の第２回転軸７１１の回転を、アーム部７３０の動力源となる第１回転軸７２１に伝達をする接続部７２０とを備えている。駆動工程Ｓ７００は、衝突検知工程Ｓ１００で衝突検知した場合、前述の回転軸速度制御工程Ｓ５００によって、捻れ角速度閾値±αの範囲内に制御される。
【００５７】
アーム停止検知工程Ｓ８００は、アーム部７３０の停止を検知する。アーム停止検知工程Ｓ８００は、アーム部７３０の停止を検知できない場合、第１角速度検出工程Ｓ２１０と、第２角速度検出工程Ｓ２２０と、回転軸速度指令工程Ｓ２５０とから再度、回転動力源制御方法の各工程を実施する。アーム部７３０の停止を検知できた場合は、当該制御方法の各工程を終了する。
【００５８】
次に、上述した回転動力源制御方法を実行する回転動力源制御装置１について、各工程との対応関係を含めて説明する。
【００５９】
第１回転軸７２１の回転によって運動可能なアーム部７３０は、運動経路上に存在する何らかの障害物と衝突をしたことを検知する衝突検知センサー（不図示）が備えられている。アーム部７３０の衝突を検知した時、後述する第１角速度検出部２１０と、第２角速度検出部２２０と、回転軸速度指令部２５０とに当該衝突信号ＣＬを伝達する。これは、衝突検知工程Ｓ１００に対応する。
【００６０】
第１角速度検出部２１０は、アーム部７３０の運動に伴う角速度となる第１角速度ａを検出する。第１角速度検出部２１０の角速度センサー（不図示）は、アーム部７３０に取りつけられている。検出された第１角速度ａは、後述する捻れ角速度演算部３００に伝達される。これは、第１角速度検出工程Ｓ２１０に対応する。
【００６１】
第２角速度検出部２２０は、第２回転軸７１１の角速度となる第２角速度ｍを検出する。第２角速度検出部２２０は、第２回転軸７１１の角度を計測する角度センサー部２２１と、計測された角度を微分することで第２角速度ｍに変換する微分部２２２を備えている。検出された第２角速度ｍは、後述する捻れ角速度演算部３００に伝達される。これは、第２角速度検出工程Ｓ２２０に対応する。
【００６２】
捻れ角速度演算部３００は、前述の第１角速度検出部２１０と、第２角速度検出部２２０とに基づき、第１回転軸７２１と、第２回転軸７１１との接続部７２０に生じる捻れ角速度ωの演算をおこなう。演算された捻れ角速度ωは、後述する捻れ角速度制御部４００に伝達される。これは、捻れ角速度演算工程Ｓ３００に対応する。
【００６３】
回転軸速度指令部２５０は、衝突信号ＣＬを受けて予め設定されている速度指令値Ｖｃを回転軸速度制御部５００へ出力する。
【００６４】
捻れ角速度制御部４００は、前述の捻れ角速度演算部３００で演算された捻れ角速度ωと、捻れ角速度閾値設定部４５０で設定される捻れ角速度閾値αとを比較し、差分に応じた速度制御信号Ｔｓを回転軸速度制御部５００へ出力する。これは、捻れ角速度制御工程Ｓ４００に対応する。
【００６５】
捻れ角速度閾値設定部４５０は、演算された捻れ角速度閾値αを前述の捻れ角速度制御部４００に与える。これは、捻れ角速度閾値設定工程Ｓ４５０に対応する。
【００６６】
回転軸速度制御部５００は、前述の捻れ角速度制御部４００から出力された速度制御信号Ｔｓと、回転軸速度指令部２５０から出力された速度指令値Ｖｃとを比較し、動力部７１０の第２回転軸７１１の速度制御をおこなう。これは回転軸速度制御工程Ｓ５００に対応する。
【００６７】
駆動部７００は、前述の回転軸速度制御部５００によって制御され、アーム部７３０の衝突が検知された後、接続部７２０（減速機７５０）の座屈トルクτＢを超えない第１回転軸７２１と、第２回転軸７１１との捻れ角速度ωとなるように動力部７１０の回転速度を制御する。
【００６８】
上述した実施形態によれば、以下の効果が得られる。
本実施形態の回転動力源制御方法および回転動力源制御装置１によれば、動力部７１０の回転動力を取り出す第２回転軸７１１と、第２回転軸７１１の運動が伝達される第１回転軸７２１との第２角速度ｍおよび第１角速度ａを、第２角速度検出工程Ｓ２２０および第１角速度検出工程Ｓ２１０で検出する。第１角速度ａと、第２角速度ｍに基づき、捻れ角速度演算工程Ｓ３００で第１回転軸７２１と、第２回転軸７１１との接続部７２０に生じる捻れ角速度ωを演算することができる。
【００６９】
よって、第１回転軸７２１および第２回転軸７１１の少なくても一方の運動を阻害する力が加えられた時、接続部７２０に生じる捻れ角速度ωを、捻れ角速度制御工程Ｓ４００および回転軸速度制御工程Ｓ５００を構成する捻れ角速度制御部４００および回転軸速度制御部５００によって、接続部７２０（減速機７５０）が破損（座屈）しない所定の捻れ角速度ωの範囲、すなわち捻れ角速度閾値±αの範囲に制御をすることができる。
【００７０】
従って、第１回転軸７２１および第２回転軸７１１の少なくても一方の運動が阻害された場合、接続部７２０や第１回転軸７２１の回転によって運動をするアーム部７３０を破損させることなくアーム部７３０の運動を停止させることができる回転動力源制御方法と当該制御方法を実行する回転動力源制御装置１を実現することができる。
【００７１】
本実施形態の回転動力源制御装置１によれば、第２回転軸７１１の回転運動を減速して第１回転軸７２１へ伝達することで、接続部７２０を構成する減速機７５０で減速される比率（減速比）に応じたトルクを第１回転軸７２１で得ることができる。従って、第１回転軸７２１と、第２回転軸７１１と間に生じる捻れ角速度ωを捻れ角速度閾値±αの範囲内に維持をするように制御する場合、減速機７５０を備えない場合と比較して、広範に捻れ角速度閾値±αの制御範囲を設定することができる。
【００７２】
（第２実施形態）
本実施形態は、第１実施形態で述べた回転動力源制御方法と、当該制御方法を用いたロボット２である。図３は、本実施形態のロボット２の概構成を示す模式図である。図３を用いて本実施形態のロボット２について説明する。なお、第１実施形態の回転動力源制御方法と構成および工程は同様のため、同様の部分は、構成および工程に同様の符号を付して説明は省略または簡略とする。
【００７３】
図３に示すロボット２は、駆動部７００ａと、駆動部７００ｂとの２組を備える、いわゆる２軸スカラロボットを示している。なお、駆動部７００が１組以上あれば本発明をロボット２に適用できる。本実施形態では、駆動部７００ａと、駆動部７００ｂは同様の構成かつ同様の動作および制御がされるため、駆動部７００ａを代表して説明する。
【００７４】
本実施形態のロボット２は、駆動部７００ａを備え、演算制御部２０によって制御される。駆動部７００は、アーム部７３０ａと、動力部７１０ａと、接続部７２０ａとを備える。また、演算制御部２０は、第１角速度検出部２１０ａと、第２角速度検出部２２０ａと、演算制御部２０を備える。また、第２角速度検出部２２０ａは、角度センサー部２２１ａと，微分部２２２ａとで構成される。さらに、演算制御部２０は、捻れ角速度演算部３００と、捻れ角速度制御部４００と、捻れ角速度閾値設定部４５０と、回転軸速度制御部５００と、回転軸速度指令部２５０とを備える。
【００７５】
ロボット２の制御方法は、第１実施形態の回転動力源制御方法と同様に、衝突検知工程Ｓ１００と、第１角速度検出工程Ｓ２１０と、第２角速度検出工程Ｓ２２０と、回転軸速度指令工程Ｓ２５０と、捻れ角速度演算工程Ｓ３００と、捻れ角速度制御工程Ｓ４００と、捻れ角速度閾値設定工程Ｓ４５０と、回転軸速度制御工程Ｓ５００と、駆動工程Ｓ７００と、アーム停止検知工程Ｓ８００とを備える。
【００７６】
ロボット２は、アーム部７３０ａの動作支点となる関節部７０ａに、駆動部７００ａを構成する動力部７１０ａと、動力部７１０ａの回転運動を取り出す第２回転軸７１１ａ，と、第２回転軸７１１ａの運動をアーム部７３０ａに設けられた第１回転軸７２１ａに伝達する減速機７５０ａで構成される接続部７２０ａを備える。
【００７７】
ロボット２は、関節部７０ａを支点としてアーム部７３０ａを運動させる。このアーム部７３０ａが運動する際に、何らかの障害物と、アーム部７３０ａとが衝突をした場合、アーム部７３０ａの運動を停止させるため、第１角速度検出部２１０と、第２角速度検出部２２０と、演算制御部２０とによって、第１回転軸７２１ａと、第２回転軸７１１ａとの接続部７２０ａに生じる捻れ角速度ωの演算をする。演算された捻れ角速度ωを所定の範囲内に維持しつつ、第２回転軸７１１ａの回転速度を制御し、アーム部７３０ａを停止させる。
【００７８】
関節部７０ａは、動力部７１０ａと、第２回転軸７１１ａと、接続部７２０ａを構成する減速機７５０ａとを備える。また、減速機７５０ａは、第１回転軸７２１ａを有し、アーム部７３０ａに設けられている。このため、動力部７１０ａの回転運動を第２回転軸７１１ａで取り出し、減速機７５０ａを介して第１回転軸７２１ａに伝達することから、関節部７０ａを支点としてアーム部７３０ａを運動させることができる。
【００７９】
次に、ロボット２の動作について工程順に詳述する。アーム部７３０ａが運動をする際に、運動経路上で何らかの障害物がアーム部７３０ａと衝突した場合、衝突検知工程Ｓ１００によって、アーム部７３０ａと、障害物との衝突を検知し、衝突信号ＣＬを第１角速度検出工程Ｓ２１０と、第２角速度検出工程Ｓ２２０と、回転軸速度指令工程Ｓ２５０に伝達する。
【００８０】
第１角速度検出工程Ｓ２１０は、アーム部７３０ａの衝突信号ＣＬが伝達された場合、アーム部７３０ａの第１角速度ａを第１角速度検出部２１０ａによって検出し、衝突時のアーム部７３０ａの第１角速度ａを検出する。また、第１角速度検出部２１０ａの取付位置は、アーム部７３０ａに取りつけられていれば良い。本実施形態では、第１角速度検出部２１０ａは、アーム部７３０ａに取りつけられている。
【００８１】
また同時に、第２角速度検出工程Ｓ２２０は、動力部７１０ａの第２回転軸７１１ａの角度を角度センサー部２２１ａによって検出し、検出された角度を微分部２２２ａで微分することで、第２角速度ｍを検出する。また、角度センサー部２２１ａの取付位置は、第２回転軸７１１ａに取りつけられていれば良い。本実施形態では、第２角速度検出部２２０ａを構成する角度センサー部２２１ａおよび微分部２２２ａは、接続部７２０ａが備えられる第２回転軸７１１ａの端部とは反対の第２回転軸７１１ａの端部に設けられている。
【００８２】
さらに同時に、回転軸速度指令工程Ｓ２５０は、アーム部７３０ａを駆動する動力部７１０の回転軸速度制御をおこなう回転軸速度制御工程Ｓ５００に速度指令値Ｖｃを出力する。
【００８３】
捻れ角速度演算工程Ｓ３００は、前述のアーム部７３０ａの第１角速度ａと、動力部７１０ａの第２回転軸の第２角速度ｍとに基づき、アーム部７３０ａの運動をさせる第１回転軸７２１ａと、動力部７１０ａの第２回転軸７１１ａとを接続する接続部７２０ａに生じる捻れ角速度ωの演算を捻れ角速度演算部３００によって演算をする。
【００８４】
捻れ角速度制御工程Ｓ４００は、前述の捻れ角速度ωと、後述する捻れ角速度閾値設定工程Ｓ４５０で設定される捻れ角速度閾値±αとを比較し、差分に基づき速度制御信号Ｔｓを捻れ角速度制御部４００によって、回転軸速度制御工程Ｓ５００に伝達する。
【００８５】
捻れ角速度閾値設定工程Ｓ４５０は、第１実施形態と同様に、第２回転軸７１１ａの回転を第１回転軸７２１ａへ伝達する接続部７２０（減速機７５０ａ）の座屈トルクτＢに基づいて、接続部７２０ａ（減速機７５０ａ）に破損を生じさせない捻れ角速度ωの範囲、すなわち捻れ角速度閾値±αを、捻れ角速度閾値設定部４５０によって設定し、捻れ角速度制御部４００に伝達する。
【００８６】
回転軸速度制御工程Ｓ５００は、第１実施形態と同様に、回転軸速度指令部２５０から出力された速度指令値Ｖｃと、速度制御信号Ｔｓとを比較し、第２回転軸７１１ａの速度制御を回転軸速度制御部５００によっておこなう。
【００８７】
駆動工程Ｓ７００は、前述の回転軸速度制御部５００によって、動力部７１０ａの第２回転軸７１１ａの速度が制御され、アーム部７３０ａの衝突が検知された後、接続部７２０ａ（減速機７５０ａ）の座屈トルクτＢを超えない第１回転軸７２１ａと、第２回転軸７１１ａとの捻れ角速度ωとなるように制御される。
【００８８】
アーム停止検知工程Ｓ８００は、アーム部７３０ａの停止を検知する。アーム停止検知工程Ｓ８００は、アーム部７３０ａの停止を検知できない場合、第１角速度検出工程Ｓ２１０と、第２角速度検出工程Ｓ２２０と、回転軸速度指令工程Ｓ２５０とから、再度ロボット制御方法の各工程を実施する。アーム部７３０ａの停止を検知できた場合は、当該制御方法の各工程を終了する。
【００８９】
なお、障害物が動体、かつアーム部７３０ａの運動方向と対向する方向に運動する場合、アーム部７３０ａの動力源となる第１回転軸７２１ａと、第１回転軸７２１ａと、第２回転軸７１１ａとの接続部７２０ａ（減速機７５０ａ）に大きな破壊力が加わる。この場合、衝突時のアーム部７３０ａの角速度ａと、第２回転軸の第２角速度ｍに基づき、捻れ角速度制御工程Ｓ４００は、障害物の運動方向と同じ方向にアーム部７３０ａを運動させる速度制御信号Ｔｓを出力する。これによって、障害物の運動方向と同じ方向にアーム部７３０ａを運動させることで、捻れ角速度閾値±αの範囲内に動力部７１０ａを制御し、駆動部７００の破損を抑制する。
【００９０】
また、障害物が動体、かつアーム部７３０ａの運動方向と同じ方向に運動する場合、アーム部７３０ａの運動に過速力が加わり、駆動部７００に破壊力が加わる。この場合、上述と同様に、捻れ角速度制御工程Ｓ４００は、障害物と同じ方向にアーム部７３０ａを運動させる速度制御信号Ｔｓを出力し、障害物の運動方向と同じ方向にアーム部７３０ａを運動させることで、駆動部７００の破損を抑制する。
【００９１】
上述した実施形態によれば、以下の効果が得られる。
【００９２】
本実施形態のロボット２によれば、アーム部７３０ａの運動に伴う第１角速度ａを検出する第１角速度検出部２１０ａと、動力部７１０ａから回転動力を取り出し、第１回転軸７２１ａに運動を伝達する第２回転軸７１１ａと、第２回転軸の運動に伴う第２角速度ｍを検出する第２角速度検出部２２０となる角度センサー部２２１ａおよび微分部２２２ａを備える。
【００９３】
このことによって、アーム部７３０ａに障害物が衝突した際、アーム部７３０ａの第１角速度ａと、第２回転軸７１１ａの第２角速度ｍとに基づいて、第１回転軸７２１ａと、第２回転軸７１１ａとを接続する接続部７２０ａに生じる捻れ角速度ωを、捻れ角速度演算部３００で演算することができる。また、アーム部７３０ａに障害物が衝突し、アーム部７３０ａおよび第１回転軸７２１ａの運動を阻害する力が加えられた時に、第２回転軸７１１ａの回転速度を、捻れ角速度制御部４００および回転軸速度制御部５００で制御をすることができる。
【００９４】
このことによって、アーム部７３０ａの運動中にアーム部７３０ａと、障害物とが衝突した場合、接続部７２０ａに生じる捻れ角速度ωを、捻れ角速度制御部４００および回転軸速度制御部５００によって、接続部７２０ａ（減速機７５０ａ）が破損（座屈）しない所定の捻れ角速度ωの閾値である捻れ角速度閾値±αの範囲内に制御することができる。従って、障害物とアーム部７３０ａなどが衝突した場合に、アーム部７３０ａや接続部７２０ａを構成する減速機７５０を破損させることなく、アーム部７３０ａの動作を停止させることができるロボット２を実現することができる。
【００９５】
なお、上述した実施形態に限定されず、その要旨を逸脱しない範囲において種々の変更や改良等を加えて実施する事が可能である。変形例を以下に述べる。
【００９６】
（変形例１）第１実施形態および第２実施形態の動力部７１０は、動力源の種類に限定されることなく、回転運動を動力として取り出せる動力源であれば良い。例えば、電動機（モーター）を用いることができる。回転軸速度制御部５００から出力される信号に基づきモーターを回転させる電圧、電流、周波数、またはこれらを組み合わせて制御することで本発明の回転動力源制御方法および当該制御装置を実施することができる。
【００９７】
（変形例２）第１実施形態および第２実施形態の接続部７２０は、第１回転軸７２１と、第２回転軸７１１とが同軸に接続される接続部７２０、もしくは接続部７２０として減速機７５０を用いている。しかし、これに限定されることなく減速機７５０を介して第１回転軸７２１と、第２回転軸７１１とを並行に、またはＬ字に配置しても本発明の回転軸動力源制御方法および当該制御装置を実施することができる。
【００９８】
（変形例３）第１実施形態および第２実施形態の第１回転軸には、アーム部７３０が備えられている。しかし、これに限定されることなく動力部７１０の回転運動を第２回転軸で取り出し、その運動を第１回転軸に伝達をする動力伝達機構にも本発明の回転動力源制御方法および当該制御装置を実施することができる。
【符号の説明】
【００９９】
１…回転動力源制御装置、２…ロボット、２０…演算制御部、７０…関節部、２１０…第１角速度検出部、２２０…第２角速度検出部、２２１…角度センサー部、２２２…微分部、２５０…回転軸速度指令部、３００…捻れ角速度演算部、４００…捻れ角速度制御部、４５０…捻れ角速度閾値設定部、５００…回転軸速度制御部、７００…駆動部、７１０…動力部、７２０…接続部、７３０…アーム部、７５０…減速機、Ｓ１００…衝突検知工程、Ｓ２１０…第１角速度検出工程、Ｓ２２０…第２角速度検出工程、Ｓ２２１…回転軸角度検出工程、Ｓ２２２…回転軸角度微分工程、Ｓ２５０…回転軸速度指令工程、Ｓ３００…捻れ角速度演算工程、Ｓ４００…捻れ角速度制御工程、Ｓ４５０…捻れ角速度閾値設定工程、Ｓ５００…回転軸速度制御工程、Ｓ７００…駆動工程、Ｓ８００…アーム停止検知工程。
【図１】
0005849455.tif 000011
【図２】
0005849455.tif 000012
【図３】
0005849455.tif 000013