JPH0553590B2 - - Google Patents

Description

【発明の詳細な説明】

〔産業上の利用分野〕 本発明は、基本３軸と、ロボツトアームに対し
てオフセツトしている手首３軸を有するの６軸関
節形ロボツトを制御する方法に関する。 〔従来の技術〕 基本３軸と手首部３軸からなる６軸関節形ロボ
ツトは、第５図に示すように、次の６軸から構成
される。 (1) 地面に固定された脚１１の上部に設けられ、
垂線（Z_p軸）回りに自由度を有する胴１２。 (2) 胴１２の先端部に設けられ、Z_p軸に垂直なZ₁
軸回りに自由度を有する下腕１３。 (3) 下腕１３の先端部に設けられ、Z₁軸と平行な
Z₂軸回りに自由度を有する上腕１４。 (4) 上腕１４の先端部に設けられ、Z₂軸に垂直な
Z₃軸回りに自由度を有する手首１５。 (5) 手首１５の先端部に設けられ、Z₃軸に垂直な
Z₄軸回りに自由度を有する手首１６。 (6) 手首１６の先端部に設けられ、Z₄軸に垂直な
Z₅軸回りに自由度を有する手首４７。 以上、各関節が１自由度づつ有するもので、こ
のロボツトは全体で６自由度となる。 このような６軸関節形ロボツトにおいては、手
首１７は位置のみならず、姿勢も制御可能である
ことは、例えば米国バデユー大学教授R.P.Paul
氏が著した『ROBOT MANIPULATORS：
MATHEMATICS、PROGRAMING AND
CONTROL』（1981年）にも詳しい。 なお、この著書は、ロボツトの計算機制御につ
いて世界で初めて体系的かつ学術的記述を行なつ
たものとして有名であり、日本では『ロボツト・
マニビユレータ』（吉川恒夫訳、コロナ社）とし
て発行されている。 さて、これらの従来技術によれば、各関節ごと
に次の手順で各座標系を固定していけば、６関節
目の手首１７の姿勢が確定できる。 Z_o-1（ｎ＝１〜６）軸回りの角度θ_oだけ回転
させる。 Z_o-1軸に沿つて距離d_oだけ並進させる。 回転後のX_o-1軸、すなわちX_o軸に沿つて長
さa_oだけ並進させる。 X_o軸回りにねじれ角α_oだけ回転させる。す
なわち、ロボツトをZ_p軸回りに角度θ₁だけ回転
させ、次にZ_p軸に沿つて距離d₁だけ並進させ、
次に回転後のX_p軸、すなわちX₁時に沿つてロ
ボツトを長さa_pだけ情進させ、X₁軸回りにね
じれ角α₁だけ回転させる。これにより、下腕１
３の座標系X₁Y₁Z₁が固定される、以後、同様
にして、上腕１４、手首１５，１６，１７の座
標系X₂Y₂Z₂、X₃Y₃Z₃、X₄Y₄Z₄、X₅Y₅Z₅が固
定され、手首１７の姿勢が確立する。 以上の操作はリンク_oの座標をリンク_o-1に関係
づける４つの同次変換の積として表現される（こ
れをＡ行列と呼ぶ）。 すなわち、 A_o＝ROT（Ｚ、θ_o）trans（０、０、d_o）trans（
a_o０、０）Rot（ｘ、α_o） ＝cosθ_o sin_oθ ０ ０−sinθ_o cosθ_o ０ ００ ０ １ ００ ０ ０ １１ ０ ０ ａ ０ １ ０ ０ ０ ０ １ ｄ ０ ０ ０ １１ ０ ０ ００ cosα_o sinα_o ００ −sinα_o cosα_o ００ ０ ０ １ となる。そこで、手首の先端をT₆とすれば、 T₆＝Ｚ・A₁A₂A₃A₄A₅A₆ ただし、 Ｚ＝１ ０ ０ P_x ０ １ ０ P_y ０ ０ １ P_z 0001 となり、固定座標軸（X_O、Y_O、Z_p）の方向余弦
（n_x、n_y、n_z）で表現すると、 T₆＝n_x n_y n_z ０0_x 0_y 0_z ０a_x a_y a_z ０P_x P_y P_z １ となる。 つまり、T₆の各要素を指定して、リンク角θ₁
〜θ₆を求めるのが従来の演算法である。 この際、リンク角θ₁を求める演算式は、T₆よ
り第６図ａに示すように手首部がオフセツトして
いない（その手首部の座標系の原点が、上腕の座
標系のｘ軸上にある）ロボツトの場合は、 −sinθ₁・P_x＋cosθ₁・P_y＝d₂ となり、これは三角法的代入により下記のように
解くことができる。 第８図はリンク角θ₁と位置Px、Pyと距離d₂の
間の関係を示す図である。 Px＝Mcosθ₀、Py＝Msinθ₀であり、これを−
sinθ₁・Px＋cosθ₁・Py＝d₂に代入すると −sinθ₁・Mcosθ₀＋cosθ₁・Msinθ₀＝d₂ Msin（θ₀−θ₁）＝d₂ sin（θ₀−θ₁）＝d₂／Ｍ θ₀−θ₁＝ωとおくと、tanω＝d₂／Ｋ θ₁＝θ₀−ω tanθ₁＝tan（θ₁−ω）＝tanθ₀−tanω／１＋tanθ₀
・tanω ＝Py／Px−d₂／Ｋ／１＋Py／Px・d₂／Ｋ＝PyK−Pxd₂
／PxK−Pyd₂ よつてθ₁が求まる。 〔発明が解決しようとする問題点〕 ところが、第６図ｂに示すように手首部がオフ
セツトしている（その手首部の座標系の原点が、
上腕の座標系のｘ軸上にない、つまり手首部の軸
線がアームの軸線に対して平行移動している）ロ
ボツトの場合は、 −sinθ₁・P_xcosθ₁・P_y ＝d₂＋cosθ₄・d₅ となる。 この式は、前述のリンク角θ₁を求める演算式の
右辺に手首１６のオフセツトに相当する距離
cosθ₄・d₅を加味したもので、リンク角θ₁とθ₄が
変数として存在しているため、三角法的代入では
解くことができない。 したがつて、従来では、手首がオフセツトして
いないロボツトしか制御できなかつた。 ちなみに、特開昭60−48291には、手首部分の
構造を工夫してオフセツトしないようにした技術
が提案されている。 しかしながら、このようにオフセツトしていな
い手首は、第７図に示すように、自分自身によつ
て、作動範囲が制約をうけるという致命的欠点を
有するほか、どうしても構造が複雑になり、手首
部分が必要以上に重くなるなど、種々の欠点があ
る。 本発明の目的は、手首部がオフセツトしている
ロボツトの制御方法を提供することである。 〔問題点を解決するための手段〕 本発明はロボツトの制御方法は、手首部の位置
および姿勢制御を行なう場合に、 位置データから基本３軸のリンク角を演算する
第１のステツプと、 姿勢データと第１のステツプで求めた基本３軸
のリンク角から手首部のリンク角を演算する第２
のステツプと、 位置デーダと、第２のステツプで求めた手首部
のリンク角から基本３軸のリンク角を再演算する
第３のステツプとを繰返すことにより各リンク角
の指令値を得ることを特徴とする。 〔作用〕 本発明は、従来技術で演算不可能であつた原因
は、１回の演算で算出しようとしたところにある
との知見を得てなされたもので、以上の一連の演
算処理を繰り返し行なうことにより、各リンク角
の指令値を求めるものである、この処理を複数回
繰り返せば、解に限りなく近づいてくことにな
る。なお、繰り返しの回数は、要求される精度や
ロボツト制御装置の演算処理速度によつて求めら
れるが、８回以内の繰り返しによつてほぼ解に収
束する。特に、極端な精度を必要としない分野で
は２〜３回でも充分であることが実験によつて確
認されている。 〔実施例〕 以下、本発明の実施例について図面を参照して
説明する。 第１図は本発明のロボツトの制御方法の一実施
例を示すフローチヤート、第２図はロボツトの固
定座標軸（X_p、Y_p、Z_p）のX_p−Y_p平面への投影
図、第３図はロボツトのM_p−Z_p平面への投影図
である。なお、制御されるロボツトは第５図に示
すものとする。 まず、ロボツトの手首部１７にある、ロボツト
の制御点Ｐの位置データPx、Py、Pz（固定座標
系X_pY_pZ_pを示す）が与えられ（ステツプ１）手
首１５，手首１６，手首１７は動作しないものと
して、基本３軸（胴１２，下腕１３，上腕１４）
のリンク角θ₁、θ₂、θ₃の演算を行なう。（ステツ
プ２）。 この演算は次の手順で行なう。第２図に示すよ
うに、固定座標軸のX_p−Y_p平面にロボツトを投
影させてP_x、P_yを求める。ここで、第２図はロ
ボツトの第１軸回転中心（Z_p軸）からロボツトの
制御点Ｐまでをロボツトの固定座標X_pY_pへ投影
したものである。 すると、リンク角θ₁は θ₁＝tan^-1（P_y／P_x）により求まる。次に、制御
点ＰまでのペクトルをM_pとして、 M_p−Z_p平面を想定する。 そして、第３図に示すようにM_p−Z_p平面つま
り、X_p−Z_p2平面にロボツトを投影させて、Pz′、
Pz″、M′、M″を求める。 すると、リンク角θ₂、リンク角θ₃は、 θ₂＝tan^-1（Pz″／Ｍ） θ₃＝tan^-1（Pz′／Ｍ） により求まる。ここで、リンクθ₁、θ₂、θ₃の具体
的演算式は幾何学関係により求まるが、その誘導
は極めて煩雑なので、ここでは説明しない。 以上で、リンク角θ₁、θ₂、θ₃が求まつた。ただ
し、これらの角度θ₁θ₂、θ₃は、手首を固定したも
のとしての解であり、あくまでも仮の解である。 次に、姿勢データn_x、n_y、n_z、O_x、O_y、O_z、
a_x、a_y、a_zを入力し（ステツプ３）、これら姿勢
データと先に求めたリンク角θ₁、θ₂、θ₃（仮の解）
により手首部にリンク角θ₄、θ₅、θ₆を計算する。
これは従来の姿勢制御方法の手首部を求める演算
方法を用いる。この場合、位置データは考慮しな
い。 前述のT₆＝A₁A₂A₃A₄A₅A₆から A₃ ^-1A₂ ^-1A₁ ^-1T₆＝A₄A₅A₆

