JPH0550386A - ７自由度マニプレータの位置教示方式 - Google Patents

Abstract

(57)【要約】 【目的】 ７自由度マニプレータに対して位置・姿勢を
簡単に教示できるようにすること。 【構成】 マニプレータは７つの関節を有する。肩部１
１に各関節の回転軸が１点で交差する３つの関節Ｊ１，
Ｊ２，Ｊ３を有する。肩部１１に接続された上腕部１２
と前腕部１４を結合する肘部１３に１つの関節Ｊ４を有
する。手首部に回転軸が１点で交差する３つの関節Ｊ
５，Ｊ６，Ｊ７を有する。教示操作盤３よりハンドを直
交座標系の各軸に平行に移動させる直線移動指令もしく
は各軸に対して回転指令が入力されると肘部１３の（Ｏ
ｓ−Ｏｗ）回りの回転角度を変えずにハンドの位置及び
姿勢を変える。肘回転指令が入力されると、ハンドの位
置及び姿勢を変えずに肘部の回転角のみを変える（セル
フ・モーション）。これらの指令によりマニプレータに
位置・姿勢を教示する。

Description

【発明の詳細な説明】

【０００１】

【産業上の利用分野】本発明は、関節形マニプレータの
位置教示方式に関し、特に、人間の腕と同等の動きを行
なう７自由度のマニプレータの位置教示方式に関する。

【０００２】

【従来の技術】産業用のマニプレータまたはロボットの
アームの先端に取り付けられたハンド（エンドエフェク
タ）を、このマニプレータ（ロボット）の動作範囲内で
空間の任意の位置に任意の方向で位置決めするには、マ
ニプレータ（ロボット）のアーム機構が６自由度を有す
ることが必要十分条件である。そのため、既存のマニプ
レータ及び産業用ロボットは６自由度もしくは、動作を
制限した６自由度未満の機構で構成されている。しか
し、６自由度のマニプレータやロボットでは、ハンドを
空間内である位置と姿勢に位置決めする場合、アーム機
構の姿勢は有限個の姿勢に限られ、障害物と干渉を起こ
す場合がある。

【０００３】そこで、上記６自由度のマニプレータやロ
ボットの不具合を解消するために、７自由度を有したマ
ニプレータやロボットが開発されている（特公昭５５−
４８９５６号公報参照）。特に人間の腕の動きと近似し
た動作を行なうティーチング−プレイバック式の７自由
度のマニプレータまたはロボットが本願出願人によって
開発されている（特願平３−８４１６７号参照）。この
７自由度マニプレータまたはロボットは人間の肩に対応
する部分に３つの関節を有し、手首に対応する部分に３
つの関節を設け、肩部と手首部間を肘に対応する関節を
介して結合し、人間の腕と同等の動きを行なう７自由度
のマニプレータまたはロボットを構成している。このよ
うな７自由度を有するマニプレータ若しくはロボットで
は位置，姿勢を決めるために必要な６自由度に加えて１
つの冗長度を有し、この１つの冗長度により複雑な動作
や器用な動きを実現できる。

【０００４】例えば、人間の腕で説明すると、人間の腕
も７自由度を有し、手の位置，姿勢を決定するために必
要な６自由度に加え１つの冗長自由度を有している。こ
の冗長度があるために人間の腕では前方にある物体を把
持し、手の位置，姿勢を固定したまま肘を動かすことが
可能である。この肘の動きを積極的に利用し障害物を回
避しつつ物体を把持するような動作を行なうことができ
る。このように人間の腕と同じように冗長自由度を持つ
マニプレータまたはロボットは、ハンドの位置及び姿勢
を変化させないアームの動きが可能である。このアーム
の動きは“セルフ・モーション”と呼ばれている。上述
した人間の腕の例では、手の位置と姿勢を動かさないで
肘の位置を動かす動きがセルフ・モーションである。

【０００５】一方、マニプレータやロボットの動作をプ
ログラムする方法として、テーチング・プレイバック方
式が一般的に採用され、馴れ親しまれている。テーチン
グ・プレイバック方式のマニプレータまたはロボットで
は、操作者がマニプレータやロボットのハンドを所定の
位置及び姿勢に位置決めするために、通常、教示操作盤
上を操作してハンドの位置及び姿勢を動かす下記（１）
及び（２）の機能が用意されている。そして、操作者は
これらの機能を用いてマニプレータ若しくはロボットの
ハンドを所定の位置及び姿勢に位置決めすることにより
教示を行なうようになっている。 （１）各軸動作 マニプレータまたはロボットの各関節を個別に動かし
て、ハンドの位置及び姿勢を所定の位置に位置決めする
方法。教示操作盤に各関節の動作に対応するキーが設け
られており、特定のキーを押すと対応する関節の軸が動
作する。 （２）直交座標系動作 空間内の特定の直交座標系に対して、この座標系のＸ
軸，Ｙ軸，Ｚ軸の各軸に平行にハンドを移動させる機能
と、Ｘ軸，Ｙ軸，Ｚ軸の各座標軸の回りにハンドを回転
させる機能を持ち、平行移動によりハンドの位置を、回
転移動によりハンドの姿勢を変化させて所定の位置及び
姿勢に位置決めする方法。

