JPH0768480A - マニピュレータの関節角度制御方法 - Google Patents

Abstract

(57)【要約】 【目的】 マニピュレータ手先誤差に及ぼす影響の大き
い関節の角度を効果的に補正でき、マニピュレータ手先
の絶対精度を向上する。 【構成】 位置計測器２により測定されたマニピュレー
タ手先９（もしくは位置制御点）の位置姿勢とマニピュ
レータ各関節に設置された関節角度検出器５と各関節間
のリンク長さにより算出されたマニピュレータ手先９の
位置姿勢との差、すなわち、補正すべき量をマニピュレ
ータ手先９の位置姿勢と関節角度の関係を示すヤコビ行
列を用いて関節角補正量に帰着させる。更にその補正量
に重み付けを行なうべく、各関節からマニピュレータ手
先９までのリンク長により予め設定される関節リンク長
ゲインを乗じ、マニピュレータ各関節角補正量を導出し
て補正を行ない、これを繰り返し行なってマニピュレー
タ関節のオフセット量を固定する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、マニピュレータの関節
角度制御方法に関する。

【０００２】

【従来の技術】マニピュレータ（ロボット）の関節角度
制御に適用される最も一般的なキャリブレーション方法
は、関節角度センサに原点出し機能を持ったエンコーダ
を用い、この機能である原点合わせを用いて行なってい
る。しかし、この方法は、エンコーダの取り付け誤差
や、その他メカニズムの持つ誤差（加工上、組み立て
上）を何ら解決することはできない。

【０００３】また、一般に言われるキャリブレーション
方法は、ロボットの実環境と、その画像データ（ＴＶ画
像）との誤差修正を行なうものが多く、マニピュレータ
の持つメカニズム上の誤差解決とはならない。本発明で
示す内容はロボットの位置決め精度を向上させるための
方法であり、従来はこの方法がなかったので、または製
品化に当って困難であったためロボットの位置決め精度
は相対位置決め精度に依存しているのが現状である。

【０００４】

【発明が解決しようとする課題】マニピュレータがある
実環境で作業を行なう場合、その作業環境内でのマニピ
ュレータ手先の位置決め精度が問題となる。すなわち、
作業環境内に設定された基準点からの３次元的な位置決
め精度（絶対精度）が高ければ、例えば作業対象とする
ハンドリング物体の位置姿勢を先の基準点からの３次元
座標で記述してやることでマニピュレータによる作業が
可能となる。

【０００５】しかし、マニピュレータのメカニズム上の
誤差（リンクの曲り、組み付け誤差等）が存在するため
マニピュレータ各関節に設置された関節角度センサ値と
各関節間のリンク長より算出されたマニピュレータ手先
の位置姿勢の値は実環境にての実際の位置姿勢に対して
必ず誤差を含んでいる。

【０００６】したがって、マニピュレータの作業に関し
ては前述の絶対位置決め精度に依存せず、誤差を含んだ
形で算出される関節角度センサ値、各関節リンク長を用
いたマニピュレータ位置姿勢を主体とする、すなわち相
対位置決め精度に依存して作業を行なわせている。

【０００７】よって、作業環境内のハンドリング物体の
教示を考えた場合、マニピュレータ相対位置決め精度に
依存せざるを得ないので、実際にマニピュレータ手先を
人間が持って、例えば物体を把持させるならば、実際に
マニピュレータで把持させるという、ティーチングプレ
イバックによる教示が必要である。そのため、環境教示
に非常に手間がかかるといった問題がある。

【０００８】近年、このマニピュレータの自動（自律）
化に対する教示の低減という課題が重要になっている。
そのため、教示低減のできるＣＡＤ情報等を用いたオフ
ラインティーチングによる教示の実現が必要である。し
かし、これにはマニピュレータの絶対位置決め精度の向
上が重要な過大であり、大きな問題でもある。

【０００９】本発明は上記実情に鑑みてなされたもの
で、マニピュレータ手先誤差に及ぼす影響の大きい関節
の角度を効果的に補正でき、マニピュレータ手先の絶対
精度を向上し得るマニピュレータの関節角度制御方法を
提供することを目的とする。

【００１０】

【課題を解決するための手段】本発明に係るマニピュレ
ータの関節角度制御方法は、任意の計測器により測定さ
れたマニピュレータ手先もしくは位置制御点の位置姿勢
とマニピュレータ各関節に設置された角度センサと各関
節間のリンク長さにより算出されたマニピュレータ手先
の位置姿勢との差をマニピュレータ手先の位置姿勢と関
節角度の関係を示すヤコビ行列を用いて関節角補正量に
帰着させ、更にその補正量に重み付けを行なうべく、各
関節からマニピュレータ手先までのリンク長により予め
設定される関節リンク長ゲインを乗じマニピュレータ各
関節角補正量を導出して補正を行ない、これを繰り返し
行ないマニピュレータ関節のオフセット量を固定するこ
とを特徴とする。

【００１１】

