JPH0411340B2 - - Google Patents

Description

【発明の詳細な説明】 (A) 発明の技術分野 本発明はロボツトアーム座標決定法に関し、と
くに多関節を有するロボツトアーム座標決定法に
関する。

(B) 技術の背景 一般にロボツトは物品の組立作業や運搬作業に
用いられるが複雑な作業には多関節アームを有す
るロボツトが適しており、更に高度な精密作業に
これを用いるためには高い位置決め精度を有する
多関節アームを具えたロボツトが要望される。

(C) 従来技術と問題 ロボツトの駆動には教示−再生（テイーチング
プレイバツク）方式が広く用いられている。

つまり人間が直接にロボツトアームの先端を保
持して目標位置に位置付けするか、又は教示箱
（テイーチングボツクス）に取付けたスイツチに
より遠隔操作してアームの先端を位置付けした
後、各関節に設けられている回転角度検出器の値
を関節の回転角として記憶するという操作の繰返
しによつて一連のアーム動作に応じたデータ列を
作成する。

次にロボツトアームの駆動時にはこのデータ列
を記憶部から読出し、各関節の駆動回路へ指令値
へ入力する。このような駆動制御方式のロボツト
においては、教示した点にアーム先端は正しく位
置付けされる。しかしロボツトを更に高度な作
業、例えば所定の平面上に複数の物品をマトリツ
クス状に整列配置する作業、CADによつてプロ
グラミングされた作業データで駆動する場合、あ
るいテレビカメラ等を用いた視覚認識装置により
検知された位置座標値に基いてロボツトを駆動
し、物体を把持する作業等においてはロボツトの
関節構成により定まる姿勢変換式を解いて絶対座
標系をロボツトの所要の関節角度のデータに変換
する必要がある。

ところで上記姿勢変換式には各関節の腕の長さ
及び関節原点がパラメータとして含まれており、
これらパラメータに誤差が含まれていると、ロボ
ツトの移動する軌跡は指示した軌跡とは異なり、
上記作業には不適当になる。つまりロボツトの組
立作業時に派生する組立誤差（各アーム及び各関
節の取付け誤差）が存在するため、上述の姿勢変
換式を解いてロボツトに指示値を与えてもロボツ
トは所望の動きをせず高度な精密作業には使用で
きない。

(D) 発明の目的 本発明はかかる従来の欠点に鑑みなされたもの
で比較的単純な操作によつてアームを所望の位置
へ正確に到達させることができるロボツトアーム
座標決定法を提供することを目的とする。

(E) 発明の構成 ロボツトアームを指令値に基づいて正確に動作
させるためのロボツト座標決定法であつて、 予め、複数個の関節部を有するロボツトアーム
の先端をｎ（ｎ≦２）個の固定点の各々に順次任
意の姿勢によつて位置付けし、該複数の固定点に
おける各姿勢形成時の該ロボツトアームの各関節
部の回転角を該各関節部に設けた回転角検出手段
で検出し、検出された各関節毎の複数の検出角か
ら初期の各回転角検出手段の計測原点との原点誤
差を求め、各原点誤差に基いて該ロボツトアーム
の各関節の前記計測原点を決定するようにしたこ
とを特徴とするロボツトアーム座標決定法を提供
することにより達成される。

(F) 発明の実施例 発明の実施例の説明に先立つて本発明の基本的
考え方について述べる。

第１図はロボツトの要部構成を模式的に示した
ものでA₁〜A₇はアーム、K₁〜K₆は関節であり、
このうちA₂，A₅，A₇は各々アームA₁，A₄，A₆
を回転中心として任意角度の回転が可能であり、
関節K₁〜K₃は各々アームA₂，A₃；A₃，A₄；A₅，
A₆で構成される平面内において任意角度の回転
が可能である。アームA₇の先端A₀をロボツトの
アームに固定した座標系（ハンド座標系）の原点
とし、アームA₇に対して垂直方向にこの原点よ
り距離ｈだけ離れた点Akを計測基準点とする。
原点を計測基準点とせずにこのように別個に計測
基準点を設けるのは関節K₆の回転によつてはア
ームA₇の先端の位置の変化は生じないが、計測
基準点はこの関節K₆の回転により変位すること
により関節R₃の回転をも考慮したロボツトの動
作の制御が必要になるからである。