【０００６】教示操作盤に座標軸に沿った平行移動また
は軸回りの回転移動を指令するキーが各軸毎に対応させ
て設けられており、所定のキーを押すことによって対応
する平行移動または回転移動を行なう。

【０００７】

【発明が解決しようとする課題】テーチングにおける各
軸動作では、マニプレータ若しくはロボットの１つの関
節を動作させるとハンドの位置及び姿勢の両方が変化す
る。そのため、ハンドを操作者が望む位置及び姿勢に動
かすには複数の関節を同時に動かす必要があるが、ハン
ドの位置及び姿勢の動きと各関節の動きの関係は一般に
複雑であり、操作者が直観的に理解することは困難であ
る。関節が増加する毎に両者の関係は複雑さを増し、特
に、１つの冗長自由度を有する７自由度マニプレータ若
しくはロボットに対して各軸動作で教示することは非常
に困難である。

【０００８】一方、直交座標系動作でティーチングする
場合、ハンドの位置及び姿勢の自由度が６であることに
対して、７自由度のマニプレータ若しくはロボットでは
１自由度の冗長度があるため、ハンドの位置及び姿勢を
指示するだけではマニプレータ若しくはロボットを動作
させることは不可能である。これは、ハンドの位置及び
姿勢を指定しても、まだ上述した“セルフ・モーショ
ン”の自由度が残っているからである。７自由度のマニ
プレータまたはロボツトを直交座標系で動作させるに
は、ハンドの位置及び姿勢に加えてセルフ・モーション
を指定する必要があるが、従来の教示方式では６自由度
以下のマニプレータ若しくはロボットを対象としたもの
であるため、ハンドの位置と姿勢のみしか指定できな
く、７自由度のマニプレータ若しくはロボットに適用で
きない。そこで、本発明の目的は、７自由度のマニプレ
ータ若しくはロボットに対して簡単にハンドの位置，姿
勢及びアームの姿勢を教示できる教示方式を提供するこ
とにある。

【０００９】

【課題を解決するための手段】本発明は、３つの関節を
有し各関節の回転軸が空間上の１点で交差する肩部と、
該肩部に接続された上腕部に接続され前腕部を結合する
１つの関節からなる肘部と、前腕部の他端に接続された
３つの関節を有し該３つの関節の回転軸が１点で交差す
る手首部とで構成される７自由度マニプレータにおける
位置教示方式において、マニプレータのエンドエフェク
タを直交座標系の各軸に平行に移動させる直線移動指令
手段と、各軸の回りに回転させる回転指令手段と、エン
ドエフェクタの位置及び姿勢を変えずに肩部と手首を結
ぶ直線の回りに肘を回転させる肘回転指令手段を設け、
上記直線移動指令手段もしくは回転指令手段が操作され
たときには肩部と手首を結ぶ直線の回りの肘回転角度を
変えずにエンドエフェクタの位置及び姿勢を変え、上記
肘回転指令手段が操作されたときには、エンドエフェク
タの位置及び姿勢を変えずに上記肘部回転角度のみを変
えることによってマニプレータに位置・姿勢を教示する
ことによって７自由度マニプレータに対して、簡単にテ
ィーチングができるようにした。

【００１０】

【作用】直線移動指令手段もしくは回転指令手段が操作
されたときには、肩部と手首を結ぶ直線の回りの肘回転
角度を変えずにエンドエフェクタを任意の位置に移動さ
せると共にエンドエフェクタの姿勢も任意に変更して、
エンドエフェクタの位置・姿勢を教示する。また、肘回
転指令手段が操作されたときには、エンドエフェクタの
位置及び姿勢を固定して変化させずに肘部回転角度のみ
を変えることによって（セルフ・モーション）、障害物
を回避しつつ物体を把持するような動作をマニプレータ
に教示する。

【００１１】

【実施例】図１は本発明の１実施例における７自由度マ
ニプレータの構成図である。図中１は支持体Ｂに支持さ
れたマニプレータのアーム機構部で、２は該マニプレー
タを駆動制御するマニプレータ制御部であり、プロセッ
サ，メモリ，各関節の軸を駆動するサーボモータ等のア
クチュエータ（図示せず）の軸制御回路等を有する従来
から公知のものである。３は教示操作盤でマニプレータ
を動かし、所定の位置及び姿勢に位置決めしてその位
置，姿勢を教示するものである。

