WO1985003784A1 - Procede de regulation a vitesse constante - Google Patents

Description

明 細 書

等速制御方法

技術分野

本発明は直線運動を回転運動に変換する直線一回転変 換機構を備え た シ ス テ ム に お い て 、 該回転運動の回転速 度を一定にす る等速制御方法に関する 。

背景技術

直線運動を回転運動に変換す る直線一回転変換機構が あ る 。 かか る変換機構は第 1 の可動部を直線移動さ せ、 該第 1 可動部の直線移動に応じ て第 2 の可動部を回転移 動さ せ る も のであ る 。 と こ ろで、 かか る直線一回転変換 機構において は単に第 1. の可動部を等速で直線移動さ せ て も 第 2 の可動部を等速で回転さ せ る こ と がで き ない。

以上か ら 、 本発明の目的は直線一回転変換機構におい て簡単な方法で回転運動を等速にす る こ と がで き る等速 制御方法を提供す る こ と で あ る 。

本発明の別の目的は可動部の直線軸上の位置を監視し 該直線軸上の位置に応じ て可動部の直線軸上の速度を コ ン ト ロ ール し 、 こ れに よ り 回転軸の回転速度を一定にす る等速制御方法を提供する こ と で あ る 。

発明の開示 —— 本 ¾明は直線一回転変換機構にお い て 、 回転運動を等 速にす る等速制御方法であ り 、 直線軸上の可動部の位置 を監視し 、 該位置に応じ て可動部の直線軸上の移動速度 を制御して 回転軸の回転速度一定にす る 。 « 図面の簡単な説明

第 1 図は直線運動を回転運動に変換する直線一回転変 換機搆を備えた関節 σ ボッ ト の外観図、 第 2 図は第 1 図 の動作を説明する概念図、 第 3 図は本発明方法を実現す る シ ス テ ム の ブ ロ ジ ク 図、 第 4 図は本発明方法の処理の 流れ図であ る 。

発明を実施する ための最良の形態

第 1 図は本発明方法を適用で き る直線一回転変換機構 を備え た関節ロ ボッ ト の外観図、 第 2 図は第 1 図の動作 概念図であ る 。 基台 1 の腕 1 a には支点 Ρ を中心に回転 可能に直線驟動部 2 が設け られ、 又 ¾台 1 には支点 Q を 中心に回転可能に関節軸 3 が設け られて い る 。 直線駆動 部 2 はボー ル ネ ジ 2 a と 、 該ボ一ル ネ ジ 2 a を回乾させ る モ ー タ 2 b と 、 ボ ール ネ ジ 2 a に螺合す る ナ ツ ト 部 を 有する可動部 2 c を備え、 可動部 2 c と基台 2 の支点 Q 閩ば一定の長さ r を有す る リ ン ク 4 に よ り 接続さ れて い る 。 尚、 リ ン ク 4 は支点 Q を中心に回転可能であ り 、 該 回転に よ り 図示し ない機構を介して関節軸 3 が回転する 関節軸 3 の先端には他の関節軸 5 が結合さ れ、 該関節軸 5 の先端には手首機搆 6 が設け られてい る 。 モータ 2 b を回転させて 、 ボ ー ル ネ ジ 2 a を回転さ せ る と 、 可動部 2 e は ポ 一 ル ネ ジ ' 2 a 上を該ボ ー ル ネ ジ の回転方向に応 じ た方向に移動し 、 リ ン ク 4 は支点 Qを中心に回転する 又、 直線駆動部 2 全体が支点 P を中心に回転する 。 こ の 結果可動部 2 c は支点 Q を中心に半径 r の円弧上を移動 す る こ と に な る 。 すな わち 、 ボ ー ル ネ ジ 2 a を 回転 さ せ て可動部 2 c を第 2 図実線位置か ら一点鎖線位置、 点線 位置へ と順次移動させる と該可動部 2 c は支点 Qを中心 に半径 r の円弧 7 上を移動する こ と に な り 、 リ ン ク ⁴ も 可動部 2 c と一体に回転運動を行 う 。 そ して、 リ ン ク ⁴ が回転すれば図示し ない機構に よ り 関節軸 3 が同様に支 点 Q を中心に して回転す る 。 尚、 かかる直線一回転変換 機構においては可動部 2 c を等速でボ ー ル ネ ジ 2 a '上を 移動させて も 、 該可動部は円弧 ⁷ に沿って等速で移動せ ず、 当然の こ と なが ら関節軸 3 も 等速度で回転し ない。

以下、 第 2 図を参照し て本発明にかかる等速制御方法 の概略を説明す る 。

第 2 図に おいて支点 Pか ら可動部 2 c 迄の距離を x 、 リ ン ク 4 が直線 PQと なす角度を Θ と すれば次式

