(57)【特許請求の範囲】
【請求項１】
先端を被押圧物体の表面に沿って移動させながら前記被押圧物体に当接させて押圧するアームの先端に、前記被押圧物体の押圧によって前記アームの先端に作用する力を検出するための力センサーを設け、この力センサーの検出値に基づいて前記被押圧物体の剛性係数を推定し、同剛性係数から前記被押圧物体との粘性係数を目標粘性として算出して、この目標粘性と前記剛性係数に応じて前記アームを移動させるロボットの力制御方法において、
前記アームの先端に作用する力Ｆを［fx(t)  ０  fz(t)］、
前記アームの先端の位置を［x(t) y(t) z(t)  φ(t)  θ(t)  ψ(t)］、
前記被押圧物体の粘性係数をBm、剛性係数をKm、アームの先端が前記被押圧物体との接触により受ける外力をＦとし、
Ｌをラベル数として前件部ファジィ集合を
【数１】
0004094781.tif 000070
とし、ｘ方向のファジィ規則を
【数２】
0004094781.tif 000071
として、
【数３】
0004094781.tif 000072
の推論入力により、Km1i(i=1,...,L)を後件部定数値として、
【数４】
0004094781.tif 000073
の推定規則を得て、ｘ方向の剛性係数の推定値を、メンバーシップ関数の中心値をαj、標準偏差の逆数をβj、添え字ｊをファジィルールの番号（j=1,...,L)を表すものとして、
【数５】
0004094781.tif 000074
【数６】
0004094781.tif 000075
【数７】
0004094781.tif 000076
とし、同様に、
【数８】
0004094781.tif 000077
の推論入力によりｚ方向の剛性係数の推定値を求めて前記被押圧物体の剛性係数を推定し、
Ｍを実慣性行列、Ｊをヤコビアン、Ｓをスイッチ行列、Ｅを単位行列、Ｋfを力フィードバックゲイン行列として、前記剛性係数の推定値から臨界減衰条件により算出される剛性係数
【数９】
0004094781.tif 000078
を目標粘性とし、
この目標粘性と、剛性係数の推定値から、前記アームの位置の目標値x(k)を位置指令型操作量の再帰式
【数１０】
0004094781.tif 000079
に基づいて計算し、この目標値x(k)に基づいて前記アームを移動させることを特徴とするロボットの力制御方法。
【発明の詳細な説明】
【０００１】
【発明の属する技術分野】
本発明は、ロボットの力制御方法に関するものである。
【０００２】
【従来の技術】
従来、ロボットの位置を制御する方法としては、様々なものが考えられており、ロボットに塗装、溶接、シーリング等の各種作業を行わせるようにしている。
【０００３】
かかる塗装、溶接、シーリング等の作業においては、ロボットのアームの先端の軌跡を制御するだけでよく、アームの先端が被押圧物体を押圧する押圧力を制御する必要がなかった。
【０００４】
しかし、例えば、木質材料の研磨作業等をロボットに行わせるためには、アーム先端の押圧力を高精度に制御する必要がある。
【０００５】
これまでに提案されている力制御方法としては、ロボットのアームの各軸の自由度単位で位置／コンプライアンス／力制御をハイブリッド化する制御方法（桂川・五百井・久保田・野呂、産業用ロボットにおけるハイブリッド・コンプライアンス／力制御、日本ロボット学会誌、12-6(1994)、893-898 ）や、被押圧物体のダイナミクスを考慮した力制御方法（吉川・梅野、対象物体のダイナミクスを考慮した動的ハイブリッド制御、日本ロボット学会誌、11-8(1993),1229-1235.）等がある。
【０００６】
これらの力制御方法においては、被押圧物体の表面に沿ってロボットのアームを移動させる倣い制御を行っているが、被押圧物体の形状に関する情報が未知の場合には、オペレーターがロボットのアームを目標とする軌道に沿わせて移動し、各点の位置及びその位置でのアームの姿勢を制御装置に順次入力して、予め目標軌道を生成しておくといった教示作業を行っていた。
【０００７】
【発明が解決しようとする課題】
ところが、上記従来のロボットの力制御方法にあっては、力制御を行う際に、予め教示作業を行う必要があり、作業が煩雑なものとなっていた。
【０００８】
しかも、教示作業には、オペレーターの熟練を要するとともに、入力されたアームの位置や姿勢のデータに誤差や不備があると、正確な倣い制御を行うことができなかった。
【０００９】
更には、安定的な力制御を行うためには、シミュレーションにより目標とする粘性係数を絞り込まなければならなかったり、或いは、被押圧物体の剛性係数が既知でなければならないといった欠点があった。
【００１０】
【課題を解決するための手段】
そこで、本発明では、先端を被押圧物体の表面に沿って移動させながら前記被押圧物体に当接させて押圧するアームの先端に、前記被押圧物体の押圧によって前記アームの先端に作用する力を検出するための力センサーを設け、この力センサーの検出値に基づいて前記被押圧物体の剛性係数を推定し、同剛性係数から前記被押圧物体との粘性係数を目標粘性として算出して、この目標粘性と前記剛性係数に応じて前記アームを移動させるロボットの力制御方法において、
前記アームの先端に作用する力Ｆを［fx(t)  ０  fz(t)］、
前記アームの先端の位置を［x(t) y(t) z(t)  φ(t)  θ(t)  ψ(t)］、
前記被押圧物体の粘性係数をBm、剛性係数をKm、アームの先端が前記被押圧物体との接触により受ける外力をＦとし、
Ｌをラベル数として前件部ファジィ集合を
【数１１】
0004094781.tif 000002
とし、ｘ方向のファジィ規則を
【数１２】
0004094781.tif 000003
として、
【数１３】
0004094781.tif 000004
の推論入力により、Km1i(i=1,...,L)を後件部定数値として、
【数１４】
0004094781.tif 000005
の推定規則を得て、ｘ方向の剛性係数の推定値を、メンバーシップ関数の中心値をαj、標準偏差の逆数をβj、添え字ｊをファジィルールの番号（j=1,...,L)を表すものとして、
【数１５】
0004094781.tif 000006
【数１６】
0004094781.tif 000007
【数１７】
0004094781.tif 000008
とし、同様に、
【数１８】
0004094781.tif 000009
の推論入力によりｚ方向の剛性係数の推定値を求めて前記被押圧物体の剛性係数を推定し、
Ｍを実慣性行列、Ｊをヤコビアン、Ｓをスイッチ行列、Ｅを単位行列、Ｋfを力フィードバックゲイン行列として、前記剛性係数の推定値から臨界減衰条件により算出される剛性係数
【数１９】
0004094781.tif 000010
を目標粘性とし、
この目標粘性と、剛性係数の推定値から、前記アームの位置の目標値x(k)を位置指令型操作量の再帰式
【数２０】
0004094781.tif 000011
に基づいて計算し、この目標値x(k)に基づいて前記アームを移動させることとした。
【００１１】
【発明の実施の形態】
本発明に係るロボットの力制御方法は、ロボットのアームの先端で被押圧物体を押圧する際の押圧力を制御するための力制御方法において、被押圧物体をＮＣ工作機械によって加工する際に用いられるＣＬデータに基づいてアームの位置及び姿勢を算出し、アームが被押圧物体の表面に沿って移動しながらアームの先端で被押圧物体を押圧すべく制御するものである。
【００１２】
そのため、煩雑な教示作業を行わないでもロボットのアームを被押圧物体の表面に沿って移動させる倣い制御を行うことができる。
【００１３】
また、アームの先端に設けた力センサーの検出値から目標とする押圧力との差である力偏差を算出し、サンプリング時間毎の力偏差と同力偏差の変化量とに基づいてアームの位置及び姿勢を算出し、アームが被押圧物体の表面に沿って移動しながらアームの先端で被押圧物体を押圧すべく制御することにより、被押圧物体を押圧する押圧力の偏差を抑制することができるものである。
【００１４】
特に、アームの先端に設けた力センサーの検出値に基づいてファジィ環境モデルを用いて被押圧物体の剛性係数を推定し、同剛性係数からロボットと被押圧物体との間の目標とする粘性係数を算出し、同粘性係数に応じた押圧力でアームが被押圧物体を押圧すべく制御することにより、被押圧物体の物理特性が未知であっても、オーバシュートや振動を抑えたロボットの力制御を行うことができ、これにより、ロボットに木質材料の研磨作業等のアームと被押圧物体とが所定の押圧力で接触した状態で行う作業を行わせることができるものである。
【００１５】
また、遺伝的アルゴリズムを用いてファジィ環境モデルを学習させることにより、学習効果を得ることができ、汎用性を有する力制御を行うことができるものである。
【００１６】
また、被押圧物体の剛性係数として推定した剛性係数を用いた場合に、ロボットと被押圧物体とで構成する系が臨界減衰条件を満足する粘性係数を算出することにより、力制御の応答が、オーバシュートや振動の抑えられた好ましい応答である臨界減衰応答となり、安定したロボットの力制御を行うことができるものである。
【００１７】
【実施例】
以下に、本発明の実施例について図面を参照しながら説明する。
【００１８】
まず、位置指令型インピーダンス制御法の導出を行う。
【００１９】
図１に、６自由度を有するロボット１の模式図を示す。ロボット１は、アーム２の先端で被押圧物体３に当接しており、アーム２の先端には、力センサーを配設している。かかるロボット１のアーム２の先端の望ましいインピーダンス特性として次式を考える。
【数２１】
0004094781.tif 000012
ここで、
【数２２】
0004094781.tif 000013
は、作業座標系でのアーム２の先端の位置、速度、加速度ベクトル、Ｍx(θ) は実慣性行列、ＦはＦT ＝［ｆT ｎT ］で定義されるアーム２の先端に作用する力ベクトルｆとモーメントベクトルｎからなる力・モーメントベクトル、Ｋf は力フィードバックゲイン行列である。
【００２０】
また、
【数２３】
0004094781.tif 000014
は、それぞれ目標の位置、速度、力・モーメントベクトルと粘性係数行列、剛性係数行列であり、Ｓ、Ｅはそれぞれスイッチ行列diag（Ｓ1,...,Ｓ6 ）と単位行列である。尚、Ｋf 、Ｂd 、Ｋd は正定な対角行列とする。
【００２１】
式(1) は、ＨＣＣ( 桂川・五百井・久保田・野呂、産業用ロボットにおけるハイブリッド・コンプライアンス／力制御、日本ロボット学会誌、12-6(1994)、893-898 ）の望ましいインピーダンス特性の目標剛性Ｋd にかかるスイッチ行列Ｓを省いた形となっている。対象物の特性が既知である場合には、目標位置も考慮することで応答を改善することができるので、力制御を行っている方向への位置制御も扱うものとする。但し、目標位置に現在の位置を入力すれば、この項の影響は相殺できる。
【００２２】
式(1) は、Ｓ＝Ｅのとき全方向コンプライアンス制御系となり、Ｓ＝０のとき全方向力制御系となる。
【００２３】
次に、作業座標系でのマニピュレータの運動方程式を次式とする。
【数２４】
0004094781.tif 000015
ここで、
【数２５】
0004094781.tif 000016
は、関節角度と関節角速度、
【数２６】
0004094781.tif 000017
とＧｘ( θ) は、作業座標系での遠心力・コリオリ力項と重力項、Ｊ( θ) は、ヤコビアン、τは、関節駆動トルクである。
【００２４】
式(1) の望ましい応答において、加速度が安定性に影響しないとすることで実慣性項を無視できることから( 桂川・五百井・久保田・野呂、産業用ロボットにおけるハイブリッド・コンプライアンス／力制御、日本ロボット学会誌、12-6(1994),893-898) 、Ｘ：＝ｘ−ｘd とおき、システム行列をＡ、制御行列をＢ、制御入力をｕとすると式(3) に示す状態方程式が得られる。
【数２７】
0004094781.tif 000018
但し、
【数２８】
0004094781.tif 000019
とする。この式の解は、周知のように次式となる。
【数２９】
0004094781.tif 000020
【００２５】
このとき、サンプリング幅△ｔを用いて式(4) の離散時刻ｋでの解を求める。
【００２６】
まず、
【数３０】
0004094781.tif 000021
とおき、Δt(k-1)≦ｔ＜Δtkの区間で入力ｕが定数ベクトルであり、システム行列Ａと制御行列Ｂも定数行列とみなせるものとする。式(4) でt=△tkとおいて積分区間を分けると次式のようになる。
【数３１】
0004094781.tif 000022
【００２７】
よって、Ｘ(k)=ｘ(k) −ｘd (k) とすると次の離散時間の位置指令型操作量の再帰式が得られる。
【数３２】
0004094781.tif 000023
【００２８】
次に、図１に示すように、アーム２の先端を傾斜角度θの被押圧物体３に垂直方向から目標力s ｆd で接触させる力制御を行う場合を考える。但し、添え字Ｓはセンサー座標系での値を示す。この場合、ロボットベース座標系にもとづくセンサー座標系の回転行列B Ｒs は次のようになる。
【数３３】
0004094781.tif 000024
【００２９】
よって、ロボットベース座標系での目標力ｆd は次式のようになる。
ｆd = BＲs s ｆd （８）
【００３０】
例えば、s ｆd ［０ ０ sｆdz］［Ｎ］とすれば、ｆd ＝［−s f dZ sin( θ) ０ s ｆdZ cos( θ) ］［Ｎ］となる。
【００３１】
次に、被押圧物体３の物理特性として粘性係数Ｂm と剛性係数Ｋm を考慮する。この場合、アーム２の先端が被押圧物体３との接触により次式の外力Ｆを受けるものとする。
【数３４】
0004094781.tif 000025
但し、Ｂm 、Ｋm は正定な対角行列とする。χm は最初の接触地点の位置ベクトルである。式(10)には、式(1) の臨界減衰条件から求めた(Nagata, F. 、Watanabe,K. 、Sato,K. 、Izumi,K.and Suehiro,T.、Impedance Control for Articulated Robot of 6 Degree-of-Freedom in Consideration of Critically Damped Condition with an Object Dynamics、Proc.of the 36th SICE Annual Conf.、(1997)、1119-1124)目標粘性
【数３５】
0004094781.tif 000026
を示す。
【数３６】
0004094781.tif 000027
ここに、添え字ｉは、ｉ番目（ｉ＝1,...,６）の対角要素を表わし、Ｍは関節座標系での慣性行列とする。
【００３２】
提案する位置指令型インピーダンス制御法は、オーバーシュートや振動を抑えた力制御を実現するために、目標粘性の指針値としてこの
【数３７】
0004094781.tif 000028
を用いる。
【００３３】
次に、位置指令型インピーダンス制御法を用いて力制御を行う場合、被押圧物体３の粘性に関する情報が力制御特性にどのような影響を及ぼすかを検証する。
【００３４】
まず、剛性と粘性がそれぞれ表１、表２のように異なる計16とおりの被押圧物体３で式(9) に示す外力を考え、図１の力制御実験を行う。
【表１】
0004094781.tif 000029
【表２】
0004094781.tif 000030
【００３５】
但し、被押圧物体３のｘ、ｚ方向の物理特性は等しい、つまりＫm1＝Ｋm3、Ｂm1＝Ｂm3、と仮定する。このとき、式(10)の目標粘性
【数３８】
0004094781.tif 000031
には、Ｂm=diag(15,0,15,0,0,0) Ｎs ／ｍ と固定して計算した値を用いる。
【００３６】
これにより、被押圧物体３の粘性係数が正確に推定されている場合とそうでない場合との力制御特性を比較する。シミュレーションは、ＰＵＭＡ560 マニピュレータの運動学パラメータと動力学パラメータを用い(Corke,P. 、A Robotics Toolbox for MATLAB 、IEEE Robotics & Automation Magazine 、March(1996) 、24-32.) 、ルンゲクッタ法により行った。シミュレーション時間は２ s 、サンプリング間隔は10 ms である。
【００３７】
図２〜図５はそれぞれ、表１の各Ｋm に対するＢm の組み合わせを表２のように４とおりに変えたときの力の応答を比較したものである。
【００３８】
その結果、Ｂm の違いによる４とおりの力応答の差異はほとんど見当たらないことから、被押圧物体３の粘性が正確に推定されなくても、力制御系の安定性には影響ないことがわかる。これは、マニピュレータの力制御における不安定性の原因として、被押圧物体３の剛性の影響が非常に大きいというAnらの報告と一致している。
【００３９】
以上の予備的実験結果から、被押圧物体３の剛性を推定することができれば、オーバーシュートや振動を抑えた安定な力制御を実現できることがわかった。
【００４０】
そこで、以下では遺伝的アルゴリズム（ＧＡ）で学習したファジィ環境モデルを用いて被押圧物体３の剛性係数を推定する位置指令型インピーダンス制御法について考える。
【００４１】
ファジィ環境モデルは、単一入出力の簡略化推論法で被押圧物体３の剛性の推定を行う。ここでは、図１の力制御問題を例にして、ファジィ環境モデルの構成法について述べる。
【００４２】
まず、アーム２の先端に作用する力とアーム２の先端の位置をそれぞれ、ｆT＝［ｆx(t) ０ ｆz(t)］、ｘT ＝［x(t) y(t) z(t)φ(t) θ(t) ψ(t) ］とし、ｘ、ｚ方向への推論を独立に行うものとする。但し、それぞれの推論入力は、次式のようにする。
【数３９】
0004094781.tif 000032
【００４３】
このとき、ｘ方向のファジィ規則は以下のように構成する。
【数４０】
0004094781.tif 000033
但し、
【数４１】
0004094781.tif 000034
は、前件部ファジィ集合を表わし、Ｌは、ラベル数とする。これらに並列的に剛性係数行列Ｋm のｘ方向成分Ｋm1を推定するとすれば、次の推定規則が得られる。
【数４２】
0004094781.tif 000035
ここで、Ｋm1i (i=1,...,L) は後件部定数値である。これらの規則からｘ方向の剛性係数の推定値
【数４３】
0004094781.tif 000036
は、従来の前件部適合ルールの重み付き平均値として
【数４４】
0004094781.tif 000037
となる。但し、ｐi (x*(t)）は、正規化適合度であり、
【数４５】
0004094781.tif 000038
とする。さらに、前件部の適合度
【数４６】
0004094781.tif 000039
は、以下のガウス型メンバシップ関数で表わす。
【数４７】
0004094781.tif 000040
ここで、αj は、メンバーシップ関数の中心値、βj は、標準偏差の逆数、添え字ｊは、ファジィルールの番号(j=1,...,L) を表わす。また、後件部定数値をγj とする。ｚ方向の剛性係数の推定値
【数４８】
0004094781.tif 000041
は、式(12)を推論への入力とすることで同様に求めることができる。
【００４４】
図６に、ファジィ環境モデルを用いた位置指令型インピーダンス制御法のブロック線図を示す。ファジィ環境モデルは、力センサからのフィードバック情報をもとに被押圧物体３の剛性係数を推定し、この推定値
【数４９】
0004094781.tif 000042
を用いて系が臨界減衰条件を満たす目標粘性
【数５０】
0004094781.tif 000043
を生成する。位置指令型インピーダンス制御は、
【数５１】
0004094781.tif 000044
を用いてサーボ系への目標値ｘ(k) を出力する。
【００４５】
従来、ファジィルールの設計や調整は、経験的に得られた知識を用いて、思考錯誤により行われていたため、設計者の大きな負担となっていた。これは、αj、βj 、γj の選定方法には確定的なものがないことを意味する。
【００４６】
そこで、ＧＡを用いてファジィ環境モデルを効率的に自動生成する方法を考える。
【００４７】
まず、ファジィルールの表現型をαj 、βj 、γj とし、遺伝子コードはそれぞれの値を16ビットで２値化する。１個体は、図７のように５ルール(L=5) で構成する。
【００４８】
次に、各個体の評価は、図６の提案手法を図１に示した力制御問題に適用することで行う。各個体の適応度Ｅv は、式(16)に示すようにサンプリング間隔△tごとのアーム２の先端に作用する力ｆと目標の接触力ｆd との誤差の２乗和とする。
【数５２】
0004094781.tif 000045
ここで、Ｔは、シミュレーション時間である。これは、適応度を最小とする個体を最も優れた個体として残す最小化問題である。また、力制御におけるオーバーシュートや振動、非接触状態は好ましくないため、これらが発生した場合は大きな値の適応度をダミーでセットし淘汰されるようにした。表３に、ＧＡ操作における使用したパラメータを示す。
【表３】
0004094781.tif 000046
【００４９】
力制御特性には被押圧物体３の剛性が大きく作用し、粘性の影響は非常に小さいことから、粘性をＢm ＝diag(15,0,15,0,0,0) Ｎs ／ｍ に固定し、剛性のみを表４のように変える７とおりの条件で学習させた。
【表４】
0004094781.tif 000047
【００５０】
図８は、実際の被押圧物体３の剛性係数Ｋm1( またはＫm3) と定常状態でのファジィ環境モデルの学習により得られた推定剛性係数
【数５３】
0004094781.tif 000048
との関係をプロットし、それらを２次多項式の回帰曲線でグラフ化したものである。
【００５１】
図９の実線のグラフは、
【数５４】
0004094781.tif 000049
を用いたときの目標粘性
【数５５】
0004094781.tif 000050
であり、点線のグラフは、Ｋm1、Ｋm3を既知と仮定して求めた目標粘性
【数５６】
0004094781.tif 000051
である。
【００５２】
これらの学習結果から、次のことがいえる。被押圧物体３の剛性係数が1000 N/m 付近までは系の臨界減衰応答を満足する目標粘性を用いることで、オーバーシュートや振動のない好ましい力制御応答を得ることができる。しかし、被押圧物体３の剛性係数がそれ以上になるとオーバーシュートや振動を抑えることができなくなり、より大きな目標粘性が必要となる。このため、被押圧物体３が硬くなるにつれて、ファジィ環境モデルから推定された剛性係数は真値よりも非常に大きくなっている。さらに、被押圧物体３の剛性係数の真値と望ましい粘性係数には、比例に近い関係があることがわかる。
【００５３】
これら７とおりの条件における学習効果の具体例として、Ｋm ＝diag(17500,0,17500,0,0,0) のときの各世代の平均適応度と最小適応度の変化を図10に示す。遺伝的操作により、世代交代とともにより優れた個体が深索されていることがわかる。
【００５４】
図11と表５には、100 世代終了時の最小適応度をもつ個体の前件部メンバシップ関数と後件部定数値をそれぞれ示す。
【表５】
0004094781.tif 000052
【００５５】
図12は、この学習されたファジィ環境モデルから被押圧物体３の剛性係数を推定し、推定値を式(10)に代入することで得られる目標粘性を用いたときの力応答のシミュレーション結果である。確かにオーバーシュートや振動のない好ましい力制御特性が得られていることがわかる。尚、図13と図14には、このときの被押圧物体３の剛性係数の推定値と目標粘性の変化をそれぞれ示す。
【００５６】
被押圧物体３の剛性変化に対してロバストな力制御機能を実現するために、ここでは汎化性を有するファジィ環境モデルの構築を試みる。そのため、ファジィ環境モデルの後件部定数値に図８の推定値を積極的に用いる方法を提案する。
【００５７】
まず、前件部ファジィ集合は、ラベル数を６とするガウス型メンバシップ関数で構成する。それらを台集合［3500,21000］上に等間隔に配置するため、各メンバシップ関数の中心値は、表４(2)-(7) に示したＫm1（またはＫm3）とする。
【００５８】
また、隣り合うメンバシップ関数同士が、適合度0.5 で交わるように標準偏差を1750とする。後件部の設計の際には、前件部のラベル数と同数の後件部モデルを作成する。後件部定数値は、前件部の各メンバシップ関数の中心値を図８の横軸の値とすることで得られる推定剛性係数を用いる。
【００５９】
図15と表６には、設計した前件部メンバシップ関数と後件部定数値をそれぞれ示す。
【表６】
0004094781.tif 000053
【００６０】
次に、設計したファジィ環境モデルを用いて位置指令型インピーダンス制御法のロバスト性を評価した。ここでは、表７に示す４とおりの学習していない条件の下で力制御シミュレーション実験を行った。
【表７】
0004094781.tif 000054
【００６１】
図16に、その結果を示す。アーム２の先端に作用する外力が、どの条件に対してもオーバーシュートや振動を伴うことなく、力の目標値の変化に対して良好に追従していることがわかる。このように、提案方法は学習していない条件の下でも有効であることが確認できる。
【００６２】
以上のように、本実施例では、ロボット１のアーム２の先端に力センサーを設けておき、同力センサーの検出値に基づいてファジィ環境モデルを用いて被押圧物体３の剛性係数を推定し、同剛性係数からロボット１のアーム２の先端と被押圧物体３との間で目標とする粘性係数を算出し、同粘性係数に応じた押圧力でアーム２が被押圧物体３を押圧すべく制御するようにしている。
【００６３】
そのため、被押圧物体３の物理特性が未知であっても、オーバシュートや振動を抑えたロボット１の力制御を行うことができる。
【００６４】
しかも、遺伝的アルゴリズムを用いてファジィ環境モデルを学習させることにより、学習効果を得ることができ、汎用性を有する力制御を行うことができる。
【００６５】
また、被押圧物体の剛性係数として推定した剛性係数を用いた場合に、ロボット１と被押圧物体３とで構成させる系が臨界減衰条件を満足する粘性係数を算出することにより、力制御の応答がオーバシュートや振動の抑えられた好ましい応答である臨界減衰応答となり、安定したロボット１の力制御を行うことができる。
【００６６】
次ぎに、被押圧物体をＮＣ工作機械によって加工する際に用いられるＣＬデータに基づいてアームの位置及び姿勢を算出し、アームが被押圧物体の表面に沿って移動しながらアームの先端で被押圧物体を押圧すべく制御する方法について説明する。
【００６７】
まず、前述したと同様に、６自由度を有するロボット１のアーム２の先端の望ましいインピーダンス特性として式(1) を考える。
【００６８】
式(1) の望ましい応答において、加速度は制御の対象としないものとして、実慣性項を無視し、サンプリング幅Δｔを用いて離散時刻ｋでの解を求めると、次式の位置指令型操作量の再帰式が得られる。
【数５７】
0004094781.tif 000055
ここで、x(k)は位置ベクトル［x(k) y(k) z(k)］T と姿勢ベクトルであるＺ−Ｙ−Ｚオイラー角［φ(k) θ(k) ψ(k) ］T で構成する。
【００６９】
式(17)から得られる作業座標系での操作量x(k)を逆キネマティクスにより各関節角度θ(k) に変換し、サンプリング間隔毎にθ(k) をサーボ系への目標値として与えることで、アーム２と被押圧物体３との間に作用する接触力Ｆを目標値Ｆd に追従させることができる。
【００７０】
一方、被押圧物体３がＣＡＤ／ＣＡＭシステムで設計され、ＮＣ工作機械で加工されている場合には、被押圧物体３の形状に関する情報としてＣＬデータが生成される。かかるＣＬデータのｎステップ成分（ＣＬｎ∈Ｒ6 ）は、位置ベクトル［ｘn ｙn ｚn ］T とその点における法線ベクトル［ｖｘn ｖｙn ｖｚn ］Tから構成され、次式のように与えられる。
【数５８】
0004094781.tif 000056
【００７１】
本実施例では、式(18)で与えられる位置・姿勢情報をもとにサンプリング間隔毎の目標とする軌道を生成可能な位置補償器を提案する。すなわち、式(18)で与えられる位置・姿勢情報を、力制御を行う方向のフィードフォワード量として、或いは、位置／姿勢制御を行う方向の目標軌道として用い、式(17)のx d (k) に入力することにより、教示作業を伴わない倣い制御を提供する。
【００７２】
位置補償器は、ロボット１のアーム２を被押圧物体３の表面に法線方向から目標の力で接触させたまま移動させるために、ＣＬデータを用いて目標軌道を算出する。
【００７３】
本実施例では、アーム２のｙ方向への傾斜はなく（ｖｙ＝０）、ｙ方向における位置は一定（ｙ＝constant）と仮定し、操作量x(k)はＣＬデータの刻みに比べて非常に微少であることから、ｘ方向の移動速度を基準として、直線近似により求める（図１７参照）。
【００７４】
ｚ方向の目標の位置ｚd (k) は次式から求めることができる。
【数５９】
0004094781.tif 000057
【００７５】
目標とする姿勢ベクトル［ｖｘn ｖｙn ｖｚn ］T も同様に求めることができる。この姿勢ベクトルは、逆キネマティクスへ入力するためにＺ−Ｙ−Ｚオイラー角［φ(k) θ(k) ψ(k) ］T に変換する。
【００７６】
図１８及び図１９にこの位置補償器を用いた制御系のブロック線図を示す。ここでは、倣い制御を行うためにスイッチ行列Ｓ' ＝diag（S'1,....,S'6）を用いて位置・姿勢制御系と力制御系をハイブリッドに制御している。すなわち、対角成分が０の方向は力制御系となり、対角成分が１の方向は位置・姿勢制御系となる。
【００７７】
図１８において、力制御を行う方向では、力偏差（ｆ−ｆd ）に力フィードバックゲインＫf と目標粘性Ｂd の逆数を乗じ、それに位置補償器からの補正量［x d (k) −x d (k-1) ］と
【数６０】
0004094781.tif 000058
を加えることで位置指令値
【数６１】
0004094781.tif 000059
を生成している。
【００７８】
また、位置・姿勢制御を行う方向では、位置補償器からの目標軌道との位置偏差(xd (k)-x)に位置フィードバックゲインＫp を乗じたものを位置指令値としている。
【００７９】
それぞれの方向の指令値を加えたx(k)は、逆キネマティクスにより各関節の角度ベクトルθ(k) に変換され、サーボへの目標値として入力される。
【００８０】
このように、本実施例では、被押圧物体３をＮＣ工作機械によって加工する際に用いられるＣＬデータに基づいてアーム２の位置及び姿勢を算出し、アーム２が被押圧物体３の表面に沿って移動しながらアーム２の先端で被押圧物体３を押圧すべく制御しているため、煩雑な教示作業を行わないでもロボット１のアーム２を被押圧物体３の表面に沿って移動させる倣い制御を行うことができる。
【００８１】
しかも、位置指令型の力制御であることから、オープンアーキテクチャ型の産業用ロボットに広く容易に適用することができる。
【００８２】
次ぎに、本実施例の位置補償器を用いた力制御方法を実際にオープンアーキテクチャ型の産業用ロボットに適用し、木質系の被押圧物体３に対して制御実験を行うことで、その有効性について検証する。尚、実験で使用した産業用ロボットは川崎重工業製のＰＵＭＡ型マニピュレータＪＳ−１０であり、アーム２の取付位置に力センサー４としてニッタ製の６軸力トルクセンサを取付けている。また、実験システムはロボット本体、サーボコントローラ、制御用コンピュータ（ＰＣＦＭＶ−６２６６Ｔ６：富士通製）から構成し、プログラム開発はＷｉｎｄｏｗｓＮＴ上で川崎重工業が提供するＶｉｓｕａｌＣ＋＋用のＡＰＩ（Application Programming Interface ）を用いて行った。このＡＰＩを用いた場合、サーボコントローラには最短で４msec毎に関節角度指令で操作量を与えることができる。
【００８３】
図２０に、実験用のワークとして用いた被押圧物体３の断面図を示す。被押圧物体３としては、ＮＣ加工後の木材を用いており、表面に逆目が発生していて、指で触るとかなりざらざらした粗い表面となっている。
【００８４】
ロボット座標系におけるワークの位置決めは、手作業で行って、意図的に人為的な位置決め誤差を発生させることとした。
【００８５】
実験では、金属製のアーム２を被押圧物体３に衝突させた後、目標の接触力２［ｋｇｆ］で接触させたままＰ点からＱ点に移動させる倣い制御を行った。倣いの基準となるｘ方向の速度は、４［mm/s］とし、力制御は、目標粘性を0.1 ［kgfs/mm ］、力フィードバックゲインを0.35又は1.5 とし、位置フィードバックゲインを0.001 として、式(17)を用いて行った。尚、サンプリング幅は10［msec］とした。
【００８６】
実験は、以下の３とおりを行った。
Case１：位置補償器を用いない場合
Case２：位置補償器を用いる場合（但し、アーム２の姿勢は図２０に示したようにＺ軸方向で固定した）
Case３：位置補償器を用いる場合（但し、アーム２が被押圧物体３に法線方向から接触するように姿勢も制御した）
【００８７】
図２１は、Case１における力の応答を示す。このとき、姿勢を表すＺ−Ｙ−Ｚオイラー角は（０ π ０）に固定し、力のフィードバックのみで制御を行った。図２１より、被押圧物体３の表面形状に応じて力の偏差が発生していることがわかる。
【００８８】
図２２は、Case２における力の応答を示す。このとき、Case１と同様に姿勢は固定されたままであるが、ＣＬデータ（ＣＬｎ＝［ｘn ｙn ｚn ］）に基づく位置補償器を用いた。
【００８９】
図２３は、Case３における力の応答を示す。この場合には、ＣＬデータ( ＣＬｎ＝［ｘn ｙn ｚn ｖｘn ｖｙn ｖｚn ］）に基づく位置補償器を用いており、アーム２の姿勢も制御している。また、工具座標系におけるｚ方向の力フィードバックゲインをＫf3＝1.5 と大きくすることで、より敏感な特性が得られている。
【００９０】
以上の実験結果から、本実施例に係る位置補償器を用いることで、力の応答を改善できるだけでなく、煩わしい教示作業を行うことなく倣い制御を実現できることを確認した。
【００９１】
次ぎに、アーム２の先端に設けた力センサー４の検出値から目標とする押圧力との差である力偏差を算出し、サンプリング時間毎の力偏差と同力偏差の変化量とに基づいてアーム２の位置及び姿勢を算出し、アーム２が被押圧物体３の表面に沿って移動しながらアーム２の先端で被押圧物体３を押圧するロボット１の力制御方法について説明する。
【００９２】
ロボット１が倣い制御に適用される場合には、材料の剛性や形状の変化よって力偏差が発生するが、かかる力偏差は、目標の接触力との誤差であり、倣い制御中は限りなく零であることが望ましい。そこで、本実施例では、力偏差を抑制するためのファジィ補償器を提供する。
【００９３】
ファジィ補償器は、サンプリング時間毎の力偏差とその変化量に応じて、フィードフォワードコマンドとしての位置の補正値を生成するようにしている。以下に、ｘ方向の力制御を例にファジィ補償器の動きを説明する。
【００９４】
力偏差ｅx (k) と力偏差の変化量Δｅx (k) を次式のように定義する。
【数６２】
0004094781.tif 000060
但し、ｍとｎはウインドウの幅である。
【００９５】
本実施例に係るファジィ補償器では、例えばロボット座標系においてｘ方向のみの情報を用いる場合、ファジィルールは次のようになる。
【数６３】
0004094781.tif 000061
ここで、
【数６４】
0004094781.tif 000062
と
【数６５】
0004094781.tif 000063
はそれぞれ力偏差ｅx (k) と力偏差の変化量Δｅx (k) のためのｉ番目の前件部ファジィ集合、ｃi は後件部定数値、Ｌはファジィルール数である。
【００９６】
i 番目のルールにおける前件部の適合度は、次式から得られる。
【数６６】
0004094781.tif 000064
ここで、
【数６７】
0004094781.tif 000065
は、ラベルＸのファジィ集合の適合度を示す。ファジィ集合は、次のガウス型メンバーシップ関数とする。
【数６８】
0004094781.tif 000066
但し、αはメンバーシップ関数の中心値、βは標準偏差の逆数である。
【００９７】
ファジィ補償器が生成する位置の補償値は、次式で計算する。尚、その他の方向についても同様に計算することができる。
【数６９】
0004094781.tif 000067
【００９８】
図２４及び図２５は、位置指令型インピーダンス制御法にファジィ補償器を適用した場合のブロック線図を示す。
【００９９】
次ぎに、ＰＵＭＡ５６０マニピュレータを用いた倣い制御のシミュレーション実験によって、ファジィ補償器の有効性を検証する。
【０１００】
倣い制御のシミュレーション実験は、図２４及び図２５に示す手法を用いて、図２６に示すように、ロボット１のアーム２の先端で被押圧物体３を法線方向から目標とする力で押圧しながら点Ｐから点Ｑに移動させる。この場合に、被押圧物体３のロボット座標系におけるｘ方向及びｚ方向の剛性は、図２７に示すように、時間とともに動的に変化するものとする。
【０１０１】
図２８及び図２９は、前件部のメンバーシップ関数をそれぞれ示している。また、表８は、後件部の定数値を示す。
【表８】
0004094781.tif 000068
【０１０２】
シミュレーションは、ＰＵＭＡ５６０マニピュレータの運動学と動力学パラメータを用いた次の運動方程式を、ルンゲクッタ法で数値積分することで行った。
【数７０】
0004094781.tif 000069
【０１０３】
図３０は、ファジィ補償器を用いていない場合の力の応答を示している。被押圧物体３の剛性の変化により、力の応答が振動していることがわかる。
【０１０４】
一方、図３１は、制御開始１秒後にファジィ補償器をＯＮにした場合の力の応答を示している。このときにファジィ補償器が生成した位置の補償値を図３２に示す。
【０１０５】
これらの実験結果から、被押圧物体３の特性の変化に起因する力偏差を抑制するために、ファジィ補償器が有効であることが確認できる。尚、図３３に示すように、ファジィ補償器と前記位置補償器とを併用することもでき、この場合には、教示作業を行わないでも、ロボット１のアーム２を被押圧物体３の表面に沿って移動させる倣い制御を行うことができる。
【０１０６】
以上に説明したように、本実施例では、ロボット１のアーム２の先端で被押圧物体３を押圧する際の押圧力を制御するための力制御方法において、被押圧物体３をＮＣ工作機械によって加工する際に用いられるＣＬデータに基づいてアーム２の位置及び姿勢を算出し、アーム２が被押圧物体３の表面に沿って移動しながらアーム２の先端で被押圧物体を押圧すべく制御しているため、煩雑な教示作業を行わないでもロボット１のアーム２を被押圧物体３の表面に沿って移動させる倣い制御を行うことができる。
【０１０７】
また、アーム２の先端に設けた力センサー４の検出値から目標とする押圧力との差である力偏差を算出し、サンプリング時間毎の力偏差と同力偏差の変化量とに基づいてアーム２の位置及び姿勢を算出し、アーム２が被押圧物体３の表面に沿って移動しながらアーム２の先端で被押圧物体３を押圧すべく制御しているため、被押圧物体３を押圧する押圧力の偏差を抑制することができる。
【０１０８】
さらに、アーム２の先端に設けた力センサー４の検出値に基づいてファジィ環境モデルを用いて被押圧物体３の剛性係数を推定し、同剛性係数からロボット１と被押圧物体３との間の目標とする粘性係数を算出し、同粘性係数に応じた押圧力でアーム２が被押圧物体３を押圧すべく制御しているため、被押圧物体３の物理特性が未知であっても、オーバシュートや振動を抑えたロボット１の力制御を行うことができ、これにより、ロボット１に木質材料の研磨作業等のアーム２と被押圧物体３とが所定の押圧力で接触した状態で行う作業を行わせることができる。
【０１０９】
【発明の効果】
本発明は、以上説明したような形態で実施され、以下に記載されるような効果を奏する。
【０１１０】
本願発明のロボットの力制御方法では、アームの先端に設けた力センサーの検出値に基づいてファジィ環境モデルを用いて被押圧物体の剛性係数を推定し、同剛性係数からロボットと被押圧物体との間の目標とする粘性係数を算出し、同粘性係数に応じた押圧力でアームが被押圧物体を押圧すべく制御しているので、被押圧物体の物理特性が未知であっても、オーバシュートや振動を抑えたロボットの力制御を行うことができる。
【０１１１】
したがって、ロボットに、木質材料の研磨作業等のように、アームと被押圧物体とが所定の押圧力で接触した状態で行う作業を極めて容易に行わせることができる。
【図面の簡単な説明】
【図１】シミュレーション時におけるロボットを示す模式図。
【図２】Ｋm1＝Ｋm3＝5000の場合のシミュレーション結果を示すグラフ。
【図３】Ｋm1＝Ｋm3＝10000 の場合のシミュレーション結果を示すグラフ。
【図４】Ｋm1＝Ｋm3＝15000 の場合のシミュレーション結果を示すグラフ。
【図５】Ｋm1＝Ｋm3＝20000 の場合のシミュレーション結果を示すグラフ。
【図６】ロボットを制御する制御装置のブロック図。
【図７】遺伝子コードに変換されたファジィルールを示す模式図。
【図８】実際の剛性係数と推定した剛性係数との関係を示すグラフ。
【図９】実際の剛性係数と目標粘性との関係を示すグラフ。
【図１０】平均適応度と最小適応度の変化を示すグラフ。
【図１１】前件部メンバシップ関数を示すグラフ。
【図１２】力応答のシミュレーション結果を示すグラフ。
【図１３】剛性係数の変化を示すグラフ。
【図１４】粘性係数の変化を示すグラフ。
【図１５】統合化された前件部メンバシップ関数を示すグラフ。
【図１６】学習していない環境でのシミュレーション結果を示すグラフ。
【図１７】ＣＬデータとx d (k) との関係を示す説明図。
【図１８】ロボットを制御する制御装置のブロック図。
【図１９】ロボットを制御する制御装置のブロック図。
【図２０】実験用のワークを示す説明図。
【図２１】Case１の力の応答を示すグラフ。
【図２２】Case２の力の応答を示すグラフ。
【図２３】Case３の力の応答を示すグラフ。
【図２４】ロボットを制御する制御装置のブロック図。
【図２５】ロボットを制御する制御装置のブロック図。
【図２６】シミュレーション実験の概略を示す説明図。
【図２７】被押圧物体の剛性の変化を示すグラフ。
【図２８】e x (k) のための前件部ファジィ集合を示すグラフ。
【図２９】Δe x (k) のための前件部ファジィ集合を示すグラフ。
【図３０】ファジィ補償器がない場合の力の応答を示すグラフ。
【図３１】ファジィ補償器を用いた場合の力の応答を示すグラフ。
【図３２】ファジィ補償器が生成した位置の補償値を示すグラフ。
【図３３】ロボットを制御する制御装置のブロック図。
【符号の説明】
１ ロボット
２ アーム
３ 被押圧物体
４ 力センサー
【図１】
0004094781.tif 000080
【図２】
0004094781.tif 000081
【図３】
0004094781.tif 000082
【図４】
0004094781.tif 000083
【図５】
0004094781.tif 000084
【図６】
0004094781.tif 000085
【図７】
0004094781.tif 000086
【図８】
0004094781.tif 000087
【図９】
0004094781.tif 000088
【図１０】
0004094781.tif 000089
【図１１】
0004094781.tif 000090
【図１２】
0004094781.tif 000091
【図１３】
0004094781.tif 000092
【図１４】
0004094781.tif 000093
【図１５】
0004094781.tif 000094
【図１６】
0004094781.tif 000095
【図１７】
0004094781.tif 000096
【図１８】
0004094781.tif 000097
【図１９】
0004094781.tif 000098
【図２０】
0004094781.tif 000099
【図２１】
0004094781.tif 000100
【図２２】
0004094781.tif 000101
【図２３】
0004094781.tif 000102
【図２４】
0004094781.tif 000103
【図２５】
0004094781.tif 000104
【図２６】
0004094781.tif 000105
【図２７】
0004094781.tif 000106
【図２８】
0004094781.tif 000107
【図２９】
0004094781.tif 000108
【図３０】
0004094781.tif 000109
【図３１】
0004094781.tif 000110
【図３２】
0004094781.tif 000111
【図３３】
0004094781.tif 000112