【００１２】アーム機構部１は肩部１１、上腕１２、肘
部１３、前腕１４、手首部１５で構成されている。上記
肩部１１は支持部Ｂに直接支持された回転軸ａ１のまわ
りに回転可能な肩部第１関節Ｊ１、第１関節Ｊ１とクラ
ンク形のリンクを介して接続され回転軸ａ１と直交する
回転軸ａ２のまわりに回転可能な肩部第２関節Ｊ２、回
転軸ａ２上のリンク１１ｂによって肩部第２関節に接続
され、回転軸ａ２と直交する回転軸ａ３のまわりに回転
可能な肩部第３関節Ｊ３とを備え、上記３つの肩部関節
の動作軸線ａ１，ａ２，ａ３は肩部１１の点Ｏｓで一点
で交わるように構成されている。上腕１２を構成するリ
ンクの一端は上記肩部第３関節Ｊ３に接続され回転軸ａ
３のまわりに回転可能に取り付けられている。また、上
腕１２の他端は肘部１３が取り付けられ、該肘部１３は
回転軸ａ４のまわりに回転可能な１つの関節Ｊ４で構成
されている。また、肘部１３には回転軸ａ４のまわりに
回転可能に前腕１４が取り付けられている。なお、上腕
１２の軸線と前腕１４の軸線は上記回転軸ａ４上の１点
Ｏｅで交差している。

【００１３】手首部１５は第１，第２，第３関節Ｊ５，
Ｊ６，Ｊ７で構成されている。すなわち、上記前腕１４
の中間部には前腕１４の軸線を回転軸ａ５として両端が
回転可能に手首部第１関節Ｊ５が連結され、さらに、前
腕１４の先端には回転軸ａ６のまわりに回転可能な手首
部第２関節Ｊ６が設けられ、第２関節Ｊ６にはリンク１
５ａが接続され、該リンク１５ａの先端には手首部第３
関節Ｊ７が接続され、該手首部第３関節Ｊ７にはリンク
１５ｂが接続されており、上記手首部第３関節Ｊ７によ
り、上記リンク１５ａ，１５ｂの軸線と一致する回転軸
ａ７のまわりにリンク１５ａ，１５ｂは相対的に回転可
能になっている。そして、上記手首の３つの回転軸ａ
５，ａ６，ａ７は手首部１５の点Ｏｗの一点で交わるよ
うに構成されている。なお、ハンド等のエンドエフェク
タはリンク１５ｂの先端に取り付けられる。

【００１４】以上の構成により、肩部１１に３つの関節
Ｊ１，Ｊ２，Ｊ３を有し、手首部１５に３つの関節Ｊ
５，Ｊ６，Ｊ７を備え、肩部１１と手首部１５間に肘部
１３の関節Ｊ４を備え、かつ肩部１１の３つの関節の回
転軸ａ１，ａ２，ａ３は１点Ｏｓで交差し、手首部１５
の３つの関節の回転軸ａ５，ａ６，ａ７は１点Ｏｗで交
差するようにされているので、ハンドの位置と姿勢の解
析を肩部から肘部間での４つの関節Ｊ１，Ｊ２，Ｊ３，
Ｊ４と手首部の３つの関節Ｊ５，Ｊ６，Ｊ７を分割して
取り扱うことが可能となり、運動学計算が簡単となる。
そして、このアーム機構１は、人間の肩から手までの機
能に近い動作機能を発揮できるものとなり、人間の腕と
同じように、ハンド（エンドエフェクタ）の位置・姿勢
を決めるに必要な６自由度に対して１つ自由度の多い７
自由度となり、ハンドを３次元空間で位置及び姿勢の制
御を行なうとに、障害物を迂回したり、障害物の裏側に
まわりこんで位置ぎめができることになる。

【００１５】図２は、本実施例において肩部１１の３つ
の関節Ｊ１，Ｊ２，Ｊ３を動かすことによって手首位置
（手首部１５の３つの関節の回転軸ａ５，ａ６，ａ７の
交点）Ｏｗを変化させず肘部１３の位置Ｏｅを動かすこ
とが可能なことを示す説明図である。手首位置Ｏｗを変
化させないことから、この位置Ｏｗは固定された位置で
ある。また、肩位置Ｏｓは肩部１１の３つの関節の回転
軸ａ１，ａ２，ａ３が１点で交差する位置であるから、
肩部１１の各関節Ｊ１，Ｊ２，Ｊ３がどのように動くと
も一定の位置である。そのため、肩位置Ｏｓと手首位置
Ｏｗを結ぶ直線は一定である。一方肩位置Ｏｓから肘位
置Ｏｅの長さは上腕１２の長さであり一定である。また
肘位置Ｏｅから手首位置Ｏｗまでの長さは前腕１４の長
さでこれも一定である。そのため、上腕１２と前腕１４
間の挟み角は一定となり、肘部１３の関節Ｊ４の回転角
は一定の角度となる。また、手首部１５の３つの関節Ｊ
５，Ｊ６，Ｊ７は手首１５の姿勢を決めるものであり、
手首位置Ｏｗの変化に寄与しないものであるから、手首
位置Ｏｗを固定して肘部１３の位置Ｏｅを移動させる運
動には関係ない関節である。その結果、手首位置Ｏｗを
変化させず、肩部１１の３つの関節Ｊ１，Ｊ２，Ｊ３を
動かすことによって、肘部１３を肩位置Ｏｓと手首位置
Ｏｗを結ぶ直線のまわりの円弧Ｃｅ上に沿って移動させ
ることができる（詳細は先に出願した特願平３−８４１
６７号参照）。この直線のまわりの肘部１３の回転角度
をφとすると、この回転角φが上述したセルフ・モーシ
ョンを規定するパラメータとなる。なお、肘部１３を動
かすとき前腕１４の方向も変化するので、ハンドの姿勢
は変化するが、手首部１５の３つの関節Ｊ５，Ｊ６，Ｊ
７を適切に制御することによってハンドの姿勢も変化し
ないようにすることができる。