【作用】図１は、本発明を説明する上での原理図を示し
たものである。図１において、 ｘo −ｙo −ｚo ：基準となる座標系（基準座標） ｘm −ｙm −ｚm ：マニピュレータの基準座標（マニピ
ュレータ座標） Ｔo ：外部計測器により計測される基準座標からマニピ
ュレータ座標迄の位置姿勢 Ｔs ：外部計測器により計測される基準座標からマニピ
ュレータ座標手先迄の位置姿勢 Ｔm ：マニピュレータ座標を基準として関節角度、リン
ク長より算出されるマニピュレータ手先の位置姿勢 である。ただし、Ｔαとは

【００１２】

【数１】 にて表現される４×４行列である。

【００１３】ここで、マニピュレータ手先位置姿勢Ｔm
は、マニピュレータ１ａの関節やリンクの誤差を含んで
いる。そこで、外部計測器により計測されるＴo 、Ｔs
を用いて ＾Ｔm ＝Ｔs ・Ｔo ^-1 （２） を求める。この値は外部計測器により計測されているの
で、位置姿勢Ｔm に比べてより確かな精度を有する。次
に位置姿勢Ｔm 、＾Ｔm によりマニピュレータ手先位置
姿勢の誤差をΔｒを求める。

【００１４】

【数２】

【００１５】この誤差Δｒがキャリブレーションにより
補正すべき量で、マニピュレータ手先の位置姿勢と各関
節角の変換となるヤコビ行列を用いて関節角でのキャリ
ブレーション量に帰着させる。

【００１６】

【数３】

【００１７】この（４）式におけるΔθが各関節での補
正すべきオフセット量と推定されるが、マニピュレータ
手先位置姿勢に及ぼす各関節誤差の影響はどれも均一で
なく根元関節ほど大きな誤差影響となる。この点に着目
して先の関節オフセット量Δθに重み付けを行なう。そ
のため関節リンク長ゲインＫｅとして

【００１８】

【数４】 を用い、 Δθ補正＝Ｋｅ・Δθ （６） にて最終的な補正量を導出する。

【００１９】関節リンク長ゲインＫｅは、例えば６軸マ
ニピュレータを例にすると図２、図３に示すように各関
節からマニピュレータ手先迄の全リンク長さに応じて求
め、一般的にＮ自由度マニピュレータにおいては

【００２０】

【数５】 にて設定する。

【００２１】そして、次式に示すように補正量Δθ補正
をマニピュレータ各関節センサの読み値θs にオフセッ
トを加える。 θs ＝θs ＋Δθ補正 （１０） これにより、マニピュレータは、補正量Δθ補正に応じ
て誤差を解消する。

【００２２】同じようにして位置姿勢Ｔm 、＾Ｔm を算
出、計測して前述の手順を繰り返す。この繰り返しを手
先誤差Δｒが次式 Δｒ＜Δｒe （１１） に示すようにある許容値Δｒe 以下となるまで行なうこ
とでキャリブレーションが完了する。

【００２３】このように本発明による方法では、マニピ
ュレータ手先誤差Δｒを外部計測器等を用いて求めた
後、この手先誤差Δｒを補正するマニピュレータ各関節
での誤差量Δθを求める。

【００２４】そして、マニピュレータ手先誤差を各関節
にて補正する場合、図４に示すようにマニピュレータ手
先より遠いところに配置される（根元に配置される）関
節ほど、大きな影響を及ぼすことに着目して、各関節で
の誤差量Δθに重み付けを行なう。この重み付けのゲイ
ンは、各関節から手先までのリンク長に基づいて決定す
る。マニピュレータ手先誤差に及ぼす影響の大きい関節
が、この重み付けにより効果的に補正されるので、マニ
ピュレータ手先誤差の補正も効果的に実現できる。

【００２５】

【実施例】以下、図面を参照して本発明の一実施例を説
明する。図５は、６軸マニピュレータの例について示し
たものである。同図に示すように６軸マニピュレータ１
は、制御アンプ４、関節角度検出器５及び制御計算機６
により、図６に示すブロック図にて位置制御される。

【００２６】今、この６軸マニピュレータ１は、手先位
置姿勢Ｔm にて制御されている。この時（マニピュレー
タは静止状態）、位置計測器２を用いて基準座標７から
マニピュレータ手先９迄の位置姿勢Ｔs 、基準座標から
マニピュレータ座標８迄の位置姿勢Ｔo を計測し、基準
座標７を基本としたマニピュレータ座標８でのマニピュ
レータ手先位置姿勢＾Ｔm を求める。

【００２７】次に、このＴm 、＾Ｔm よりマニピュレー
タ手先９上での誤差Δｒを求める。この誤差Δｒは、上
記（３）式にて求める。マニピュレータ制御計算機６に
おいては、図６に示すように常にヤコビ行列２０を計算
し、制御アンプ４を介してマニピュレータ１に出力す
る。

【００２８】この場合、ヤコビ行列２０を用いて手先誤
差Δｒを関節誤差Δθに（４）式を用いて帰着させる。
この関節誤差Δθに対して、図２，図３にて求められる
関節リンク長ゲインＫｅを用いて重み付けを行ない、関
節補正量Δθ補正を（６）式にて求める。