(dx/d t) = (dx/d^) · {ά θ/ά t) (1)

が成立する 。 従って、 回転速度 （dSZd t) は次式

{ά θ/ά t) = (dx/d t) · (d <9ズ dx) (2)

で与え られ" "る 。 一方、 支点 P と支点 Q閻の長さ を L 、 リ ン ク 4 の長さ を r と すれば三角関数の公式か ら次式

X L²+r²-2 し， C 0 s S (3)

が成立す る 。 従っ て 、 （ 2 ) 式に お け る （d SZdx) は （ 3 ) 式を X で微分す る こ と に よ り 次式

{ά θ/ά χ) =x/ (r · L · s i η <9) (4)

で与え られる 。 又、 （ 4 ) 式の s i n は （ 3 ) 式と 次式

(5) , と 力 > ら 求 ま る o

さ て 、 指令回転速度を V、 可動部 2 c の直線軸上の位 置を と すれば、 （ 2 ) 式よ り 次式

V= (dx/d t) · (d^/dx) _χ=χβ (6)

が成立する 。 従って、 直線軸上の可動部 2 c の移動速度 (dxZd t) を そ の現在位置： s a に応じて （ 6 ) 式が満足す る よ う に制御すれば回転速度を V—定にする こ と がで き る 0 .

第 3 図は本発明を実現する制御装置のブロッ ク 図、 第 4 図ば本発明の処理の流れ図であ り 、 以下の処理ステツ プの審号を第 4 図中の各ブロ ッ ク 右肩に付してい る。

第 3 図に'お いて .、 1 1 は プ ロ セ ッ サ ； 1 2 は制御プ ロ グ ラ ム を記億する R O M、 1 3 は π ボッ ト指令データ そ の他の処理結果を記億する R A M、 1 4 は操作盤、 1 5 は パ ル ス分配器、 1 6 は サ ー ボ回路、 1 7 は直線軸 （第 1 図におけ る ボー ル ネ ジ 2 a ) を駆動する モータ （第 1 図で は 2 b ) 、 1 8 は ロ ボッ ト 、 1 9 は ロ ボッ ト と制御 装置閩のデータ 授受を制御する ィ ン タ フ ： c — ス 回路であ る 。 尚、 以下にお いては関節軸 3 (第 1 図参照） を ジ ョ グ送 り する場合について本発明の等速制御処理を説明す る

(1)まず、 関節軸 3 を ジ ョ グ送 り するため に操作盤 1 4 上に設けた図示し ない ジョ グボタ ン を押圧する 。

(2)ジ ョ グ送 リ 速度を F (既知） 、 操作盤 1 4 上のォ ー バ ラ イ ド ス ィ ッ チ に よ り 設定されて-い る オ ーバ ラ イ ド値 を R と すれば、 プ ロ セ ッ サ 1 1 は関節軸 3 の ジ ョ グ ボ タ ン が押 さ れ る と 次式

V=F - R (7)

に よ り ォ 一 バ ラ ィ ド値を考慮し た 回転速度 V を演算す る 。

(3)つ いで 、 プ ロ セ ッ サ 1 1 は X = X a ( X a は現在位 置で R A M 1 3 に格納 さ れて い る ） に おけ る c o s ^を （ 3 ) 式か ら求め 、 し か る 後 （ 5 ) 式に よ り s i n^を演算す る 。

(4) s i n が求ま -れば、 プ ロ セ ッ サ 1 1 は該 s i n 6»を （ 4 ) 式に代入す る と 共に 、 （ 4 ) 式お いて x = x a と し て

(d Zdx) _x=xaを演算す る 。

(5)し か る 後、 （d Zdx) _x=xaを用 いて （ 6 ) 式 よ り 可動部 2 <3 の移 ¾速度 （ = d_X/d t) を演算す る 。

(6)直線軸上の移動速度 V _X が求 ま れば、 プ ロ セ ッ サ

1 1 は予め定め ら れて い る 時閩 Δ Τ 秒 （ た と え ば 1 6 m s e c ) の間 に移動すべ き ィ ン ク リ メ ン タ ル値 Δ Χ を次 式

AX = Vx · AT (8)

に よ り 求め る 。

(7) し カゝ る 後、 該 Δ X を パ ル ス 分配器 1 5 に 入力す る 。

(8)パ ル ス 分配器 1 5 は Δ X に基づ いて パ ル ス 分配演算 を実行し て分配パ ル ス X P を発生 し 、 該分配パ ル ス を サ — ボ 回路 1 6 に入力す る 。 こ の結果関節軸 3 は速度 V で 回転す る 。