【００１６】図３は本実施例において使用される教示操
作盤３の一例で、Ｋ１〜Ｋ16はキーであり、キーＫ１〜
Ｋ14はマニプレータのアーム機構を動かすためのキーで
あり、キーＫ15，Ｋ16は教示時における各軸動作と直交
座標系動作を切換えるためのキーである。また、３１は
マニプレータのアーム機構１の位置，姿勢を数値で表示
するための表示部である。各キーと動作の関係を表すと
次のようになる。

【００１７】 キー 直交座標系動作時 各軸動作時 Ｋ１ Ｘ軸＋方向にハンド移動 関節Ｊ１が＋方向に回転 Ｋ２ Ｘ軸−方向にハンド移動 関節Ｊ１が−方向に回転 Ｋ３ Ｘ軸回り＋方向にハンド姿勢が変化 関節Ｊ２が＋方向に回転 Ｋ４ Ｘ軸回り−方向にハンド姿勢が変化 関節Ｊ２が−方向に回転 Ｋ５ Ｙ軸＋方向にハンド移動 関節Ｊ３が＋方向に回転 Ｋ６ Ｙ軸−方向にハンド移動 関節Ｊ３が−方向に回転 Ｋ７ Ｙ軸回り＋方向にハンド姿勢が変化 関節Ｊ４が＋方向に回転 Ｋ８ Ｙ軸回り−方向にハンド姿勢が変化 関節Ｊ４が−方向に回転 Ｋ９ Ｚ軸＋方向にハンド移動 関節Ｊ５が＋方向に回転 Ｋ10 Ｚ軸−方向にハンド移動 関節Ｊ５が−方向に回転 Ｋ11 Ｚ軸回り＋方向にハンド姿勢が変化 関節Ｊ６が＋方向に回転 Ｋ12 Ｚ軸回り−方向にハンド姿勢が変化 関節Ｊ６が−方向に回転 Ｋ13 肘が＋方向に移動 関節Ｊ７が＋方向に回転 Ｋ14 肘が−方向に移動 関節Ｊ７が−方向に回転 Ｋ15 “各軸動作”を選択 ← Ｋ16 “直交座標系動作”を選択 ← このような教示操作盤３で教示を行なうとき、例えば、
キーＫ15を押すと、教示モードは“各軸動作モード”と
なり、キーＫ１〜Ｋ14のいずれかが押されると、対応す
るマニプレータの関節Ｊ１〜Ｊ７を動かすように制御装
置２からマニプレータの対応するサーボモータ等のアク
チュエータに指令値が送出される。例えば、キーＫ１が
押されると肩部１１の第１関節Ｊ１が回転軸ａ１の回り
に＋方向に回転することになる。また、キーＫ２が押さ
れると関節Ｊ１が回転軸ａ１の回りに−方向に回転する
ことになる。これらの各軸動作による教示におけるキー
と各関節の動きは従来の６自由度のマニプレータのにお
ける各軸動作キー指令と各関節の動きと同一である。た
だし、本実施例では７自由度であるため、６自由度のマ
ニプレータと比較し１軸多くなっている点のみであり、
各軸動作については詳細な説明は省略する。

【００１８】次に、キーＫ16を押した場合には、“直交
座標系動作モード”となり、制御装置２のプロセッサは
図４，図５にフローチャートで示す処理を所定周期毎実
行し、キーＫ１〜Ｋ12が押されれば肘の回転角度φ（セ
ルフ・モーション）を変化させずにハンドの位置，姿勢
を変化させることになる。また、キーＫ13，Ｋ14が押さ
れれば、ハンドの位置，姿勢を変化させないで、肘の回
転角度φ（セルフ・モーション）のみを変化させる動作
を実行する。そこでまず、図４，図５に示す処理におい
て使用する行列等について説明する。リンクｉ−１（関
節Ｊi-1)の座標系（ｘi-1 ，ｙi-1，ｚi-1 ）を基準と
したリンクｉ（関節Ｊｉ）の座標系（ｘi ，ｙi ，ｚi
）を表わす４×４同時変換行列をＡi （以下Ａ行列と
いう）とし、ベース座標系（ｘ0 ，ｙ0 ，ｚ0 ）を基準
とした座標系ｊを表わす同時変換行列をＴｊとする。ま
た、手首１５に取り付けられたハンドのトゥールセンタ
ポイント（ＴＣＰ）の位置（ｘ，ｙ，ｚ）及び姿勢
（α，β，γ）を表わす６次元ベクトルをＰとする。そ
して、この実施例におけるリンクパラメータ（Ｄ−Ｈ記
法による）は次のようになる。

