ロボットアームの制御方法
【課題】 柔軟機構を有するロボットアームにおいて、障害物への衝突衝撃を効果的に緩和することを可能としたロボットアームの制御方法を提供する。
【解決手段】 衝突検知部２は、角度センサ６８の出力情報を基に角度情報判定部４が判定した角度情報に基づいて逆動力学演算部２１と外乱トルク推定部２２により外乱トルクを推定して、衝突判定部２３により障害物への衝突を判定する。障害物への衝突を判定した場合には、修正運動目標値生成部１２は、衝突時の関節角度を基にして運動目標を修正し、指令値調整部１３により目標角度または角速度を調整して指令値切換部１４から制御指令部３へと駆動モータ６０の制御指令を送出する。
【解決手段】 衝突検知部２は、角度センサ６８の出力情報を基に角度情報判定部４が判定した角度情報に基づいて逆動力学演算部２１と外乱トルク推定部２２により外乱トルクを推定して、衝突判定部２３により障害物への衝突を判定する。障害物への衝突を判定した場合には、修正運動目標値生成部１２は、衝突時の関節角度を基にして運動目標を修正し、指令値調整部１３により目標角度または角速度を調整して指令値切換部１４から制御指令部３へと駆動モータ６０の制御指令を送出する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明はロボットアームの制御方法に関し、特に、関節に柔軟機構を有するロボットアームの障害物への衝突時の制御方法に関する。
【背景技術】
【０００２】
産業用ロボットなどのロボットアームに所定の運動をさせる際に、その経路上に障害物が存在すると、障害物によりアームの駆動が妨げられる。この状態で、関節の駆動指令が継続されると、駆動用モータや関節部あるいはアーム本体が破損に至るおそれがある。このような事態の発生を防ぐため、衝突の発生を検知して、モータを直ちに停止させる技術が知られている（例えば、特許文献１参照）。
【０００３】
この制御のためには、衝突発生時に速やかに衝突を検知する必要があり、特許文献１には、駆動軸モータの外乱トルクを推定して衝突を判定する技術が開示されている。
【特許文献１】特開平１１−２５４３８０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、関節部に柔軟機構を利用することで、柔軟な動きを実現するとともに、障害物への衝突時に当該障害物に対する衝突衝撃を緩和させることを可能としたロボットアームがある。この種のロボットアームでは、衝突発生時にモータを直ちに停止させたとしても柔軟機構を校正するバネやダンパーの減衰等の影響でリンク角度とモータ角度に不一致が生ずる。このため、駆動関節の先のアーム部を直ちに停止させることはできず、障害物への衝突衝撃を十分に緩和することができず、関節部やアーム本体の破損を招く可能性がある。
【０００５】
そこで本発明は、柔軟機構を有するロボットアームにおいて、障害物への衝突衝撃を効果的に緩和することを可能としたロボットアームの制御方法を提供することを課題とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するため、本発明にかかるロボットアームの制御方法は、関節部に柔軟機構を有するロボットアームの制御方法であって、（１）柔軟機構の各関節部の変位角を検出する工程と、（２）検出した変位角情報を基にして各関節に働く外乱トルクを推定する工程と、（３）推定した外乱トルクから障害物との衝突の有無を検知する工程と、（４）衝突が検知された場合に、障害物に与える衝突力に応じて新たな運動目標値を設定する工程と、（５）設定した運動目標値に対して動特性を与え、運動目標値を修正する工程と、（６）衝突を検知していない場合には、通常の運動目標値により、衝突検知後は修正した運動目標値により関節部を駆動する工程と、を備えていることを特徴とする。
【０００７】
衝突現象により生じる関節トルクを、関節部の変位角情報から推定した外乱トルクにより推定することで衝突判定を行う。柔軟機構を有するロボットアームでは、衝突直後にモータを停止させたとしても柔軟機構を構成する弾性体の影響でアームの動きを直ちに停止させることができない。そこで、モータを停止させるだけでなく、衝突力に応じてアームを停止させるのに必要な運動目標値を設定し、これを動特性に応じて修正して関節部を駆動することで、衝突時にアームを停止させたり、障害物から回避する等の行動をとる。
【発明の効果】
【０００８】
本発明によれば、衝突を検知して衝突力に応じて運動目標値を修正し、関節部を駆動することで、衝突時にアームの動きを直ちに停止させたり、衝突後に直ちに障害物から離隔することができる。これにより、衝突衝撃を緩和するとともに、障害物への接触時間を短縮して衝撃力を緩和することができる。また、運動目標値を適宜修正することで、障害物の動きに追従した動きを行わせることも可能となる。
【発明を実施するための最良の形態】
【０００９】
以下、添付図面を参照して本発明の好適な実施の形態について詳細に説明する。説明の理解を容易にするため、各図面において同一の構成要素に対しては可能な限り同一の参照番号を附し、重複する説明は省略する。
【００１０】
図１は、本発明に係るロボットアームの制御方法により制御されるロボットアームの概略図である。このロボットアーム１００は、多関節型の多軸式で多自由度を有するロボットアーム１００であり、右腕部の構成を示している。躯体５０には、上腕部５１と下腕部５２を経て、ハンド部５３が接続されている。上腕部５１、下腕部５２の両者には腕の延長方向に対して３軸方向に駆動を行う関節部が搭載されている。図２は各関節の構成を模式的に示す図である。
【００１１】
各柔軟関節は、躯体側のリンク本体に固定されているモータ６０の出力軸と、手先側のリンク本体６６の入力軸との間にプーリー６１、６２とギヤボックス６３、ダンパ６４、ばね６５が接続されて構成されている。プーリー６１、６２とギヤボックス６３はモータ６０の出力を減速する減速要素であり、ダンパ６４、ばね６５は、外部からの衝突等の突発外乱の衝撃を吸収するための減衰要素、弾性要素としてそれぞれ機能する。ここで、ダンパ６４、ばね６５は並列に配置されて減衰要素・弾性要素系を構成し、これと各減速要素６１〜６３とは直列に接続されている。
【００１２】
図３は、このロボットアーム１００の制御部２００のブロック構成図である。制御部２００は、ＣＰＵ、ＲＯＭ、ＲＡＭ等によって構成されており、指令値作成部１と、衝突検知部２、制御指令部３、角度情報判定部４を備える。指令値作成部１は、初期運動目標値生成部１１と、修正運動目標値生成部１２と、指令値調整部１３と、指令値切換部１４とを有する。衝突検知部２は、逆動力学演算部２１と、外乱トルク推定部２２と、衝突判定部２３とを備える。制御指令部３は、ロボットアーム１００の各駆動モータ６０に駆動信号を出力する。また、各関節部に配置される角度センサ６８の出力が角度情報判定部４に入力され、角度情報判定部４の判定結果が逆動力学演算部２１と、外乱トルク推定部２２および修正運動目標値生成部１２、指令値調整部１３へと入力されている。
【００１３】
以下、本実施形態におけるロボットアームの制御方法について具体的に説明する。ここでは、衝突検知および検知後の衝撃力緩和について述べる。図４は、その制御フローチャートである。
【００１４】
まず、角度情報判定部４が、各角度センサ６８から現在の関節部の各角度情報とモータ回転角情報を得る（ステップＳ１）。次に、角度情報判定部４は、関節部の各角度情報とモータの回転角情報をそれぞれ時間微分ないし２階微分することで角速度、角加速度情報を得る（ステップＳ３）。得られた角度情報、角速度情報、角加速度情報から逆動力学演算部２１が逆動力学的手法に基づいて関節駆動トルクを求める（ステップＳ５）。次に、関節駆動トルクからリンクに作用する外乱トルクを推定する（ステップＳ７）。
【００１５】
図５に示されるような１リンクアームを例にとると、その運動方程式は次式（１．１）（１．２）で表せる。
【００１６】
【数１】
【００１７】
ここで、θｍ、θｌはそれぞれモータとリンクの回転角度を、τｆは関節駆動トルクを、τｄはリンクに作用する外乱トルクをＪｍ、Ｊｌは、それぞれモータとリンクの慣性モーメントを、Ｇ（θ）は重力成分を、Ｋは関節部の弾性係数を、Ｄは関節部の粘性摩擦係数をそれぞれ示している。
【００１８】
ここで、柔軟要素で発生する力は（２）式で表せる
【００１９】
【数２】
【００２０】
柔軟要素の各パラメータは既知であり、モータ回転角、リンク回転角、これらの角速度は算出可能であるから、リンク部に働くトルク外乱は（３）式で表せる。
【００２１】
【数３】
【００２２】
（３）式から慣性項と重力項を実時間で計算することにより、外乱トルクτｄを算出可能であることがわかる。しかしながら、摩擦力については実験値などから推定せざるを得ないため、以下推定値については、上に記号＾を付加することで区別することとすると、トルク外乱の推定値は（４）式により表せる。
【００２３】
【数４】
【００２４】
推定値と真値が等しければ、理想的な非接触状態においてトルク外乱推定値は０となる。しかし、実際にはパラメータ誤差や摩擦等の影響により、非接触状態においてもトルク外乱推定値は０にならない場合が多い。そこで、閾値μを設定し、衝突判定部２３は、推定したトルク外乱値がμ以上の場合に、接触ありと判定し、μ未満の場合には、接触なしと判定する（ステップＳ９）。
【００２５】
【数５】
【００２６】
ここで、（５）式において、Ｈ＝１が接触状態、Ｈ＝０が非接触状態を意味する。非接触状態の場合には、ステップＳ１９へと移行し、初期運動目標値生成部１１で運動目標値θmrefを設定し、指令値作成部１は、この運動目標値θmrefに基づいてモータ駆動量を設定し（ステップＳ１５）、各駆動モータ６０を制御する（ステップＳ１７）ことによりロボットアームの駆動制御を行う。
【００２７】
ここで、初期運動目標値生成部１１は、関節を剛体とみなして駆動制御を行う。図５の１リンクアームにおいて、関節を剛体とみなすと、その運動方程式は次式により表せる。
【００２８】
【数６】
【００２９】
一方、ステップＳ９で接触状態と判定した場合には、衝撃力緩和制御に入る。ここで、修正運動目標値生成部１２、指令値調整部１３、指令値切換部１４、角度情報判定部４は、駆動モータ６０、柔軟要素、リンク系とともに、図６に示されるフィードバック制御構造を構成する。
【００３０】
ここで、衝突後のリンク目標角度（衝突後のアーム挙動を定義するものであり、衝突時点でアームを停止させるほか、アームを戻して障害物から回避する動作等が設定される。）をθlref、図６において修正運動目標値生成部１２におけるフィードバック制御１によって設定される駆動系の目標角度をθmrefとすると、指令値調整部１３におけるフィードバック制御２として、以下の式で表される制御を用いる。
【００３１】
【数７】
【００３２】
ここで、制御ゲインであるＫP2、Ｋi2、Ｋd2は、式（６）が安定化するように選定される。一方、目標指令を修正するためのフィードバック制御１としては、以下の式で表される制御を用いる。
【００３３】
【数８】
【００３４】
ここで、制御ゲインであるＫP1、Ｋi1、Ｋd1は、衝撃力が目標値に収まるように調整される。具体的には、リンク目標角度θlrefを設定し（ステップＳ１１）、フィードバック制御１により駆動系の目標角度θmrefを設定して（ステップＳ１３）、フィードバック制御２により、モータの駆動量ｕを設定する（ステップＳ１５）。設定した駆動量に応じてモータを制御する（ステップＳ１７）ことで、所望のリンク運動を実現する。
【００３５】
このようにして、衝突を検知した場合には、運動目標値を切り換えて、柔軟要素の挙動を考慮したフィードバック制御を行うことにより、衝突検出後、直ちにアームの運動を所望の状態に追従させることができ、衝突時の衝撃力を緩和することが可能となる。ここでは、１リンクアームの場合を例に説明したが、多関節（多リンク）の場合であっても各関節について同様の制御を行うことで、適用可能である。
【００３６】
本発明によるロボットアームの制御方法によるアーム挙動と従来の制御方法による衝突時のアーム挙動とをシミュレーションにより比較した結果を以下に述べる。図７に、シミュレーションの対象としたリンク系を示す。ここでは、図５に示されるリンク系と同様の１リンクアームを考える。モータ６０は、並列接続されたダンパ６４とばね６５を介してリンク６６に接続され、バネ要素を有する障害物６７と接触する。
【００３７】
ここでは、リンク角度０°から２．５°／秒の一定角速度で２秒後にリンク角度５°まで達し、５秒後までその状態を維持し、その後、−２．５°／秒の一定角速度でリンク角度０°まで戻す目標軌道を設定し、リンク角度４°の位置で障害物に接触するものとする。シミュレーションにおいては、同一のリンク系に対して、衝突後も目標軌道を変更せず維持するケース１と、衝突後に、衝突した瞬間のリンク角度に目標値を変更するケース２と、ケース２に加えてフィードバック制御１を追加したケース３（本発明にかかる制御手法に該当する。）について比較を行った。
【００３８】
シミュレーションの結果を図８〜図１１に示す。このうち、図８〜図１０は、それぞれケース１〜３の目標値とリンク角度、モータ角度の時間変化を示す図であり、図１１は、衝突衝撃力の時間変化を示す図である。ケース１の場合には、図８に示されるように、衝突後もモータ角度θｍを目標値に合致させようとするため、リンク角度θｌが衝突角度を超えている。つまり、障害物にアームが食い込む形になり、障害物の変形、破損をもたらす可能性がある。
【００３９】
ケース２の場合には、図９に示されるように、衝突直後にモータ角度θｍを速やかに追随させることはできるが、柔軟機構を有するため慣性力によってリンク角度θｌが追従するのに時間遅れを生ずる。この結果、リンク角度θｌは約０．２秒間に渡って衝突角度を超えるため、障害物へ衝撃力が付与されてしまう。
【００４０】
これに対して、ケース３の場合には、図１０に示されるように、衝突の瞬間にケース１、ケース２に比べて目標角度を進行方向とは逆方向へと大きく転ずることにより、モータを反転させ（モータ角度θｍが逆方向へ転ずる）ることで、リンクを速やかに障害物から回避させる。このため、リンク角度が衝突角度を超える時間、量を短縮して衝突衝撃を緩和する。このとき、リンク角度が衝突角度をほとんど超えずにすむため、障害物の変形や破損を防止することができる。特に、人体等へ衝突した場合には、衝突した人を傷つけるおそれが少なく、また、衝突回避行動をとるため、静止している人や物体のみならず、移動中の人や物体に対しても適切な回避行動を行い、衝突衝撃を緩和することもできる。
【００４１】
図１１に示されるように、ケース１の場合には、衝突後の衝撃力が大きく、その後も当初設定された目標角度が衝突角度を下回るまでの長時間にわたって障害物へ大きな力が付与されることになる。ケース２の場合には、衝撃力は、ケース１の場合に比較して、衝突の発生直後の短時間に限定され、衝突時の衝撃力もケース１の約３分の１程度まで低下する。さらに、ケース３の場合には、衝突力の発生は、衝突時の一瞬（ケース２の約３分の１）にすぎず、発生する衝突力もケース１の場合の約１０分の１まで低下する。このようにケース３、つまり、本発明にかかるロボットアームの制御方法においては、衝突衝撃を緩和する効果が高いことがわかった。
【図面の簡単な説明】
【００４２】
【図１】本発明に係るロボットアームの制御方法により制御されるロボットアームの概略図である。
【図２】図１のロボットアームの各関節の構成を模式的に示す図である。
【図３】図１のロボットアームの制御部のブロック構成図である。
【図４】本発明に係るロボットアームの制御方法の制御フローチャートである。
【図５】１リンクアームモデルを示す図である。
【図６】本発明におけるフィードバック制御構造を示す図である。
【図７】比較シミュレーションの対象としたリンク系を示す図である。
【図８】ケース１のシミュレーション結果を示すグラフである。
【図９】ケース２のシミュレーション結果を示すグラフである。
【図１０】ケース３のシミュレーション結果を示すグラフである。
【図１１】ケース１〜３のシミュレーション結果を示す図である。
【符号の説明】
【００４３】
１…指令値作成部、２…衝突検知部、３…制御指令部、４…角度情報判定部、１１…初期運動目標値生成部、１２…修正運動目標値生成部、１３…指令値調整部、１４…指令値切換部、２１…逆動力学演算部、２２…外乱トルク推定部、２３…衝突判定部、５０…躯体、５１…上腕部、５２…下腕部、５３…ハンド部、６０…駆動モータ、６１…プーリー、６３…ギヤボックス、６４…ダンパ、６５…ばね、６６…リンク本体、６７…障害物、６８…角度センサ、１００…ロボットアーム、２００…制御部。
【技術分野】
【０００１】
本発明はロボットアームの制御方法に関し、特に、関節に柔軟機構を有するロボットアームの障害物への衝突時の制御方法に関する。
【背景技術】
【０００２】
産業用ロボットなどのロボットアームに所定の運動をさせる際に、その経路上に障害物が存在すると、障害物によりアームの駆動が妨げられる。この状態で、関節の駆動指令が継続されると、駆動用モータや関節部あるいはアーム本体が破損に至るおそれがある。このような事態の発生を防ぐため、衝突の発生を検知して、モータを直ちに停止させる技術が知られている（例えば、特許文献１参照）。
【０００３】
この制御のためには、衝突発生時に速やかに衝突を検知する必要があり、特許文献１には、駆動軸モータの外乱トルクを推定して衝突を判定する技術が開示されている。
【特許文献１】特開平１１−２５４３８０号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
ところで、関節部に柔軟機構を利用することで、柔軟な動きを実現するとともに、障害物への衝突時に当該障害物に対する衝突衝撃を緩和させることを可能としたロボットアームがある。この種のロボットアームでは、衝突発生時にモータを直ちに停止させたとしても柔軟機構を校正するバネやダンパーの減衰等の影響でリンク角度とモータ角度に不一致が生ずる。このため、駆動関節の先のアーム部を直ちに停止させることはできず、障害物への衝突衝撃を十分に緩和することができず、関節部やアーム本体の破損を招く可能性がある。
【０００５】
そこで本発明は、柔軟機構を有するロボットアームにおいて、障害物への衝突衝撃を効果的に緩和することを可能としたロボットアームの制御方法を提供することを課題とする。
【課題を解決するための手段】
【０００６】
上記課題を解決するため、本発明にかかるロボットアームの制御方法は、関節部に柔軟機構を有するロボットアームの制御方法であって、（１）柔軟機構の各関節部の変位角を検出する工程と、（２）検出した変位角情報を基にして各関節に働く外乱トルクを推定する工程と、（３）推定した外乱トルクから障害物との衝突の有無を検知する工程と、（４）衝突が検知された場合に、障害物に与える衝突力に応じて新たな運動目標値を設定する工程と、（５）設定した運動目標値に対して動特性を与え、運動目標値を修正する工程と、（６）衝突を検知していない場合には、通常の運動目標値により、衝突検知後は修正した運動目標値により関節部を駆動する工程と、を備えていることを特徴とする。
【０００７】
衝突現象により生じる関節トルクを、関節部の変位角情報から推定した外乱トルクにより推定することで衝突判定を行う。柔軟機構を有するロボットアームでは、衝突直後にモータを停止させたとしても柔軟機構を構成する弾性体の影響でアームの動きを直ちに停止させることができない。そこで、モータを停止させるだけでなく、衝突力に応じてアームを停止させるのに必要な運動目標値を設定し、これを動特性に応じて修正して関節部を駆動することで、衝突時にアームを停止させたり、障害物から回避する等の行動をとる。
【発明の効果】
【０００８】
本発明によれば、衝突を検知して衝突力に応じて運動目標値を修正し、関節部を駆動することで、衝突時にアームの動きを直ちに停止させたり、衝突後に直ちに障害物から離隔することができる。これにより、衝突衝撃を緩和するとともに、障害物への接触時間を短縮して衝撃力を緩和することができる。また、運動目標値を適宜修正することで、障害物の動きに追従した動きを行わせることも可能となる。
【発明を実施するための最良の形態】
【０００９】
以下、添付図面を参照して本発明の好適な実施の形態について詳細に説明する。説明の理解を容易にするため、各図面において同一の構成要素に対しては可能な限り同一の参照番号を附し、重複する説明は省略する。
【００１０】
図１は、本発明に係るロボットアームの制御方法により制御されるロボットアームの概略図である。このロボットアーム１００は、多関節型の多軸式で多自由度を有するロボットアーム１００であり、右腕部の構成を示している。躯体５０には、上腕部５１と下腕部５２を経て、ハンド部５３が接続されている。上腕部５１、下腕部５２の両者には腕の延長方向に対して３軸方向に駆動を行う関節部が搭載されている。図２は各関節の構成を模式的に示す図である。
【００１１】
各柔軟関節は、躯体側のリンク本体に固定されているモータ６０の出力軸と、手先側のリンク本体６６の入力軸との間にプーリー６１、６２とギヤボックス６３、ダンパ６４、ばね６５が接続されて構成されている。プーリー６１、６２とギヤボックス６３はモータ６０の出力を減速する減速要素であり、ダンパ６４、ばね６５は、外部からの衝突等の突発外乱の衝撃を吸収するための減衰要素、弾性要素としてそれぞれ機能する。ここで、ダンパ６４、ばね６５は並列に配置されて減衰要素・弾性要素系を構成し、これと各減速要素６１〜６３とは直列に接続されている。
【００１２】
図３は、このロボットアーム１００の制御部２００のブロック構成図である。制御部２００は、ＣＰＵ、ＲＯＭ、ＲＡＭ等によって構成されており、指令値作成部１と、衝突検知部２、制御指令部３、角度情報判定部４を備える。指令値作成部１は、初期運動目標値生成部１１と、修正運動目標値生成部１２と、指令値調整部１３と、指令値切換部１４とを有する。衝突検知部２は、逆動力学演算部２１と、外乱トルク推定部２２と、衝突判定部２３とを備える。制御指令部３は、ロボットアーム１００の各駆動モータ６０に駆動信号を出力する。また、各関節部に配置される角度センサ６８の出力が角度情報判定部４に入力され、角度情報判定部４の判定結果が逆動力学演算部２１と、外乱トルク推定部２２および修正運動目標値生成部１２、指令値調整部１３へと入力されている。
【００１３】
以下、本実施形態におけるロボットアームの制御方法について具体的に説明する。ここでは、衝突検知および検知後の衝撃力緩和について述べる。図４は、その制御フローチャートである。
【００１４】
まず、角度情報判定部４が、各角度センサ６８から現在の関節部の各角度情報とモータ回転角情報を得る（ステップＳ１）。次に、角度情報判定部４は、関節部の各角度情報とモータの回転角情報をそれぞれ時間微分ないし２階微分することで角速度、角加速度情報を得る（ステップＳ３）。得られた角度情報、角速度情報、角加速度情報から逆動力学演算部２１が逆動力学的手法に基づいて関節駆動トルクを求める（ステップＳ５）。次に、関節駆動トルクからリンクに作用する外乱トルクを推定する（ステップＳ７）。
【００１５】
図５に示されるような１リンクアームを例にとると、その運動方程式は次式（１．１）（１．２）で表せる。
【００１６】
【数１】
【００１７】
ここで、θｍ、θｌはそれぞれモータとリンクの回転角度を、τｆは関節駆動トルクを、τｄはリンクに作用する外乱トルクをＪｍ、Ｊｌは、それぞれモータとリンクの慣性モーメントを、Ｇ（θ）は重力成分を、Ｋは関節部の弾性係数を、Ｄは関節部の粘性摩擦係数をそれぞれ示している。
【００１８】
ここで、柔軟要素で発生する力は（２）式で表せる
【００１９】
【数２】
【００２０】
柔軟要素の各パラメータは既知であり、モータ回転角、リンク回転角、これらの角速度は算出可能であるから、リンク部に働くトルク外乱は（３）式で表せる。
【００２１】
【数３】
【００２２】
（３）式から慣性項と重力項を実時間で計算することにより、外乱トルクτｄを算出可能であることがわかる。しかしながら、摩擦力については実験値などから推定せざるを得ないため、以下推定値については、上に記号＾を付加することで区別することとすると、トルク外乱の推定値は（４）式により表せる。
【００２３】
【数４】
【００２４】
推定値と真値が等しければ、理想的な非接触状態においてトルク外乱推定値は０となる。しかし、実際にはパラメータ誤差や摩擦等の影響により、非接触状態においてもトルク外乱推定値は０にならない場合が多い。そこで、閾値μを設定し、衝突判定部２３は、推定したトルク外乱値がμ以上の場合に、接触ありと判定し、μ未満の場合には、接触なしと判定する（ステップＳ９）。
【００２５】
【数５】
【００２６】
ここで、（５）式において、Ｈ＝１が接触状態、Ｈ＝０が非接触状態を意味する。非接触状態の場合には、ステップＳ１９へと移行し、初期運動目標値生成部１１で運動目標値θmrefを設定し、指令値作成部１は、この運動目標値θmrefに基づいてモータ駆動量を設定し（ステップＳ１５）、各駆動モータ６０を制御する（ステップＳ１７）ことによりロボットアームの駆動制御を行う。
【００２７】
ここで、初期運動目標値生成部１１は、関節を剛体とみなして駆動制御を行う。図５の１リンクアームにおいて、関節を剛体とみなすと、その運動方程式は次式により表せる。
【００２８】
【数６】
【００２９】
一方、ステップＳ９で接触状態と判定した場合には、衝撃力緩和制御に入る。ここで、修正運動目標値生成部１２、指令値調整部１３、指令値切換部１４、角度情報判定部４は、駆動モータ６０、柔軟要素、リンク系とともに、図６に示されるフィードバック制御構造を構成する。
【００３０】
ここで、衝突後のリンク目標角度（衝突後のアーム挙動を定義するものであり、衝突時点でアームを停止させるほか、アームを戻して障害物から回避する動作等が設定される。）をθlref、図６において修正運動目標値生成部１２におけるフィードバック制御１によって設定される駆動系の目標角度をθmrefとすると、指令値調整部１３におけるフィードバック制御２として、以下の式で表される制御を用いる。
【００３１】
【数７】
【００３２】
ここで、制御ゲインであるＫP2、Ｋi2、Ｋd2は、式（６）が安定化するように選定される。一方、目標指令を修正するためのフィードバック制御１としては、以下の式で表される制御を用いる。
【００３３】
【数８】
【００３４】
ここで、制御ゲインであるＫP1、Ｋi1、Ｋd1は、衝撃力が目標値に収まるように調整される。具体的には、リンク目標角度θlrefを設定し（ステップＳ１１）、フィードバック制御１により駆動系の目標角度θmrefを設定して（ステップＳ１３）、フィードバック制御２により、モータの駆動量ｕを設定する（ステップＳ１５）。設定した駆動量に応じてモータを制御する（ステップＳ１７）ことで、所望のリンク運動を実現する。
【００３５】
このようにして、衝突を検知した場合には、運動目標値を切り換えて、柔軟要素の挙動を考慮したフィードバック制御を行うことにより、衝突検出後、直ちにアームの運動を所望の状態に追従させることができ、衝突時の衝撃力を緩和することが可能となる。ここでは、１リンクアームの場合を例に説明したが、多関節（多リンク）の場合であっても各関節について同様の制御を行うことで、適用可能である。
【００３６】
本発明によるロボットアームの制御方法によるアーム挙動と従来の制御方法による衝突時のアーム挙動とをシミュレーションにより比較した結果を以下に述べる。図７に、シミュレーションの対象としたリンク系を示す。ここでは、図５に示されるリンク系と同様の１リンクアームを考える。モータ６０は、並列接続されたダンパ６４とばね６５を介してリンク６６に接続され、バネ要素を有する障害物６７と接触する。
【００３７】
ここでは、リンク角度０°から２．５°／秒の一定角速度で２秒後にリンク角度５°まで達し、５秒後までその状態を維持し、その後、−２．５°／秒の一定角速度でリンク角度０°まで戻す目標軌道を設定し、リンク角度４°の位置で障害物に接触するものとする。シミュレーションにおいては、同一のリンク系に対して、衝突後も目標軌道を変更せず維持するケース１と、衝突後に、衝突した瞬間のリンク角度に目標値を変更するケース２と、ケース２に加えてフィードバック制御１を追加したケース３（本発明にかかる制御手法に該当する。）について比較を行った。
【００３８】
シミュレーションの結果を図８〜図１１に示す。このうち、図８〜図１０は、それぞれケース１〜３の目標値とリンク角度、モータ角度の時間変化を示す図であり、図１１は、衝突衝撃力の時間変化を示す図である。ケース１の場合には、図８に示されるように、衝突後もモータ角度θｍを目標値に合致させようとするため、リンク角度θｌが衝突角度を超えている。つまり、障害物にアームが食い込む形になり、障害物の変形、破損をもたらす可能性がある。
【００３９】
ケース２の場合には、図９に示されるように、衝突直後にモータ角度θｍを速やかに追随させることはできるが、柔軟機構を有するため慣性力によってリンク角度θｌが追従するのに時間遅れを生ずる。この結果、リンク角度θｌは約０．２秒間に渡って衝突角度を超えるため、障害物へ衝撃力が付与されてしまう。
【００４０】
これに対して、ケース３の場合には、図１０に示されるように、衝突の瞬間にケース１、ケース２に比べて目標角度を進行方向とは逆方向へと大きく転ずることにより、モータを反転させ（モータ角度θｍが逆方向へ転ずる）ることで、リンクを速やかに障害物から回避させる。このため、リンク角度が衝突角度を超える時間、量を短縮して衝突衝撃を緩和する。このとき、リンク角度が衝突角度をほとんど超えずにすむため、障害物の変形や破損を防止することができる。特に、人体等へ衝突した場合には、衝突した人を傷つけるおそれが少なく、また、衝突回避行動をとるため、静止している人や物体のみならず、移動中の人や物体に対しても適切な回避行動を行い、衝突衝撃を緩和することもできる。
【００４１】
図１１に示されるように、ケース１の場合には、衝突後の衝撃力が大きく、その後も当初設定された目標角度が衝突角度を下回るまでの長時間にわたって障害物へ大きな力が付与されることになる。ケース２の場合には、衝撃力は、ケース１の場合に比較して、衝突の発生直後の短時間に限定され、衝突時の衝撃力もケース１の約３分の１程度まで低下する。さらに、ケース３の場合には、衝突力の発生は、衝突時の一瞬（ケース２の約３分の１）にすぎず、発生する衝突力もケース１の場合の約１０分の１まで低下する。このようにケース３、つまり、本発明にかかるロボットアームの制御方法においては、衝突衝撃を緩和する効果が高いことがわかった。
【図面の簡単な説明】
【００４２】
【図１】本発明に係るロボットアームの制御方法により制御されるロボットアームの概略図である。
【図２】図１のロボットアームの各関節の構成を模式的に示す図である。
【図３】図１のロボットアームの制御部のブロック構成図である。
【図４】本発明に係るロボットアームの制御方法の制御フローチャートである。
【図５】１リンクアームモデルを示す図である。
【図６】本発明におけるフィードバック制御構造を示す図である。
【図７】比較シミュレーションの対象としたリンク系を示す図である。
【図８】ケース１のシミュレーション結果を示すグラフである。
【図９】ケース２のシミュレーション結果を示すグラフである。
【図１０】ケース３のシミュレーション結果を示すグラフである。
【図１１】ケース１〜３のシミュレーション結果を示す図である。
【符号の説明】
【００４３】
１…指令値作成部、２…衝突検知部、３…制御指令部、４…角度情報判定部、１１…初期運動目標値生成部、１２…修正運動目標値生成部、１３…指令値調整部、１４…指令値切換部、２１…逆動力学演算部、２２…外乱トルク推定部、２３…衝突判定部、５０…躯体、５１…上腕部、５２…下腕部、５３…ハンド部、６０…駆動モータ、６１…プーリー、６３…ギヤボックス、６４…ダンパ、６５…ばね、６６…リンク本体、６７…障害物、６８…角度センサ、１００…ロボットアーム、２００…制御部。
【特許請求の範囲】
【請求項１】
関節部に柔軟機構を有するロボットアームの制御方法であって、
前記柔軟機構の各関節部の変位角を検出する工程と、
検出した変位角情報を基にして各関節に働く外乱トルクを推定する工程と、
推定した外乱トルクから障害物との衝突の有無を検知する工程と、
衝突が検知された場合に、障害物に与える衝突力に応じて新たな運動目標値を設定する工程と、
設定した運動目標値に対して動特性を与え、運動目標値を修正する工程と、
衝突を検知していない場合には、通常の運動目標値により、衝突検知後は修正した運動目標値により関節部を駆動する工程と、
を備えていることを特徴とするロボットアームの制御方法。
【請求項１】
関節部に柔軟機構を有するロボットアームの制御方法であって、
前記柔軟機構の各関節部の変位角を検出する工程と、
検出した変位角情報を基にして各関節に働く外乱トルクを推定する工程と、
推定した外乱トルクから障害物との衝突の有無を検知する工程と、
衝突が検知された場合に、障害物に与える衝突力に応じて新たな運動目標値を設定する工程と、
設定した運動目標値に対して動特性を与え、運動目標値を修正する工程と、
衝突を検知していない場合には、通常の運動目標値により、衝突検知後は修正した運動目標値により関節部を駆動する工程と、
を備えていることを特徴とするロボットアームの制御方法。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２００６−１６７８２０（Ｐ２００６−１６７８２０Ａ）
【公開日】平成１８年６月２９日（２００６．６．２９）
【国際特許分類】
【出願番号】特願２００４−３６０３３２（Ｐ２００４−３６０３３２）
【出願日】平成１６年１２月１３日（２００４．１２．１３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成１８年６月２９日（２００６．６．２９）
【国際特許分類】
【出願日】平成１６年１２月１３日（２００４．１２．１３）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]