【００２９】このΔθ補正を関節角度検出器５で検出さ
れる関節角度θs にオフセット量として（１０）式に示
すように補正する。これにより、図６に示すブロック図
中のマニピュレータ現在位置姿勢２１が変化し、従来の
目標値Ｔm との偏差を生じ、目標値Ｔm への位置へ新た
にマニピュレータ手先９が移動する。

【００３０】そして、マニピュレータ１の動作が静定し
た後、図７のフローチャート（アルゴリズム）に示すよ
うに、再度、位置計測器２を用いて誤差量を検出する。
すなわち、マニピュレータ座標を基準として関節角度、
リンク長よりマニピュレータ手先の位置姿勢Ｔm を導出
し（ステップＡ1 ）、次いで、外部計測器により計測さ
れる位置姿勢Ｔo 、Ｔs を用いて（２）式によりマニピ
ュレータ手先位置姿勢＾Ｔm を求める（ステップＡ2
）。更に、（３）式によりマニピュレータ手先位置姿
勢誤差Δｒを求める。

【００３１】そして、この手先位置姿勢誤差Δｒがある
許容値Δｒe より小さいか否かを判断し（ステップＡ4
）、誤差Δｒが許容値Δｒe より大きければ、（４）
式により関節補正量Δθを算出し（ステップＡ5 ）、次
いで（６）式によりΔθ補正を求めて関節リンク長ゲイ
ンＫｅによる重み付けを行なう（ステップＡ6 ）。更
に、（１０）式に示すように補正量Δθ補正を関節セン
サの読み値θs にオフセットして加え（ステップＡ7
）、その後ステップＡ1 に戻る。

【００３２】以下、同様の動作を繰り返し、ステップＡ
4 において、誤差Δｒがある許容値Δｒe より小さくな
ったと判断されると処理を終了する。以上のようにして
手先誤差Δｒが許容誤差Δｒe より小さくなるように前
述の計算を繰り返して行く。これにより位置姿勢Ｔm と
＾Ｔm の誤差がなくなる。

【００３３】すなわち、任意の計測器により測定された
マニピュレータ手先（もしくは位置制御点）の位置姿勢
とマニピュレータ各関節に設置された関節角度検出器５
と各関節間のリンク長さにより算出されたマニピュレー
タ手先９の位置姿勢との差、すなわち補正すべき量をマ
ニピュレータ手先９の位置姿勢と関節角度の関係を示す
ヤコビ行列を用いて関節角補正量に帰着させ、更にその
補正量に重み付けを行なうべく、各関節からマニピュレ
ータ手先９までのリンク長により予め設定される関節リ
ンク長ゲインを乗じマニピュレータ各関節角補正量を導
出して補正を行ない、これを繰り返し行ないマニピュレ
ータ関節のオフセット量を固定するようにしたもので、
これによりマニピュレータ１の基準座標に対する手先位
置決め精度（絶対精度）を向上する。

【００３４】

【発明の効果】以上詳記したように本発明によれば、マ
ニピュレータ手先に生ずるメカニカルな避けられない誤
差をマニピュレータ各関節角の誤差に変換し、その関節
角の誤差を重み付けすることにより、マニピュレータ手
先誤差に及ぼす影響の大きい関節が効果的に補正される
ので、マニピュレータ手先の絶対精度を向上することが
できる。

【図面の簡単な説明】

【図１】本発明に係るマニピュレータの関節角度制御方
法の基本原理図。

【図２】本発明に係る関節ゲイン長決定の説明図。

【図３】本発明に係る関節ゲイン長決定の説明図。

【図４】関節角誤差と手先誤差の関係を示す図。

【図５】本発明の一実施例に係るマニピュレータの関節
角度制御方法の構成図。

【図６】同実施例におけるマニピュレータ位置制御ブロ
ック図。

【図７】同実施例における処理動作を示すフローチャー
ト。

【符号の説明】

１ ６軸マニピュレータ ２ 位置計測器 ３ 視覚センサコントローラ ４ 制御アンプ ５ 関節角度検出器 ６ 制御計算機 ７ 基準座標 ８ マニピュレータ座標 ９ マニピュレータ手先 ２０ ヤコビ行列 ２１ マニピュレータ現在位置姿勢

Claims (1)

【特許請求の範囲】
1. 【請求項１】 任意の計測器により測定されたマニピュ
   レータ手先もしくは位置制御点の位置姿勢とマニピュレ
   ータ各関節に設置された角度センサと各関節間のリンク
   長さにより算出されたマニピュレータ手先の位置姿勢と
   の差をマニピュレータ手先の位置姿勢と関節角度の関係
   を示すヤコビ行列を用いて関節角補正量に帰着させ、更
   にその補正量に重み付けを行なうべく、各関節からマニ
   ピュレータ手先までのリンク長により予め設定される関
   節リンク長ゲインを乗じマニピュレータ各関節角補正量
   を導出して補正を行ない、これを繰り返し行ないマニピ
   ュレータ関節のオフセット量を固定することを特徴とす
   るマニピュレータの関節角度制御方法。