JP2676721B2 - 多関節型ロボットの制御装置 - Google Patents

Description

【発明の詳細な説明】 （発明の技術分野） 本発明は、多関節型ロボットの手先位置及び姿勢の制
御装置に関する。 （従来技術と問題点） 従来において、例えば、多関節型ロボットは第１図に
示すように複数個のアーム10,11,12,……と、各アーム
を連結する屈曲関節21,22,……と、アームに組み込まれ
た回転関節31,32,……と、アーム先端に取り付けられた
手先40からなる。この他にアームにスライド機構を組み
込んでアームの長さを可変にしたものもある。作業内容
はある位置で部品を把持し、それを他の位置へ運んでそ
こへ置く若しくは取り付ける等であるが、かかる運動に
必要な自由度は手先位置決定用に３軸、手先の姿勢制御
用に３軸、合わせて６軸である。第１図の例では屈曲関
節が21,22,23の３つ、回転関節が31,32,33の３つあるの
で６関節型であり、上記作業が可能である。 各関節にある屈曲または回転角を与えれば、それに応
じて手先の位置および姿勢が定まる。各関節の屈曲、回
転角を知って手先の位置、姿勢を求めるという方法がロ
ボットの１制御法であり、これとは逆に、先ず手先の位
置、姿勢を定め、かかる位置、姿勢にするのに必要な各
関節の屈曲、回転角を求めるという方法が、ロボットの
他の制御法である。前者は計算が比較的容易であり、各
アームの長さ、各関節の屈曲回転角、座標変換行列など
を用いて手先の位置、姿勢を求めることができる。以下
に、手先の位置、姿勢を求める方法について説明する。 第１図において、O₀−x₀y₀x₀はロボットに対して定義
された絶対座標系、O_i−x_iy_iz_i（１≦ｉ≦6;以下同様と
する）は各関節およびハンドに定義された座標系であ
る。θ₁,θ₂,θ₃,θ₄,θ₅,θ_６は、アームをX₀軸方向に
ねじれなく真直ぐに伸ばしたときを初期状態（θｉ＝
０）とする、つまり屈曲関節θ_２、θ_３、θ_５の各回転
中心軸が平行に整列した状態の時、各回転関節θ_１、θ
_４、θ_６が回転していてもそれらの回転角度を０゜とし
て扱い、同時に各屈曲関節の回転角度を０゜とする、各
関節の回転角度を表し、L1,L2,L3＋L4,L5＋L6は、それ
ぞれ腰の高さを示すアーム10の長さ（回転関節31の上下
両側のアームを含む長さ）、上腕のアーム11の長さ、前
腕のアーム12の長さ、手首からハンドの根本までのアー
ム13の長さを表す。 手先の位置・姿勢のベクトルを （ここで、「α，β，γ角」＝「オイラー角」であり、
^Ｔは転置ベクトルを意味する。なお「オイラー角」の定
義は“合田周平、木下源一郎共著「ロボット工学」（コ
ロナ社，昭和52年８月10日発行）第159頁”の記載に従
い、絶対座標系であるO₀−x₀y₀z₀に対して姿勢ベクトル
がなす角をオイラー角としている）と表すと、関節角ベ
クトルΘ＝（θ₁,θ₂,θ₃,θ₄,θ₅,θ_６）^Ｔが与えられ
た時、 の値は次のようにして求められる。第２図にオイラー角
α，β，γを示す。O_i-1−x_i-1y_i-1z_i-1からO_i−x_iy_iz_i
への座標変換行列を とすると、O_i-1−x_i-1y_i-1z_i-1でのある点の座標値を O_i−x_iy_iz_iでのそれを としたとき、 の関係が成り立つ。ここで は、次のような４×４行列である。 但し、Riは座標系の回転を表す３×３行列、 は座標系の平行移動を表す３次元ベクトル、 は３次元零ベクトルである。は次のようになる。 従って、O₀−x₀y₀z₀からO₆−x₆y₆z₆へのトータルな座
標変換行列 は、 と表されるから、絶対座標系O₀−x₀y₀z₀における手先の
位置 は、ハンドに定義された座標系O₆−x₆y₆z₆におけるその
位置を とすると、次のように計算される。 また、第２図に示すようにx₀軸、y₀軸、z₀軸に対する
x₆軸、y₆軸、z₆軸の方向余弦を順にH1,H2,H3とし、O₆−
x₆y₆z₆の原点の座標値を とすると、前述した座標変換行列 とは、次の関係が成り立つ。 従って、絶対座標系O₀−x₀y₀z₀における手先の姿勢 を、ハンドに定義された座標系O₅−x₅y₆z₆の姿勢と同じ
であると定義することにより、α，β，γはオイラー角
の定義より、 と計算される。 以上のように、関節角ベクトルΘが与えられると、手
先の位置・姿勢ベクトル は一意に求めることができる。 しかしながら、ロボットの使用上からは後者の方法が
重要視される。 すなわち、ロボットにおいては、その動作指定は教示
データで行っている。 つまり、ティーチングボックス等を用い、またはロボ
ットの先端を持ってロボットを移動させ、各点の座標に
おける各アームの関節角を記憶させた後、この記憶させ
た教示データに基づいて、ロボットを動作させている。 しかしながら、このようなロボットのティーチング動
作は、時間がかかると共に、その操作は、大変煩わしい
という欠点がある。このため、近年、ロボットの動作指
定は、ロボット言語あるいは、CAD（Computer Aided De
sign）出力に応じて行おうとしている。 つまり、この出力は具体的な移動量だけの指令値であ
り、ロボットは、この指令値に応じた動作を行えること
が要求されている。しかしながら、この方法は、与えら
れた手先の位置、姿勢から各関節の回転角を求めなけれ
ばならず、計算が厄介である。 以下に、従来の手先の位置、姿勢のベクトルが与えら
れたときの各関節の回転角を行列を用いて求める方法に
ついて説明する。 前述したように、本発明ではオイラー角は絶対座標系
O₀−x₀y₀z₀で定義されるから、第１図に示すように、手
先の位置、姿勢ベクトル が与えられたとき、ハンドに定義された座標系の原点 は、次のように求められる。 オイラー角（α，β，γ）の定義より、絶対座標系O₀
−x₀y₀z₀におけるz₀軸、y₀軸、x₀軸回りの回転の変換行
列は、順に である。 このとき、ハンドに定義された座標系O₆−x₆y₆z₆のX₆
軸、Y₆軸、Z₆軸に対する方向余弦 となる。ここで、座標変換行列 とは、次の関係が成り立つ。 以上により、 が得られる。 上記得られた▲ｘ⁰ ₆▼，▲ｙ⁰ ₆▼，▲ｚ⁰ ₆▼に関する
各式を用いて、各関節の関節角を求める。まず、 より第５関節に定義された座標系の原点 を求める。 これよりθ_１は、 次に、 の第４列ベクトルは等値であるから、 これを変形して、次のようにする。 次に、^２＋^２の演算を行い、 を得る。 このに，を代入して を得る。 左辺に正弦・余弦の合成公式を適用して （ここで、φ＝tan^-1（Q/R）） ゆえに、θ_２は、 また、，より、θ_３は、 より得られる。 次に、X₆軸に対するの第１列ベクトルは等値であるから、 次に、 cosθ_１×＋sinθ_１×の演算を行い、 cos（α−θ_１）・cosβ＝ cos（θ_２＋θ_３）・cosθ_５ −sin（θ_２＋θ_３）・cosθ_４・sinθ_５ …… sin（θ_２＋θ_３）×＋cos（θ_２＋θ_３）× sinβ・sin（θ_２＋θ_３） ＋cos（α−θ_１）cosβ・cos（θ_２＋θ_３） ＝cosθ_５ … これより、θ_５は、 θ_５＝cos^-1〔sinβ・sin（θ_２＋θ_３） ＋cos（α−θ_１）・cosβ・cos（θ_２＋θ_３）〕 次に、−sinθ_１×＋cosθ_１×の演算を行い、 sin（α−θ_１）・cosβ＝sinθ_４・sinθ_５をに代入して sinβ・cos（θ_２＋θ_３） −cos（α−θ_１）・cosβ・sin（θ_２＋θ_３） ＝cosθ_４・sinθ_５ これより、θ_４は、 次に、 の第３行ベクトルの関係より、 次に、cosθ_５×−sinθ_６×の演算を行い、 cosβ・sin（θ_６−γ）＝ −cos（θ_２＋θ_３）・sinθ_４ 次に、sinθ_６×−cosθ_６×の演算を行い、 cosβ・cos（θ_６−γ）＝ cos（θ_２＋θ_３）・cosθ_４・cosθ_５ −sin（θ_２＋θ_３）・sinθ_５ これより、θ_６は、 上述したように、θ_１〜θ_６の計算は非常に厄介であ
り、ハードウェア化が困難であると共に、ソフトウェア
による処理時間が長大化するという欠点がある。また、
ハードウェア化したとしても専用化してしまい、他の関
節構成のロボットを作った時にこのハードウェアを利用
できないという欠点がある。 また関節型ロボットは６関節あれば手先の位置、姿勢
を所望通りにすることができるが、周囲に障害物があり
それは回避しながら任意の姿勢をとる、あるいは関節型
ロボットが２つあり、２本腕で強調動作をするなどの場
合には６関節では困難がある。これに対しては自由度を
更に増して、即ち冗長自由度を持たせた７関節型が有効
である。 （発明の目的） 本発明の目的は手先の位置、姿勢を所望通りにするた
めの各関節の屈曲、回転角を、ハードウェア化が容易で
計算機による演算時間が長大化することなく求めること
ができ、高速動作が可能で精度を向上し得る多関節ロボ
ットの制御方法を提供することにある。 （発明の構成） 本発明は複数個のアームを関節により連結して構成し
た多関節型ロボットの制御装置において、上位装置より
指示される前記複数個のアームのうちの手先の位置ベク
トル及び姿勢ベクトルに基づいて各アームの方向単位ベ
クトル及び各アームをそれぞれ含む複数の面の各法線単
位ベクトルを求めるベクトル算出手段と、前記方向単位
ベクトル及び前記法線単位ベクトルに基づいて各関節の
回転角を求める回転角算出手段と、前記求められた回転
角に応じて各関節を駆動する駆動手段とを具備してなる
ことを特徴とする。 （発明の実施例） 以下、本発明の多関節型ロボットの制御装置がその基
礎としている、多関節ロボットの制御方法の実施例を図
面を用いて詳細に説明する。 第３図は第１図に示す６関節型ロボットのベクトルの
定義を示す図である。第３図を用いてベクトルを用いた
関節角の算出方法について説明する。 （１）面ベクトルの導入による関節の拘束規約 第３図に示すように、各回転関節毎に対象とする面が
切り替わるので、回転関節の切れ目毎に、π₁,π₂,π₃,
π面のように複数の面πｉを設定し、各ベクトルを次の
ように定義する。 L₁,L₂,L₃＋L₄,L₅＋L₆:順に、腰の高さ、上腕の長さ、前
腕の長さ、手首からハンド根本までの長さ なお第３図において、 のベクトルはx₆軸からずれた位置にあるが、これはロボ
ットのハンドにエアーガンやエアードライバーを取り付
けた場合、その先端位置がO₆−x₆y₆z₆の座標系から だけ平行移動した位置にあるからである。 （ａ）屈曲関節の拘束規約 第４図（ａ）に示すように屈曲関節は面を共有してい
るから、 と直交しているから、 また、関節角θｋは次式で与えられる。 （ｂ）回転関節の拘束規約 第４図（ｂ）に示すように回転関節は軸を共有してい
るから、 と直交しているから、 また、関節角θｋは次式で与えられる。 したがって、第３図に示す構成の６関節ロボットの場
合、 が求まれば、関節角ベクトルΘ＝（θ₁,θ₂,θ₃,θ₄,θ
₅,θ_６）^Ｔを決定することができる。 （２）面ベクトル、方向ベクトルの決定条件 （ａ）面ベクトルと方向ベクトルの直交関係 上段に面ベクトル、下段に方向ベクトルを配置し、直
交しているベクトルを線で結んで表示した図を直交ダイ
ヤグラムと呼ぶ。第３図に示す構成の多完成ロボットの
場合、（１）で示した関節の拘束規約（，）を用い
て直交関係を摘出すると第５図に示す直交ダイヤグラム
となる。 （ｂ）位置ベクトル と方向ベクトルの関係 （ｃ）各アームの長さの規約 アームは回転関節、屈曲関節で連結して構成されてい
るから、各アームの長さは不変である。従って、 以上の３条件を用いて、面ベクトル、方向ベクトルを
決定する。 （３）関節角の導出（面ベクトル、方向ベクトルの決
定） （ａ）根本ベクトル は定義によって決められているベクトルであり、値は不
変である。 ここで、位置ベクトル は、第１図を用いて説明した、手先の位置、姿勢ベクト
ル が与えられたときのハンドに定義された座標系の原点 であり、 ハンドに定義された座標系における手先の位置 が与えられると、次式で求められる。 また、手先の姿勢ベクトル をハンドに定義された座標系の姿勢ベクトルと同じであ
ると定義しているので、 はオイラー角の定義から、 である。結果として は第３図のx₆と同一の方向を持つ単位ベクトルとなり、 はy₆と同一の方向を持つ単位ベクトルとなる。 （ｃ）面ベクトル の決定 第３図に示すベクトル を導入する。 はπ_２面上に存在し、π_２面の法線ベクトルが である。そのためロボットの関節がどのように屈曲回転
しても、 と直交（直交ダイヤグラムは第５図に示す）している。
このベクトル は、次式で表される既知ベクトルである。 従って、 は次式で決定される。 （ｄ）方向ベクトル の決定 前述した はπ_２面上にあるから、次のように表せる。 ここで、π_２面上に新たに、互いに直交するベクトル を定義する。 このとき、 を用いて表せる。 ，を，に代入して、 との内積を求めると Ａ＝L₂a₁＋（L₃＋L₄）・b₁ …… ０＝L₂a₂＋（L₃＋L₄）・b₂ …… また、 であるから ，，より の係数を，に代入して、 が得られる。 （ｅ） の決定 以上の を用いて関節角Θは決定される。 次に、第６図を用いて７関節型ロボットの場合につい
て説明する。第３図と比較すれば明らかなようにアーム
11にも回転関節を加えて屈曲関節３、回転関節４の７関
節としている。このロボットは人間の腕によく似た動作
を行うことができる。 図において、21,22,23は屈曲関節、31,32,33,34は回
転関節、 は夫々アーム10〜13の方向単位ベクトル、L₁,L₂＋L₇,L₃
＋L₄,L₅＋L₆は夫々アーム10〜13の長さ、θ_１〜θ_７は
各関節の屈曲、回転（以下回転と称する）角、 はアームの手先40の目標位置、 は手先40の位置及び姿勢ベクトルである。 また、各回転関節毎に対象とする面が切り替わる（本
実施例の場合、屈曲関節は面が切り替わらないと定義し
て説明しているが、後で第７図（ａ）と共に説明するよ
うに面の定義の仕方により、屈曲関節毎に面が切り替わ
る）ので、回転関節の切れ目毎に、π₁,π₂,π₃,π₄,π
_５面とし、これらの法線ベクトルを とする。また、アーム11と12は腕と肘に相当するが、こ
れらのアーム及び屈曲関節21と23を結ぶ線分（このベク
トルをとする。 ベクトル は、不変ベクトルともいう）が構成する三角形の、 を軸とする傾斜角をη（η＝０は関節22を上にして該三
角形がＺ軸と整列した状態とする）とする。つまり第11
図（ａ）に示すように、ベクトル を軸としてθ_３を右ネジの進む方向へ回転させたときの
回転角がηであり、このときの回転方向が回転角の増加
方向となる。第６図に示される構成のロボットでは、適
当な回転関節と屈曲関節の間のアームの長さを０にする
ことにより、ベクトル を軸にして関節22を回転させることができる。例えば第
６図において、回転関節θ_１と屈曲関節θ_２の間のアー
ムの長さを０として２自由度運動機構とすればよく、同
様にθ_７とθ_３の間あるいはθ_４とθ_５の間の各アーム
の長さを０にすれば、θ_３は第11図（ｂ）に示すように
ベクトル を軸にして回転することができる。 このロボットではアーム10の始端は直交座標軸の原点
０に固定され、アーム10は上向き（Ｚ軸方向）に固定さ
れている。従って方向単位ベクトル 面π_１の法線単位ベクトル であり、アーム10をベクトル表示すると となる。 ここで との関係を第７図で説明するに、第７図（ａ）は屈曲関
節24,25が90゜方向を変えてアーム14〜16に取り付けら
れた例を示し、アーム14と15はπ_６面に、アーム15と16
はπ_７面にあるとしている。屈曲関節ではアーム例えば
14,15のなす角に拘わらず、該アーム14,15は同じ平面上
にあるから面π_６とπ_７は直交しており、従って面π₆,
π_７の法線単位ベクトル の内積は０即ち、 である。また方向単位ベクトル は面π_６とπ_７を共有しているから の関係もある。ここで などの添字₅,₆,……はi,j,……（これらは₁,₂,₃,……の
１つ）と一般化できるものであり、この点は以下でも同
様である。関節24における屈折角θは で与えられる。第７図（ｂ）は回転関節35と屈曲関節24
を用いた例で、アーム14はπ_６面にあり、アーム15と16
はπ_７面にあるとしている。この場合の方向単位ベクト
ルは に直交しているから である。関節35の回転角θは である。 第７図（ｃ），（ｄ）はスライド関節を示し、（ｃ）
は平行スライド、（ｄ）は一般スライドである。45はス
ライドを示す。平行スライドでは であり、アームの長さはスライド量だけ伸縮する。一般
スライドでは などは構造により定まる値をとる。 なお一般的には、面ベクトルや方向ベクトルからスラ
イド関節のスライド量を演算することはできない。しか
し、スライド関節のスライド量が、他の関節角θ₁,θ₂,
θ_３とは独立に定められる条件、即ち，面ベクトルや方
向ベクトルを用いることなくスライド関節のスライド量
を定められるような条件がある場合、例えば、スカラー
型（水平多関節型）ロボットで水平方向にはスライド関
節がなく先端部分にのみ鉛直方向のスライド関節を持っ
ている場合などには、本発明をスライド関節を有するロ
ボットに適用することが可能にある。 方向単位ベクトル（方向ベクトルと略） と法線単位ベクトル（面ベクトルと略す） とは直交し、多関節型ロボットではこれらが継続して行
くことになる。この関係は第８図に示すように上段に面
ベクトル、下段に方向ベクトルを配置し、直交関係にあ
るベクトルを直線で結んで表すと、全体の関係が一目で
分かるようになる。第８図（ａ）は第６図のロボットを
例にとったものである。 に直交し、 に直交し……という関係が一目瞭然である。第８図
（ｂ）は同図（ｃ）のロボットの 関係図である。このロボットでは回転関節のないアーム
があり、また90゜向きを変えた屈曲関節25があるので 関係は複雑であるが、第８図（ｂ）の関係図を画いてみ
ると、 は同一平面上にあり、また、 は同一平面上にあり、 は正規直交系を形成する。等が直ちに分かる。この第８
図の直交ダイヤグラムはロボットのアーム方向及び全関
節間の規約を全て表している。 再び第６図に戻ると、アーム10のベクトルは であり、同様な方法でアーム11〜13を表すと、ハンドに
定義された座標系の原点を表すベクトルは である。これはベクトル ηにほかならず（なお手先ベクトル はハンドに取り付けられた塗装用エアーガンやネジ締め
ドライバーなどの先端を示す）、ハンドに定義された座
標系を表す方向ベクトルは ハンドに定義された座標系を表す面ベクトルは であり、これらより各部の方向ベクトル を求めることができる。式で示すと次の如くなる。 ハンドに定義された座標系、姿勢、傾き角 結果として、 は第３図のx₆と同一の方向を持つ単位ベクトルとなり、 はy₆と同一の方向を持つ単位ベクトルとなる。 （ただしZ₁は＋１または−１をとる符号であり、詳細は
後述される。） を軸にして関節22が回転しπ_３面が回転したときのπ_３
面の回転につれて回転したπ_３面の法線ベクトル を与える。 ところで第６図の７関節ロボットは１冗長関節を含ん
でおり、このままでは解析的に解くことはできない。そ
こでここではηを外部から与えた。そしてθ_７＝０のと
きη＝０とし、このときの とした。従ってηは とのなす角即ちcosηとなる。 と直交するので またベクトル の に垂直な は同一平面に存在するので、これより上述の が得られる。 ここで の演算中に導入されている符号Z₁はロボットを円滑に動
かすために有効なもので後で説明する の演算中にもそれぞれZ₂,Z₃として導入されている。こ
れらの符号Ｚ（Z₁,Z₂,Z₃）はたとえば第７図（ａ）の24
の如き屈曲関節なら、この関節24の屈曲角をθとして θ＝Z|θ｜ により求まり、また第７図（ｂ）の35の如き回転関節な
らばこの関節の回転角をθとして θ＝Z|θ｜ により求まる。ここでSingは、 との内積の符号のみをとることを示す。 と同方向または逆方向であり、従ってその内積には正、
負があり、この正、負はθの正、負を示している。 次に の演算におけるZ₁の意味を説明する。前述したように、 であり、これについて第12図（ａ），（ｂ），（ｃ）に
示すθ_２＞０、θ_２＝０、θ_２＜０の３つの場合を考え
る。なお説明を簡単にするために、第６図においてθ_１
＝０゜、θ_７＝０゜、θ_４＝０゜とする。これにより第
12図（ａ）の場合 の方向はY₀軸の方向と同一になりり、第12図（ｂ）の場
合 となる。また第12図（ｃ）の場合 は−Y₀軸の方向と同一になる。 ここで、ロボットが第12図（ａ）の状態から第12図
（ｃ）の状態へ連続的に動いた場合を想定すると、 の方向はY₀軸の方向を向いていたものが急に−Y₀軸方向
へ向いてしまう。ところでθ_７＝０゜のとき であり、符号Z₁を考える前の であるから、符号Z₁を考慮しない 用いてθ_７を求めると、 であり、第12図（ａ）の場合θ_１＝０゜なので、 となり、 θ_７＝cos^-1（１）＝０゜ となる。次に第12図（ｃ）の場合を考える。θ_０＝０
゜， なので、 になり、 θ_７＝cos^-1（１）＝180゜ となってしまい、最初の仮定であるθ_７＝０゜を満足す
ることができず、矛盾を起こす。これは制御を不安定に
する要因となり得る。 そこでこのようなことにならないように、第12図
（ａ）の始点の状態の符号を保存しておいて、ロボット
が連続動作しているときの の符号が変化した時点で符号を元に戻している。符号Z₁
はこのために用いられ、第12図（ａ）の状態でZ₁＝１と
しておき、 とする。そして の符号が変化した時点でＺ＝−１とし、 とすることにより、上述した矛盾が起こらないようにす
る。また第12図（ｂ）の状態は一般に特異点といわれる
状態であり、θηの解が不定となる。しかしこの場合は を予め一意に決定することにより回避することができ
る。 の演算 a₁＝｛A²＋（L₂＋L₇）^２−（L₃＋L₄）^２｝/2A（L₂＋L
₇） b₂＝−a₂・L₂/L₃ （ただしZ₂,Z₃は＋１または−１をとる符号であり、詳
細は後述される。） ここでZ₂の意味を説明する。説明を簡単にするため、
第６図においてθ_１＝０゜、θ_７＝０゜と仮定し、θ_２
については、第13図（ａ），（ｂ），（ｃ）に示すよう
にθ_２＞0,θ_２＝0,θ_２＜０の３つの場合を考える。第
13図（ａ）の場合、 の方向はY₀の方向（0,1,0）と同一となり、第13図
（ｂ）の場合は、 そして第13図（ｃ）の場合 の方向は−Y₀軸の方向（0,−1,0）と同一になる。θ_１
は のなす角であり、 であるから、ここでロボットが第13図（ａ）の状態から
第13図（ｃ）の状態へ連続に動いた場合、 の方向はＹ軸方向から−Ｙ軸方向に変わり、θ_１は０゜
から180゜に急に変化して、θ_１＝０゜とした最初の仮
定との矛盾が起こる。Z₂はこの の符号が変わった時点で符号を元に戻し、矛盾が起こら
ないようにする。 次にZ₃の意味を説明する。この場合も説明を簡単にす
るためθ_４＝０゜と仮定し、θ_５について、第14図
（ａ），（ｂ），（ｃ）の３つの場合を考える。第14図
（ａ）では はY₀軸方向を向き、第14図（ｂ）では 第14図（ｃ）では−Y₀軸方向を向く。Z₁,Z₂の場合と同
様にロボットが第14図（ａ）の状態から第14図（ｃ）の
状態へ動い場合、 の向きが変わり、θ_４＝０゜とした仮定との矛盾が生じ
るのでこれを回避するためにZ₃が用いられている。 Z₁,Z₂,Z₃の意味については上述した通りであり、今用
いる所の符号Ｚを記憶しておいて次に進むべき所の符号
Ｚと比較し、符号が変化した場合のみＺの符号を元の符
号に強制的に戻すものであるが、このようなＺを用いて
符号を強制的に元に戻すことをしなかった場合にどうな
るかというと、一般のロボットでは、ロボットの操作者
によって与えられた目標の位置・姿勢に到達すべく，現
在いる位置・姿勢と目標位置・姿勢の間を50msecから10
0msec位（この時間間隔は、ロボットの制御計算機の能
力による）のサンプリング間隔で補間しながら移動する
が、符号が反転したことによって50msecから100msec位
の間に180゜近い関節の回転が要求されることになり、
現在あるモータの性能ではとても回転しきれない値とな
ってしまう。つまり高速制御性能に悪影響が生じる。 θｉの演算 上記演算状況をみれば明らかなように、殆どが単純な
計算で済みsin,cosが入り組んだ複雑な式ではない。従
って解析は容易で、短時間で済む。 例えば、 のような外積演算、 のような内積演算と逆三角関数の組み合わせ演算がほと
んどであり、この部分をハードウェア化することによ
り、高速演算が可能である。また、この演算は、６関節
でも７関節ロボットでも共通した部分でこの部分をハー
ドウェア化してもこのハードウェアを汎用的に使うこと
が出来る。ロボットは最終位置、姿勢の指定だけでな
く、中間点も幾つか指定し、該中間的の間は補間すると
いう方法で所望の軌跡に沿った運動をさせるので、計算
量は膨大である。従って迅速に計算できる関係式でない
とロボットの動きは遅く、鈍くならざるを得ないが、こ
れを改善することができる。また計算が迅速に済めば中
間点を多数指定でき、予定の軌跡に沿った正確なロボッ
ト制御が可能になる。 第９図は、本発明にかかる多関節型ロボットの制御装
置の実施例を説明するブロック図であり、第10図は、６
関節型ロボットの場合の各関節角を求めるブロック図で
ある。 図において、50は上位装置からの指令値に基づいて、
各関節の方向単位ベクトル、法線単位ベクトルを求める
ベクトル算出手段、60はベクトル算出手段により求めら
れた各ベクトルに基づいて、各関節の関節角度θｉを求
める回転角算出手段、70はロボットアーム80に回転角算
出手段60により求めた関節角度をとらせるための駆動手
段である。 （発明の効果） 以上説明したように本発明では手先の目標位置及び姿
勢ベクトルから各アームの方向ベクトルおよび面ベクト
ルを求め、これらより各関節の回転角を求めるようにし
たので、計算を容易、迅速になし得、ひいてはロボット
の動作を正確、迅速にすることができる。尚、本発明は
上記実施例に示す構成のロボットに限られるものではな
く、各種適用できることは云うまでもない。