【００１９】 なお、変数θｉ（ｉ＝１，２，…７）は関節Ｊｉの座標
系のＺ軸を回転軸ａｉとしてその回転角、すなわち関節
Ｊｉの回転角。αは関節Ｊi-1 の座標系に対する関節Ｊ
ｉのＸ軸回りのねじれ角、ａは関節Ｊｉ（Ｊ3 ）と関節
Ｊi+1 （Ｊ4 ）間の距離、ｄは関節Ｊi-1 （Ｊ4 ）と関
節Ｊi （Ｊ5 ）間の距離である。そして、Ａ行列Ａｉは
次の数式１で示され、

【００２０】

【数１】 上記数式１より、Ａ行列Ａ1 〜Ａ7 は次の数式２とな
る。ただしＣｉ＝ｃｏｓθi ，Ｓｉ＝ｓｉｎθi であ
る。

【００２１】

【数２】 また、ベース座標系を基準としてリンク１１ｂに固定さ
れた座標系Ｔ２、上腕１２に固定される座標系Ｔ３、前
腕１４に固定される座標系Ｔ４、ＴＣＰを表わす座標系
Ｔ７は次のようになる。 Ｔ２＝Ａ１Ａ２ …（３） Ｔ３＝Ａ１Ａ２Ａ３ …（４） Ｔ４＝Ａ１Ａ２Ａ３Ａ４ …（５） Ｔ７＝Ａ１Ａ２Ａ３Ａ４Ａ５Ａ６Ａ７ …（６） 上記数式３より、肩位置Ｏｓのベース座標系上の位置は
ｘs ＝ｙs ＝ｚs ＝０となる。また、肘位置Ｏｅのベー
ス座標系上の位置（ｘe ，ｙe ，ｚe ）は上記４式より
次のようになる。 ｘe ＝（Ｃ1 Ｃ2 Ｃ3 ＋Ｓ1 Ｓ3 ）ａ3 ｙe ＝（Ｓ1 Ｃ2 Ｃ3 −Ｃ1 Ｓ3 ）ａ3 ｚe ＝Ｓ2 Ｃ3 ａ3 また、手首位置Ｏw のベース座標系の位置（ｘw ，ｙw
，ｚw ）は上記５式より次のようになる。 ｘw ＝（−Ｃ1 Ｃ2 Ｓ34＋Ｓ1 Ｃ34）ｄ5 ＋（Ｃ1 Ｃ2 Ｃ3 ＋Ｓ1 Ｓ3 ）ａ3 ｙw ＝（−Ｓ1 Ｃ2 Ｓ34−Ｃ1 Ｃ34）ｄ5 ＋（Ｓ1 Ｃ2 Ｃ3 −Ｃ1 Ｓ3 ）ａ3 ｚw ＝−Ｓ2 Ｓ34ｄ5 ＋Ｓ2 Ｃ3 ａ3 ただし、Ｓ34＝ｓｉｎ（θ3 ＋θ4 ），Ｃ34＝ｃｏｓ
（θ3 ＋θ4 ）である。

【００２２】セルフ・モーションのパラメータとなる肘
回転角度φは、手首位置Ｏｗと肩位置Ｏｓを含みベース
座標系のＸ−Ｙ平面に垂直な平面（肘位置Ｏｅが最も高
い位置もしくは最も低い位置の姿勢の時に相当する）を
基準平面とし、手首位置Ｏｗ，肩位置Ｏｓ，肘位置Ｏｅ
の３点を通る平面が上記基準平面となす角度とする。す
なわち、肘部１３の関節Ｊ４の回転軸ａ４と上腕１２、
前腕１４とは直交するものであるから、手首位置Ｏｗ，
肩位置Ｏｓ，肘位置Ｏｅの３点を通る平面の法線ベクト
ルは回転軸ａ４となる。そしてこの回転軸ａ４は座標系
Ｔ３のＺ軸向きである。そのため、基準平面上における
関節Ｊ４の回転軸ａ４が示す法線ベクトルと肘回転角度
がφの時の回転軸ａ４で示される法線ベクトルとがなす
角が肘回転角度φとなる。図２で示す例で説明すると、
破線で示す位置が基準位置であり、実線で示す位置が肘
回転角φの位置である。手首位置Ｏｗ，肩位置Ｏｓ，肘
位置Ｏｅの３点を通る平面に対し、肘部１３の関節Ｊ４
の回転軸ａ４は、回転軸ａ４と上腕１２、前腕１４とは
直交するものであるから常に該平面の法線方向を示し、
かつ、図２に示すように円弧Ｃｅの接線方向を示すこと
になる。そして、基準面上の回転軸ａ４と肘回転角度φ
の時の回転軸ａ４とのなす角が肘の回転角度φを示すこ
とになる。そのため、この回転軸ａ４である座標系Ｔ３
のＺ軸の方向を求めれば、回転角度φは求められる。

