JP2582445B2 - 関節形マニュピュレータの手先方向演算装置 - Google Patents
関節形マニュピュレータの手先方向演算装置Info
- Publication number
- JP2582445B2 JP2582445B2 JP1266219A JP26621989A JP2582445B2 JP 2582445 B2 JP2582445 B2 JP 2582445B2 JP 1266219 A JP1266219 A JP 1266219A JP 26621989 A JP26621989 A JP 26621989A JP 2582445 B2 JP2582445 B2 JP 2582445B2
- Authority
- JP
- Japan
- Prior art keywords
- hand
- vector
- roll
- yaw
- pitch
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Expired - Lifetime
Links
Landscapes
- Numerical Control (AREA)
- Manipulator (AREA)
Description
【発明の詳細な説明】 〈産業上の利用分野〉 本発明は、関節形マニュピュレータの手先の方向を制
御する際、現在の手先の方向と目標方向との偏差を演算
する関節形マニュピュレータの手先方向演算装置に関す
る。
御する際、現在の手先の方向と目標方向との偏差を演算
する関節形マニュピュレータの手先方向演算装置に関す
る。
〈従来の技術〉 従来、関節形マニュピュレータは塗装作業や溶接作業
をはじめとして工場の生産ライン等で広く用いられてい
る。その形は第２図にその一例を示すように、一般に人
間の腕によく似たものとなっている。
をはじめとして工場の生産ライン等で広く用いられてい
る。その形は第２図にその一例を示すように、一般に人
間の腕によく似たものとなっている。
即ち、関節形マニュピュレータ１は肩関節に相当する
第２関節３と肘関節に相当する第３関節４を持ち、人間
の手先に似た手先６を備えており、この手先６で物をつ
かむ。
第２関節３と肘関節に相当する第３関節４を持ち、人間
の手先に似た手先６を備えており、この手先６で物をつ
かむ。
手先６の位置は、通常関節形マニュピュレータ１本体
に定義された座標系（点Ｏを原点とする直交座標系）か
らみらＸ座標,Y座標,Z座標で示される。
に定義された座標系（点Ｏを原点とする直交座標系）か
らみらＸ座標,Y座標,Z座標で示される。
一方、ベクトルを用いて表現すれば、手先６の位置は
点Ｏを始点とするベクトルで示すことができる。これ
に加えて、手先６は今、どの方向に向いているかを示す
必要がある。手先６の方向の記述によく用いられる方法
としては第３図に示すベクトル，，がある。ベク
トルは手先６が、目標物に接近する際の方向にとら
れ、アプローチベクトルと呼ばれる。ベクトルは手先
６の片方の指先6aからもう一方の指先6bへ向う方向へと
られ、手先６の方向を指定するのでオリエンテーション
ベクトルと呼ばれる。ベクトルは３本のベクトルが右
手系をなすようにとられる。従って、は＝×で
与えられる（但し、×はベクトルの外積を示す）。
点Ｏを始点とするベクトルで示すことができる。これ
に加えて、手先６は今、どの方向に向いているかを示す
必要がある。手先６の方向の記述によく用いられる方法
としては第３図に示すベクトル，，がある。ベク
トルは手先６が、目標物に接近する際の方向にとら
れ、アプローチベクトルと呼ばれる。ベクトルは手先
６の片方の指先6aからもう一方の指先6bへ向う方向へと
られ、手先６の方向を指定するのでオリエンテーション
ベクトルと呼ばれる。ベクトルは３本のベクトルが右
手系をなすようにとられる。従って、は＝×で
与えられる（但し、×はベクトルの外積を示す）。
，，はそれぞれ の３つの要素からなる。
ところで、手先６の方向を示す別な方法として、第４
図に示すように、Yaw,Pitch,Rollがあり、Yawはｘ軸回
りの回転θYaw、Pitchはｙ軸回りの回転θPitch、Roll
がｚ軸回りの回転θRollで示される。
図に示すように、Yaw,Pitch,Rollがあり、Yawはｘ軸回
りの回転θYaw、Pitchはｙ軸回りの回転θPitch、Roll
がｚ軸回りの回転θRollで示される。
第５図に示すように、各関節2,3,4,5に設けられた位
置センサ（図示せず）より、各関節2,3,4,5の角度を示
す信号12を角度−ベクトル変換器13に入力し、，，
を算出する。演算装置14では、，，を入力し、 を用いて、手先６の向いている方向を演算し減算回路15
へ出力する。
置センサ（図示せず）より、各関節2,3,4,5の角度を示
す信号12を角度−ベクトル変換器13に入力し、，，
を算出する。演算装置14では、，，を入力し、 を用いて、手先６の向いている方向を演算し減算回路15
へ出力する。
そして、手先６を向かせたい方向を角度で示し、Yaw
を角度θYawr、Pitchを角度θPitchr、Rollを角度θ
Rollrとして減算回路15に入力し、演算されたθYaw，θ
Pitch，θRollとのそれぞれの角度偏差ｅYaw,ePitch,e
Rollを次式 ｅYaw＝θYawr−θYaw ……（５） ｅPitch＝θPitchr−θPitch ……（６） ｅRoll＝θRollr−θRoll ……（７） で求め、偏差を示すベクトル ＝〔ｅYaw,ePitch,eRoll〕T（Ｔは転置を示す）を求
める。このをゲイン乗算器16に入力し、ゲイン乗算器
16に設定されているゲイン行列Kpを乗じ、修正速度を求
め、直線−回転速度変換器17へ出力する。直線−回転速
度変換器17には逆ヤコビ行列J-1（θ）が設定されてお
り、この逆ヤコビ行列J-1（θ）を前記修正速度に乗じ
て各関節2,3,4,5の修正速度に変換し、マニュピュレー
タ１を駆動している。
を角度θYawr、Pitchを角度θPitchr、Rollを角度θ
Rollrとして減算回路15に入力し、演算されたθYaw，θ
Pitch，θRollとのそれぞれの角度偏差ｅYaw,ePitch,e
Rollを次式 ｅYaw＝θYawr−θYaw ……（５） ｅPitch＝θPitchr−θPitch ……（６） ｅRoll＝θRollr−θRoll ……（７） で求め、偏差を示すベクトル ＝〔ｅYaw,ePitch,eRoll〕T（Ｔは転置を示す）を求
める。このをゲイン乗算器16に入力し、ゲイン乗算器
16に設定されているゲイン行列Kpを乗じ、修正速度を求
め、直線−回転速度変換器17へ出力する。直線−回転速
度変換器17には逆ヤコビ行列J-1（θ）が設定されてお
り、この逆ヤコビ行列J-1（θ）を前記修正速度に乗じ
て各関節2,3,4,5の修正速度に変換し、マニュピュレー
タ１を駆動している。
〈発明が解決しようとする課題〉 上記従来の方法では、手先６の方向を示すが座標系
のＺ軸方向と同じ方向を向いている場合、＝〔ｎx,
ny,nz〕T＝〔0,0,±１〕Tとなり（２）式において、 となりθRollを求めることができない。従って、角度偏
差ｅYaw,ePitch,eRollが求められず手先６の方向の制御
ができない。
のＺ軸方向と同じ方向を向いている場合、＝〔ｎx,
ny,nz〕T＝〔0,0,±１〕Tとなり（２）式において、 となりθRollを求めることができない。従って、角度偏
差ｅYaw,ePitch,eRollが求められず手先６の方向の制御
ができない。
本発明は、上記問題点に鑑み、外積を用いて角度の偏
差を求める関節形マニュピュレータの手先方向演算装置
を提供することを目的とする。
差を求める関節形マニュピュレータの手先方向演算装置
を提供することを目的とする。
〈課題を解決するための手段〉 本発明は、上記目的を達成するために、関節形マニュ
ピュレータの手先を向けたい方向を、前記手先が目標位
置に接近する際の手先の方向を示すアプローチベクトル
r，rと直交し手先の一方の指先から他方の指先への
方向を示すオリエンテーションベクトルr及びrと
rとに直交するベクトルrとして入力するとともに、現
在の手先の方向を示すアプローチベクトルr、オリエ
ンテーションベクトルr及びrとrとに直交するベ
クトルを入力して、外積 の和を演算し、偏差を示すベクトルr＝〔ｅYaw,e
Pitch,eRoll〕Tとして出力することを特徴とする。
ピュレータの手先を向けたい方向を、前記手先が目標位
置に接近する際の手先の方向を示すアプローチベクトル
r，rと直交し手先の一方の指先から他方の指先への
方向を示すオリエンテーションベクトルr及びrと
rとに直交するベクトルrとして入力するとともに、現
在の手先の方向を示すアプローチベクトルr、オリエ
ンテーションベクトルr及びrとrとに直交するベ
クトルを入力して、外積 の和を演算し、偏差を示すベクトルr＝〔ｅYaw,e
Pitch,eRoll〕Tとして出力することを特徴とする。
〈作用〉 手先の動かしたい目標方向を示すアプローチベクトル
r、オリエンテーションベクトルr及びrとrと互
いに直交するベクトルrを入力するとともに、また、
現在手先の向いている方向を示すアプローチベクトル
、オリエンテーションベクトル、及び、と互い
に直交するベクトルを入力し、外積 が演算される。偏差を示すベクトルを示せば〔ｅ
Yaw,ePitch,eRoll〕Tであり、外積との関係は、次式に
よって表わされ、角度ｅYaw,ePitch,eRollが出力され
る。
r、オリエンテーションベクトルr及びrとrと互
いに直交するベクトルrを入力するとともに、また、
現在手先の向いている方向を示すアプローチベクトル
、オリエンテーションベクトル、及び、と互い
に直交するベクトルを入力し、外積 が演算される。偏差を示すベクトルを示せば〔ｅ
Yaw,ePitch,eRoll〕Tであり、外積との関係は、次式に
よって表わされ、角度ｅYaw,ePitch,eRollが出力され
る。
ｅYaw＝0.5×｛（ｎyｎzr−ｎzｎyr）＋（ｏyｏzr−ｏz
ｏyr）＋（ａyａzr−ａzａyr）｝ ……（８） ｅPitch＝0.5×｛（ｎzｎxr−ｎxｎzr）＋（ｏzｏxr−
ｏxｏzr）＋（ａzａxr−ａxａzr）｝ ……（９） ｅRoll＝0.5×｛（ｎxｎyr−ｎyｎxr）＋（ｏxｏyr−ｏ
yｏxr）＋（ａxａyr−ａyａxr）｝ ……（10） 〈実施例〉 以下、本発明の一実施例を図面に従って説明する。
ｏyr）＋（ａyａzr−ａzａyr）｝ ……（８） ｅPitch＝0.5×｛（ｎzｎxr−ｎxｎzr）＋（ｏzｏxr−
ｏxｏzr）＋（ａzａxr−ａxａzr）｝ ……（９） ｅRoll＝0.5×｛（ｎxｎyr−ｎyｎxr）＋（ｏxｏyr−ｏ
yｏxr）＋（ａxａyr−ａyａxr）｝ ……（10） 〈実施例〉 以下、本発明の一実施例を図面に従って説明する。
尚、従来の技術と同一構成のものは同一符号を用い、
説明を省略する。
説明を省略する。
第１図は第２図に示す関節形マニュピュレータ１の制
御装置の構成図である。
御装置の構成図である。
マニュピュレータ１の第１関節２、第２関節３、第３
関節４、第４関節５にはそれぞれ関節を動かすアクチュ
エータ（図示せず）と位置センサ（図示せず）が設けら
れている。マニュピュレータ１の先端部には手先６が設
けられ、手先６の方向は第４図に示すようにRoll,Pitc
h,Yawで示される。
関節４、第４関節５にはそれぞれ関節を動かすアクチュ
エータ（図示せず）と位置センサ（図示せず）が設けら
れている。マニュピュレータ１の先端部には手先６が設
けられ、手先６の方向は第４図に示すようにRoll,Pitc
h,Yawで示される。
手先６をある方向に動かしたい時、制御装置には、動
かしたい方向をRoll,Pitch,Yawのそれぞれの角度を示す
信号θRollr，θPitchr，θYawrを入力する。これらの
信号は、角度−ベクトル変換器７に入力し、アプローチ
ベクトルr、オリエンテーションベクトルr、及びこ
れらのベクトルと右手系をなすベクトルrに変換され
る。r ，r，rを行列で表現して とすれば、角度−ベクトル変換器７においては次式によ
る計算が行われ、行列として出力される。
かしたい方向をRoll,Pitch,Yawのそれぞれの角度を示す
信号θRollr，θPitchr，θYawrを入力する。これらの
信号は、角度−ベクトル変換器７に入力し、アプローチ
ベクトルr、オリエンテーションベクトルr、及びこ
れらのベクトルと右手系をなすベクトルrに変換され
る。r ，r，rを行列で表現して とすれば、角度−ベクトル変換器７においては次式によ
る計算が行われ、行列として出力される。
ｎxr＝cosθRollr・cosθPitchr ｎyr＝sinθRollr・cosθPitchr ｎzr＝−sinθPitchr ｏxr＝cosθRollr・sinPitchr・sinθYawr−sinθRollr
・cosθYawr ｏyr＝sinθRollr・sinPitchr・sinθYawr＋cosθRollr
・cosθYawr ｏzr＝cosPitchr・sinθYawr ａxr＝cosθRollr・sinPitchr・cosθYawr＋sinθRollr
・sinθYawr ａyr＝sinθRollr・sinθPitchr・cosθYawr−cosθ
Rollr・sinθYawr ａzr＝cosθPitchr・cosθYawr ……（11） また、手先６の現在の向きは、関節2,3,4,5の各位置
センサより角度を角度−ベクトル変換器８へ入力し、ア
プローチベクトル、オリエンテーションベクトル、
及びとと共に右手系をなすベクトルを算出して出
力する。
・cosθYawr ｏyr＝sinθRollr・sinPitchr・sinθYawr＋cosθRollr
・cosθYawr ｏzr＝cosPitchr・sinθYawr ａxr＝cosθRollr・sinPitchr・cosθYawr＋sinθRollr
・sinθYawr ａyr＝sinθRollr・sinθPitchr・cosθYawr−cosθ
Rollr・sinθYawr ａzr＝cosθPitchr・cosθYawr ……（11） また、手先６の現在の向きは、関節2,3,4,5の各位置
センサより角度を角度−ベクトル変換器８へ入力し、ア
プローチベクトル、オリエンテーションベクトル、
及びとと共に右手系をなすベクトルを算出して出
力する。
角度−ベクトル変換器７から出力されr，r，
r、及び角度−ベクトル変換器８から出力された，
，は演算装置９に入力する。演算装置９では外積 を計算し総和を求める。
r、及び角度−ベクトル変換器８から出力された，
，は演算装置９に入力する。演算装置９では外積 を計算し総和を求める。
手先６の現在向いている方向と手先６を向かせたい方
向との偏差は偏差をベクトル＝〔ｅYaw,ePitch,
eRoll〕Tで表わせば、は（８），（９），（10）式で
求められる。
向との偏差は偏差をベクトル＝〔ｅYaw,ePitch,
eRoll〕Tで表わせば、は（８），（９），（10）式で
求められる。
偏差は、ゲイン乗算器10で、ゲインマトリクスＫp
を乗算され、直線−回転速度変換器11へ出力される。直
線−回転速度変換器11では逆ヤコビ行列Ｊ-1（θ）にゲ
インマトリクスＫpを乗算されたを入力して各関節2,
3,4,5の修正速度に変換され、各関節2,3,4,5のアクチ
ュエータに出力され、マニュピュレータ１の手先６の方
向が修正される。この修正に伴い、各関節の角度θは更
新され、新たな，，がそれぞれ求まる。そして、
がｏベクトルとなるように手先６が動作する。
を乗算され、直線−回転速度変換器11へ出力される。直
線−回転速度変換器11では逆ヤコビ行列Ｊ-1（θ）にゲ
インマトリクスＫpを乗算されたを入力して各関節2,
3,4,5の修正速度に変換され、各関節2,3,4,5のアクチ
ュエータに出力され、マニュピュレータ１の手先６の方
向が修正される。この修正に伴い、各関節の角度θは更
新され、新たな，，がそれぞれ求まる。そして、
がｏベクトルとなるように手先６が動作する。
〈発明の効果〉 以上説明したように、手先を示すベクトルの各要素か
ら角度の偏差を求めることができ、手先の方向を示すベ
クトルが座標系のＺ軸、または−Ｚ軸方向となった場
合でも、手先の方向の制御が可能となる。
ら角度の偏差を求めることができ、手先の方向を示すベ
クトルが座標系のＺ軸、または−Ｚ軸方向となった場
合でも、手先の方向の制御が可能となる。
第１図と第２図は本発明の一実施例を示し、第１図は実
施例の構成図、第２図は関節形マニュピュレータの説明
図、第３図から第５図までは従来の技術を示し、第３図
はベクトルo,a,1nの説明図、第４図はYaw,Pitch,Rollの
説明図、第５図は従来の手先の方向の位置制御方法のブ
ロック図である。 図中、 １は関節形マニュピュレータ、２は第１関節、３は第２
関節、４は第３関節、５は第４関節、６は手先、7,8は
角度−ベクトル変換器、９は演算装置、10はゲイン乗算
器、11は直線−回転速度変換器である。
施例の構成図、第２図は関節形マニュピュレータの説明
図、第３図から第５図までは従来の技術を示し、第３図
はベクトルo,a,1nの説明図、第４図はYaw,Pitch,Rollの
説明図、第５図は従来の手先の方向の位置制御方法のブ
ロック図である。 図中、 １は関節形マニュピュレータ、２は第１関節、３は第２
関節、４は第３関節、５は第４関節、６は手先、7,8は
角度−ベクトル変換器、９は演算装置、10はゲイン乗算
器、11は直線−回転速度変換器である。
Claims (1)
- 【請求項１】関節形マニュピュレータの手先を向けたい
方向を、前記手先が目標位置に接近する際の手先の方向
を示すアプローチベクトルr，rと直交し手先の一方
の指先から他方の指先への方向を示すオリエンテーショ
ンベクトルr及びrとrとに直交するベクトルrと
して入力するとともに、現在の手先の方向を示すアプロ
ーチベクトル、オリエンテーションベクトル及び
ととに直交するベクトルを入力して、外積 の和を演算し、偏差を示すベクトル＝〔ｅYaw,
ePitch,eRoll〕Tとして出力することを特徴とする関節
形マニュピュレータの手先方向演算装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP1266219A JP2582445B2 (ja) | 1989-10-16 | 1989-10-16 | 関節形マニュピュレータの手先方向演算装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP1266219A JP2582445B2 (ja) | 1989-10-16 | 1989-10-16 | 関節形マニュピュレータの手先方向演算装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| JPH03131485A JPH03131485A (ja) | 1991-06-05 |
| JP2582445B2 true JP2582445B2 (ja) | 1997-02-19 |
Family
ID=17427917
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP1266219A Expired - Lifetime JP2582445B2 (ja) | 1989-10-16 | 1989-10-16 | 関節形マニュピュレータの手先方向演算装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JP2582445B2 (ja) |
Families Citing this family (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP3473834B2 (ja) | 1999-11-29 | 2003-12-08 | 株式会社安川電機 | ロボットの制御装置 |
-
1989
- 1989-10-16 JP JP1266219A patent/JP2582445B2/ja not_active Expired - Lifetime
Also Published As
| Publication number | Publication date |
|---|---|
| JPH03131485A (ja) | 1991-06-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
| JPS5840761B2 (ja) | 人間腕形マニピュレ−タの制御装置 | |
| WO1992012473A1 (en) | Method of correcting deflection of robot | |
| CN105563482A (zh) | 用于工业机器人的末端执行器的旋转运动规划方法 | |
| JP2582445B2 (ja) | 関節形マニュピュレータの手先方向演算装置 | |
| JPS62194513A (ja) | ロボツトの円弧補間姿勢制御装置 | |
| JPH035605B2 (ja) | ||
| JP2737325B2 (ja) | ロボットの軌道生成方式 | |
| JPS5927306A (ja) | ロボツトの制御方法 | |
| JPS63184806A (ja) | ロボツトによる相対座標系情報テイ−チング方式 | |
| JPH02205494A (ja) | マニピュレータ画像追従方法及び装置並びに該装置を備えたマニピュレータ装置 | |
| JP2592636B2 (ja) | 倣い教示制御方式 | |
| JPH02121007A (ja) | ロボットの教示用制御方式 | |
| JPS60217418A (ja) | 多関節ロボツトの追従制御方式 | |
| JP2737326B2 (ja) | ロボットの軌道探索方式 | |
| JPS6198404A (ja) | 産業用ロボツトの座標変換方式 | |
| JPH03111184A (ja) | ロボット制御装置 | |
| JP2613076B2 (ja) | 倣い教示制御方式 | |
| JPS63149707A (ja) | ロボツトの速度指令生成方法 | |
| JPS62295115A (ja) | ロボツトの制御方法 | |
| JPS63256380A (ja) | 多関節型ハンドの制御装置 | |
| JPS62127905A (ja) | ロボツトア−ムの動作シミユレ−タ装置 | |
| JP2751967B2 (ja) | 倣い制御装置 | |
| JPS61250705A (ja) | 産業用ロボツトの座標変換方式 | |
| JPS6132691B2 (ja) |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20081121 Year of fee payment: 12 |
|
| FPAY | Renewal fee payment (event date is renewal date of database) |
Free format text: PAYMENT UNTIL: 20091121 Year of fee payment: 13 |
|
| EXPY | Cancellation because of completion of term |