JPH0743606B2 - サ−ボ機構の円弧軌道生成方法 - Google Patents

Description

【発明の詳細な説明】

〔産業上の利用分野〕 この発明は、ロボツトやNCガス切断機などのサーボ機構
の円弧軌道生成方法に関するものである。 〔従来の技術〕 現在、ロボツトやNC（数値制御装置）ガス切断機などの
サーボ機構のエンド・エフエクタ（溶接トーチや切断ト
ーチなど）を円弧軌道に沿つて移動する場合、与えられ
た複数個の点の間を円弧補間してサンプリング時間ごと
の目標位置を定め、各駆動軸のサーボ制御を行う方法が
実用されている。 ここで、第４図に示すように、空間上の位置（Ａ），
（Ｂ），（Ｃ）から規定される円弧軌道に沿つて、エン
ド・エフエクタを位置（Ａ）から動かし始め、位置
（Ｂ）を経由して位置（Ｃ）で止める作業を考える。こ
の場合、第５図に示すようなフローチヤート、すなわ
ち、ステツプ（10）で円弧軌道上の次の目標位置（Pi）
を求め、次いでステツプ（11）でこの目標位置（Pi）を
直交座標系基準の座標値として求める（これを軌道生成
という）。次にステツプ（12）では、ステツプ（11）で
求められた座標値に対応する駆動軸（モータなどの出力
軸）の目標位置を求める（これを座標変換という）。次
いでステツプ（13）では駆動軸ステツプ（12）で求めた
目標位置になるように位置決め制御をする（これをサー
ボ制御という）。これらの動作を一定時間（これをサン
プリング時間という）おきに、終点に到達するまで繰り
返し行う（ステツプ（14），（15）参照）。 〔発明が解決しようとする問題点〕 上記のような円弧軌道生成方法は、離散時間制御である
からそのサーボ制御性を損なわないためには、サンプリ
ング時間をなるべく短くすることが必要である。これ
は、通常の多関節形ロボツトの場合、約30m sec以下の
時間が望ましい。また、サーボ機構として多関節形ロボ
ツトを考える場合、直交座標系基準のエンド・エフエク
タの位置と、各駆動軸の位置との関係式は複雑な非線形
方程式となり、直交座標系基準のエンド・エフエクタの
位置に対応する各駆動軸の位置を求める座標変換には、
かなりの演算時間を必要とする。そして、この座標変換
のアルゴリズムは、駆動軸の構成に応じてほぼ１つの方
法のみが見出だせるだけであり、工夫の余地は少ない。
このため、与えられた円弧軌道上の直交座標系基準のエ
ンド・エフエクタ位置を求める軌道生成を、できるだけ
短い演算時間で行なうことが強く要求されている。 第６図は現在一般に行なわれている円弧軌道生成法を説
明するための原理的説明図である。なお、ここで求めた
いのは、位置（Ａ），（Ｂ），（Ｃ）で規定される円弧
軌道上の目標位置（Pi）である。つまり、基準直交座標
系の原点（０）からの距離（▲▼）が求まればよ
い。以下にその手順を説明する。 位置（Ａ），（Ｂ），（Ｃ）から規定される円の中
心位置（Ｇ）を求め、そしてベクトル（▲▼）を求
める。 面（A,B,C）の単位法線ベクトル（）を求める。 ベクトル（）に関して、任意のベクトルを（Δ
θ）だけ回転させる。そして３×３の変換行列Ｒ（Δ
θ）を求める。 kykxvers（Δθ）−kzsin（Δθ） ky²vers（Δθ）＋cos（Δθ） kykzvers（Δθ）＋kxsin（Δθ） ただし、vers（Δθ）＝１−cos（Δθ）である。 ベクトル（）に関して、ベクトル（▲
▼）を角度（Δθ）だけ回転させたものが、（▲
▼）であるから、 ▲▼＝▲▼＋▲▼ …〔４〕 より、目標位置（Pi）を求める。ただし（▲▼）
の初期値は（▲▼）である。 ここで、円弧軌道上て一定線速で移動するために、中心
角（θ）を角速度（）一定で定める方法を用いた場合
には、位置（Ａ）での始動時と位置（Ｃ）での停止時に
円弧軌道上の線速が不連続となるため、機構に大きな振
動が発生する。そのため、一般的には何等かの規範で中
間位置（A⁺），（C^-）を定め、位置（Ａ）から位置
（A⁺）までの間で角速度（）を零からある定められた
ある値（_ｓ）まで加速し、その後一定角速度（_ｓ）
で位置（C^-）まで移動する。そして位置（C^-）から位置
（Ｃ）までの間で（）を（_ｓ）から零まで減速する
ように中心角（θ）を定める方法が用いられている。つ
まり、Δθ（＝θｉ−θｉ−１）は一定値でない。 〔発明が解決しようとする問題点〕 ところで、前述した手順〜を予め行うこと、すなわ
ち、目標位置（Pi）を予め全て求めてこれを記憶してお
く方法は、膨大な記憶容量が必要となるので、現実的で
はない。そこで、一度だけ行えば良い手順，だけは
予め行なつておき、手順，は機構の駆動時にサンプ
リング時間ごとに行う方法がとられる（なお、前明の手
順，は（Δθ）が一定値でないために、目標位置ご
とに異なつた演算となる）。 ここで、上記手順の式〔２〕はsin,cosおよび多くの
加減乗算を含む演算式であるからかなりの演算時間がか
かり、これが原因で結果的にはサーボのサンプリング時
間を充分に短くすることができなくなり、サーボ制御性
が損なわれるという問題があつた。 この発明は、このような状況に鑑みてなされたものであ
り、軌道生成のための演算時間を短くし、また、他の直
線軌道と連結するときには滑らかな連結を可能にしたサ
ーボ機構の円弧機構生成方法を提供することを目的とす
る。 ［問題点を解決するための手段及び作用］ この発明に係るサーボ機構の円弧軌道生成方法は、円弧
軌道の始点及び終点を含む教示点（A,C）を指定する工
程と、各教示点についてその点に位置する時刻よりも一
定時間だけ前後の時刻に位置すべき２点（A^-,A⁺,C^-,
C⁺）の位置をそれぞれ求める工程と、各教示点及びその
前後に位置する上記２点によって時間に関する４次多項
式を定め、これにより生成される加減速軌道をそれぞれ
求める工程とを有し、その加減速軌道を円弧軌道の始終
点部として用いる。 例えば円弧軌道の始点から移動を開始し、又は終点にお
いては停止する場合は、始点に位置する時刻よりも一定
時間だけ前の時刻に位置すべき点（A^-）と始点（Ａ）と
を一致させ、又は終点に位置する時刻よりも一定時間だ
け後の時刻に位置すべき点（C⁺）と終点（Ｃ）とを一致
させて前記の４次多項式を定める。その結果、始点
（Ａ）と点（A⁺）とを結ぶ加減速軌道又は点（C^-）と終
点（Ｃ）とを結ぶ加減速軌道は直線で表され、その演算
は簡単であり演算時間は従来の演算方法に比べて短くな
る。 また、円弧軌道の始点及び終点がそれぞれ他の直線軌道
と連結している場合には、始点に位置する時刻よりも一
定時間だけ前の時刻に位置すべき点（A^-）及び終点に位
置する時刻よりも一定時間だけ後の時刻に位置すべき点
（C⁺）はそれぞれ直線軌道上に求められ、ここで生成さ
れた加減速軌道は曲線軌道となり他の直線軌道と滑らか
に連結する。 〔発明の実施例〕 以下、第１図を用いてこの発明による円弧軌道の生成方
法を説明する。いま、位置（Ａ）から（Ｂ）を経由して
位置（Ｃ）まで円弧軌道上を一定線速で移動する問題を
考える。 （イ） 位置（Ａ）から（A⁺）までの第１軌道生成方
法。 まず、加速に要する時間（2ta）を定め、位置（Ａ）か
ら与えられた一定線速（Ｖ_ｓ）で時間（ta）だけ直線移
動したときに到達する円弧軌道上の位置を（A⁺）と定め
る。また、位置（Ａ）から動き始める予定時間より時刻
（ta）だけ前にいた位置を（A^-）と定める。この場合、
位置（Ａ）から動き始める前は停止していたのであるか
らA^-＝Ａである。なお、第２図は時間に関する４次多項
式の直線軌道生成方法を（ｘ）座標についての説明図で
ある。ここで、第１図における位置（Ａ）から位置
（A⁺）までの途中位置（Pi）の座標値（xi），（yi），
（zi）を次のように定める。 ここで、hi＝（ti＋ta）/2ta（ただし、−ta≦ti≦ta） このように軌道生成すれば、位置（Ａ）と位置（A⁺）と
を結ぶ直線軌道上を線速零から線速（Ｖ_ｓ）まで加速し
ながら、時間（2ta）で移動する。また、位置（Ａ）お
よび位置（A⁺）では線加速度が共に零になるので、この
方法で作る直線軌道の前後に他の等線速軌道（線加速度
が零の軌道）を連結する場合には、端点での線加速度を
連続にできる。ここで、位置（A⁺）と（A^-）すなわち
（xa⁺），（ya⁺）（za⁺），（xa^-），（ya^-），（za^-）
は予め求めておけるので、式〔５〕だけを機構の駆動時
にサンプリング時間ごとに演算すればよい。 これは上記の従来方法に比べて、単純な演算であるか
ら、短時間で軌道の生成ができる。 ところで、第１図に示すように、位置（Ａ）から位置
（A⁺）までは、本来の円弧軌道から偏差が生じる。しか
し、円弧軌道の精度が要求されるのは、溶接などの超低
速動作に対してのみであり、（AA⁺）間の移動に要する
時間（2ta）は短いので、距離（AA⁺）は数mmほどとな
る。それゆえに、実用される曲率半径の大きさの円弧軌
道に対しては、実用上問題となるほどの大きさの偏差は
生じない。 （ロ） 位置（A⁺）から（C^-）までの第２軌道生成方
法。 まず、減速に要する時間（2ta）を定め、次いで位置
（Ｃ）からある与えられた一定線速（Ｖ_ｓ）で時間（t
a）だけ直線移動したときに到達する円弧軌道上の位置
を（C^-）と定める。ここで、位置（A⁺）から位置（C^-）
までの途中位置（Pi）を次のように定める。 位置（A⁺），（Ｂ），（C^-）から規定される円の中
心位置（Ｇ）を求め、そしてベクトル（▲▼）を
求める。 面（A⁺,B,C^-）の単位法線ベクトル（）を求め
る。 与えられた一定線速（Ｖ_ｓ）より、サンプリング時
間ごとの中心角の増分値（Δθ）を求める。 ベクトル（）に関して、任意のベクトルを（Δ
θ）だけ回転させる３×３の変換行列Ｒ（Δθ）を求め
る。 kykxvers（Δθ）−kzsin（Δθ） ky₂vers（Δθ）＋cos（Δθ） kykzvers（Δθ）＋kxsin（Δθ） ただし、vers（Δθ）＝１−cos（Δθ）である。 ベクトル（）に関して、ベクトル（▲
▼）を角度（Δθ）だけ回転させたものが、ベクトル
（▲▼）であるから、 ▲▼＝▲▼＋▲▼ …

