JPH0584677A - マニピユレータ制御装置 - Google Patents
マニピユレータ制御装置Info
- Publication number
- JPH0584677A JPH0584677A JP24377591A JP24377591A JPH0584677A JP H0584677 A JPH0584677 A JP H0584677A JP 24377591 A JP24377591 A JP 24377591A JP 24377591 A JP24377591 A JP 24377591A JP H0584677 A JPH0584677 A JP H0584677A
- Authority
- JP
- Japan
- Prior art keywords
- target
- joint
- angle
- axis
- tip
- 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
- Manipulator (AREA)
- Control Of Position Or Direction (AREA)
Abstract
(57)【要約】
【目的】 任意の目標位置・姿勢に対して各関節の急激
な角度変化なくして移行させることができるようにした
制御装置を得ること。 【構成】 一点で交わる３関節軸交点の目標位置を実現
する関節角度を計算する座標変換装置１１と、先端３軸
の根元側のロール軸座標変換装置１３と、先端３軸の２
つのロール軸の軸線が或る基準以上接近している場合
に、根元側のロール軸の目標角度を基準角度に接近する
ように修正するロール軸目標角度修正装置１４と、上記
根元側のロール軸の修正目標角度に対応して上記ロール
軸以外の先端２軸の目標関節角度を算出する先端関節軸
目標角度計算部１５と、各関節軸を目標関節角度に回動
制御する関節軸制御装置１６とを有する。
な角度変化なくして移行させることができるようにした
制御装置を得ること。 【構成】 一点で交わる３関節軸交点の目標位置を実現
する関節角度を計算する座標変換装置１１と、先端３軸
の根元側のロール軸座標変換装置１３と、先端３軸の２
つのロール軸の軸線が或る基準以上接近している場合
に、根元側のロール軸の目標角度を基準角度に接近する
ように修正するロール軸目標角度修正装置１４と、上記
根元側のロール軸の修正目標角度に対応して上記ロール
軸以外の先端２軸の目標関節角度を算出する先端関節軸
目標角度計算部１５と、各関節軸を目標関節角度に回動
制御する関節軸制御装置１６とを有する。
Description
【０００１】
【産業上の利用分野】本発明は、少なくとも６関節を有
し、先端の３関節軸が一点で交わるように構成されたマ
ニピュレータの制御装置に関する。
し、先端の３関節軸が一点で交わるように構成されたマ
ニピュレータの制御装置に関する。
【０００２】
【従来の技術】一般に、６関節軸を有し、先端の３関節
がロール・ピッチ・ロール構成でそれらの関節軸が一点
で交わるように構成されたマニピュレータが、工場内で
使用される産業用ロボットを中心として多く使われてい
る。
がロール・ピッチ・ロール構成でそれらの関節軸が一点
で交わるように構成されたマニピュレータが、工場内で
使用される産業用ロボットを中心として多く使われてい
る。
【０００３】図７は、上記６軸多関節、先端３軸がロー
ル・ピッチ・ロール構成で３軸が一点で交わるように構
成されたマニピュレータの一例を示す図であって、基体
１には第１のリンク２の基端が水平軸（第一軸）及び垂
直軸（第二軸）回りに回動可能に連結されており、その
第１のリンク２の先端には、第２のリンク３の基端がそ
の第２のリンク３の長手方向軸線と直交する軸線（第三
軸）及び上記第２のリンク３の長手方向軸線（第四軸）
の回りにそれぞれ回動可能に連結されている。そして、
上記第２のリンク３の先端には、上記第四軸に直交する
軸線（第五軸）及びその第五軸に直交する軸線（第六
軸）回りに回動可能にハンド部４が連結されている。
ル・ピッチ・ロール構成で３軸が一点で交わるように構
成されたマニピュレータの一例を示す図であって、基体
１には第１のリンク２の基端が水平軸（第一軸）及び垂
直軸（第二軸）回りに回動可能に連結されており、その
第１のリンク２の先端には、第２のリンク３の基端がそ
の第２のリンク３の長手方向軸線と直交する軸線（第三
軸）及び上記第２のリンク３の長手方向軸線（第四軸）
の回りにそれぞれ回動可能に連結されている。そして、
上記第２のリンク３の先端には、上記第四軸に直交する
軸線（第五軸）及びその第五軸に直交する軸線（第六
軸）回りに回動可能にハンド部４が連結されている。
【０００４】そこで、このようなマニピュレータは、６
関節を有するために、各関節における適宜回動によって
動作範囲内であれば３次元空間の任意の目標位置及び姿
勢を実現することができる。
関節を有するために、各関節における適宜回動によって
動作範囲内であれば３次元空間の任意の目標位置及び姿
勢を実現することができる。
【０００５】目標位置及び姿勢としては、マニピュレー
タ先端すなわちハンド部４の位置及び姿勢、或は先端３
関節軸（第四軸、第五軸、第六軸）交点の位置及び先端
姿勢を外部から指定される場合がある。しかして、この
マニピュレータの制御装置では、通常指定された目標位
置・姿勢を各関節角度に座標変換し、その後にそれらの
目標関節角度を実現するように各関節軸を制御する制御
系を構成しているが、特に後者の目標位置・姿勢の与え
方の場合には、マニピュレータを、基部側の３軸で目標
位置を、先端の３軸で目標姿勢を実現する３次元の位置
決め機構として扱われ、目標位置・姿勢から目標関節角
度への座標変換式は容易に導かれる。
タ先端すなわちハンド部４の位置及び姿勢、或は先端３
関節軸（第四軸、第五軸、第六軸）交点の位置及び先端
姿勢を外部から指定される場合がある。しかして、この
マニピュレータの制御装置では、通常指定された目標位
置・姿勢を各関節角度に座標変換し、その後にそれらの
目標関節角度を実現するように各関節軸を制御する制御
系を構成しているが、特に後者の目標位置・姿勢の与え
方の場合には、マニピュレータを、基部側の３軸で目標
位置を、先端の３軸で目標姿勢を実現する３次元の位置
決め機構として扱われ、目標位置・姿勢から目標関節角
度への座標変換式は容易に導かれる。
【０００６】
【発明が解決しようとする課題】ところが、上述の如く
先端３関節軸がロール・ピッチ・ロール構成の場合には
任意の姿勢を実現することはできるけれども下記のよう
な問題がある。
先端３関節軸がロール・ピッチ・ロール構成の場合には
任意の姿勢を実現することはできるけれども下記のよう
な問題がある。
【０００７】すなわち、まず、２つのロール軸線が一致
する姿勢になった場合、すなわち、図７においてリンク
３の軸線とハンドの軸線が一致する場合には、ヨー方向
の姿勢変化が不可能になるという特異姿勢になることが
ある。また、２つのロール軸線が完全に一致しない場合
にはヨー方向への姿勢変化は可能であるが、微妙なヨー
方向の姿勢変化を実現するために２つのロール軸に急激
な角度変化を与えなければならないことがあり、そのよ
うな目標関節角度に正確に追従させることが制御的に実
現できない場合がある。
する姿勢になった場合、すなわち、図７においてリンク
３の軸線とハンドの軸線が一致する場合には、ヨー方向
の姿勢変化が不可能になるという特異姿勢になることが
ある。また、２つのロール軸線が完全に一致しない場合
にはヨー方向への姿勢変化は可能であるが、微妙なヨー
方向の姿勢変化を実現するために２つのロール軸に急激
な角度変化を与えなければならないことがあり、そのよ
うな目標関節角度に正確に追従させることが制御的に実
現できない場合がある。
【０００８】また、ロール軸の可動範囲が±９０度程度
のものの場合には、目標先端姿勢によってはロール軸の
目標関節角度が＋９０度から−９０度へ或は−９０度か
ら＋９０度へと不連続的に変化する不連続点が存在す
る。ロール軸の可動範囲が±９０度でも任意の目標姿勢
を実現することは可能であるが、先端３軸で実現すべき
姿勢をロール・ピッチ・ロールで表現した場合に、ピッ
チ角度が０度の前後でロール軸の目標関節角度が不連続
的に変化する。
のものの場合には、目標先端姿勢によってはロール軸の
目標関節角度が＋９０度から−９０度へ或は−９０度か
ら＋９０度へと不連続的に変化する不連続点が存在す
る。ロール軸の可動範囲が±９０度でも任意の目標姿勢
を実現することは可能であるが、先端３軸で実現すべき
姿勢をロール・ピッチ・ロールで表現した場合に、ピッ
チ角度が０度の前後でロール軸の目標関節角度が不連続
的に変化する。
【０００９】このような特異姿勢或は不連続点はいずれ
も目標関節角度への移行中に急激な角度変化を引き起こ
すために好ましくなく、実際の運用においてはこれらの
特異姿勢・不連続領域を使用しないようなロボットの動
きをさせることによって回避しているが、ロボットの動
きが大きくなる場合には途中でこのような特異姿勢・不
連続領域に接近しないようなロボットの動きを決定する
にはかなりの注意が必要で、場合によっては試行錯誤を
繰り返さなければならない等の問題がある。
も目標関節角度への移行中に急激な角度変化を引き起こ
すために好ましくなく、実際の運用においてはこれらの
特異姿勢・不連続領域を使用しないようなロボットの動
きをさせることによって回避しているが、ロボットの動
きが大きくなる場合には途中でこのような特異姿勢・不
連続領域に接近しないようなロボットの動きを決定する
にはかなりの注意が必要で、場合によっては試行錯誤を
繰り返さなければならない等の問題がある。
【００１０】近年、ロボットの動作教示に計算機を使用
するオフラインティーチングシステムが広まりつつあ
り、ロボットの動作軌道を自動的に計算する軌道生成機
能を部分的に備えるものも増加しつつあるが、軌道の自
動計算を実現する際に、上記特異姿勢・不連続領域に接
近しない軌道を確実に作成することは不可能ではない
が、それでなくても多大な計算量を必要とする軌道生成
の負荷をさらに増大することとなり、ロボットの教示自
動化を難しくする原因ともなる等の問題がある。
するオフラインティーチングシステムが広まりつつあ
り、ロボットの動作軌道を自動的に計算する軌道生成機
能を部分的に備えるものも増加しつつあるが、軌道の自
動計算を実現する際に、上記特異姿勢・不連続領域に接
近しない軌道を確実に作成することは不可能ではない
が、それでなくても多大な計算量を必要とする軌道生成
の負荷をさらに増大することとなり、ロボットの教示自
動化を難しくする原因ともなる等の問題がある。
【００１１】本発明はこのような点に鑑み、マニピュレ
ータの可動範囲内であれば、任意の目標位置・姿勢に対
して各関節の急激な角度変化なくして移行させることが
できる各関節の目標関節角度を算出する座標変換装置を
設けたマニピュレータの制御装置を得ることを目的とす
る。
ータの可動範囲内であれば、任意の目標位置・姿勢に対
して各関節の急激な角度変化なくして移行させることが
できる各関節の目標関節角度を算出する座標変換装置を
設けたマニピュレータの制御装置を得ることを目的とす
る。
【００１２】
【課題を解決するための手段】第１の発明は、少なくと
も６関節を有し、先端の３関節がロール・ピッチ・ロー
ル構成でそれらの関節軸が一点で交わるようにしたマニ
ピュレータにおける、上記３関節軸の交点の３次元位置
及び先端姿勢を目標値に制御するマニピュレータ制御装
置において、上記一点で交わる３関節軸交点の目標位置
を実現する関節角度を計算する座標変換装置と、先端３
軸の根元側のロール軸座標変換装置と、先端３軸の２つ
のロール軸の軸線が或る基準以上接近している場合に、
２つのロール軸の軸線方向の差となるピッチ軸の関節角
度の値に従って、根元側のロール軸の目標角度を基準角
度に接近するするように修正するロール軸目標角度修正
装置と、上記根元側のロール軸の修正目標角度に対応し
てマニピュレータ先端部の目標姿勢を実現するように上
記ロール軸以外の先端２軸の目標関節角度を算出する先
端関節軸目標角度計算部と、各関節軸を目標関節角度に
回動制御する関節軸制御装置とを有することを特徴とす
る。
も６関節を有し、先端の３関節がロール・ピッチ・ロー
ル構成でそれらの関節軸が一点で交わるようにしたマニ
ピュレータにおける、上記３関節軸の交点の３次元位置
及び先端姿勢を目標値に制御するマニピュレータ制御装
置において、上記一点で交わる３関節軸交点の目標位置
を実現する関節角度を計算する座標変換装置と、先端３
軸の根元側のロール軸座標変換装置と、先端３軸の２つ
のロール軸の軸線が或る基準以上接近している場合に、
２つのロール軸の軸線方向の差となるピッチ軸の関節角
度の値に従って、根元側のロール軸の目標角度を基準角
度に接近するするように修正するロール軸目標角度修正
装置と、上記根元側のロール軸の修正目標角度に対応し
てマニピュレータ先端部の目標姿勢を実現するように上
記ロール軸以外の先端２軸の目標関節角度を算出する先
端関節軸目標角度計算部と、各関節軸を目標関節角度に
回動制御する関節軸制御装置とを有することを特徴とす
る。
【００１３】また、第２の発明は、７関節を有し、先端
の３関節がロール・ピッチ・ロールの構成でそれらの関
節軸が一点で交わるようにしたマニピュレータの制御装
置において、上記一点で交わる関節軸交点の目標位置を
実現する関節角度を計算する座標変換装置が、先端３関
節で実現している姿勢をロール・ピッチ・ヨーで表現し
たときのピッチの角度がＯ度の近傍に限って、上記目標
位置を実現しつつ、先端３関節軸交点でのリンク先端方
向と目標先端方向ができるだけ等しくなるような目標関
節角度を算出するようにしたことを特徴とする。
の３関節がロール・ピッチ・ロールの構成でそれらの関
節軸が一点で交わるようにしたマニピュレータの制御装
置において、上記一点で交わる関節軸交点の目標位置を
実現する関節角度を計算する座標変換装置が、先端３関
節で実現している姿勢をロール・ピッチ・ヨーで表現し
たときのピッチの角度がＯ度の近傍に限って、上記目標
位置を実現しつつ、先端３関節軸交点でのリンク先端方
向と目標先端方向ができるだけ等しくなるような目標関
節角度を算出するようにしたことを特徴とする。
【００１４】
【作用】第１の発明によれば、先端３軸の２つのロール
軸の軸線が基準以上接近している場合には、２つのロー
ル軸の軸線方向の差となるピッチ軸の関節角度の値に従
って根元側のロール軸の目標角度を基準角度に接近する
ように修正し、その修正したロール軸の目標角度に対応
してヨー方向を除く他の２方向の目標姿勢を実現する先
端２軸の目標関節角度を算出するため、たとえ先端の２
つのロール軸の軸線が一致する特異姿勢近傍状態にある
とき、目標姿勢がヨー方向に多少変化したとしても、先
端３軸の中の根元側のロール軸の目標関節角度は基準角
度近傍から急激には変化せず、また先端２軸の目標関節
角度の値は目標姿勢の値の変化に対して急激に変化する
ことがなく、先端３軸の角度が急激に変化することがな
い。
軸の軸線が基準以上接近している場合には、２つのロー
ル軸の軸線方向の差となるピッチ軸の関節角度の値に従
って根元側のロール軸の目標角度を基準角度に接近する
ように修正し、その修正したロール軸の目標角度に対応
してヨー方向を除く他の２方向の目標姿勢を実現する先
端２軸の目標関節角度を算出するため、たとえ先端の２
つのロール軸の軸線が一致する特異姿勢近傍状態にある
とき、目標姿勢がヨー方向に多少変化したとしても、先
端３軸の中の根元側のロール軸の目標関節角度は基準角
度近傍から急激には変化せず、また先端２軸の目標関節
角度の値は目標姿勢の値の変化に対して急激に変化する
ことがなく、先端３軸の角度が急激に変化することがな
い。
【００１５】また、第２の発明によれば、目標ピッチ角
度が或る基準角度例えば１０度以下になった場合には、
マニピュレータの冗長自由度を利用して、目標位置を実
現しながら先端３関節軸の交点での先端方向姿勢を先端
目標姿勢に一致させることによって、目標姿勢を実現す
るために先端３軸全てを使用する必要がなくなる。つま
り、先端３軸の中の根元側ロール軸の関節角度を基準角
度に一定に保っても先端姿勢を実現することができる。
度が或る基準角度例えば１０度以下になった場合には、
マニピュレータの冗長自由度を利用して、目標位置を実
現しながら先端３関節軸の交点での先端方向姿勢を先端
目標姿勢に一致させることによって、目標姿勢を実現す
るために先端３軸全てを使用する必要がなくなる。つま
り、先端３軸の中の根元側ロール軸の関節角度を基準角
度に一定に保っても先端姿勢を実現することができる。
【００１６】さらに、第１の発明と第２の発明とを組み
合わせることによって、ロール軸の可動範囲が±９０度
程度のマニピュレータにおいても、ロール軸の目標関節
角度を不連続に変化させることはなく、任意の目標姿勢
を実現することができる。
合わせることによって、ロール軸の可動範囲が±９０度
程度のマニピュレータにおいても、ロール軸の目標関節
角度を不連続に変化させることはなく、任意の目標姿勢
を実現することができる。
【００１７】
【実施例】図１は、図７で示したマニピュレータの制御
装置を示すブロック図であって、マニピュレータの先端
すなわちハンド部の目標位置が外部から与えられると、
マニピュレータの先端３関節軸の交点の目標位置が、１
〜３軸座標変換装置１１に入力され、そこでその目標位
置及び現在の関節角度をもとに１〜３軸の目標関節角度
への座標変換が行なわれる。
装置を示すブロック図であって、マニピュレータの先端
すなわちハンド部の目標位置が外部から与えられると、
マニピュレータの先端３関節軸の交点の目標位置が、１
〜３軸座標変換装置１１に入力され、そこでその目標位
置及び現在の関節角度をもとに１〜３軸の目標関節角度
への座標変換が行なわれる。
【００１８】一方、マニピュレータ先端の目標姿勢は先
端３関節目標角度変換装置１２の４軸座標変換装置１３
に入力され、そこで現在の関節角度等をもとに解析的に
目標姿勢を正確に実現するための第４軸の目標関節角度
が算出される。そして、その算出結果は４軸目標角度修
正部１４に入力される。
端３関節目標角度変換装置１２の４軸座標変換装置１３
に入力され、そこで現在の関節角度等をもとに解析的に
目標姿勢を正確に実現するための第４軸の目標関節角度
が算出される。そして、その算出結果は４軸目標角度修
正部１４に入力される。
【００１９】上記４軸目標角度修正部１４では現在の第
５軸の関節角度が所定基準値△以下、例えば３度以下か
どうかが判断され、第５軸の関節角度の絶対値が上記基
準値以下の場合には、４軸座標変換装置１３ですでに算
出されている第４軸の目標関節角度をそのままの値で出
力する。
５軸の関節角度が所定基準値△以下、例えば３度以下か
どうかが判断され、第５軸の関節角度の絶対値が上記基
準値以下の場合には、４軸座標変換装置１３ですでに算
出されている第４軸の目標関節角度をそのままの値で出
力する。
【００２０】一方、第５軸の関節角度の絶対値が上記基
準値△未満の場合には、４軸座標変換装置１３で算出さ
れた第４軸の目標関節角度の値が下記の計算式に従って
修正される。
準値△未満の場合には、４軸座標変換装置１３で算出さ
れた第４軸の目標関節角度の値が下記の計算式に従って
修正される。
【００２１】 θdn4 ＝θd4×ｇain 2 （１） ｇain ＝ｓｉｎ（｜θ5 ｜×π／（△・２）） θd4 ：４軸座標変換装置で算出された第４軸目標関節
角度 θ5 ：第５軸関節角度 △ ：４軸目標角度修正部で修正するか否かの判定を
行なう基準角度 θdn4 ：４軸目標角度修正部の出力となる修正第４軸目
標角度 （１）式により、５軸の関節角度の絶対値が基準値△付
近では、４軸の目標角度は目標姿勢→４軸座標変換装置
３で算出された値とほとんど等しく、特異姿勢であるθ
5 ＝０度付近に近づくにつれて４軸目標角度の値が０度
に近づく。θ5 ＝０度付近では、（１）式のθd4の値に
かかわらずθdn4 の値はほとんど０度になるので、たと
え目標姿勢のヨー角度の値が多少変動したためにθd4の
値が急激に変化することがあったとしてもθdn4 の値は
ほとんど変化しない。また、特異姿勢付近では４軸の角
度変化に対する先端姿勢のヨー角度の感度が小さくな
り、特に、特異姿勢であるθ5 ＝０度では、４軸の角度
をいくらにしてもヨー角度は０度のまま全く変化しなく
なる。従って、（１）式のように４軸目標角度の値を修
正しても先端姿勢の変化は小さい。
角度 θ5 ：第５軸関節角度 △ ：４軸目標角度修正部で修正するか否かの判定を
行なう基準角度 θdn4 ：４軸目標角度修正部の出力となる修正第４軸目
標角度 （１）式により、５軸の関節角度の絶対値が基準値△付
近では、４軸の目標角度は目標姿勢→４軸座標変換装置
３で算出された値とほとんど等しく、特異姿勢であるθ
5 ＝０度付近に近づくにつれて４軸目標角度の値が０度
に近づく。θ5 ＝０度付近では、（１）式のθd4の値に
かかわらずθdn4 の値はほとんど０度になるので、たと
え目標姿勢のヨー角度の値が多少変動したためにθd4の
値が急激に変化することがあったとしてもθdn4 の値は
ほとんど変化しない。また、特異姿勢付近では４軸の角
度変化に対する先端姿勢のヨー角度の感度が小さくな
り、特に、特異姿勢であるθ5 ＝０度では、４軸の角度
をいくらにしてもヨー角度は０度のまま全く変化しなく
なる。従って、（１）式のように４軸目標角度の値を修
正しても先端姿勢の変化は小さい。
【００２２】前記４軸座標変換装置１３で算出された第
４軸目標関節角度θd4或は上記修正された修正値θdn4
は５、６軸目標関節角度計算部１５に入力され、そこで
上記入力値、目標姿勢のうち残りのロール・ピッチの値
及びマニピュレータ各関節角度の値等によって解析的に
第５軸、第６軸の目標関節角度を算出し、第４軸の目標
関節角度及び１〜３軸座標変換装置１１で算出される第
１〜３軸の目標関節角度とともに、１〜６軸制御装置１
６に入力し、各関節の関節角度が目標角度に追従するよ
うに各関節を制御する。
４軸目標関節角度θd4或は上記修正された修正値θdn4
は５、６軸目標関節角度計算部１５に入力され、そこで
上記入力値、目標姿勢のうち残りのロール・ピッチの値
及びマニピュレータ各関節角度の値等によって解析的に
第５軸、第６軸の目標関節角度を算出し、第４軸の目標
関節角度及び１〜３軸座標変換装置１１で算出される第
１〜３軸の目標関節角度とともに、１〜６軸制御装置１
６に入力し、各関節の関節角度が目標角度に追従するよ
うに各関節を制御する。
【００２３】このようにして、マニピュレータの全ての
関節が急激に変化することもなく、目標位置・姿勢をほ
ぼ実現することができる。
関節が急激に変化することもなく、目標位置・姿勢をほ
ぼ実現することができる。
【００２４】図２、図３、図４は本発明により生成され
る第４軸、第５軸、第６軸の目標関節角度変化及び目標
姿勢から正確に解析的に第４軸、第５軸、第６軸の目標
関節角度を算出した結果を比較したものである。すなわ
ち、上記各図はマニピュレータ先端目標の姿勢の中で、
ロール、ヨー方向の姿勢を一定にしながらピッチ方向の
姿勢のみを−１０°から１０°まで変化させたときの、
本発明により算出した関節目標角度の値Ａと通常の解析
的な計算で算出した値Ｂとの比較をした図である。
る第４軸、第５軸、第６軸の目標関節角度変化及び目標
姿勢から正確に解析的に第４軸、第５軸、第６軸の目標
関節角度を算出した結果を比較したものである。すなわ
ち、上記各図はマニピュレータ先端目標の姿勢の中で、
ロール、ヨー方向の姿勢を一定にしながらピッチ方向の
姿勢のみを−１０°から１０°まで変化させたときの、
本発明により算出した関節目標角度の値Ａと通常の解析
的な計算で算出した値Ｂとの比較をした図である。
【００２５】また、図５は２つの方法により計算された
第４、第５、第６軸の目標関節角度により実現される先
端のヨー方向の姿勢の変化を示したものである。マニピ
ュレータは６軸多関節、先端３軸はロール・ピッチ・ロ
ール構成で一点で交わるものとする。４、６軸の可動範
囲は±１８０度とする。
第４、第５、第６軸の目標関節角度により実現される先
端のヨー方向の姿勢の変化を示したものである。マニピ
ュレータは６軸多関節、先端３軸はロール・ピッチ・ロ
ール構成で一点で交わるものとする。４、６軸の可動範
囲は±１８０度とする。
【００２６】計算条件は、マニピュレータの１、２、３
軸関節角度は０度で一定。目標姿勢の中のロール軸の角
度＝０度、ヨー＝１度で固定、ピッチを−１０度から１
０度まで変化させている。
軸関節角度は０度で一定。目標姿勢の中のロール軸の角
度＝０度、ヨー＝１度で固定、ピッチを−１０度から１
０度まで変化させている。
【００２７】図をみるとわかるように、ピッチが０度付
近でヨー方向に誤差を生じるものの、第４軸、第６軸の
角度変化量が解析的に算出した場合に比べてかなり小さ
くなっているのがわかる。関節の目標角度変化が急激な
場合、マニピュレータのアクチュエータ性能などによっ
ては実際のマニピュレータの動きが目標角度の変化に正
確に追従できなくなる場合もあるので、アクチュエータ
・コントローラ性能によっては実際のマニピュレータの
先端姿勢の目標値からの誤差は本発明の方が解析的に算
出した場合よりも小さくなることもある。また、マニピ
ュレータが７関節以上有し、先端３関節のロール・ピッ
チ・ロール構成で、目標位置を根元側４関節で実現し、
目標姿勢を先端３関節で実現するような場合には、特異
姿勢付近での先端ヨー方向姿勢を根元側４関節で実現す
ることにより、先端姿勢の誤差もなくすことが可能であ
る。
近でヨー方向に誤差を生じるものの、第４軸、第６軸の
角度変化量が解析的に算出した場合に比べてかなり小さ
くなっているのがわかる。関節の目標角度変化が急激な
場合、マニピュレータのアクチュエータ性能などによっ
ては実際のマニピュレータの動きが目標角度の変化に正
確に追従できなくなる場合もあるので、アクチュエータ
・コントローラ性能によっては実際のマニピュレータの
先端姿勢の目標値からの誤差は本発明の方が解析的に算
出した場合よりも小さくなることもある。また、マニピ
ュレータが７関節以上有し、先端３関節のロール・ピッ
チ・ロール構成で、目標位置を根元側４関節で実現し、
目標姿勢を先端３関節で実現するような場合には、特異
姿勢付近での先端ヨー方向姿勢を根元側４関節で実現す
ることにより、先端姿勢の誤差もなくすことが可能であ
る。
【００２８】図７は、第２の発明のマニピュレータ制御
装置を示すブロック図であり、マニピュレータが７関節
を有するものに適用するものである。
装置を示すブロック図であり、マニピュレータが７関節
を有するものに適用するものである。
【００２９】図７において、外部から与えられたマニピ
ュレータの先端３関節軸の交点の目標位置は、１〜４軸
座標変換装置２０に入力され、マニピュレータ先端の目
標姿勢は先端３関節目標角度変換装置２１に入力され
る。
ュレータの先端３関節軸の交点の目標位置は、１〜４軸
座標変換装置２０に入力され、マニピュレータ先端の目
標姿勢は先端３関節目標角度変換装置２１に入力され
る。
【００３０】１〜４軸座標変換装置２０では、まず先端
３関節軸の現在の関節角度から先端３関節のみで実現さ
れている姿勢を算出し、そのピッチ方向角度の値が基準
値△（例えば１０度）以下である場合には、座標変換に
際して、目標位置を実現するだけでは４関節の関節角度
に−自由度残ることを利用して、目標位置を満たしなが
ら先端３関節軸交点でのリンク先端方向姿勢と目標先端
姿勢ができるだけ等しくなるような目標関節角度を算出
するようにしてある。
３関節軸の現在の関節角度から先端３関節のみで実現さ
れている姿勢を算出し、そのピッチ方向角度の値が基準
値△（例えば１０度）以下である場合には、座標変換に
際して、目標位置を実現するだけでは４関節の関節角度
に−自由度残ることを利用して、目標位置を満たしなが
ら先端３関節軸交点でのリンク先端方向姿勢と目標先端
姿勢ができるだけ等しくなるような目標関節角度を算出
するようにしてある。
【００３１】すなわち、１〜４軸座標変換装置２０での
計算は以下のように行なわれる。
計算は以下のように行なわれる。
【００３２】今、マニピュレータの先端３軸交点位置を
ｘ、関節軸ベクトルをθ、としたときに、ｘとθの間の
関係は以下のように表現することができる。
ｘ、関節軸ベクトルをθ、としたときに、ｘとθの間の
関係は以下のように表現することができる。
【００３３】 ｘ＝ｆ（θ）＝（ｆ１，ｆ２，ｆ３）t （２） すると、ｘの微小変化△ｘと関節角度の微小変化△θの
間には以下の関係が成り立つ。
間には以下の関係が成り立つ。
【００３４】
【数１】 Ｊ（θ）はヤコビアン行列と呼ばれる。（３）式の△ｘ
と△θの関係を逆にすると以下の式により△ｘを与えた
ときの△θの値を計算することができる。
と△θの関係を逆にすると以下の式により△ｘを与えた
ときの△θの値を計算することができる。
【００３５】
【数２】 従って、（２）式により現在の関節角度から現在の先端
３関節軸交点を算出、先端３関節軸交点の目標位置との
差をとり△ｘを求め、（４）式の計算を行ない、現在の
関節角度に（４）式で算出された△θの値を加算するこ
とにより目標関節角度が求まる。△ｘの値が大きくなる
と、（４）式は正確には成り立たなくなるが、マニピュ
レータの制御装置においては、数ｍsec から数１０ｍse
c という短い周期で繰り返しこの計算を行なうので問題
はない。
３関節軸交点を算出、先端３関節軸交点の目標位置との
差をとり△ｘを求め、（４）式の計算を行ない、現在の
関節角度に（４）式で算出された△θの値を加算するこ
とにより目標関節角度が求まる。△ｘの値が大きくなる
と、（４）式は正確には成り立たなくなるが、マニピュ
レータの制御装置においては、数ｍsec から数１０ｍse
c という短い周期で繰り返しこの計算を行なうので問題
はない。
【００３６】また、（４）式におけるｋの値は、先端３
関節軸の現在の関節角度から、先端３関節のみで実現さ
れている姿勢を算出、そのピッチ軸方向角度の値が基準
値△（例えば１０度）以上である場合には、例えば
（４）式のｋベクトルの値を０にして計算すればよい。
ピッチ軸方向角度の値が基準値△未満の場合には、ｋベ
クトルを以下のようにして算出することにより目標位置
を満たしながら先端３関節軸交点でのリンク先端方向姿
勢と目標先端姿勢ができるだけ等しくなるような目標関
節角度を算出することができる。 １） 与えられた先端目標姿勢の値から、マニピュレー
タ先端での先端方向単位ベクトルｐ＝（ｐ1 ，ｐ2 ，ｐ
3 ）を算出する。 ２） マニピュレータ先端３軸の交点での先端方向単位
ベクトルを第１〜第４軸の関節角度θｉで表したものを
ｑ（θ１，θ２，θ３，θ４）＝（ｑ1 ，ｑ2 ，ｑ3 ）
としたときに、
関節軸の現在の関節角度から、先端３関節のみで実現さ
れている姿勢を算出、そのピッチ軸方向角度の値が基準
値△（例えば１０度）以上である場合には、例えば
（４）式のｋベクトルの値を０にして計算すればよい。
ピッチ軸方向角度の値が基準値△未満の場合には、ｋベ
クトルを以下のようにして算出することにより目標位置
を満たしながら先端３関節軸交点でのリンク先端方向姿
勢と目標先端姿勢ができるだけ等しくなるような目標関
節角度を算出することができる。 １） 与えられた先端目標姿勢の値から、マニピュレー
タ先端での先端方向単位ベクトルｐ＝（ｐ1 ，ｐ2 ，ｐ
3 ）を算出する。 ２） マニピュレータ先端３軸の交点での先端方向単位
ベクトルを第１〜第４軸の関節角度θｉで表したものを
ｑ（θ１，θ２，θ３，θ４）＝（ｑ1 ，ｑ2 ，ｑ3 ）
としたときに、
【００３７】
【数３】 のようにしてｋベクトルを求める。ただし、Ｌは、マニ
ピュレータ先端での先端方向単位ベクトルｐとマニピュ
レータ先端３軸の交点での先端方向単位ベクトルｑとの
距離であり、 Ｌ＝（ｐ1 −ｑ1 ）2 ＋（ｐ2 −ｑ2 ）2 ＋（ｐ3 −ｑ3 ）2 （６） のように表わされる。
ピュレータ先端での先端方向単位ベクトルｐとマニピュ
レータ先端３軸の交点での先端方向単位ベクトルｑとの
距離であり、 Ｌ＝（ｐ1 −ｑ1 ）2 ＋（ｐ2 −ｑ2 ）2 ＋（ｐ3 −ｑ3 ）2 （６） のように表わされる。
【００３８】しかして、（５）式により算出されたｋベ
クトルを用いて（４）式を計算し、１〜４軸の目標関節
角度を計算することにより、目標位置を満たしながらＬ
ができるだけ小さくなる目標関節角度、つまりマニピュ
レータ先端方向とマニピュレータ先端３軸の交点での先
端方向ができるだけ一致するように目標関節角度が算出
される。
クトルを用いて（４）式を計算し、１〜４軸の目標関節
角度を計算することにより、目標位置を満たしながらＬ
ができるだけ小さくなる目標関節角度、つまりマニピュ
レータ先端方向とマニピュレータ先端３軸の交点での先
端方向ができるだけ一致するように目標関節角度が算出
される。
【００３９】一方、先端３関節目標角度への変換装置２
１では、第１の発明と同様の処理により第５〜７軸の目
標関節角度を算出する。ただし、本実施例では、先端３
関節軸交点でのリンク先端方向姿勢と目標先端姿勢がほ
ぼ等しくなるように制御されるので、外部から与えられ
る目標姿勢の値にかかわらず、５〜７軸で実現すべき姿
勢変化のヨー方向成分はほぼ０となっており、第１実施
例のような目標先端姿勢との誤差は生じない。また、目
標位置→１〜４軸座標変換装置２０及び先端３関節目標
角度への変換装置２１で算出された１〜７軸の目標関節
角度の値は１〜７軸制御装置２５に入力され、各関節の
関節角度が目標関節角度に追従するように各関節が制御
される。このようにして、マニピュレータの全ての関節
が急激に変化することもなく目標位置姿勢を実現するこ
とができる。
１では、第１の発明と同様の処理により第５〜７軸の目
標関節角度を算出する。ただし、本実施例では、先端３
関節軸交点でのリンク先端方向姿勢と目標先端姿勢がほ
ぼ等しくなるように制御されるので、外部から与えられ
る目標姿勢の値にかかわらず、５〜７軸で実現すべき姿
勢変化のヨー方向成分はほぼ０となっており、第１実施
例のような目標先端姿勢との誤差は生じない。また、目
標位置→１〜４軸座標変換装置２０及び先端３関節目標
角度への変換装置２１で算出された１〜７軸の目標関節
角度の値は１〜７軸制御装置２５に入力され、各関節の
関節角度が目標関節角度に追従するように各関節が制御
される。このようにして、マニピュレータの全ての関節
が急激に変化することもなく目標位置姿勢を実現するこ
とができる。
【００４０】
【発明の効果】以上のように本発明によれば、先端３関
節軸の２つのロール軸が一致する特異姿勢や、先端３軸
で実現すべき姿勢をロール・ピッチ・ロールで表現した
場合に、ピッチ角度が０度付近でロール軸の目標関節角
度が急激に変化することなしに、連続な目標位置・姿勢
に対して急激に変化しない連続的な目標関節角度を算出
することができる。
節軸の２つのロール軸が一致する特異姿勢や、先端３軸
で実現すべき姿勢をロール・ピッチ・ロールで表現した
場合に、ピッチ角度が０度付近でロール軸の目標関節角
度が急激に変化することなしに、連続な目標位置・姿勢
に対して急激に変化しない連続的な目標関節角度を算出
することができる。
【図１】第１の発明におけるマニピュレータ制御装置の
ブロック図
ブロック図
【図２】本発明と通常の解析的計算による第４軸の目標
関節角度の比較を示す図
関節角度の比較を示す図
【図３】本発明と通常の解析的計算による第５軸の目標
関節角度の比較を示す図
関節角度の比較を示す図
【図４】本発明と通常の解析的計算による第６軸の目標
関節角度の比較を示す図
関節角度の比較を示す図
【図５】本発明により計算された目標関節角度による先
端ヨー方向姿勢変化を示す図
端ヨー方向姿勢変化を示す図
【図６】第２の発明におけるマニピュレータ制御装置の
ブロック図
ブロック図
【図７】６軸多関節、先端３軸がロール・ピッチ・ロー
ル構成で３軸が一点で交わるようにしたマニピュレータ
の概略構成を示す図。
ル構成で３軸が一点で交わるようにしたマニピュレータ
の概略構成を示す図。
１１ １〜３軸座標変換装置 １２ 先端３関節目標角度変換装置 １３ ４軸座標変換装置 １４ ４軸目標角度修正部 １５ ５，６軸目標関節角度計算部 １６ １〜６軸制御装置 ２０ １〜４軸座標変換装置 ２１ 先端３関節目標角度変換装置。
Claims (2)
- 【請求項１】少なくとも６関節を有し、先端の３関節が
ロール・ピッチ・ロール構成でそれらの関節軸が一点で
交わるようにしたマニピュレータにおける、上記３関節
軸の交点の３次元位置及び先端姿勢を目標値に制御する
マニピュレータ制御装置において、上記一点で交わる３
関節軸交点の目標位置を実現する関節角度を計算する座
標変換装置と、先端３軸の根元側のロール軸座標変換装
置と、先端３軸の２つのロール軸の軸線が或る基準以上
接近している場合に、２つのロール軸の軸線方向の差と
なるピッチ軸の関節角度の値に従って、根元側のロール
軸の目標角度を基準角度に接近するするように修正する
ロール軸目標角度修正装置と、上記根元側のロール軸の
修正目標角度に対応してマニピュレータ先端部の目標姿
勢を実現するように上記ロール軸以外の先端２軸の目標
関節角度を算出する先端関節軸目標角度計算部と、各関
節軸を目標関節角度に回動制御する関節軸制御装置とを
有することを特徴とする、マニピュレータ制御装置。 - 【請求項２】７関節を有し、先端の３関節がロール・ピ
ッチ・ロールの構成でそれらの関節軸が一点で交わるよ
うにしたマニピュレータの制御装置において、上記一点
で交わる関節軸交点の目標位置を実現する関節角度を計
算する座標変換装置が、先端３関節で実現している姿勢
をロール・ピッチ・ヨーで表現したときのピッチの角度
がＯ度の近傍にある場合に限って、上記目標位置を実現
しつつ、先端３関節軸交点でのリンク先端方向と目標先
端方向ができるだけ等しくなるような目標関節角度を算
出するようにしたことを特徴とする、マニピュレータ制
御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP24377591A JPH0584677A (ja) | 1991-09-24 | 1991-09-24 | マニピユレータ制御装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP24377591A JPH0584677A (ja) | 1991-09-24 | 1991-09-24 | マニピユレータ制御装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH0584677A true JPH0584677A (ja) | 1993-04-06 |
Family
ID=17108792
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP24377591A Pending JPH0584677A (ja) | 1991-09-24 | 1991-09-24 | マニピユレータ制御装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH0584677A (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116277010A (zh) * | 2023-03-28 | 2023-06-23 | 杭州唯精医疗机器人有限公司 | 机器人运动控制方法、电子设备及计算机可读存储介质 |
-
1991
- 1991-09-24 JP JP24377591A patent/JPH0584677A/ja active Pending
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116277010A (zh) * | 2023-03-28 | 2023-06-23 | 杭州唯精医疗机器人有限公司 | 机器人运动控制方法、电子设备及计算机可读存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US11667035B2 (en) | Path-modifying control system managing robot singularities | |
| US6845295B2 (en) | Method of controlling a robot through a singularity | |
| JP5114019B2 (ja) | エフェクタの軌道を制御するための方法 | |
| US5781705A (en) | Method and apparatus for controlling the motion of a redundancy manipulator | |
| JPH0144476B2 (ja) | ||
| JPH079606B2 (ja) | ロボット制御装置 | |
| KR100253898B1 (ko) | 한 평면내 다자유도 스카라형 로봇의 궤적 제어장치 및 궤적제어방법 및 한 평면내 다자유도 스카라형 로봇의 궤적 제어프로그램을 기록한 컴퓨터 판독 가능한 기록매체 | |
| CN109176487A (zh) | 一种联动关节段标定方法、系统、设备、存储介质 | |
| JP6652310B2 (ja) | ロボットシステム、ロボットアームの制御方法、プログラム、記録媒体、及び物品の製造方法 | |
| WO2018199035A1 (ja) | 多関節ロボットおよび多関節ロボットシステム | |
| JPH0693209B2 (ja) | ロボツトの円弧補間姿勢制御装置 | |
| JP2019093487A (ja) | ロボットの制御装置及びロボットの逆変換処理方法 | |
| CN116852363B (zh) | 连续体机械臂末端位姿控制方法、计算机设备及可读存储介质 | |
| JPH05177563A (ja) | マスタスレーブマニピュレータの制御方法 | |
| JPH0584677A (ja) | マニピユレータ制御装置 | |
| JPH035605B2 (ja) | ||
| CN116852397A (zh) | 负压理疗机器人理疗力度及理疗路径的自适应调节方法 | |
| JPH05228863A (ja) | マニピュレータ制御装置 | |
| JPH0647689A (ja) | 多関節形ロボットの制御方法 | |
| JPS60118478A (ja) | 関節形ロボツトの位置制御装置 | |
| JP3930956B2 (ja) | 一平面内多自由度スカラ型ロボットの軌跡制御装置および軌跡制御方法、並びに、一平面内多自由度スカラ型ロボットの軌跡制御プログラムを記録したコンピュータ読み取り可能な記録媒体 | |
| JP2676721B2 (ja) | 多関節型ロボットの制御装置 | |
| JP4647919B2 (ja) | 制御方法および制御装置 | |
| JP2947417B2 (ja) | ロボットの制御方法 | |
| CN118254150B (zh) | 7自由度冗余机械臂及其逆运动学求解方法 |