ここで第１図のロボツトの各パラメータを以下
のように定める。

a_i；アームA_i（ｉ＝１〜７）の長さ θ_i；関節K_i（ｉ＝１〜６）の真の回転角 Δθ_i；関節K_iの機械的設定誤差に基づく関節Ki
の回転角誤差 〓；アーム先端の位置ベクトル 〓_i座標変換マトリツクス 〓₆；ハンド座標系における位置座標 ハンド座標系における位置座標〓₆をロボツト座
標系（絶対座標系（X₀，Y₀，Z₀））における位置
座標〓′で表わすと、 〓′＝〓₁，〓₂，〓₃，〓₄，〓₅，〓₆ ＝〓_T・〓₆ となる。

但し〓_T＝〓₁，〓₂，〓₃，〓₄，〓₅，〓₆ 〓，〓_i（ｉ＝１〜６）は関節の回転角〓（θ₁，
θ₂，θ₃，θ₄，θ₅，θ₆）の関数であるから回転角の
原点誤差Δ〓（Δθ₁，Δθ₂，Δθ₃，Δθ₄，Δθ₅，
Δθ₆）を含んだ関節角に対するロボツト座標系上
の基準点位置ベクトル〓（〓＋Δ〓）は次式で表
わされる。

〓（〓＋Δ〓）＝〓（〓）＋₆ 〓ⁱ⁼¹ ∫_i∂〓（〓＋Δ〓／∂〓_i ……(2) δ_iΔθ_i ハンド座標系上のある点のロボツト座標系におけ
る座標〓（〓）とそのときの各関節に設けられて
いるロータリエンコーダの読取値（出力値）〓＋
Δ〓よりδ_iを未知数とする連立方程式(2)よりδ_iの
値を求めることができる。

ここで であり、 〓₁＝cosθ sinθ ０ ０ −sinθ cosθ ０ ０ ０ ０ １ ０ ０ ０ a₁ １ ……(4-1) 〓₂＝cosθ₂ ０ −sinθ₂ ０ ０ １ ０ ０ sinθ₂ ０ cosθ₂ ０ a₂sinθ₂ ０ a₂cosθ₂ １ ……（４−２） 〓₃＝cosθ₃ ０ −sinθ₃ ０ ０ １ ０ ０ sinθ₃ ０ cosθ₃ ０ a₃sinθ₃ ０ a₃cosθ₃ １ ……（４−３） 〓₄＝１ ０ ０ ０ ０ cosθ₄ sinθ₄ ０ ０ −sinθ₄ cosθ₄ ０ a₄ ０ ０ ０
……（４−４） 〓₅＝cosθ₅ ０ −sinθ₅ ０ ０ １ ０ ０ sinθ₅ ０ cosθ₅ ０ a₅cosθ₅ ０ −a₅sinθ₅ １ ……（４−５） 〓₆＝１ ０ ０ ０ ０ cosθ₆ sinθ₆ ０ ０ −sinθ₆ cosθ₆ ０ a₆ ０ ０ １ ……(4-6) δ_iが求まると第(2)式及び次式 〓（〓＋Δ〓）＝A_T（〓＋Δ〓）〓₆ を用いて関節部おける機械的設定誤差Δ〓が求ま
る。この設定誤差Δ〓が判明すればロボツトに対
する指示値〓（〓）を算出する基礎とる関節の回
転角〓を指示値として与えれば第(2)式よりアーム
の位置〓（〓＋Δ〓）が判る。

次に本発明の好ましい実施例について説明す
る。

第２図は本発明の実施例構成図であつて、Ｂは
ロボツト基底部、Ａはアーム部であつて前述のア
ームA₁〜A₇及び関節K₁〜K₆により構成される。
Ｈはハンドで変位検出Ｔを把握する。変位検出子
Ｔの動きは変位検出器Ｇの内部に設けたセンサ
ー、例えば歪ゲージ等により電気信号に変換す
る。Ｙは歪ゲージの出力信号をＸ，Ｙ，Ｚ方向の
変位量に変換する変位検出部、Ｍは座標決定部、
Ｃは制御部、Ｚは関節角の検出部であつて関節
K₁〜K₆に設けられており、関節角を与えるロー
タリエンコーダ（図示せず）の出力値を受けて関
節角を検出する。