〔９〕 より、目標位置（Pi）を求める。ただし（▲▼）
の初期値は（GA⁺）である。 上記のように軌道生成すれば、位置（A⁺），（Ｂ）と位
置（C^-）とを結ぶ円弧軌道上を一定線速（Ｖ_ｓ）で等速
移動する。ここで、位置（A⁺）と位置（C^-）および（Δ
θ）（一定値）は予め行なうことができ、そして手順
だけを機構の駆動時にサンプリング時間ごとに行なえば
よい。つまり、上述の従来の円弧軌道生成方法と比べる
と、従来の手順すなわち、ベクトルを回転させる３×
３の交換行列を求める演算を予め行なえることになる。
それゆえに、サンプリング時間ごとに行なう演算の量が
著しく減るので、短時間に軌道が生成できる。 （ハ） 位置（C^-）から（Ｃ）までの第３軌道生成方
法。 まず、位置（Ｃ）で止まる予定時刻より時間（ta）だけ
後にいるべき位置を（C⁺）と定める。この場合、位置
（Ｃ）で止まり続けるのでC⁺＝Ｃである。ここで、位置
（C^-）から（Ｃ）までの途中位置（Pi）の座標値（x
i），（yi），（zi）を前記（イ）項と同様に次のよう
に定める。 ここで、hi＝（ti＋ta）/2ta（ただし、−ta≦ti≦ta） このように軌道生成すれば、位置（C^-）と位置（Ｃ）と
を結ぶ直線軌道上を、線速（Ｖ_ｓ）から線速零まで減速
しながら時間（2ta）で移動する。また、位置（C^-）お
よび（Ｃ）では、線加速度がともに零となる。 この方法は、（イ）項で述べたのと同様に従来の円弧軌
道生成方法に比べて単純な演算であるから、短時間に軌
道が生成できる。 第３図は円弧軌道と直線軌道との連結状態を説明するた
めの説明図である。すなわち、位置（Ａ）に到達する予
定の時刻よりも時間（ta）だけ前にいるはずの他の直線
軌道上のを位置（A^-）として定めれば、この発明の円弧
軌道生成方法をそのまゝ用いることにより、他の直線軌
道から円弧軌道へとその線速度，線加速度を変えながら
円滑に連結することができる。この逆に、位置（Ｃ）に
到達する予定の時刻よりも時間（ta）だけ後にいるはず
の他の直線上の点を位置（C⁺）として定めれば、前述と
同様に円弧軌道から直線軌道への連結が円滑にできる。 〔発明の効果〕 以上のようにこの発明によれば、円弧軌道の始点及び終
点及びその近傍についての軌道を時間に関する４次多項
式によって求めるようにしたので、従来の方法に比べて
演算時間が短縮され、従って、その分だけ従来よりもサ
ーボサンプリング周期を短くすることができる。特に、
時間に関する４次多項式によって軌道を生成するように
したので、速度及び加速度の双方が連結点のそれと一致
しこの点からも円滑な連結が可能になっている。また、
直線軌道と円弧軌道とを連結するために特別な他の方法
を用いる必要がないので、軌道生成のソフトウエア作成
が容易であり、かつその連結も円滑である。

