JPH0713642A - マニピュレータのコンプライアンス制御装置 - Google Patents
マニピュレータのコンプライアンス制御装置Info
- Publication number
- JPH0713642A JPH0713642A JP5287093A JP5287093A JPH0713642A JP H0713642 A JPH0713642 A JP H0713642A JP 5287093 A JP5287093 A JP 5287093A JP 5287093 A JP5287093 A JP 5287093A JP H0713642 A JPH0713642 A JP H0713642A
- Authority
- JP
- Japan
- Prior art keywords
- manipulator
- external force
- vector
- force vector
- singular point
- 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.)
- Pending
Links
Landscapes
- Control Of Position Or Direction (AREA)
- Control Of Non-Electrical Variables (AREA)
- Manipulator (AREA)
Abstract
(57)【要約】
【目的】特異点領域を通過するような軌道を与えた場合
にも、仮想外力ベクトルとを与えることにより、特異点
領域から外れる方向に動作させることによって、操作者
の負担を軽減する。 【構成】特異点領域の幾何形状情報とマニピュレータ手
先点の現在位置をもとに、マニピュレータ手先点が特異
点領域に近付けば近付くほど大きさが大きくなり、マニ
ピュレータ手先点が特異点領域から遠ざかる向きを持つ
ような仮想外力ベクトルを求める。手先点に設けられた
力センサの出力する実際の外力ベクトルと前記仮想外力
ベクトルとの和である合成外力ベクトルを求める。合成
外力ベクトルが手先点に加えられているものとして特性
方程式を解く。
にも、仮想外力ベクトルとを与えることにより、特異点
領域から外れる方向に動作させることによって、操作者
の負担を軽減する。 【構成】特異点領域の幾何形状情報とマニピュレータ手
先点の現在位置をもとに、マニピュレータ手先点が特異
点領域に近付けば近付くほど大きさが大きくなり、マニ
ピュレータ手先点が特異点領域から遠ざかる向きを持つ
ような仮想外力ベクトルを求める。手先点に設けられた
力センサの出力する実際の外力ベクトルと前記仮想外力
ベクトルとの和である合成外力ベクトルを求める。合成
外力ベクトルが手先点に加えられているものとして特性
方程式を解く。
Description
【０００１】
【産業上の利用分野】本発明はマニピュレータのコンプ
ライアンス制御装置に関し、特に、マニピュレータの特
異点領域情報をもとにマニピュレータの稼働範囲内に仮
想力場を設定することによってマニピュレータ手先点の
特異点領域回避を実現するマニピュレータのコンプライ
アンス制御装置に関する。
ライアンス制御装置に関し、特に、マニピュレータの特
異点領域情報をもとにマニピュレータの稼働範囲内に仮
想力場を設定することによってマニピュレータ手先点の
特異点領域回避を実現するマニピュレータのコンプライ
アンス制御装置に関する。
【０００２】
【従来の技術】コンプライアンス制御は、マニピュレー
タの先端に仮想的なバネ、ダンパ、慣性パラメータを設
定し、マニピュレータ先端にかかる力に対して軟らかい
動きをさせる制御手法である。具体的には、次に示す
(1) 式を満足するようにマニピュレータを制御するもの
である。
タの先端に仮想的なバネ、ダンパ、慣性パラメータを設
定し、マニピュレータ先端にかかる力に対して軟らかい
動きをさせる制御手法である。具体的には、次に示す
(1) 式を満足するようにマニピュレータを制御するもの
である。
【０００２】
【数１】
【０００３】従来の装置の概念を図２に示して説明する
と、マニピュレータ１の各関節に取りつけられたエンコ
ーダ２の検出角度θを前向き座標変換部３により位置ｒ
に変換し、本来の目標位置ｒO および力センサ４からの
力フィードバツクＦexから、コンプライアンス制御部５
でマニピュレータ１が追従すべき今回位置ｒを前記(1)
式より求め、その前回値との差（１回微分値）を速度と
して、逆向き座標変換部６からマニピュレータの各関節
の回転角速度に変換して、コントローラ７に与えてい
る。ところが、マニピュレータの機構上、目標位置によ
っては各関節の回転角が一意に決まらない領域がある。
このような領域を特異点領域と呼び、特異点領域では逆
向き座標変換が不可能となるため、制御不能に陥ってし
まう。そこで従来は、あらかじめ目標位置が特異点領域
に入らないように目標軌道を与える、もしくは特異点領
域に入ったらマニピュレータの動作を停止するという方
法をとっている。
と、マニピュレータ１の各関節に取りつけられたエンコ
ーダ２の検出角度θを前向き座標変換部３により位置ｒ
に変換し、本来の目標位置ｒO および力センサ４からの
力フィードバツクＦexから、コンプライアンス制御部５
でマニピュレータ１が追従すべき今回位置ｒを前記(1)
式より求め、その前回値との差（１回微分値）を速度と
して、逆向き座標変換部６からマニピュレータの各関節
の回転角速度に変換して、コントローラ７に与えてい
る。ところが、マニピュレータの機構上、目標位置によ
っては各関節の回転角が一意に決まらない領域がある。
このような領域を特異点領域と呼び、特異点領域では逆
向き座標変換が不可能となるため、制御不能に陥ってし
まう。そこで従来は、あらかじめ目標位置が特異点領域
に入らないように目標軌道を与える、もしくは特異点領
域に入ったらマニピュレータの動作を停止するという方
法をとっている。
【０００３】
【発明が解決しようとする課題】従来の方法では、特異
点領域に入る度にマニピュレータが停止するので、作業
効率が悪く、特異点領域に入らないように作業空間を狭
くする必要がある。また、特異点領域に入りにくいよう
にマニピュレータを配置しなければならない。さらに、
特異点領域に入らないように操作者が注意しなければな
らないという問題があった。そこで本発明は、マニピュ
レータ手先点の目標軌道として特異点領域を通過するよ
うな軌道を与えた場合にも、マニピュレータを停止せし
めることなく、安定に動作させることによって、操作者
の負担を軽減し、効率良い作業を可能とするマニピュレ
ータのコンプライアンス制御装置を提供することを目的
とする。
点領域に入る度にマニピュレータが停止するので、作業
効率が悪く、特異点領域に入らないように作業空間を狭
くする必要がある。また、特異点領域に入りにくいよう
にマニピュレータを配置しなければならない。さらに、
特異点領域に入らないように操作者が注意しなければな
らないという問題があった。そこで本発明は、マニピュ
レータ手先点の目標軌道として特異点領域を通過するよ
うな軌道を与えた場合にも、マニピュレータを停止せし
めることなく、安定に動作させることによって、操作者
の負担を軽減し、効率良い作業を可能とするマニピュレ
ータのコンプライアンス制御装置を提供することを目的
とする。
【０００４】
【課題を解決するための手段】本発明は、マニピュレー
タの特異点領域の幾何形状情報を記憶する記憶手段と、
前記特異点領域の幾何形状情報とマニピュレータ手先点
の現在位置をもとに、マニピュレータ手先点が特異点領
域に近付けば近付くほど大きさが大きくなり、マニピュ
レータ手先点が特異点領域から遠ざかる向きを持つよう
な仮想外力ベクトルを求める手段と、手先点に設けられ
た力センサの出力する実際の外力ベクトルと前記仮想外
力ベクトルとの和である合成外力ベクトルを求める手段
と、前記合成外力ベクトルが手先点に加えられているも
のとして特性方程式を解くコンプライアンス制御手段と
を備えたことを特徴とするものである。
タの特異点領域の幾何形状情報を記憶する記憶手段と、
前記特異点領域の幾何形状情報とマニピュレータ手先点
の現在位置をもとに、マニピュレータ手先点が特異点領
域に近付けば近付くほど大きさが大きくなり、マニピュ
レータ手先点が特異点領域から遠ざかる向きを持つよう
な仮想外力ベクトルを求める手段と、手先点に設けられ
た力センサの出力する実際の外力ベクトルと前記仮想外
力ベクトルとの和である合成外力ベクトルを求める手段
と、前記合成外力ベクトルが手先点に加えられているも
のとして特性方程式を解くコンプライアンス制御手段と
を備えたことを特徴とするものである。
【０００５】
【作用】上記手段により、マニピュレータ手先点が特異
点に近付くに従って、マニピュレータ手先点に特異点領
域に近付けさせないような仮想外力が加わので、マニピ
ュレータ手先点が特異点領域に侵入することを防ぐこと
ができる。
点に近付くに従って、マニピュレータ手先点に特異点領
域に近付けさせないような仮想外力が加わので、マニピ
ュレータ手先点が特異点領域に侵入することを防ぐこと
ができる。
【０００６】
【実施例】以下、本発明の具体的実施例を説明する。図
１に本発明の制御ブロック図を示す。図２と異なる点
は、特異点情報記憶部８と仮想外力ベクトル計算部９
と、外力ベクトル加算器１０を設けたことにある。特異
点情報記憶部８はマニピュレータ１の特異点領域の幾何
形状情報を記憶するものである。仮想外力ベクトル計算
部９は、特異点領域から前記現在位置までの最短距離を
与える相対位置ベクトルを算出し、前記相対位置ベクト
ルと向きを同じくする単位ベクトルを算出し、前記相対
位置ベクトルの長さを変数とする関数を算出し、前記単
位ベクトルに前記関数値を乗じた仮想外力ベクトルを算
出することにより仮想外力ベクトルを求めるものであ
る。外力ベクトル加算器１０は、力センサの出力する実
際の外力ベクトルと前記仮想外力ベクトルとの和である
合成外力ベクトルを求めるものである。
１に本発明の制御ブロック図を示す。図２と異なる点
は、特異点情報記憶部８と仮想外力ベクトル計算部９
と、外力ベクトル加算器１０を設けたことにある。特異
点情報記憶部８はマニピュレータ１の特異点領域の幾何
形状情報を記憶するものである。仮想外力ベクトル計算
部９は、特異点領域から前記現在位置までの最短距離を
与える相対位置ベクトルを算出し、前記相対位置ベクト
ルと向きを同じくする単位ベクトルを算出し、前記相対
位置ベクトルの長さを変数とする関数を算出し、前記単
位ベクトルに前記関数値を乗じた仮想外力ベクトルを算
出することにより仮想外力ベクトルを求めるものであ
る。外力ベクトル加算器１０は、力センサの出力する実
際の外力ベクトルと前記仮想外力ベクトルとの和である
合成外力ベクトルを求めるものである。
【０００７】以下、本発明を図３に示す回転関節型３自
由度マニピュレータに適用した本実施例の動作を説明す
る。図３において、第１関節は地面に垂直な回転軸を有
し、第２、第３関節は地面に水平な回転軸を有する。第
２関節と第３関節の回転軸は並行である。L0はマニピュ
レータ基礎の長さであり、以下順にL1が第１リンクの、
L2が第２リンクの、l3が第３リンクの長さである。x-y-
z は手先点を記述する座標系である。図３に示すマニピ
ュレータにおいてマニピュレータ手先点の特異点領域
は、第１関節の回転軸上（図４(a) の点線）と、第２関
節を中心とした半径 L2 + L3の球面上（図４(b) の点
線）であり、直線 x = y = 0 (2) と、球面 x2 + y2 + (z - L1)2 = (L2 + L3)2 (3) である。前者を該マニピュレータの内部特異点領域と呼
び、後者を外部特異点領域と呼ぶ。このような特異点領
域はマニピュレータの構造により定まるもので、構造が
わかっていれば既知である。手先点の現在位置 (x, y,
z) において、内部特異点領域による仮想外力ベクトル
は、
由度マニピュレータに適用した本実施例の動作を説明す
る。図３において、第１関節は地面に垂直な回転軸を有
し、第２、第３関節は地面に水平な回転軸を有する。第
２関節と第３関節の回転軸は並行である。L0はマニピュ
レータ基礎の長さであり、以下順にL1が第１リンクの、
L2が第２リンクの、l3が第３リンクの長さである。x-y-
z は手先点を記述する座標系である。図３に示すマニピ
ュレータにおいてマニピュレータ手先点の特異点領域
は、第１関節の回転軸上（図４(a) の点線）と、第２関
節を中心とした半径 L2 + L3の球面上（図４(b) の点
線）であり、直線 x = y = 0 (2) と、球面 x2 + y2 + (z - L1)2 = (L2 + L3)2 (3) である。前者を該マニピュレータの内部特異点領域と呼
び、後者を外部特異点領域と呼ぶ。このような特異点領
域はマニピュレータの構造により定まるもので、構造が
わかっていれば既知である。手先点の現在位置 (x, y,
z) において、内部特異点領域による仮想外力ベクトル
は、
【０００８】
【数２】
【０００９】であり、外部特異点領域による仮想外力ベ
クトルは、
クトルは、
【数３】
【００１０】である。これらの和である
【００１１】
【数４】
【００１２】を仮想外力ベクトルとして手先点に働かせ
る。Fs' およびFs''は、それぞれ、 (x2+y2)1/2 (7) および L2 + L3 - (x2+y2+(L1-z)2)1/2 (8) を変数とする関数であり、以下にいくつか具体例を挙げ
る。 Fs(r) = A (r/σ) -n (9-1) Fs(r) = A ln((r+1)/r) (9-2) Fs(r) = A exp(-( αr-β/r)) (9-3) Fs(r) =∞ ( r≦σのとき) 、0 ( r ＞σのとき) (9-
4) ただし、A およびα、β、σは正の実数定数、n は自然
数定数である。このとき、特性方程式は次のようにな
る。
る。Fs' およびFs''は、それぞれ、 (x2+y2)1/2 (7) および L2 + L3 - (x2+y2+(L1-z)2)1/2 (8) を変数とする関数であり、以下にいくつか具体例を挙げ
る。 Fs(r) = A (r/σ) -n (9-1) Fs(r) = A ln((r+1)/r) (9-2) Fs(r) = A exp(-( αr-β/r)) (9-3) Fs(r) =∞ ( r≦σのとき) 、0 ( r ＞σのとき) (9-
4) ただし、A およびα、β、σは正の実数定数、n は自然
数定数である。このとき、特性方程式は次のようにな
る。
【００１３】
【数５】
【００１４】(1)'式を解いて得られるｒを、該マニピュ
レータの手先点のとるべき挙動として用いる。図５(a)
および図５(b) のような目標軌道が与えられた場合、実
際のマニピュレータ手先点は特異点領域に近付かないよ
うな軌道上を通過する。
レータの手先点のとるべき挙動として用いる。図５(a)
および図５(b) のような目標軌道が与えられた場合、実
際のマニピュレータ手先点は特異点領域に近付かないよ
うな軌道上を通過する。
【００１５】
【発明の効果】以上述べたように、本発明によれば、マ
ニピュレータの手先位置をコンプライアンス制御する際
に、マニピュレータ手先点の目標軌道として、特異点領
域もしくは特異点領域近傍を通るような目標軌道が与え
られた場合でも、実際のマニピュレータ手先点は特異点
領域に近付かない軌道上を通過するため、マニピュレー
タの動作を停止させることなく、安定に動作させること
によって、操作者の負担を軽減することができ、効率良
い作業が可能となる。
ニピュレータの手先位置をコンプライアンス制御する際
に、マニピュレータ手先点の目標軌道として、特異点領
域もしくは特異点領域近傍を通るような目標軌道が与え
られた場合でも、実際のマニピュレータ手先点は特異点
領域に近付かない軌道上を通過するため、マニピュレー
タの動作を停止させることなく、安定に動作させること
によって、操作者の負担を軽減することができ、効率良
い作業が可能となる。
【図１】本発明の実施例を示すブロック図
【図２】従来の技術を示すブロック図
【図３】実施例における回転関節型３自由度マニピュレ
ータ
ータ
【図４】特異点領域の説明図であり、(a) は内部特異点
領域、(b) は外部特異点領域を示す図
領域、(b) は外部特異点領域を示す図
【図５】目標軌道と実軌道の例を示す図であり、(a) は
内部特異点領域、(b) は外部特異点領域を示す図
内部特異点領域、(b) は外部特異点領域を示す図
１ マニピュレータ ２ エンコーダ ３ 前向き座標変換部 ４ 力センサ ５ コンプライアンス制御部 ６ 逆向き座標変換部 ７ コントローラ ８ 特異点情報記憶部 ９ 仮想外力ベクトル計算部 １０ 外力ベクトル加算器
Claims (3)
- 【請求項１】 複数の関節を有するマニピュレータと、
前記マニピュレータの各関節位置を検出する位置検出手
段と、前記マニピュレータに作用する外力ベクトルを検
出する力検出手段を有するマニピュレータのコンプライ
アンス制御装置において、 マニピュレータの特異点領域の幾何形状情報を記憶する
記憶手段と、 前記特異点領域の幾何形状情報とマニピュレータ手先点
の現在位置をもとに、マニピュレータ手先点が特異点領
域に近付けば近付くほど大きさが大きくなり、マニピュ
レータ手先点が特異点領域から遠ざかる向きを持つよう
な仮想外力ベクトルを求める手段と、 手先点に設けられた力センサの出力する実際の外力ベク
トルと前記仮想外力ベクトルとの和である合成外力ベク
トルを求める手段と、 前記合成外力ベクトルが手先点に加えられているものと
して特性方程式を解くコンプライアンス制御手段とを備
えたことを特徴とするマニピュレータのコンプライアン
ス制御装置。 - 【請求項２】 前記仮想外力ベクトルを求める手段は、
前記特異点領域から前記現在位置までの最短距離を与え
る相対位置ベクトルを算出し、前記相対位置ベクトルと
向きを同じくする単位ベクトルを算出し、前記相対位置
ベクトルの長さを変数とする関数を算出し、前記単位ベ
クトルに前記関数値を乗じた仮想外力ベクトルを算出す
ることにより仮想外力ベクトルを求めるものである請求
項１記載のマニピュレータのコンプライアンス制御装
置。 - 【請求項３】 前記コンプライアンス制御手段は、現在
位置と前記合成外力ベクトルに対して設定したコンプラ
イアンスモデルから目標位置を算出し、前回目標位置と
の差より目標速度を出力するものである請求項１または
２記載のマニピュレータのコンプライアンス制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP5287093A JPH0713642A (ja) | 1993-02-17 | 1993-02-17 | マニピュレータのコンプライアンス制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP5287093A JPH0713642A (ja) | 1993-02-17 | 1993-02-17 | マニピュレータのコンプライアンス制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH0713642A true JPH0713642A (ja) | 1995-01-17 |
Family
ID=12926917
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP5287093A Pending JPH0713642A (ja) | 1993-02-17 | 1993-02-17 | マニピュレータのコンプライアンス制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH0713642A (ja) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2016193776A (ja) * | 2015-03-31 | 2016-11-17 | ブラザー工業株式会社 | 制御システム |
| US9561815B2 (en) | 2013-07-22 | 2017-02-07 | Sejin Engineering Co., Ltd. | Cart with easy accommodation of handle by spring |
| WO2023079809A1 (ja) * | 2021-11-02 | 2023-05-11 | 株式会社日立製作所 | 軌道計画装置、軌道計画方法および軌道計画プログラム |
-
1993
- 1993-02-17 JP JP5287093A patent/JPH0713642A/ja active Pending
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9561815B2 (en) | 2013-07-22 | 2017-02-07 | Sejin Engineering Co., Ltd. | Cart with easy accommodation of handle by spring |
| JP2016193776A (ja) * | 2015-03-31 | 2016-11-17 | ブラザー工業株式会社 | 制御システム |
| WO2023079809A1 (ja) * | 2021-11-02 | 2023-05-11 | 株式会社日立製作所 | 軌道計画装置、軌道計画方法および軌道計画プログラム |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| JP3207728B2 (ja) | 冗長マニピュレータの制御方法 | |
| Hollerbach et al. | Wrist-partitioned inverse kinematic accelerations and manipulator dynamics | |
| JPH05197423A (ja) | 移動体の制御装置 | |
| JPS5840761B2 (ja) | 人間腕形マニピュレ−タの制御装置 | |
| KR100253898B1 (ko) | 한 평면내 다자유도 스카라형 로봇의 궤적 제어장치 및 궤적제어방법 및 한 평면내 다자유도 스카라형 로봇의 궤적 제어프로그램을 기록한 컴퓨터 판독 가능한 기록매체 | |
| JPH0713642A (ja) | マニピュレータのコンプライアンス制御装置 | |
| JPH0693209B2 (ja) | ロボツトの円弧補間姿勢制御装置 | |
| JP2739764B2 (ja) | ロボットの倣い速度制御方式 | |
| JPH06312392A (ja) | 多関節ロボットの制御装置 | |
| JPS643661Y2 (ja) | ||
| JPH0829517B2 (ja) | ロボツトの衝突回避制御方法 | |
| JP2615423B2 (ja) | マニピュレータの運動特性の評価および作業計画法 | |
| JP2737325B2 (ja) | ロボットの軌道生成方式 | |
| JPH0468646B2 (ja) | ||
| KR0155281B1 (ko) | 다관절 로보트의 직선보간방법 | |
| JP3086550B2 (ja) | 運動体の運動同定方法及び運動体の運動同定装置 | |
| JPS63289606A (ja) | 冗長自由度形ロボット | |
| JP2552271B2 (ja) | ロボツトの適応制御方法 | |
| JP2676721B2 (ja) | 多関節型ロボットの制御装置 | |
| JP2932443B2 (ja) | ロボットの円弧補間姿勢制御方法 | |
| JPH077300B2 (ja) | ロボット制御装置 | |
| JPS6160447B2 (ja) | ||
| JPH03262009A (ja) | 多自由度ロボットの制御方法 | |
| JP2914719B2 (ja) | 工業用ロボット | |
| JPH02269586A (ja) | マニピュレータ駆動制御装置 |