【表】 ただし、Ｓ、Ｌ、Ｕ、Ｒ、Ｂ、Ｔはそれぞれ
Z₁、Z₂、…、Z₆軸 SS＝SiN（θ₁）、S_LU＝SiN（θ₂＋θ₃）、S_u＝SiNθ₃ CS＝COS（θ₁）、C_LU＝COS（θ₂＋θ₃）、C_u＝COSθ₃ SR＝SiN（θ₄）、SB＝SiN（θ₅）、ST＝SiN（θ₆） CR＝COS（θ₄）、CB＝COS（θ₅）、CT＝COS（θ₆） a₂＝Ｌ軸アーム長 d₂＝Ｓ時回転中心とＲ軸回転中心の距離 d₄＝Ｕ軸アーム長 d₅＝Ｒ軸回転中心とＴ軸回転中心の距離 (1)式より −CR・SB＝a_xCS・S_LU ＋a_ySS・S_LU−a_zC_LU ……(2) −SR・SB＝−a_xSS＋a_yCS ……(3) (2)、(3)式より θ₄＝tan^-1｛a_xSS−a_yCS／a_zC_LU−S_LU（a_xCS＋a_ySS）
｝ また、 A₄ ^-1A₃ ^-1A₂ ^-1A₁ ^-1T₆＝A₅A₆

