JPH07319530A - ３次元姿勢制御方法及びその装置 - Google Patents
３次元姿勢制御方法及びその装置Info
- Publication number
- JPH07319530A JPH07319530A JP10943094A JP10943094A JPH07319530A JP H07319530 A JPH07319530 A JP H07319530A JP 10943094 A JP10943094 A JP 10943094A JP 10943094 A JP10943094 A JP 10943094A JP H07319530 A JPH07319530 A JP H07319530A
- Authority
- JP
- Japan
- Prior art keywords
- posture
- parameter
- angular distance
- trajectory
- path
- 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
Abstract
(57)【要約】
【目標】 本発明は、所与の３次元姿勢の動作経路に沿
って被制御機構の３次元的な姿勢を制御する３次元姿勢
制御方法及びその装置を提供する。 【構成】 本発明による３次元姿勢制御方法は、姿勢パ
ラメータと名付けるパラメータを導入し、事前に生成さ
れた３次元姿勢の時間軌道から角速度軌道を生成するス
テップと、前記角速度軌道から角距離軌道を生成するス
テップと、該角距離軌道を、前記姿勢パラメータをパラ
メータとして角距離経路に変換するステップと、初期３
次元姿勢と、前記角距離経路とから姿勢パラメータをパ
ラメータとして姿勢経路を生成するステップと、目標と
する姿勢変化速度から姿勢パラメータ指令値を生成する
ステップと、前記姿勢経路と姿勢パラメータ指令値から
姿勢目標値を生成するステップと、該姿勢目標値により
被制御機構の姿勢を制御するステップとを含む。
って被制御機構の３次元的な姿勢を制御する３次元姿勢
制御方法及びその装置を提供する。 【構成】 本発明による３次元姿勢制御方法は、姿勢パ
ラメータと名付けるパラメータを導入し、事前に生成さ
れた３次元姿勢の時間軌道から角速度軌道を生成するス
テップと、前記角速度軌道から角距離軌道を生成するス
テップと、該角距離軌道を、前記姿勢パラメータをパラ
メータとして角距離経路に変換するステップと、初期３
次元姿勢と、前記角距離経路とから姿勢パラメータをパ
ラメータとして姿勢経路を生成するステップと、目標と
する姿勢変化速度から姿勢パラメータ指令値を生成する
ステップと、前記姿勢経路と姿勢パラメータ指令値から
姿勢目標値を生成するステップと、該姿勢目標値により
被制御機構の姿勢を制御するステップとを含む。
Description
【０００１】
【産業上の利用分野】本発明は、事前に与えられた３次
元姿勢の動作経路に沿ってロボット等の機械的な機構の
３次元的な姿勢を制御する３次元姿勢制御方法及びその
装置に関し、特に、姿勢経路を倣う動作速度として角速
度の大きさを任意に指定できる３次元姿勢制御方法及び
その装置に関するものである。
元姿勢の動作経路に沿ってロボット等の機械的な機構の
３次元的な姿勢を制御する３次元姿勢制御方法及びその
装置に関し、特に、姿勢経路を倣う動作速度として角速
度の大きさを任意に指定できる３次元姿勢制御方法及び
その装置に関するものである。
【０００２】
【従来の技術】例えばロボットに磨き作業のような加工
作業を行なわせる場合を考えると、ロボットの手先に装
着された工具の姿勢を予め指定された姿勢経路に沿って
動作をさせることが要求され、その上で加工状況に応じ
た動作の速度の調節を行なう必要がある。
作業を行なわせる場合を考えると、ロボットの手先に装
着された工具の姿勢を予め指定された姿勢経路に沿って
動作をさせることが要求され、その上で加工状況に応じ
た動作の速度の調節を行なう必要がある。
【０００３】このようなロボット等の機械的な機構を制
御する場合、姿勢の変化する経路を指示し、かつその経
路を倣う速度（以下経路速度）を調節したいことがあ
る。そこで姿勢の経路を以下に述べるように一つの変数
でパラメータ表現しておき、このパラメータの変化速度
を調節することにより、経路速度を調節する方法があ
る。
御する場合、姿勢の変化する経路を指示し、かつその経
路を倣う速度（以下経路速度）を調節したいことがあ
る。そこで姿勢の経路を以下に述べるように一つの変数
でパラメータ表現しておき、このパラメータの変化速度
を調節することにより、経路速度を調節する方法があ
る。
【０００４】しかし、従来の方法では、角速度などの、
運動の状態を表す物理的に意味のある量で姿勢に関する
経路速度の調節を行なうことができなかった。一般的
に、３次元の姿勢の表現方法は種々ある。
運動の状態を表す物理的に意味のある量で姿勢に関する
経路速度の調節を行なうことができなかった。一般的
に、３次元の姿勢の表現方法は種々ある。
【０００５】３次元の姿勢に関する自由度は３であるの
で、最低３つの変数を用いることにより、姿勢を一意に
示すことができる。例えば、姿勢を表現する変数とし
て、（１）ロール・ピッチ・ヨー角、（２）オイラー
角、（３）機構を動作させるアクチュエータの位置、
（４）姿勢を表すツール先端に張り付けられた直交座標
系の３つの軸を表す互いに直交する３つの単位ベクト
ル、などを用いる方法が一般的に知られている。
で、最低３つの変数を用いることにより、姿勢を一意に
示すことができる。例えば、姿勢を表現する変数とし
て、（１）ロール・ピッチ・ヨー角、（２）オイラー
角、（３）機構を動作させるアクチュエータの位置、
（４）姿勢を表すツール先端に張り付けられた直交座標
系の３つの軸を表す互いに直交する３つの単位ベクト
ル、などを用いる方法が一般的に知られている。
【０００６】これらの表現方法は相互に変換可能であ
る。ここでは、本発明でも用いる表現方法（４）につい
て説明しておく。図２に示すように、３次元の姿勢は次
のように表現することで一意に指示できる。
る。ここでは、本発明でも用いる表現方法（４）につい
て説明しておく。図２に示すように、３次元の姿勢は次
のように表現することで一意に指示できる。
【０００７】まず機構の外に固定した直交座標系（以
下、ワールド座標系：Σworld と呼ぶ）を考える。ま
た、機構の姿勢を制御したい部位（以下、部位と略す）
に仮想的の張り付けた適当な直交座標系（以下、姿勢座
標系：Σtoolと呼ぶ）を考える。
下、ワールド座標系：Σworld と呼ぶ）を考える。ま
た、機構の姿勢を制御したい部位（以下、部位と略す）
に仮想的の張り付けた適当な直交座標系（以下、姿勢座
標系：Σtoolと呼ぶ）を考える。
【０００８】姿勢座標系の各軸の向きを表す３つの単位
ベクトルをｅx 、ｅy 、ｅz とし、単位ベクトルの成分
をワールド座標系で表し、 ｅx ＝［ｅx1，ｅx2，ｅx3］T ， ｅy ＝［ｅy1，ｅy2，ｅy3］T ， ｅz ＝［ｅz1，ｅz2，ｅz3］T とすると、上記の９個の変数ｅx1，…ｅz3で姿勢が決定
できる。
ベクトルをｅx 、ｅy 、ｅz とし、単位ベクトルの成分
をワールド座標系で表し、 ｅx ＝［ｅx1，ｅx2，ｅx3］T ， ｅy ＝［ｅy1，ｅy2，ｅy3］T ， ｅz ＝［ｅz1，ｅz2，ｅz3］T とすると、上記の９個の変数ｅx1，…ｅz3で姿勢が決定
できる。
【０００９】ただし、３次元の姿勢は実際には３自由度
しか持たないので、各変数には次の従属関係がある。 ｜ｅx ｜＝｜ｅy ｜＝｜ｅz ｜＝１，ｅx ×ｅy ＝ｅz …（１） 以上より姿勢をＴ＝［ｅx ，ｅy ，ｅz ］という３行３
列の行列（以下、姿勢行列と呼ぶ）で表現する。
しか持たないので、各変数には次の従属関係がある。 ｜ｅx ｜＝｜ｅy ｜＝｜ｅz ｜＝１，ｅx ×ｅy ＝ｅz …（１） 以上より姿勢をＴ＝［ｅx ，ｅy ，ｅz ］という３行３
列の行列（以下、姿勢行列と呼ぶ）で表現する。
【００１０】この方法は３つの自由度を９個の冗長な数
の変数で表現するため、各変数に従属関係があるという
複雑さはあるが、部位に固定された座標軸の方向を直接
表現するという意味で直観的に理解がしやすいという特
徴をもつ。
の変数で表現するため、各変数に従属関係があるという
複雑さはあるが、部位に固定された座標軸の方向を直接
表現するという意味で直観的に理解がしやすいという特
徴をもつ。
【００１１】従来、経路をパラメータ表現する方法とし
ては、上記の例で示したような、姿勢を一意に指示可能
な適切な複数の変数で姿勢を表現し、各変数を共通な一
つの変数をパラメータとした関数で記述することのよっ
て、姿勢の経路をパラメータ表現する方法が用いられて
いる。
ては、上記の例で示したような、姿勢を一意に指示可能
な適切な複数の変数で姿勢を表現し、各変数を共通な一
つの変数をパラメータとした関数で記述することのよっ
て、姿勢の経路をパラメータ表現する方法が用いられて
いる。
【００１２】例えば、上記表現方法（１）の場合の例に
とると、ロール・ピッチ・ヨー角をそれぞれθ、φ、ψ
とすれば、それぞれをパラメータ変数ｓの連続関数とし
てθ(s) 、φ(s) 、ψ(s) と記述する。
とると、ロール・ピッチ・ヨー角をそれぞれθ、φ、ψ
とすれば、それぞれをパラメータ変数ｓの連続関数とし
てθ(s) 、φ(s) 、ψ(s) と記述する。
【００１３】パラメータ変数ｓを指定してやることによ
りθ、φ、ψが決まり姿勢が一意に決定される。パラメ
ータｓを変化させることにより、θ、φ、ψが変化しそ
の軌跡が姿勢経路となる。
りθ、φ、ψが決まり姿勢が一意に決定される。パラメ
ータｓを変化させることにより、θ、φ、ψが変化しそ
の軌跡が姿勢経路となる。
【００１４】すなわち、時間的にｓを変化させることに
よって姿勢が経路上を移動することになる。この時間的
にｓを変化させる割合が経路速度となる。
よって姿勢が経路上を移動することになる。この時間的
にｓを変化させる割合が経路速度となる。
【００１５】
【発明が解決しようとする課題】しかしながら、以上の
従来の技術では、姿勢の経路を姿勢の位置や角速度とい
った運動の状態量を表す物理的に意味を持ったパラメー
タで表現することができず、その結果、以下の例で示す
ように、運動の状態量を表す物理的な意味を持つ量で経
路速度を指定することができないという欠点を有してい
た。
従来の技術では、姿勢の経路を姿勢の位置や角速度とい
った運動の状態量を表す物理的に意味を持ったパラメー
タで表現することができず、その結果、以下の例で示す
ように、運動の状態量を表す物理的な意味を持つ量で経
路速度を指定することができないという欠点を有してい
た。
【００１６】例えば、姿勢の表現方法として上記表現方
法（１）もしくは（２）の変数表示を用いた場合、ロー
ル・ピッチ・ヨー角あるいはオイラー角といった各変数
が物理的な意味をあらわさないため、これらの変数をパ
ラメータで表現してもそのパラメータに物理的な意味を
もたせることができない。
法（１）もしくは（２）の変数表示を用いた場合、ロー
ル・ピッチ・ヨー角あるいはオイラー角といった各変数
が物理的な意味をあらわさないため、これらの変数をパ
ラメータで表現してもそのパラメータに物理的な意味を
もたせることができない。
【００１７】また、上記表現方法（３）を用いた場合、
アクチュエータの位置はアクチュエータレベルでは位置
の次元の物理的意味を持つが、注目すべき部位の姿勢お
よびその変化速度との関係は機構に依存し、アクチュエ
ータの位置指令は注目している部位においては必ずしも
物理的意味が明確ではない。
アクチュエータの位置はアクチュエータレベルでは位置
の次元の物理的意味を持つが、注目すべき部位の姿勢お
よびその変化速度との関係は機構に依存し、アクチュエ
ータの位置指令は注目している部位においては必ずしも
物理的意味が明確ではない。
【００１８】一方、上記表現方法（４）を用いると３つ
の単位ベクトルｅx 、ｅy 、ｅz が部位の姿勢を直接表
し、また、各ベクトルの角速度ベクトルが部位の姿勢の
角速度ベクトルとなり、姿勢を決定する変数として物理
的な意味を考えることができる。
の単位ベクトルｅx 、ｅy 、ｅz が部位の姿勢を直接表
し、また、各ベクトルの角速度ベクトルが部位の姿勢の
角速度ベクトルとなり、姿勢を決定する変数として物理
的な意味を考えることができる。
【００１９】しかし、この表現方法（４）では、ｅx1，
…，ｅx3の９個の変数が表面的に現れるが、本来姿勢を
表すために３つの変数が必要十分であるため、上述の９
個の変数には（１）式の従属関係がある。
…，ｅx3の９個の変数が表面的に現れるが、本来姿勢を
表すために３つの変数が必要十分であるため、上述の９
個の変数には（１）式の従属関係がある。
【００２０】そのため、ｅx1，…，ｅx3の各変数を、物
理的な意味のある一つのパラメータで表すことは困難で
ある。このような理由から表現方法（４）を用いた場合
でも各変数を物理的な意味のあるパラメータで表す方法
は提案されていなかった。
理的な意味のある一つのパラメータで表すことは困難で
ある。このような理由から表現方法（４）を用いた場合
でも各変数を物理的な意味のあるパラメータで表す方法
は提案されていなかった。
【００２１】そこで、本発明は以上のような点に鑑みて
なされたもので、運動の状態を表す物理的な意味をもつ
パラメータで経路速度を指定することができないという
問題点を解決したもので、姿勢を表現する変数としては
前記表現方法（４）を用いるが、各変数を、その時間微
分値が角速度の大きさを表すという意味では物理的な意
味を持つ姿勢パラメータで表現することにより、姿勢変
化の角速度の大きさで姿勢の経路速度の調節を行なうこ
とができる３次元姿勢制御方法及びその装置を提供する
ことを目的とする。
なされたもので、運動の状態を表す物理的な意味をもつ
パラメータで経路速度を指定することができないという
問題点を解決したもので、姿勢を表現する変数としては
前記表現方法（４）を用いるが、各変数を、その時間微
分値が角速度の大きさを表すという意味では物理的な意
味を持つ姿勢パラメータで表現することにより、姿勢変
化の角速度の大きさで姿勢の経路速度の調節を行なうこ
とができる３次元姿勢制御方法及びその装置を提供する
ことを目的とする。
【００２２】
【課題を解決するための手段】本発明によると、上記課
題を解決するために、角速度ベクトルを時間に関して積
分した量である角距離ベクトルと、この角距離ベクトル
の空間上での距離を表す角距離および、角距離空間上で
の経路に沿う道程を表し、その時間微分が姿勢変化の角
速度の大きさを表す姿勢パラメータと名付けるパラメー
タを導入し、事前に生成された３次元姿勢の時間軌道
（以下、時間軌道を軌道と略す）から角速度軌道を生成
するステップと、前記角速度軌道から、角速度軌道を時
間について積分した角距離軌道を生成するステップと、
前記角距離軌道を、前記姿勢パラメータをパラメータと
して前記角距離ベクトルの空間上での時間に依存しない
空間的な経路として表した角距離経路に変換するステッ
プと、初期３次元姿勢と、前記角距離経路とから、姿勢
パラメータをパラメータとして空間的な３次元姿勢の経
路を表した姿勢経路を生成するステップと、目標とする
姿勢変化速度から姿勢パラメータ指令値を生成するステ
ップと、前記姿勢経路と姿勢パラメータ指令値から姿勢
目標値を生成するステップと、前記姿勢目標値に従って
被制御機構の姿勢を制御するステップとを具備してなる
ことを特徴とする３次元姿勢制御方法が提供される。
題を解決するために、角速度ベクトルを時間に関して積
分した量である角距離ベクトルと、この角距離ベクトル
の空間上での距離を表す角距離および、角距離空間上で
の経路に沿う道程を表し、その時間微分が姿勢変化の角
速度の大きさを表す姿勢パラメータと名付けるパラメー
タを導入し、事前に生成された３次元姿勢の時間軌道
（以下、時間軌道を軌道と略す）から角速度軌道を生成
するステップと、前記角速度軌道から、角速度軌道を時
間について積分した角距離軌道を生成するステップと、
前記角距離軌道を、前記姿勢パラメータをパラメータと
して前記角距離ベクトルの空間上での時間に依存しない
空間的な経路として表した角距離経路に変換するステッ
プと、初期３次元姿勢と、前記角距離経路とから、姿勢
パラメータをパラメータとして空間的な３次元姿勢の経
路を表した姿勢経路を生成するステップと、目標とする
姿勢変化速度から姿勢パラメータ指令値を生成するステ
ップと、前記姿勢経路と姿勢パラメータ指令値から姿勢
目標値を生成するステップと、前記姿勢目標値に従って
被制御機構の姿勢を制御するステップとを具備してなる
ことを特徴とする３次元姿勢制御方法が提供される。
【００２３】また、本発明によると、上記課題を解決す
るために、角速度ベクトルを時間に関して積分した量で
ある角距離ベクトルと、この角距離ベクトルの空間上で
の距離を表す角距離と、角距離空間上での経路に沿う道
程を表し、その時間微分が姿勢変化の角速度の大きさを
表す姿勢パラメータと名付けるパラメータを導入し、事
前に生成された３次元姿勢の時間軌道から角速度軌道を
生成する角速度軌道生成部と、該、角速度軌道から角速
度軌道を時間について積分した角距離軌道を生成する角
距離軌道生成部と、該、角軌道を、前記姿勢パラメータ
をパラメータとして、前記、角距離ベクトルの空間上で
の時間に依存しない空間的な経路として表した角距離経
路に変換する角距離経路生成部と、初期３次元姿勢と、
該、角距離経路とから、姿勢パラメータをパラメータと
して空間的な３次元姿勢の経路を表した姿勢経路を生成
する姿勢経路生成部と、目標とする姿勢変化速度から姿
勢パラメータ指令値を生成する姿勢パラメータ決定部
と、該、姿勢経路と姿勢パラメータ指令値から姿勢目標
値を生成する姿勢目標値生成部と、該、姿勢目標値に従
って被制御機構の姿勢を制御する機構制御部と、からな
る３次元姿勢制御装置が提供される。
るために、角速度ベクトルを時間に関して積分した量で
ある角距離ベクトルと、この角距離ベクトルの空間上で
の距離を表す角距離と、角距離空間上での経路に沿う道
程を表し、その時間微分が姿勢変化の角速度の大きさを
表す姿勢パラメータと名付けるパラメータを導入し、事
前に生成された３次元姿勢の時間軌道から角速度軌道を
生成する角速度軌道生成部と、該、角速度軌道から角速
度軌道を時間について積分した角距離軌道を生成する角
距離軌道生成部と、該、角軌道を、前記姿勢パラメータ
をパラメータとして、前記、角距離ベクトルの空間上で
の時間に依存しない空間的な経路として表した角距離経
路に変換する角距離経路生成部と、初期３次元姿勢と、
該、角距離経路とから、姿勢パラメータをパラメータと
して空間的な３次元姿勢の経路を表した姿勢経路を生成
する姿勢経路生成部と、目標とする姿勢変化速度から姿
勢パラメータ指令値を生成する姿勢パラメータ決定部
と、該、姿勢経路と姿勢パラメータ指令値から姿勢目標
値を生成する姿勢目標値生成部と、該、姿勢目標値に従
って被制御機構の姿勢を制御する機構制御部と、からな
る３次元姿勢制御装置が提供される。
【００２４】
【作用】以上のような本発明の解決手段による３次元姿
勢制御方法及びその装置は、上記の構成をとることによ
り、事前に与えられた一つの姿勢軌道から、姿勢軌道の
軌跡である空間経路の、姿勢パラメータを用いたパラメ
ータ表現が求められ、姿勢パラメータを変化させること
により、姿勢経路にそって３次元姿勢を制御することが
できる。
勢制御方法及びその装置は、上記の構成をとることによ
り、事前に与えられた一つの姿勢軌道から、姿勢軌道の
軌跡である空間経路の、姿勢パラメータを用いたパラメ
ータ表現が求められ、姿勢パラメータを変化させること
により、姿勢経路にそって３次元姿勢を制御することが
できる。
【００２５】さらに姿勢パラメータは、その時間微分が
角速度の大きさを表すことから、姿勢パラメータの変化
速度を調節することにより、姿勢変化の角速度を指定す
ることができる。
角速度の大きさを表すことから、姿勢パラメータの変化
速度を調節することにより、姿勢変化の角速度を指定す
ることができる。
【００２６】
【実施例】以下、本発明について実施例により詳細に説
明する。まず、与えられた一つの姿勢軌道から姿勢経路
のパラメータ表現を得るための手順について説明する。
明する。まず、与えられた一つの姿勢軌道から姿勢経路
のパラメータ表現を得るための手順について説明する。
【００２７】以下では、変数の変化を時間をパラメータ
として表現したものを軌道と呼び、軌道が描く空間的な
軌跡を経路と呼ぶ。また、姿勢を姿勢行列Ｔ＝［ｅx ，
ｅy ，ｅz ］で表現する姿勢軌道Ｔ（t)と角速度軌道ω
(t) には次式の関係がある。
として表現したものを軌道と呼び、軌道が描く空間的な
軌跡を経路と呼ぶ。また、姿勢を姿勢行列Ｔ＝［ｅx ，
ｅy ，ｅz ］で表現する姿勢軌道Ｔ（t)と角速度軌道ω
(t) には次式の関係がある。
【００２８】
【数１】
【００２９】ここで、Ｔ0 は、初期時刻ｔ＝０における
初期姿勢Ｔ(0) ＝Ｔ0 である。ただし、［ *×］はベク
トル積演算と等価な行列であり、ある３次元ベクトルｖ
がｖ＝［ｖ1 ，ｖ2 ，ｖ3 ］T と成分表示されるとき
初期姿勢Ｔ(0) ＝Ｔ0 である。ただし、［ *×］はベク
トル積演算と等価な行列であり、ある３次元ベクトルｖ
がｖ＝［ｖ1 ，ｖ2 ，ｖ3 ］T と成分表示されるとき
【００３０】
【数２】
【００３１】で表される。角速度ベクトルを時間に関し
て積分したベクトルを角距離ベクトル、角距離ベクトル
の空間を角距離空間と名付ける。
て積分したベクトルを角距離ベクトル、角距離ベクトル
の空間を角距離空間と名付ける。
【００３２】また、角距離空間における２点間の距離を
角距離と名付ける。ここで、角速度の軌道ω(t) が与え
られた時、ω(t) を時間に関して積分したものをξ(t)
とする。すなわち、
角距離と名付ける。ここで、角速度の軌道ω(t) が与え
られた時、ω(t) を時間に関して積分したものをξ(t)
とする。すなわち、
【００３３】
【数３】
【００３４】すると、ξ(t) は角距離空間においてある
軌道を描くことになる。この軌道を角距離軌道ξ(t) と
呼びその角距離空間における軌跡を角距離経路と呼ぶ。
次式で表されるσは角距離空間における角距離経路に沿
う道程を表し、σを姿勢パラメータと呼ぶ。
軌道を描くことになる。この軌道を角距離軌道ξ(t) と
呼びその角距離空間における軌跡を角距離経路と呼ぶ。
次式で表されるσは角距離空間における角距離経路に沿
う道程を表し、σを姿勢パラメータと呼ぶ。
【００３５】
【数４】
【００３６】すなわち、σが時間によって変化する時、
σの時間微分はその瞬間における姿勢変化の角速度の大
きさを表す。角距離空間における角距離経路を上記σを
用いて表現すると
σの時間微分はその瞬間における姿勢変化の角速度の大
きさを表す。角距離空間における角距離経路を上記σを
用いて表現すると
【００３７】
【数５】
【００３８】となる。またξ(t) の定義より、
【００３９】
【数６】
【００４０】の関係があり、これを用いて（２）式の左
辺の変数をｔからσに変更すると
辺の変数をｔからσに変更すると
【００４１】
【数７】
【００４２】となる。この（７）式は、姿勢経路がσを
パラメータとして時間に依存しない空間経路としてあら
わされたことを意味する。
パラメータとして時間に依存しない空間経路としてあら
わされたことを意味する。
【００４３】ここで、（７）式は非線形な積分方程式で
あり、（７）式を解析的に解いて求めるのは難しい。し
かし、十分細かい間隔Δσでσを離散化し、次式ような
漸化的な数値積分により数値的に求めることができる。
あり、（７）式を解析的に解いて求めるのは難しい。し
かし、十分細かい間隔Δσでσを離散化し、次式ような
漸化的な数値積分により数値的に求めることができる。
【００４４】
【数８】
【００４５】ここでＩは単位行列である。そして、
（８）式におけるσn-1 ＜σ＜σn （ｎ＝１，２，…）
の離散点間で適切に補間することにより、連続関数とし
て（７）式の空間経路を求めることができる。
（８）式におけるσn-1 ＜σ＜σn （ｎ＝１，２，…）
の離散点間で適切に補間することにより、連続関数とし
て（７）式の空間経路を求めることができる。
【００４６】また、（５）式よりσの変化速度は姿勢の
変化する角速度の大きさと一致するので、σの変化速度
を指定することにより角速度の大きさで姿勢の経路速度
を調節することができる。
変化する角速度の大きさと一致するので、σの変化速度
を指定することにより角速度の大きさで姿勢の経路速度
を調節することができる。
【００４７】以上の手順に基づき、図１に示す各構成部
の処理内容を説明する。一般に３次元姿勢の経路はティ
ーチング・データやＣＡＤデータ等を用いて、時系列デ
ータとして作成されるので、この時系列データから、３
次元姿勢経路をならう一つの姿勢軌道ａもしくは、初期
姿勢ｂと角速度軌道ｃが作成できる。
の処理内容を説明する。一般に３次元姿勢の経路はティ
ーチング・データやＣＡＤデータ等を用いて、時系列デ
ータとして作成されるので、この時系列データから、３
次元姿勢経路をならう一つの姿勢軌道ａもしくは、初期
姿勢ｂと角速度軌道ｃが作成できる。
【００４８】この姿勢軌道ａもしくは、初期姿勢ｂと角
速度軌道ｃはあらかじめ何らかの方法で作成されている
ものとする。姿勢軌道ａ：Ｔ(t) が与えられ、角速度軌
道ｃ：ω(t) が与えられない場合は角速度軌道生成部１
が（３）式に基づいて姿勢軌道ａ：Ｔ(t) から角速度軌
道ｃ：ω(t) を生成する。
速度軌道ｃはあらかじめ何らかの方法で作成されている
ものとする。姿勢軌道ａ：Ｔ(t) が与えられ、角速度軌
道ｃ：ω(t) が与えられない場合は角速度軌道生成部１
が（３）式に基づいて姿勢軌道ａ：Ｔ(t) から角速度軌
道ｃ：ω(t) を生成する。
【００４９】角距離軌道生成部２は事前に与えられる
か、もしくは、角速度軌道生成部１で生成された角速度
軌道ｃ：ω(t) から、（４）式に基づいて角距離軌道
ｄ：ξ(t）を生成する。
か、もしくは、角速度軌道生成部１で生成された角速度
軌道ｃ：ω(t) から、（４）式に基づいて角距離軌道
ｄ：ξ(t）を生成する。
【００５０】角距離経路生成部３は角距離空間における
角距離軌道ｄの軌跡を、角距離パラメータｈをパラメー
タとして角距離経路ｅとして求める。姿勢経路生成部４
は角距離経路生成部３により得られた角距離経路ｅと事
前に与えられている初期姿勢ｂ：Ｔ0 から、（７）式も
しくは（８）式に基づいて姿勢経路ｆを求める。
角距離軌道ｄの軌跡を、角距離パラメータｈをパラメー
タとして角距離経路ｅとして求める。姿勢経路生成部４
は角距離経路生成部３により得られた角距離経路ｅと事
前に与えられている初期姿勢ｂ：Ｔ0 から、（７）式も
しくは（８）式に基づいて姿勢経路ｆを求める。
【００５１】姿勢パラメータ決定部５は、何らかの手段
で設定された姿勢経路速度ｇを入力とし、これを積分す
ることにより姿勢パラメータ指令値ｈ：σを求める。こ
こで、姿勢パラメータ指令値ｈ：σの変化速度ｄσ／ｄ
ｔは、姿勢の変化する角速度に一致する。
で設定された姿勢経路速度ｇを入力とし、これを積分す
ることにより姿勢パラメータ指令値ｈ：σを求める。こ
こで、姿勢パラメータ指令値ｈ：σの変化速度ｄσ／ｄ
ｔは、姿勢の変化する角速度に一致する。
【００５２】姿勢目標値生成部６は、姿勢パラメータ決
定部５で決定された姿勢パラメータ指令値ｈ：σと、姿
勢経路生成部４で求められた姿勢経路ｆから、姿勢目標
値ｉを生成する。
定部５で決定された姿勢パラメータ指令値ｈ：σと、姿
勢経路生成部４で求められた姿勢経路ｆから、姿勢目標
値ｉを生成する。
【００５３】機構制御部７は姿勢目標値ｉに追従するよ
うに機構を制御する。このようにして、事前に与えられ
た姿勢経路に沿い、かつ、姿勢の経路速度を姿勢変化の
角速度の大きさで指定することができる、３次元姿勢制
御装置を提供することができる。
うに機構を制御する。このようにして、事前に与えられ
た姿勢経路に沿い、かつ、姿勢の経路速度を姿勢変化の
角速度の大きさで指定することができる、３次元姿勢制
御装置を提供することができる。
【００５４】ついで、本実施例の動作の詳細についての
経路生成の具体例を使って説明する。ここでは、図３に
示すように、一定の速度で回転することにより方向が時
間とともに変化する回転軸の回りに、常に一定の回転速
度で運動Ｔ(t) により生じる経路（７）式の生成を考え
る。
経路生成の具体例を使って説明する。ここでは、図３に
示すように、一定の速度で回転することにより方向が時
間とともに変化する回転軸の回りに、常に一定の回転速
度で運動Ｔ(t) により生じる経路（７）式の生成を考え
る。
【００５５】詳しく述べると、ｔ＝０で方向ベクトルｌ
＝［１，１，１］T で表される直線Ｌを考え、この直線
Ｌの回りの一定のＬ回りの回転速度αで回転する。その
上でこの回転軸Ｌが一定の角速度αでＺ軸回りに回転す
る。
＝［１，１，１］T で表される直線Ｌを考え、この直線
Ｌの回りの一定のＬ回りの回転速度αで回転する。その
上でこの回転軸Ｌが一定の角速度αでＺ軸回りに回転す
る。
【００５６】この運動Ｔ(t) は周期２π／αで一周する
周期運動であり、この周期運動により描かれる軌跡で表
される経路を考える。また、初期姿勢は
周期運動であり、この周期運動により描かれる軌跡で表
される経路を考える。また、初期姿勢は
【００５７】
【数９】
【００５８】とする。上記運動Ｔ(t) の角速度軌道は
【００５９】
【数１０】
【００６０】で与えられる。このときの角距離軌道を
（４）式に基づき求めると
（４）式に基づき求めると
【００６１】
【数１１】
【００６２】となる。この角速度軌道ω(t) と角距離軌
道ξ(t) の軌跡を描いた例を図４に示す。上記角距離軌
道ξ(t) の描く軌跡を姿勢パラメータσで表すと、
道ξ(t) の軌跡を描いた例を図４に示す。上記角距離軌
道ξ(t) の描く軌跡を姿勢パラメータσで表すと、
【００６３】
【数１２】
【００６４】となる。（１２）式で表される角距離経路
を図５に示す。図５には、角距離軌道ξ(t) も示してあ
る。
を図５に示す。図５には、角距離軌道ξ(t) も示してあ
る。
【００６５】両者が一致していることが確認できる。こ
の図より姿勢パラメータσを用いて、角距離軌道が角距
離経路として表されたことが確認できる。（９）式の初
期姿勢Ｔ0 と（１２）式の角距離軌道から、姿勢経路と
して、姿勢行列＝［ｅx （σ) ，ｅy （σ) ，ｅz
（σ) ］T を求めた結果を示すと、
の図より姿勢パラメータσを用いて、角距離軌道が角距
離経路として表されたことが確認できる。（９）式の初
期姿勢Ｔ0 と（１２）式の角距離軌道から、姿勢経路と
して、姿勢行列＝［ｅx （σ) ，ｅy （σ) ，ｅz
（σ) ］T を求めた結果を示すと、
【００６６】
【数１３】
【００６７】となる。図６に姿勢行列＝［ｅx （σ) ，
ｅy （σ) ，ｅz （σ) ］T のうち、ｅz （σ）の経路
のみ示す。
ｅy （σ) ，ｅz （σ) ］T のうち、ｅz （σ）の経路
のみ示す。
【００６８】また、σ＝０→１１．１６であり、途中σ
＝０，３．７２，７．４４の３点についてｅz （σ）を
示してある。図６には基となった周期運動の軌跡Ｔ(t)
＝［ｅx (t) ，ｅy (t) ，ｅz (t)］T のうち、ｅz (t)
の軌跡も示されている。
＝０，３．７２，７．４４の３点についてｅz （σ）を
示してある。図６には基となった周期運動の軌跡Ｔ(t)
＝［ｅx (t) ，ｅy (t) ，ｅz (t)］T のうち、ｅz (t)
の軌跡も示されている。
【００６９】ｅz （σ）とｅz (t) が一致していること
より、基になった姿勢軌道Ｔ(t) を、姿勢パラメータσ
を用いて姿勢経路として表されたことが確認できる。以
上のようにして、従来、事前に指定された姿勢経路に沿
った機構の姿勢制御を行なう場合、経路に沿った姿勢の
変化速度を角速度の大きさとして指定することができな
かったが、本発明で提案する３次元姿勢制御方法及びそ
の装置をもちいることのより、その微分値が角速度の大
きさを表すパラメータを用いて、姿勢経路の時間に依存
しない空間経路を表すパラメータ表現を得ることがで
き、その結果、事前に与えられた姿勢経路が沿い、か
つ、姿勢の経路速度を姿勢変化の角速度の大きさで指定
して機構を制御することが可能になる。
より、基になった姿勢軌道Ｔ(t) を、姿勢パラメータσ
を用いて姿勢経路として表されたことが確認できる。以
上のようにして、従来、事前に指定された姿勢経路に沿
った機構の姿勢制御を行なう場合、経路に沿った姿勢の
変化速度を角速度の大きさとして指定することができな
かったが、本発明で提案する３次元姿勢制御方法及びそ
の装置をもちいることのより、その微分値が角速度の大
きさを表すパラメータを用いて、姿勢経路の時間に依存
しない空間経路を表すパラメータ表現を得ることがで
き、その結果、事前に与えられた姿勢経路が沿い、か
つ、姿勢の経路速度を姿勢変化の角速度の大きさで指定
して機構を制御することが可能になる。
【００７０】これにより、本発明の３次元姿勢制御方法
及びその装置は例えば、予め指定された姿勢経路に沿っ
た動作を行なう場合に、姿勢経路に沿った減速区間、加
速区間の設定などによる速度調節を行なうことにより、
機構を動作させるアクチェエータの負荷等を考慮しなが
らの経路制御が可能になる。
及びその装置は例えば、予め指定された姿勢経路に沿っ
た動作を行なう場合に、姿勢経路に沿った減速区間、加
速区間の設定などによる速度調節を行なうことにより、
機構を動作させるアクチェエータの負荷等を考慮しなが
らの経路制御が可能になる。
【００７１】また、ロボットに磨き作業のような加工作
業を行なわせる場合に、ロボットの手先に装着された工
具の姿勢を予め指定された姿勢経路に沿って動作をさせ
ながら、加工対象の状況に応じた動作速度の調節を行な
うことが可能になる。
業を行なわせる場合に、ロボットの手先に装着された工
具の姿勢を予め指定された姿勢経路に沿って動作をさせ
ながら、加工対象の状況に応じた動作速度の調節を行な
うことが可能になる。
【００７２】このように、本発明による３次元姿勢制御
方法及びその装置によればティーチング・プレイバック
動作ロボットの適用範囲の拡大に貢献し、さらにはロボ
ット作業の高度化の役立つ。
方法及びその装置によればティーチング・プレイバック
動作ロボットの適用範囲の拡大に貢献し、さらにはロボ
ット作業の高度化の役立つ。
【００７３】
【発明の効果】従って、以上詳述したように、本発明に
よれば、運動の状態を表す物理的な意味をもつパラメー
タで経路速度を指定することができないという問題点を
解決したもので、姿勢を表現する変数としては前記表現
方法（４）を用いるが、各変数を、その時間微分値が角
速度の大きさを表すという意味で物理的な意味を持つ姿
勢パラメータで表現することにより、姿勢変化の角速度
の大きさで姿勢の経路速度の調節を行なうことができる
３次元姿勢制御方法及びその装置を提供することが可能
となる。
よれば、運動の状態を表す物理的な意味をもつパラメー
タで経路速度を指定することができないという問題点を
解決したもので、姿勢を表現する変数としては前記表現
方法（４）を用いるが、各変数を、その時間微分値が角
速度の大きさを表すという意味で物理的な意味を持つ姿
勢パラメータで表現することにより、姿勢変化の角速度
の大きさで姿勢の経路速度の調節を行なうことができる
３次元姿勢制御方法及びその装置を提供することが可能
となる。
【図１】本発明に係る３次元姿勢制御装置の一実施例の
構成を示すブロック線図である。
構成を示すブロック線図である。
【図２】本発明の３次元姿勢制御装置で用いる姿勢の表
現方法を説明する図で、姿勢を表現する行列を構成する
ベクトルを説明している。
現方法を説明する図で、姿勢を表現する行列を構成する
ベクトルを説明している。
【図３】本発明の実施例で用いた３次元姿勢の動作例を
説明する図である。
説明する図である。
【図４】角速度軌道および角距離軌道の例を示し、図３
の動作例の角速度軌道および角距離軌道の軌跡を示した
図である。
の動作例の角速度軌道および角距離軌道の軌跡を示した
図である。
【図５】角距離経路の例を示し、図４に示される角距離
軌道から求めた角距離空間における角距離経路を示した
図である。
軌道から求めた角距離空間における角距離経路を示した
図である。
【図６】姿勢経路の例を示し、図５に示される角距離経
路から求めた姿勢経路のうち、姿勢行列の第３列である
ベクトルｅz （σ) の経路を図示したものである。
路から求めた姿勢経路のうち、姿勢行列の第３列である
ベクトルｅz （σ) の経路を図示したものである。
１…角速度軌道生成部、２…角距離軌道生成部、３…角
距離経路生成部、４…姿勢経路生成部、５…姿勢パラメ
ータ決定部、６…姿勢目標値生成部、７…機構制御部。
距離経路生成部、４…姿勢経路生成部、５…姿勢パラメ
ータ決定部、６…姿勢目標値生成部、７…機構制御部。
Claims (2)
- 【請求項１】 角速度ベクトルを時間に関して積分した
量である角距離ベクトルと、この角距離ベクトルの空間
上での距離を表す角距離および、角距離空間上での経路
に沿う道程を表し、その時間微分が姿勢変化の角速度の
大きさを表す姿勢パラメータと名付けるパラメータを導
入し、事前に生成された３次元姿勢の時間軌道から角速
度軌道を生成するステップと、 前記角速度軌道から、角速度軌道を時間について積分し
た角距離軌道を生成するステップと、 前記角距離軌道を、前記姿勢パラメータをパラメータと
して前記角距離ベクトルの空間上での時間に依存しない
空間的な経路として表した角距離経路に変換するステッ
プと、 初期３次元姿勢と、前記角距離経路とから、姿勢パラメ
ータをパラメータとして空間的な３次元姿勢の経路を表
した姿勢経路を生成するステップと、 目標とする姿勢変化速度から姿勢パラメータ指令値を生
成するステップと、 前記姿勢経路と姿勢パラメータ指令値から姿勢目標値を
生成するステップと、 前記姿勢目標値に従って被制御機構の姿勢を制御するス
テップとを具備してなることを特徴とする３次元姿勢制
御方法。 - 【請求項２】 角速度ベクトルを時間に関して積分した
量である角距離ベクトルと、この角距離ベクトルの空間
上での距離を表す角距離および、角距離空間上での経路
に沿う道程を表し、その時間微分が姿勢変化の角速度の
大きさを表す姿勢パラメータと名付けるパラメータを導
入し、事前に生成された３次元姿勢の時間軌道から角速
度軌道を生成する角距離軌道生成部と、 前記角速度軌道から、角速度軌道を時間について積分し
た角距離軌道を生成する角距離軌道生成部と、 前記角距離軌道を、前記姿勢パラメータをパラメータと
して前記角距離ベクトルの空間上での時間に依存しない
空間的な経路として表した角距離経路に変換する角距離
経路生成部と、 初期３次元姿勢と、前記角距離経路とから、姿勢パラメ
ータをパラメータとして空間的な３次元姿勢の経路を表
した姿勢経路を生成する姿勢経路生成部と、 目標とする姿勢変化速度から姿勢パラメータ指令値を生
成する姿勢パラメータ決定部と、 前記姿勢経路と姿勢パラメータ指令値から姿勢目標値を
生成する姿勢目標生成部と、 前記姿勢目標値に従って被制御機構の姿勢を制御する機
構制御部とを具備してなることを特徴とする３次元姿勢
制御装置。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP10943094A JPH07319530A (ja) | 1994-05-24 | 1994-05-24 | ３次元姿勢制御方法及びその装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| JP10943094A JPH07319530A (ja) | 1994-05-24 | 1994-05-24 | ３次元姿勢制御方法及びその装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| JPH07319530A true JPH07319530A (ja) | 1995-12-08 |
Family
ID=14510053
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| JP10943094A Pending JPH07319530A (ja) | 1994-05-24 | 1994-05-24 | ３次元姿勢制御方法及びその装置 |
Country Status (1)
| Country | Link |
|---|---|
| JP (1) | JPH07319530A (ja) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114274147A (zh) * | 2022-02-10 | 2022-04-05 | 北京航空航天大学杭州创新研究院 | 目标跟踪控制方法及装置、机械臂控制设备和存储介质 |
-
1994
- 1994-05-24 JP JP10943094A patent/JPH07319530A/ja active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114274147A (zh) * | 2022-02-10 | 2022-04-05 | 北京航空航天大学杭州创新研究院 | 目标跟踪控制方法及装置、机械臂控制设备和存储介质 |
| CN114274147B (zh) * | 2022-02-10 | 2023-09-22 | 北京航空航天大学杭州创新研究院 | 目标跟踪控制方法及装置、机械臂控制设备和存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US7035436B2 (en) | Method of generating poses and motions of a tree structure link system | |
| JPH0319964B2 (ja) | ||
| JPH10235580A (ja) | 位置・力目標軌道生成器 | |
| JPS59218513A (ja) | 工業用ロボツトの円弧制御法 | |
| WO1989008878A1 (en) | Method of controlling tool attitude of a robot | |
| JP3341305B2 (ja) | 加減速パターン生成装置及び加減速パターン生成方法、並びにこれに用いる逆運動学問題の解法と時間軸補正方法 | |
| JP4269150B2 (ja) | ロボット制御装置 | |
| JPH07319530A (ja) | ３次元姿勢制御方法及びその装置 | |
| US4922431A (en) | Method and apparatus of tool control in arbitrary plane operations | |
| JP3902310B2 (ja) | 産業用ロボットの姿勢生成方法 | |
| CN111331577B (zh) | 机器人的控制装置及控制方法 | |
| JPH0693209B2 (ja) | ロボツトの円弧補間姿勢制御装置 | |
| JPH09190211A (ja) | 数値制御装置の経路制御方式 | |
| JPH08155647A (ja) | 溶接ロボットのウィービング制御装置 | |
| JPH0695294B2 (ja) | 産業用ロボットの軌跡補間方法 | |
| JPH07200030A (ja) | ロボット動作計画装置、コントローラ、および、最適軌道生成方法 | |
| JPS62154006A (ja) | ロボツト制御装置 | |
| JPH1148176A (ja) | ロボット位置教示装置 | |
| Funabiki et al. | Sensor-based motion-planning of a manipulator to overcome large transmission delays in teleoperation | |
| JP2932443B2 (ja) | ロボットの円弧補間姿勢制御方法 | |
| JPS6055412A (ja) | Νｃデ−タ作成方法 | |
| JPH11198072A (ja) | ロボットの最短時間速度制御装置 | |
| JP2921756B1 (ja) | ロボットの制御方法および制御装置 | |
| JPH10244481A (ja) | ロボットの動作プログラム作成方法および装置 | |
| JPH0352003A (ja) | 工業用ロボットのオフライン教示装置 |