【００２３】また、この肘回転角φは手首位置Ｏｗが一
定の位置にある状態で、肘部１３を回転させることは図
２で説明したように、肩位置Ｏｓと手首位置Ｏｗを結ぶ
線、すなわち、肩位置Ｏｓから手首位置Ｏｗまでのベク
トルｋの回りの回転角が上記肘回転角度φとなる。ベク
トルｋは肩位置Ｏｓがベース座標系上で（０，０，０）
であることから、上記手首位置Ｏｗで表わされることに
なる。また、肘位置に関する上腕に固定された座標系Ｔ
３は上記肘回転角φの関数とすることができる。そうす
ると、 Ｔ3 （φ）＝Ａ1(θ1)Ａ2(θ2)Ａ3(θ3) …（７） と表わされ、この現在位置からベクトルｋの回りにΔφ
だけ回転させた場合には、ベクトルｋの回りにΔφだけ
回転する変換を行う同時変換行列Ｒｏｔ（ｋ，Δφ）と
上記７式より次の関係が成り立つ。

【００２４】 Ｔ3 （φ＋Δφ）＝Ｒｏｔ（ｋ，Δφ）Ｔ3 （φ） …（８） そして、肘回転角が（φ＋Δφ）になる時の肩部１１の
各関節Ｊ１，Ｊ２，Ｊ３の回転角θ1'，θ2'，θ3'は次
の式より求まる。