【図面の簡単な説明】 第１図は６関節型ロボットの概略説明図、第２図はオイ
ラー角を説明するための図、第３図は第１図に示す関節
型ロボットのベクトルの定義を示す図、第４図、第７図
は方向ベクトルと面ベクトルの説明図、第５図、第８図
は直交ダイヤグラムの説明図、第６図は７関節型ロボッ
トのベクトルの定義を示す図、第９図、第10図、第11図
は、本発明の実施例を説明するための第12図，第13図，
第14図は符号Ｚの説明図である。 図において、10〜13はアーム、21〜23は屈曲関節、31〜
34は回転関節、 θ_１〜θ_７は各関節の回転角、40は手先、50はベクトル
算出手段、60は回転角算出手段、70は駆動手段、80はロ
ボットアームである。

───────────────────────────────────────────────────── フロントページの続き (72)発明者 荒木 誠 川崎市中原区上小田中1015番地 富士通 株式会社内 (56)参考文献 特開 昭52−25365（ＪＰ，Ａ) 特開 昭57−3102（ＪＰ，Ａ) 特開 昭57−33989（ＪＰ，Ａ) 「機械の研究」第25巻 第７号 （1973）第76〜80頁

Claims (1)

1. (57)【特許請求の範囲】 １．複数個のアーム（10〜13,40）を関節（21〜23,31〜
   34）により連結して構成した多関節型ロボットの制御装
   置において、 上位装置より指示される前記複数個のアーム（10〜13,4
   0）のうちの手先（40）の位置ベクトル 及び姿勢ベクトル 根本ベクトル 及びこれらのベクトルより求めた外側屈曲関節間の不変
   ベクトル に基づいて各アームの方向単位ベクトル を求め、該不変ベクトル 及び該方向単位ベクトル に基づいて回転関節で区切られた各アームを夫々含む複
   数の面（π_１〜π_５）の各法線単位ベクトルを求めるベクトル算出手段（50）と、 前記方向単位ベクトル に基づいて各屈曲関節の回転角（θ₂,θ₃,θ_５）を求
   め、前記法線単位ベクトル に基づいて各回転関節の回転角（θ₁,θ₄,θ₆,θ_７）を
   求める回転角算出手段（60）と、 前記求められた回転角（θ_１〜θ_７）に応じて各関節を
   駆動する駆動手段とを具備してなる多関節型ロボットの
   制御装置。