〔発明の効果〕

以上説明したように本発明は、基本３軸のリン
ク角を再演算することにより、ロボツトの手首部
がロボツトアームに対してオフセツトしている機
構のロボツトにおいてもオフセツトしていない機
構のロボツトと同様に姿勢制御できるという大き
な効果がある。

【図面の簡単な説明】

第１図は本発明のロボツトの制御方法の一実施
例を示すフローチヤート、第２図はロボツトの固
定座標軸（X_pY_pZ_p）のX_p−Y_p平面への投影図、
第３図はロボツトのM_p−Z_p平面への投影図、第
４図は本発明の制御方法が適用されたロボツトの
制御部のブロツク図、第５図は６軸関節形ロボツ
トの外観図、第６図ａ，ｂはオフセツトしていな
い手首部およびオフセツトしている手首部の外観
図、第７図はオフセツトしていない手首部が作動
範囲が制約される様子を示す図、第８図はリンク
角θ₁と位置Px，Pyと距離d₂の関係を示す図であ
る。 １〜７……ステツプ、１１……脚、１２……
胴、１３……下腕、１４……上腕、１５，１６，
１７……手首。

Claims (1)

1. 【特許請求の範囲】 １ 基本３軸と、ロボツトアームに対してオフセ
   ツトしている手首３軸を有する６軸関節形ロボツ
   トにおいて、その手首部の位置および姿勢制御を
   行なう場合に、 位置データから基本３軸のリンク角を演算する
   第１のステツプと、 姿勢データと第１のステツプで求めた基本３軸
   のリンク角から手首部のリンク角を演算する第２
   のステツプと、 位置データと、第２のステツプで求めた手首部
   のリンク角から基本３軸のリンク角を再演算する
   第３のステツプとを繰返すことにより各リンク角
   の指令値を得ることを特徴とするロボツトの制御
   方法。