まず変位検出器Ｇを第１図に示す位置〓₁（〓）
に設定する。この場合検出器Ｇの位置は〓（〓＋
Δ〓） 即ち第(2)式を用いて〓_T（〓＋Δ〓）〓₆で与え
られる。（関節角〓＋Δ〓はロータリエンコーダ
の出力値である。）従つて 〓₁（〓）＝〓₁（〓＋Δ〓）−₆ 〓ⁱ⁼¹ ∫_i∂〓_i（〓＋Δ〓）／∂θ_i ……(6) が成立する。

次に検出器Ｇを第１図に示す位置〓₂（〓）に設
定すると同様にして次式が成立する。

〓₂（〓）＝〓₂（〓＋Δ〓）−₆ 〓ⁱ⁼¹ ∂〓₂（〓＋Δ〓）／∂θ_i 位置ベクトル〓₁，〓₂は３次元空間における３
つの成分P_1x，P_1y，P_1z，P_2x，P_2y，P_2zを有する
ため上式(6)，(7)は６連立方程式となり６つの未知
数δ₁〜δ₆の値を求めることができる。検出器Ｇの
位置は変位検出部Ｙで検出され位置情報として座
標決定部Ｍへ入力される。座標決定部Ｍでは、検
出器Ｇの位置情報及び関節角検出部Ｚからの関節
角〓＋Δ〓及及び指示値〓に基いて式(7)，(8)の解
δ₁〜δ₆を求める。その後(2)，(5)式を用いて設定誤
差Δ〓を求める。そして〓＋Δ〓−∫_iを(2)式の〓
＋Δ〓の代わりに代入し同様の計算を行つて真の
誤差Δ〓を求めて正しい回転角の原点を得る。こ
れら算出値δ₁〜δ₆，Δ〓は制御部Ｃへ送られ指示
値θ₁〜θ₆に基いてハンドＨの位置制御が行われ
る。検出器Ｇによりアーム先端の位置の検出を行
うのが困難な場合は以下の方法によることも可能
である。

第３図に示すように平板（治具）Ｄに配された
５個のピンE₁〜E₅を順次ロボツトのハンドＨで
把持する。ロボツト座標系から治具座標系への座
標変換マトリツクスをＢとし、治具座標系の原点
のロボツト座標系における位置ベクトルをΔと
し、治具上の点の治具座標系での座標を〓とする
とこの点のロボツト座標系での位置ベクトルは 〓＝〓〓＋〓 ……(9) となる。（第４図） この式を(6)式に代入すると、 〓〓＋〓＝〓（〓＋Δ〓）−₆ 〓ⁱ⁼¹ ∫_i∂〓（〓＋Δ〓）／∂〓_i ……(10) となる。

２組のデータ（〓₁，〓＋Δ〓），（〓₂，〓₂＋
Δ〓） を上式に代入し、辺々の差をとると 〓（〓₁−〓₂）＝｛〓（〓＋Δ〓）−〓 （〓₂＋Δ〓）｝−₆ 〓ⁱ⁼¹ ∫_i｛∂〓（〓₁＋Δ〓）／∂θ_i −∂〓（〓₂＋Δ〓）／∂θ_i｝ ……(10)′ を得る。Δ〓＝〓₁−〓₂，Δ〓＝〓（〓₁＋Δ〓） −〓（〓₂＋Δ〓）Δ（∂〓／∂θ_i） ＝∂〓（〓₁＋Δ〓）／∂〓_i−∂〓（〓₂＋Δ〓）／
∂θ_i とおくことにより 〓Δ〓＝Δ〓−₆ 〓ⁱ⁼¹ ∫_iΔ（∂〓／∂θ_i） ……(11) となる。の係数を全て未知数とすれば９＋６＝15
個の未知数をもつ１次方程式とる。従つていくつ
かのデータから〓及びδ_iを求めることができる。

〓は次式で与えられる。

〓＝cosα， simα， ０ −sinα， cosα， ０， ０ ０ １ cosθ， ０， −simβ， ０， １， ０， simβ ０ cosβ０， ０， ０， ０， −simγ， cosγ， ０ −cosγ −simγ …(12) （α，β，γはロボツト座標系に対する姿勢角
となる）従つて、ロボツトのハンドＨによつて、
治具ＤのピンP₁〜P₅を順次把持し、その都度の
関節K₁〜K₆の回転角をロータリエンコーダによ
つて知り関節角検出部Ｚで検出する。他方ロボツ
ト座標決定部Ｍでは、関節角検出部Ｚから入力さ
れる関節角〓＋Δ〓をもとにして(5)，(11)式を計算
して未知数δ₁〜δ₆より誤差Δ〓を求め前述の実施
例の場合と同様の繰返し演算によつて真の誤差Δ
〓を求める。この誤差Δ〓はロボツト制御部Ｃへ
入力され、別に入力される指示値〓とに基いてロ
ボツトアームの位置制御を行う。

