[1]
【発明の詳細な説明】
【0001】
【発明の属する技術分野】本発明は、ロボットの力制御方法に関するものである。
【0002】
【従来の技術】従来、ロボットの位置を制御する方法としては、様々なものが考えられており、ロボットに塗装、溶接、シーリング等の各種作業を行わせるようにしている。
【0003】かかる塗装、溶接、シーリング等の作業においては、ロボットのアームの先端の軌跡を制御するだけでよく、アームの先端が被押圧物体を押圧する押圧力を制御する必要がなかった。
【0004】しかし、例えば、木質材料の研磨作業等をロボットに行わせるためには、アーム先端の押圧力を高精度に制御する必要がある。
【0005】これまでに提案されている力制御方法としては、ロボットのアームの各軸の自由度単位で位置/コンプライアンス/力制御をハイブリッド化する制御方法(桂川・五百井・久保田・野呂、産業用ロボットにおけるハイブリッド・コンプライアンス/力制御、日本ロボット学会誌、12-6(1994)、893-898 )や、被押圧物体のダイナミクスを考慮した力制御方法(吉川・梅野、対象物体のダイナミクスを考慮した動的ハイブリッド制御、日本ロボット学会誌、11-8(1993),1229-1235.)等がある。
【0006】
【発明が解決しようとする課題】ところが、上記従来のロボットの力制御方法にあっては、安定的な力制御を行うためには、シミュレーションにより目標とする粘性係数を絞り込まなければならなかったり、或いは、被押圧物体の剛性係数が既知でなければならないといった欠点があった。
【0007】
【課題を解決するための手段】そこで、本発明では、ロボットのアームの先端で被押圧物体を押圧する際の押圧力を制御するための力制御方法において、アームの先端に設けた力センサーの検出値に基づいてファジィ環境モデルを用いて被押圧物体の剛性係数を推定し、同剛性係数からロボットと被押圧物体との間の目標とする粘性係数を算出し、同粘性係数に応じた押圧力でアームが被押圧物体を押圧すべく制御することとした。
【0008】また、前記ファジィ環境モデルは、遺伝的アルゴリズムを用いて学習させることとした。
【0009】また、前記粘性係数は、被押圧物体の剛性係数として推定した剛性係数を用いた場合に、ロボットと被押圧物体とで構成する系が臨界減衰条件を満足する値とすることとした。
【0010】
【発明の実施の形態】本発明に係るロボットの力制御方法は、ロボットのアームの先端で被押圧物体を押圧する際の押圧力を制御するための力制御方法であって、アームの先端に設けた力センサーの検出値に基づいてファジィ環境モデルを用いて被押圧物体の剛性係数を推定し、同剛性係数からロボットと被押圧物体との間の目標とする粘性係数を算出し、同粘性係数に応じた押圧力でアームが被押圧物体を押圧すべく制御するものである。
【0011】従って、被押圧物体の物理特性が未知であっても、オーバシュートや振動を抑えたロボットの力制御を行うことができるものである。
【0012】そのため、ロボットに木質材料の研磨作業等のアームと被押圧物体とが所定の押圧力で接触した状態で行う作業を行わせることができるものである。
【0013】また、遺伝的アルゴリズムを用いてファジィ環境モデルを学習させることにより、学習効果を得ることができ、汎用性を有する力制御を行うことができるものである。
【0014】また、被押圧物体の剛性係数として推定した剛性係数を用いた場合に、ロボットと被押圧物体とで構成する系が臨界減衰条件を満足する粘性係数を算出することにより、力制御の応答が、オーバシュートや振動の抑えられた好ましい応答である臨界減衰応答となり、安定したロボットの力制御を行うことができるものである。
【0015】
【実施例】以下に、本発明の実施例について図面を参照しながら説明する。
【0016】まず、位置指令型インピーダンス制御法の導出を行う。
【0017】図1に、6自由度を有するロボット1の模式図を示す。ロボット1は、アーム2の先端で被押圧物体3に当接しており、アーム2の先端には、力センサーを配設している。かかるロボット1のアーム2の先端の望ましいインピーダンス特性として次式を考える。
【数1】
ここで、
【数2】
は、作業座標系でのアーム2の先端の位置、速度、加速度ベクトル、M X (θ)は実慣性行列、FはF T =[f T n T ]で定義されるアーム2の先端に作用する力ベクトルfとモーメントベクトルnからなる力・モーメントベクトル、K f は力フィードバックゲイン行列である。
【0018】また、
【数3】
は、それぞれ目標の位置、速度、力・モーメントベクトルと粘性係数行列、剛性係数行列であり、S、Eはそれぞれスイッチ行列diag(S 1,..., S 6 )と単位行列である。尚、K f 、B d 、K d は正定な対角行列とする。
【0019】式(1) は、HCC( 桂川・五百井・久保田・野呂、産業用ロボットにおけるハイブリッド・コンプライアンス/力制御、日本ロボット学会誌、12-6(1994)、893-898 )の望ましいインピーダンス特性の目標剛性K d にかかるスイッチ行列Sを省いた形となっている。対象物の特性が既知である場合には、目標位置も考慮することで応答を改善することができるので、力制御を行っている方向への位置制御も扱うものとする。但し、目標位置に現在の位置を入力すれば、この項の影響は相殺できる。
【0020】式(1) は、S=Eのとき全方向コンプライアンス制御系となり、S=0のとき全方向力制御系となる。
【0021】次に、作業座標系でのマニピュレータの運動方程式を次式とする。
【数4】
ここで、
【数5】
は、関節角度と関節角速度、
【数6】
とG X (θ)は、作業座標系での遠心力・コリオリ力項と重力項、J (θ) は、ヤコビアン、τは、関節駆動トルクである。
【0022】式(1) の望ましい応答において、加速度が安定性に影響しないとすることで実慣性項を無視できることから( 桂川・五百井・久保田・野呂、産業用ロボットにおけるハイブリッド・コンプライアンス/力制御、日本ロボット学会誌、12-6(1994),893-898) 、X:=x-x d とおき、システム行列をA、制御行列をB、制御入力をuとすると式(3) に示す状態方程式が得られる。
【数7】
但し、
【数8】
とする。この式の解は、周知のように次式となる。
【数9】
【0023】このとき、サンプリング幅△tを用いて式(4) の離散時刻kでの解を求める。
【0024】まず、
【数10】
とおき、Δt(k-1)≦t 【0025】よって、X(k)=x(k) -x d (k) とすると次の離散時間の位置指令型操作量の再帰式が得られる。
【数12】
【0026】次に、図1に示すように、アーム2の先端を傾斜角度θの被押圧物体3に垂直方向から目標力 s f d で接触させる力制御を行う場合を考える。但し、添え字Sはセンサー座標系での値を示す。この場合、ロボットベース座標系にもとづくセンサー座標系の回転行列 B R s は次のようになる。
【数13】
【0027】よって、ロボットベース座標系での目標力f d は次式のようになる。
f d = B R s s f d (8)
【0028】例えば、 s f d [0 0 S f dz ][N]とすれば、f d =[- S f dZ sin(θ) 0 s f dZ cos(θ) ][N]となる。
【0029】次に、被押圧物体3の物理特性として粘性係数B m と剛性係数K m を考慮する。この場合、アーム2の先端が被押圧物体3との接触により次式の外力Fを受けるものとする。
【数14】
但し、B m 、K m は正定な対角行列とする。x 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)目標粘性
【数15】
を示す。
【数16】
ここに、添え字iは、i番目(i=1,...,6)の対角要素を表わし、Mは関節座標系での慣性行列とする。
【0030】提案する位置指令型インピーダンス制御法は、オーバーシュートや振動を抑えた力制御を実現するために、目標粘性の指針値としてこの
【数17】
を用いる。
【0031】次に、位置指令型インピーダンス制御法を用いて力制御を行う場合、被押圧物体3の粘性に関する情報が力制御特性にどのような影響を及ぼすかを検証する。
【0032】まず、剛性と粘性がそれぞれ表1、表2のように異なる計16とおりの被押圧物体3で式(9) に示す外力を考え、図1の力制御実験を行う。
【表1】
【表2】
【0033】但し、被押圧物体3のx、z方向の物理特性は等しい、つまりK m1 =K m3 、B m1 =B m3 、と仮定する。このとき、式(10)の目標粘性
【数18】
には、B m =diag(15,0,15,0,0,0) Ns /m と固定して計算した値を用いる。
【0034】これにより、被押圧物体3の粘性係数が正確に推定されている場合とそうでない場合との力制御特性を比較する。シミュレーションは、PUMA560 マニピュレータの運動学パラメータと動力学パラメータを用い(Corke,P. 、A Robotics Toolbox forMATLAB、IEEE Robotics & Automation Magazine 、March(1996) 、24-32.) 、ルンゲクッタ法により行った。シミュレーション時間は2 s 、サンプリング間隔は10 ms である。
【0035】図2~図5はそれぞれ、表1の各K m に対するB m の組み合わせを表2のように4とおりに変えたときの力の応答を比較したものである。
【0036】その結果、B m の違いによる4とおりの力応答の差異はほとんど見当たらないことから、被押圧物体3の粘性が正確に推定されなくても、力制御系の安定性には影響ないことがわかる。これは、マニピュレータの力制御における不安定性の原因として、被押圧物体3の剛性の影響が非常に大きいというAnらの報告と一致している。
【0037】以上の予備的実験結果から、被押圧物体3の剛性を推定することができれば、オーバーシュートや振動を抑えた安定な力制御を実現できることがわかった。
【0038】そこで、以下ではGAで学習したファジィ環境モデルを用いて被押圧物体3の剛性係数を推定する位置指令型インピーダンス制御法について考える。
【0039】ファジィ環境モデルは、単一入出力の簡略化推論法で被押圧物体3の剛性の推定を行う。ここでは、図1の力制御問題を例にして、ファジィ環境モデルの構成法について述べる。
【0040】まず、アーム2の先端に作用する力とアーム2の先端の位置をそれぞれ、f T =[f x (t) 0 f z (t) ]、x T =[x(t) y(t) z(t) φ(t) θ(t) ψ(t)]とし、x、z方向への推論を独立に行うものとする。但し、それぞれの推論入力は、次式のようにする。
【数19】
【0041】このとき、x方向のファジィ規則は以下のように構成する。
【数20】
但し、
【数21】
は、前件部ファジィ集合を表わし、Lは、レベル数とする。これらに並列的に剛性係数行列K m のx方向成分K m1 を推定するとすれば、次の推定規則が得られる。
【数22】
ここで、K m1i (i=1,...,L) は後件部定数値である。これらの規則からx方向の剛性係数の推定値
【数23】
は、従来の前件部適合ルールの重み付き平均値として
【数24】
となる。但し、p i (x * (t) )は、正規化適合度であり、
【数25】
とする。さらに、前件部の適合度
【数26】
は、以下のガウス型メンバシップ関数で表わす。
【数27】
ここで、α j は、メンバーシップ関数の中心値、β j は、標準偏差の逆数、添え字jは、ファジィルールの番号(j=1,...,L) を表わす。また、後件部定数値をγ j とする。z方向の剛性係数の推定値
【数28】
は、式(12)を推論への入力とすることで同様に求めることができる。
【0042】図6に、ファジィ環境モデルを用いた位置指令型インピーダンス制御法のブロック線図を示す。ファジィ環境モデルは、力センサからのフィードバック情報をもとに被押圧物体3の剛性係数を推定し、この推定値
【数29】
を用いて系が臨界減衰条件を満たす目標粘性
【数30】
を生成する。位置指令型インピーダンス制御は、
【数31】
を用いてサーボ系への目標値x(k) を出力する。
【0043】従来、ファジィルールの設計や調整は、経験的に得られた知識を用いて思慮錯誤により行われていたため、設計者の大きな負担となっていた。これは、α j 、β j 、γ j の選定方法には確定的なものがないことを意味する。
【0044】そこで、GAを用いてファジィ環境モデルを効率的に自動生成する方法を考える。
【0045】まず、ファジィルールの表現型をα j 、β j 、γ j とし、遺伝子コードはそれぞれの値を16ビットで2値化する。1個体は、図7のように5ルール(L=5) で構成する。
【0046】次に、各個体の評価は、図6の提案手法を図1に示した力制御問題に適用することで行う。各個体の適応度E v は、式(16)に示すようにサンプリング間隔△tごとのアーム2の先端に作用する力fと目標の接触力f d との誤差の2乗和とする。
【数32】
ここで、Tは、シミュレーション時間である。これは、適応度を最小とする個体を最も優れた個体として残す最小化問題である。また、力制御におけるオーバーシュートや振動、非接触状態は好ましくないため、これらが発生した場合は大きな値の適応度をダミーでセットし淘汰されるようにした。表3に、GA操作における使用したパラメータを示す。
【表3】
【0047】力制御特性には被押圧物体3の剛性が大きく作用し、粘性の影響は非常に小さいことから、粘性をB m =diag(15,0,15,0,0,0) Ns /m に固定し、剛性のみを表4のように変える7とおりの条件で学習させた。
【表4】
【0048】図8は、実際の被押圧物体3の剛性係数K m1 (またはK m3 ) と定常状態でのファジィ環境モデルの学習により得られた推定剛性係数
【数33】
との関係をプロットし、それらを2次多項式の回帰曲線でグラフ化したものである。
【0049】図9の実線のグラフは、
【数34】
を用いたときの目標粘性
【数35】
であり、点線のグラフは、K m1 、K m3 を既知と仮定して求めた目標粘性
【数36】
である。
【0050】これらの学習結果から、次のことがいえる。被押圧物体3の剛性係数が1000 N/m 付近までは系の臨界減衰応答を満足する目標粘性を用いることで、オーバーシュートや振動のない好ましい力制御応答を得ることができる。しかし、被押圧物体3の剛性係数がそれ以上になるとオーバーシュートや振動を抑えることができなくなり、より大きな目標粘性が必要となる。このため、被押圧物体3が硬くなるにつれて、ファジィ環境モデルから推定された剛性係数は真値よりも非常に大きくなっている。さらに、被押圧物体3の剛性係数の真値と望ましい粘性係数には、比例に近い関係があることがわかる。
【0051】これら7とおりの条件における学習効果の具体例として、K m =diag(17500,0,17500,0,0,0)のときの各世代の平均適応度と最小適応度の変化を図10に示す。遺伝的操作により、世代交代とともにより優れた個体が深索されていることがわかる。
【0052】図11と表5には、100 世代終了時の最小適応度をもつ個体の前件部メンバシップ関数と後件部定数値をそれぞれ示す。
【表5】
【0053】図12は、この学習されたファジィ環境モデルから被押圧物体3の剛性係数を推定し、推…