(9)以上の処理 と 並行 し て プ ロ セ ッ サ 1 1 は Δ Τ 秒経過 後に R A M I 3 に記億 さ れて い る 可動部 2 c の直線軸上 の現在位置 x a を次式

に よ り 更新す る 。 '尚、 （ 9 ) 式に お いて符号は移動方向 に饯存す る 。

(10)そ し て 、 以後ジ ョ グ ボタ ンが抻圧さ れて い る か ど う か を チ ヱッ ク し 、 抻圧さ れて い る 限 り 厶 T 秒毎に （ 9 ) 式で求め ら れた現在位置 X a を用 いて上記ス テ ツ プ（3)以 降の処理を繞行すれば関節軸 3 は一定の速—度 Vで回.転す る こ と に な る 。

尚、 以上は ジ ョ グ ボ タ ン が抻 さ れた ジ ョ グ送 り の場合 で あ る が、 当然 R A M 1 3 に記億 さ れて い る ロ ボッ ト 指 令デー タ に よ る移動制海の場合に も 適用 で き る も ので あ る 0

産業上の利用可能性

以上説明 し た よ う に 、 本発明に よ れば直線一回転変換 機構に お け る 回転軸の回転速度を簡単な方法で一定に で き 、 従っ て ロ ボッ ト の関節軸を一定速度で回転 さ せ る 場 合に適用 し て好適で あ る 。

Claims

請求の範囲

1 * 第 1 の可動部を直線軸に沿って移動さ せ る と共に 、 該第 1 の可動部の直線移動に応じ て第 2 の可動部を回転 さ せ る直線一回転変換機構におけ る該第 2 の可動部を等 速回転させる等速制御方法にお いて 、 第 1 の可動部の直 線軸上の位置を監視する第 1 ス テツ プ、 第 2 の可動部を 等速回転さ せ る ための第 1 の可動部の移動速度を該第 1 の可動部の直線軸上の位置に応じて演算す る第 2 ステ ツ プ、 該演算さ れた移動速度で第 1 可動部を移動さ せ、 前 記第 2 の可動部の回転速度を等速にする第 3 ス テ ツ プを 有す る こ と を特徵 と する等速制御方法。

2 . 前記第 2 ス テ ツ プは、 第 1 の可動部の直線軸上の位 置を x 、 第 2 の可動部の回転位置を と す る と き 、 第 1 可動部の直線軸上の現在.位置 X a に おけ る （d ^Z d x) _x=Xa を求め る ス テ ツ プ、 該 （d ^Z d x) と 指令回転速度 V と か ら第 1 の可動部の移動速度を演算す る ス テ ツ プを有す る こ と を特徵と する請求の範囲第 1 項記載の等速制御方 法 0

3 · 第 1 の可動部の移動速度を VZ { ά θ / ά ^ ) _x=xaと す る こ と を特徵と す る請求の範囲第 2 項記載の等速制御方法。

4 * 前記直線一回転変換機構は、 所定の支点 P を 中心に 回転可能な直線軸 と 、 該直線軸上を移動す る第 1 の可動 部 と 、 第 1 の可動部を直線軸に沿っ て移動さ せ る モータ と 、 第 1 の可動部の直線軸上の移動に伴っ て該直線軸を 回転さ せ、 結果的に第 1 の可動部の通路執跡を 円弧にす る機構と 、 第 1 の可動部の直線軸上の位置に基づいて支 点 Q を中心に回転する第 2 の可動部を有する こ と を特徵 とする請求の範囲第 1 項記載の等速制御方法。

5 ♦ 前記第 1 の可動部の通路軌跡を円弧にする機構は、 該第 1 の可動部と支点 Q間を結合す る と共に、 支点 Q を 中心に回転可能な長さ r の リ ン ク を有する こ と を特徵と する請求の範囲第 4 項記載の制御方法。

6 * 前記第 2 ス テ ツ プは 、 第 1 の可動部の直線軸上.の位 置を x 、 第 2 の可動部の回転位置を と す る と き 、 第 1 可動部の直線軸上の現在位置 X a に お け る （d<9Zdx) _x=Xa を求め る ス テ ツ プ、 該 （d^Zd_X) _χ=Χ3 指令回転速度 V と か ら第 1 の可動部の移動速度を演算する ス テツ プを有す る こ と を特徵 と する請求の範囲第 5 項記載の等速制御方 法

7 · 支点 P Q間の距離を L と する と き 、 （ deZdx) を次 式

2 _τ 2 2

X = L + Γ - L · c 0 s Θ

n Θ +c o s Θ

よ り 求め る こ と を特徵と する請求の範囲第 S 項記戴の等 速制御方法。

8 * 第 1 の可動部の移動速度を V/ (d d x) X=Xaと す る と を特徵と する請求の範囲第 7 項記載の等速制御方法