なお治具Ｄの５つのピンE₁〜E₅の代わりに任
意の２つのピン、例えばE₁，E₂を用いて未知数δ₁
〜δ₆，誤差Δ〓を求めることもできる。

第５図及び第６図において、ピンE₁，E₂に
各々ハンドＨを順次固定し、その時の関節K₁〜
K₆に設けたロータリエンコーダの出力を関節角
検出器Ｚに入力して関節角〓＋Δ〓を検出し、演
算部Ｆへ入力する。

前述の式を用いると 〓Δ〓＝Δ〓₁−₆ 〓ⁱ⁼¹ 〓∫_iΔ（∂〓／∂〓_i）₁ ……(13) 〓Δ〓＝Δ〓₂−₆ 〓ⁱ⁼¹ ∫_iΔ（∂〓／∂〓_i）₂ ……(14) が成立する。添数字はピンE₁，E₂にハンドＨを
固定したときにアームのなす第１及び第２の姿勢
を識別するものである。

Δ〓＝〓₁−〓₂，Δ〓₁＝〓₁（〓₁＋Δ〓）−〓₂（〓
_２ ＋Δ〓）Δ〓₂＝〓₃（〓₂＋Δ〓）−〓₄（〓₄＋Δ
〓） Δ（∂〓／∂〓_i）₁＝∂〓（〓₁＋Δ〓）／∂〓_i− ∂〓（〓₂＋Δ〓）／∂〓_i Δ（∂〓／∂〓_i）₂＝∂〓（〓₄＋Δ〓）／∂〓_i− ∂〓（〓₂＋Δ〓）／∂〓_i 上記２式の辺々が等しいため₆ 〓ⁱ⁼¹ ∫_i（Δ（〓〓／〓〓_i）₁₁−Δ（〓〓／〓〓_i）₂₁） ＝Δ〓₁−Δ〓₂ ……(15) が成立する。

上式においてピンE₁，E₂にハンドＨが固定さ
れたときのハンドＨの位置ベクトル〓₁，〓₂を用
いて得られる連立方程式を演算部Ｆで解し、解δ₁
〜δ₆を求めた後、上述したと同様の方法により真
の誤差Δ〓を決定する。従つてこの誤差値Δ〓を
ロボツト制御部Ｃへ送出し以後このΔ〓を用いて
角度検出器の計測原点を設定することにより指示
値〓に基づく正しいアーム位置制御が可能とな
る。

(G) 発明の効果 以上の説明から明らかなように本発明に係るロ
ボツトのアーム座標決定法はロボツトアームを任
意の位置へ正確に駆動制御できる。

【図面の簡単な説明】

第１図はロボツトの要部構成図、第２図は本発
明の一実施例構成図、第３図〜第６図は本発明の
他の実施例説明図である。 A₁〜A₆；アーム、K₁〜K₆；関節、E₁〜E₅；固
定点、Ｈ；ハンド、Ｙ；変位検出部、Ｍ；ロボツ
ト座標決定部、Ｚ；関節角検出部、Ｃ；ロボツト
制御部。

Claims (1)

1. 【特許請求の範囲】 １ ロボツトアームを指令値に基づいて正確に動
   作させるためのロボツト座標決定法であつて、 予め、複数個の関節部を有するロボツトアーム
   の先端をｎ（ｎ≧２）個の固定点の各々に順次任
   意の姿勢によつて位置付けし、該複数の固定点に
   おける各姿勢形成時の該ロボツトアームの各関節
   部の回転角を該各関節部に設けた回転角検出手段
   で検出し、検出された各関節毎の複数の検出角か
   ら初期の各回転角検出手段の計測原点との原点誤
   差を求め、各原点誤差に基いて該ロボツトアーム
   の各関節の前記計測原点を決定するようにしたこ
   とを特徴とするロボツトアーム座標決定法。