【図面の簡単な説明】

第１図はこの発明の一実施例による円弧軌道生成方法を
説明するための原理的説明図、第２図はこの発明による
直線軌道生成方法を示す説明図、第３図はその円弧軌道
と直線軌道との連結状態を示す説明図、第４図は円弧軌
道の一般的な説明図、第５図はその動作を説明するフロ
ーチヤート、第６図は従来の円弧軌道生成方法を示す説
明図である。

Claims (3)

【特許請求の範囲】
1. 【請求項１】円弧軌道の始点及び終点を含む教示点（A,
   C）を指定する工程と、 各教示点についてその点に位置する時刻よりも一定時間
   だけ前後の時刻に位置すべき２点（A^-,A⁺,C^-,C⁺）の位
   置をそれぞれ求める工程と、 各教示点及びその前後に位置する上記２点によって時間
   に関する４次多項式を定め、これにより生成される加減
   速軌道をそれぞれ求める工程とを有し、その加減速軌道
   を円弧軌道の始終点部として用いることを特徴とするサ
   ーボ機構の円弧軌道生成方法。
2. 【請求項２】円弧軌道の始点から移動を開始し、又は終
   点においては停止する場合は、始点に位置する時刻より
   も一定時間だけ前の時刻に位置すべき点（A^-）と始点
   （Ａ）とを一致させ、又は終点に位置する時刻よりも一
   定時間だけ後の時刻に位置すべき点（C⁺）と終点（Ｃ）
   とを一致させて、前記の４次多項式を定める特許請求の
   範囲第１項記載のサーボ機構の円弧軌道生成方法。
3. 【請求項３】円弧軌道の始点及び終点がそれぞれ他の直
   線軌道と連結している場合には、始点に位置する時刻よ
   りも一定時間だけ前の時刻に位置すべき点（A^-）及び終
   点に位置する時刻よりも一定時間だけ後の時刻に位置す
   べき点（C⁺）はそれぞれ前記直線軌道上に求められる特
   許請求の範囲第１項記載のサーボ機構の円弧軌道生成方
   法。