【００２５】 Ｔ3 （φ＋Δφ）＝Ａ1(θ1') Ａ2(θ2') Ａ3(θ3') …（９） このように、ハンドの位置を変化せずに肘の回転角φの
みを変えた時の肩関節Ｊ１〜Ｊ３の回転角を求めること
ができる。

【００２６】次に、図４，図５のフローチャーと共に
“直交座標系動作モード”の処理について説明する。キ
ーＫ16を押され“直交座標系動作モード”となると、制
御装置２のプロセッサはキーＫ１〜Ｋ15が押されたか否
か判断し（ステップＳ１〜Ｓ４）、キーＫ１５が押され
れば、各軸移動モードに移行する。

【００２７】ハンドの位置・姿勢を固定して肘のみを移
動させるセルフ・モーションを行わせるとして、肘をプ
ラス方向に移動させるキー１３が押されると該処理周期
内に肘の回転角を移動させる量Δφとして設定されてい
る量＋Δφを移動量としてレジスタに格納する（ステッ
プＳ５）。また、肘をマイナス方向に移動させるキー１
３が押されると移動量として−Δφがレジスタに設定さ
れる（ステップＳ６）。次に、角関節Ｊ１〜Ｊ７の現在
位置を記憶する現在位置レジスタより現在位置θ１〜θ
７を読取り（ステップＳ７）、まず、現在の肘位置に関
する上腕１２に固定された座標系Ｔ3 を第７式により計
算する（ステップＳ８）。次に第５式より上述した手首
位置Ｏw のベース座標系の位置（ｘw，ｙw ，ｚw ）を
求めベクトルｋを求める（ステップＳ９）。そして、ス
テップＳ８で求めた座標系Ｔ3(φ) とベクトルｋの回り
に肘を回転移動量Δφだけ回転させる同時変換行列Ｒｏ
ｔ（ｋ，Δφ）より上記第８式の演算を行って座標系Ｔ
3(φ＋Δφ）を求める（ステップＳ１０）。座標系Ｔ3
は第４式によっても求められるものであるから、ステッ
プＳ１０で求めた座標系Ｔ3(φ＋Δφ）になる肩部１１
の各関節Ｊ１〜Ｊ３の回転角θ1'〜θ3'を第９式の演算
を行って求める（ステップＳ１１）。次に、肩位置Ｏｓ
と手首位置Ｏｗ間の距離は変化しないものであるから、
肘部１３の関節Ｊ４の回転角θ4 は肘回転角がφから
（φ＋Δφ）に変化しても変化せず、変化後の回転角θ
4'＝θ4 である。そのため、前腕１４に固定された座標
系Ｔ４の肘回転角が（φ＋Δφ）になった時の座標系Ｔ
4'は次のようにして算出される（ステップＳ１２）。

【００２８】 Ｔ4'＝Ａ1(θ1') Ａ2(θ2') Ａ3(θ3') Ａ4(θ4'） ＝Ｔ3 （φ＋Δφ）Ａ4(θ4'） また、ハンドの位置・姿勢は変化しないものであるた
め、ハンドの位置・姿勢Ｐを表わす座標系Ｔ７は変化せ
ず、ステップＳ７で求めた各関節回転角θ１〜θ７によ
って求まり、次の式が成立する。

【００２９】 Ｔ７＝Ａ1(θ1)Ａ2(θ2)Ａ3(θ3)Ａ4(θ4)Ａ5(θ5)Ａ6(θ6)Ａ1(θ7) ＝Ｔ4'Ａ5(θ5') Ａ6(θ6') Ａ7(θ7') …（１０） 上記第１０式よりハンドの姿勢を変更しない手首の角関
節Ｊ５〜Ｊ７の回転角θ5'〜θ6'を求める。

【００３０】以上のようにして、ステップＳ１１で求め
られた肩部の関節Ｊ１〜Ｊ３に移動位置θ1'〜θ3'、移
動位置を変更しない肘の関節Ｊ４には移動位置θ4 ＝θ
4'、手首の関節Ｊ５〜Ｊ７にはステップＳ１３で求めた
移動位置θ5'〜θ7'を出力しマニプレータを駆動する。
以下、キーＫ１３またはキー１４が押されている限り上
記処理を繰り返すことになる。

【００３１】次に、キーＫ１〜Ｋ１２の１つが押された
場合、この場合には、肘の回転角度φを変化させずにハ
ンドの位置・姿勢を変化させる（セルフ・モーション）
動作となる。まず、角関節Ｊ１〜Ｊ７の現在の回転角θ
１〜θ７を読取り（ステップＳ１５）、この回転角θ１
〜θ７より現在のハンド位置・姿勢Ｐを求めると共に、
現在の肘回転角度φを座標系Ｔ３のＺ軸方向より求める
（ステップＳ１６）。次に、ステップＳ１６で求められ
たハンドの位置・姿勢Ｐに対して、キーＫ１〜Ｋ１２の
内押されたキーに対応する軸に対して設定された移動量
ΔＰを加算し新しいハンド位置・姿勢Ｐ´を求め（ステ
ップＳ１７）、このハンド位置・姿勢Ｐ´より各関節Ｊ
１〜Ｊ７の回転角を求めるが、肩部１１の３つの関節Ｊ
１〜Ｊ３の内関節Ｊ２を固定（θ２＝一定）すれば、従
来の６自由度マニプレータと同じ構造になるので、この
関節Ｊ２の回転角θ２を現在位置に固定して、ハンド位
置・姿勢がＰ´になるように残り６関節の回転角θ１及
びθ３〜θ７を従来と同様に計算する（ステップＳ１
８）。こうして求められた回転角θ１，θ３〜θ７及
び、変更しなかったθ２に基づいてステップＳ１６と同
様に、求められた回転角θ１〜θ７における肘回転角度
φ´を求め（ステップＳ１９）、この肘回転角度φ´を
ステップＳ１６で求めたハンドの位置・姿勢を変更する
まえの肘回転角度φから減じて、肘回転角度の変化量Δ
φを求める（ステップＳ２０）。そして、ステップＳ８
以下の処理を行うことによって、指令されたハンドの位
置・姿勢Ｐ´を保持し、肘回転角度をステップＳ２０で
求めた変化量Δφだけ回転する（元に戻す）処理を行
う。そうすれば、肘回転量は変化せずφの間まで、ハン
ドの位置・姿勢だけが指令された当該処理周期における
単位量だけ移動することになる。

【００３２】すなわち、ステップＳ８でステップＳ１８
で求めた新しい各関節の回転角θ１，θ３及び変化のな
い回転角θ２によって座標系Ｔ3(φ）を求め、また、ス
テップＳ１８で求めた各回転角θ１〜θ４より手首位置
Ｏｗを求めてベクトルｋを求め（ステップＳ９）、上記
ステップＳ８で求められた座標系Ｔ3(φ）と、上記ベク
トルｋとステップＳ２０で求めた肘回転角度の変化量Δ
φで求められるベクトルｋの回りにΔφだけ回転させる
同時変換行列Ｒｏｔ（ｋ，Δφ）より、第８式の演算を
行って座標系Ｔ3(φ＋Δφ）を求める（ステップＳ１
０）。以後は前述したステップＳ１１〜ステップＳ１４
（なお、ステップＳ１３において座標系Ｔ７はステップ
Ｓ１８で求めた新しい各関節の回転角θ１，θ３〜θ７
及び変化させなかった回転角θ２によって求めるもので
ある）の処理を行って、各関節を求められた回転角度θ
1'〜θ7'に移動させる。以下、キーＫ１〜Ｋ１２の１つ
が押し続けられる間、ステップＳ１〜Ｓ４，ステップＳ
１５〜Ｓ２０，ステップＳ８〜Ｓ１４の処理を毎周期毎
繰り返し、肘の回転角度φを変えずに、ハンドの位置・
姿勢のみを変える。

【００３３】上記実施例では、肩部の第１，第２関節を
関節の回転軸がリンクの軸線方向と同一となる回転関節
を用い、第３関節を該関節の回転軸回りにリンクが回転
する旋回関節を使用し、肘の関節に旋回関節、手首の第
１関節に回転関節、第２関節に旋回関節、第３関節に回
転関節を使用したが、関節の組み合わせは他の構成でも
よい。すなわち、肩部の関節の３つの回転軸が１点で交
差し、かつ手首の３つの関節の回転軸が１点で交差する
ものであればよく、例えば、前述した特許出願、特願平
３−８４１６７号で示される各種７自由度マニプレータ
にも本発明は適用できるものである。

【００３４】

【発明の効果】本発明においては、肘の回転角を変化さ
せない（セルフ・モーションを変化させない）で、ハン
ド（エンドエフェクタ）の位置・姿勢を変化させる動
き、ハンド（エンドエフェクタ）の位置・姿勢を変えな
いで、肘の回転角のみを変化させる（セルフ・モーショ
ンのみ変化させる）動きを指令することができ、この指
令によって７自由度のマニプレータ（ロボット）に簡単
に位置・姿勢等を教示することができる。

【図面の簡単な説明】

【図１】本発明の一実施例における７自由度マニプレー
タの構成図である。

【図２】同実施例におけるセルフ・モーションの説明図
である。

【図３】同実施例における教示操作盤の外観図である。

【図４】同実施例における直交座標系動作モードにおけ
る処理のフローチャートの一部である。

【図５】図４に示すフローチャートの続きである。

【符号の説明】

１ アーム機構部 ２ マニプレータ制御装置 ３ 教示操作盤 １１ 肩部 １２ 上腕部 １３ 肘部 １４ 前腕部 １５ 手首部 Ｊ１ 肩部第１関節 Ｊ２ 肩部第２関節 Ｊ３ 肩部第３関節 Ｊ４ 肘部の関節 Ｊ５ 手首部第１関節 Ｊ６ 手首部第２関節 Ｊ７ 手首部第３関節 Ｋ１〜Ｋ16 キー

Claims (3)

【特許請求の範囲】
1. 【請求項１】 ３つの関節を有し各関節の回転軸が空間
   上の１点で交差する肩部と、該肩部に接続された上腕部
   に接続され前腕部を結合する１つの関節からなる肘部
   と、前腕部の他端に接続された３つの関節を有し該３つ
   の関節の回転軸が１点で交差する手首部とで構成される
   ７自由度マニプレータにおける位置教示方式において、
   マニプレータのエンドエフェクタを直交座標系の各軸に
   平行に移動させる直線移動指令手段と、各軸の回りに回
   転させる回転指令手段と、エンドエフェクタの位置及び
   姿勢を変えずに肩部と手首を結ぶ直線の回りに肘を回転
   させる肘回転指令手段を設け、上記直線移動指令手段も
   しくは回転指令手段が操作されたときには肩部と手首を
   結ぶ直線の回りの肘回転角度を変えずにエンドエフェク
   タの位置及び姿勢を変え、上記肘回転指令手段が操作さ
   れたときには、エンドエフェクタの位置及び姿勢を変え
   ずに上記肘部回転角度のみを変えることによって、マニ
   プレータに位置・姿勢を教示する７自由度マニプレータ
   の位置教示方式。
2. 【請求項２】 各軸動作への切換手段を有し、各軸動作
   へ切換えられると、各関節毎の移動指令手段からの指令
   により各関節が個々に駆動され、エンドエフェクタの位
   置及び姿勢を変えてマニプレータに位置・姿勢を教示す
   る請求項第１項記載の７自由度マニプレータの位置教示
   方式。
3. 【請求項３】 上記肘回転指令手段が操作されたときに
   は、肩部の各関節のその時点の回転角より上記上腕に固
   定された座標系を求め、該座標系を肩部関節の回転軸の
   交点と手首部関節の回転軸の交点を結ぶ直線の回りに肘
   部を回転させたときの肩部各関節の回転角を求め、該求
   められた肩部の各関節の回転角と肘部関節の変化しない
   回転角及び、肘部を回転させるまえのエンドエフェクタ
   の位置及び姿勢を表わす座標系とにより、手首部の３つ
   の関節の回転角を求め、各関節を求めた回転角に移動さ
   せてエンドエフェクタの位置及び姿勢を変えずに上記肘
   部回転角度のみを変え、上記直線移動指令手段もしくは
   回転指令手段が操作されたときには、肩部の第２関節の
   回転角を固定して、指令されたエンドエフェクタの位置
   及び姿勢になる各関節の回転角を求めると共に、上記肘
   部回転角度の変化量を求め、肩部の各関節の上記求めら
   れた回転角より上記上腕に固定された座標系を求め、該
   座標系を肩部関節の回転軸の交点と手首部関節の回転軸
   の交点を結ぶ直線の回りに肘部を上記変化量だけ回転さ
   せ元の肘部回転角度にするときの肩部各関節の指令回転
   角を求め、該求められた肩部の各関節の指令回転角と肘
   部関節の指令回転角となる上記求められた回転角及び、
   肘部回転角度を上記変化量だけ戻す前のエンドエフェク
   タの位置及び姿勢を表わす座標系とにより、手首部の３
   つの関節の指令回転角を求め、各関節を求めた指令回転
   角に移動させて上記肘回転角度を変化させずにエンドエ
   フェクタの位置及び姿勢を変えることによってマニプレ
   ータに位置・姿勢を教示する請求項第１項または第２項
   記載の７自由度マニプレータの位置教示方式。