計測自動制御学会東北支部 第 218 回研究集会 (2004.10.9)
資料番号 218-6

水平多関節ロボットの制御
Control of Horizontal Multi-link Robot○菊池貴行，大久保重範, 及川一美, 高橋達也
○ Takayuki Kikuchi, Shigenori Okubo,Kazumi Oikawa,Tatsuya Takahashi
山形大学

キーワード : ジャーク信号 (Jerk signal)，安定多項式 (Stable polynomial),
状態方程式実現 (Realization of state equation)

連絡先 : 〒 992-8510 米沢市城南 4-3-16 山形大学 工学部 機械システム工学科 大久保研究室
菊池貴行，Tel.: (090)7076-2486 E-mail: dmk52868@dip.yz.yamagata-u.ac.jp

1.

はじめにされることを示す。それと同時に、モデルである水平多関節ロボットに制御系をとり入れた上で、ロボットマニピュレータの制御では、定常特性や即応性、減衰性などの過渡特性に着目し、これより高度な位置決め動作を実現する事を最終目標とする。

ら三大特性を満足するように制御システムを構成しなければならない。しかし、実際には実システ

2.

水平多関節ロボットの概観

ムモデルを作成するときにはシステム構造の簡略化や物理システムに含まれる非線形性を線形近似

モータ３リンク1リンク2

関節１関節２することなどによる誤差が発生し、制御系には不リンク３安定性や応答の劣化が生じる。これらの不確定性

関節３モータ４５１３が、制御系の安定性などに対して無視できない重大な影響を与えることがある。このシステムの変関節4リンク４(ワークハンド)

動やシステムの不確定性に耐えうる性質がロバス

モータ１モータ２ト性であり、その性質を持つ制御系がロバスト制

510828御系である。本研究では、プラントに不確定性をFig. 1含むシステムパラメータを用いて、モデル追従形制御系の設計を行い、かつ入力信号にリンクパラメータの３次微分（ジャーク信号）を使用することにより、規範モデルとの出力誤差を安定にゼロへ収束させることの出来るロバスト制御系が設計

リンク２

水平多関節ロボットの概観上の図で、リンク１・２は横方向、リンク３は上下方向の移動でリンク４はワークハンドである。
また、関節１・２はＡＣサーボモータ、関節３・４はステッピングモータで構成されている。

{1{

システム構成

3.

・KXSD3D-ALZ
・KXPD3D-ABZ

ＡＣサーボモータ
ドライバ

ＰＣ
Windows NT

コントロールボード
HPC-PPD234A

ステッピングモータ
ドライバ
指令信号

接続してＡＣ／ＤＣサーボモータあるいはパルスモータの位置および速度制御を簡単に行うことが・KXS230HG-ALZ
・KXP230GD-ALZできる。

ＡＣサーボモータ

特徴

LSI : PCL5014

最高 4.9Mpps の高速制御

ステッピングモータ

Ｓ字加減速、直線加減速

・CSK566-NAP
・PMC33A-MG36

応答信号

Fig. 2

次ブロック自動実行機能

システム構成ソフトリミット機能
計算機からの数値データはコントロールボー
複数ボード間同時スタート・ストップ機能

ド内のパルス発生ＬＳＩでパルス信号に変換されて、各モータドライバに送られてモータを駆動さ現在位置読み出し（指令位置/エンコーダ位置可せている。逆にＡＣサーボモータ内のエンコーダ能）からは、モータの角度・角速度のパルス信号データ
仕様がコントロールボードに戻され数値データに変換è 制御軸−４軸位置決め（ＰＰＤ２３４Ａ）

された後、計算機の画面上に表示される。ステッピングモータにはエンコーダが搭載されていない

è 制御ＬＳＩ−ＰＣＬ５０１４（日本パので、指令値が画面に表示される。構成要素は以ルスモータ製）

下の通りである。

è 加減速制御−直線およびＳ字加減速
è 移動量− 268,435,445 パルスまたは-134,217,728

è ＯＳ：Windows NT〜+134,217,727 パルス

è プログラム開発ソフト：VisualC++

è 指令パルス速度− 0.1pps〜4.9Mpps

è コントロールボード：HPC-PPD234A

è エンコーダ信号入力−カプラ受け (差動è ＡＣサーボモータ：KXS230HG-ALZ・KXP230GD-またはオープンコレクタ）

ALZ

è 軸センサ信号入力（各軸あたり、フォトカプラ絶縁）−ストロークエンド入力

è ステッピングモータ：CSK566-NAP・PMC33A-（＋ＥＬＳ），センサ原点入力（ＯＬＳ），

MG3

原点減速センサ入力（ＤＬＳ），エン

4.

コントロールボード

4.1

コントロールボード

コーダ原点入力（Ｚ相）è サーボインターフェース（各軸あたり、フォトカプラ絶縁）−２入力、３出力

ＨＰＣ−ＰＰＤ２３４Ａは４軸位置決め制御ボードで、制御ＬＳＩとしてＰＣＬ５０１４を使4.2用している。今回使用しているボードの指令パルス出力は、パルス列入力形式のサーボドライバとパルス発生ＬＳＩＰＣＬ５０１４は、ＰＣＬ３０１３とのハード（ピン端子）、ソフトの互換仕様となっている。Ｐ{2{

ＣＬ５０１３の豊富な機能をそのままに、加速、6.

外乱を考慮したロボットの制御系設計

減速時間を別々に設定できることと、Ｓ字加減速制御ではＳ字区間の設定ができる機能が付加され(4) 式から、システムの制御対象と参照モデルている。また、最高速度の自動修正が行えるのでは (5)、(6) 式で与えられる。

滑らか動作を実現する制御ができる。

D(p)q(t)= v(t) +ロボットの運動方程式5.

+1
K(q(t); q(t);_q°(t))Åû
ñ

1d(t)
ñ

(5)

ラグランジュ法で求めた運動方程式について、

dm (p)qm (t) = rm (t)トルク、重量、粘性摩擦、外乱を含めた場合の式(6)

出力誤差は (7) 式で定める。の一般形は以下の式で与えられる。

e(t) = q(t) Ä qm (t)M (q(t))°q (t) + h(q(t); q(t))_+ g(q(t)) = ú(t) + d(t)

(7)

(1)

ここで、容易のために、外乱 d(t) を線形自由系のここで、q(t) 2 RnÉ1：リンクパラメータ、M (q(t)) 2出 力 と す る 。d(t) は (8) 式 を満 足 す る 特 性多 項 式

RnÉn：慣性行列、h(q(t); q(t))_2R

コリオリ力、V 2 R

R

nÉ1

nÉn

nÉ1

：遠心力及び：粘性摩擦行列、g(q(t)) 2：重力負荷、ú(t) 2 RnÉ1

Dd (p) を持っていると仮定する。安定多項式 T (p)を選び、(9) 式より R(p)、S(p) を定める。

：制御入力、d(t) 2

RnÉ1：外乱、である。さらに、(1) 式は確定的な関_q°(t)) と不確定性を含むシステムパ数 K(q(t); q(t);ラメータûを使って (2) 式のようになる。

K(q(t); q(t);_q°(t))û= ú(t) + d(t)Dd (p)d(t) =0(8)T (p)Dm (p) =Dd (p)D(p)R(p) + S(p) (9)

ここで、各多項式の次元は、(10) 式〜(15) 式のよ(2)うになる。

@T (p)= ö(10)@Dm= nm(11)^は公称値である。(2) 式
メータの誤差、ûは真値、û

@D(p)= np = 3(12)で入力ú(t) を (3) 式のように定める。

@Dd (p)= nd(13)@R(p)= ö+ nm Ä np Ä nd(14)@S(p)= np + nd Ä 1(15)K(q(t); q(t);_q°(t)) 2 RnÉn ; û2 RnÉ1
^ ûは不確定性のシステムパラ
ここで、Åû = ûÄ^ú(t) = ñv(t) Ä q [3] + K(q(t); q(t);_q°(t))û

(3)

ここで、v(t)：新しい制御入力、ñ：ジャーク信号dである。これの強さ、q [3] (t) = D(p)q = p3 q,p = dt

より全体の動的方程式は (4) 式で与えられる。

1
1q [3] (t) = v(t) + K(q(t); q(t);_q°(t))Åû+ d(t) (4)ññ(7) 式〜(9) 式により、T (p)Dm (p)e(t) の結果を整理すると、(16) 式〜(19) 式のようになる。

T (p)Dm (p)e(t)= T (p)dm (p)(q(t) Ä qm (t))(16)

= Dd (p)D(p)R(p) + S(p)q(t)Ä T (p)Dm (p)qm (t)(17)= Dd (p)R(p)[v(t)+{3{11K(q(t); q(t);_q°(t))Åû+ d(t)]ññ+ S(p)q(t) Ä T (p)Dm (p)qm (t)(18)= Dd (p)R(p)v(t)1+ Dd (p)R(p) K(q(t); q(t);_q°(t))Åû
ñ+ S(p)q(t) Ä T (p)rm (t)

(19)

(29)

り (30) 式となる。外部信号 vm (t) は (31) 式になる。

v(t) =(21) 式に書き直すことができる。

+e(t) =1
[Dd (p)R(p) Ä Q(p)v(t)T (p)Dm (p)+Q(p)v(t) + S(p)q(t) Ä T (p)rm (t)]vm (t) =ÄH1 ê1 (t) Ä (E2 q(t) + H2 ê2 (t))
vm (t)(30)

Em rm (t) + Hm êm (t)(31)

ここで、@Q(p) = ö+nm Änp 、öï 2np +nd Änm Ä1、Dd (p)R(p) 1
K(q(t); q(t);_q°(t))Åû
T (p)Dm (p) ñnm ï np である 。Dd (p) = p とすれ ば、nd = 1(20)である。ö = 3 として、安定多項式 T (p) を選び、多項式の次数が (10) 式〜(15) 式より、計算すると、

Q(p)
[v(t)
T (p)Dm (p)@T (p) = 3、@Dm (p) = 3、@D(p) = 3 である。しQ(p)Ä1+Q(p)Ä1+Dd (p)R(p) 1
K(q(t); q(t);_q°(t))Åû
T (p)Dm (p) ñ+_m (t) = Fm êm (t) + Gm rm (t)ê可制御正準形にとるものとする。v(t) は (23) 式よに選ぶならば、(19) 式より誤差 e(t) は (20) 式及び=(28)jpI ÄFi j = jQ(p)j(i = 1 ò 4) であり、Fi 、Gi 、Hi はQ(p) はモニックな安定多項式 (@Q(p) = ö+nm Änp )+_2 (t) = F2 ê2 (t) + G2 q(t)ê

Dd (p)R(p) Ä Q(p)v(t)S(p)q(t) Ä Q(p)

Ä1

たがって R(p) と S(p) 等を求めると、以下のようにT (p)rm (t)]

なる。

(21)
ここで、

v(t) + Q(p)Ä 1 Dd (p)R(p) Ä Q(p)v(t)+Q(p)Ä 1 S(p)q(t) Ä Q(p)Ä1 T (p)rm (t) = 0(22)T (p) =

(p + ã)3(32)Q(p) =(p + ã)3(33)Dm (p) =(p + å)3(34)D(p) =

p3(35)

R(p) =

p2 + 3(ã+ å)p+とおけば v(t) は、S(p) =v(t) = ÄQ(p)
Ä1

ÄQ(p)

Ä1Dd (p)R(p) Ä Q(p)v(t)S(p)q(t) + Q(p)

Ä1T (p)rm (t)(23)

ここで、各伝達関数の状態方程式実現を考慮すれe(t) =
H1 (pI Ä F1 )

Ä1

G1 = Q(p)Dd (p)R(p) Ä Q(p)(24)

E2 + H2 (pI Ä F2 )Ä 1 G2 = Q(p)Ä 1 S(p)(25)

Em + Hm (pI Ä Fm )Ä1 Gm = Q(p)Ä 1 T (p)(26)

これより (27) 式〜(29) 式の状態方程式実現が得ら(ã+ å)(ã2 Ä ãå+ å3 )p3+3ãå(ã2 + 3ãå+ å2 )p2+3ã2 å2 (ã+ å)p + ã3 å3(37)Dd (p)R(p) 1K(q(t); q(t);_q°(t))Åû
T (p)Dm (p) ñ(38)_q°(t) が三角関数及び多K(q(t); q(t);_q°(t)) の q(t); q(t);項式関数の積和である。ñが大きいとき、e(t) は十分小さくなる。

7.

れる。

_1 (t)ê(36)

また、(21) 式より、ば以下のようになる。
Ä13(ã2 + ãå+ å2 )今後の課題
è Mathmatica を用いたマニピュレータ運動方= F1 ê1 (t) + G1 v(t)(27){4{程式の構築

è 外乱を考慮したロボットの制御系に関する状態方程式の安定性についての考察

è 実モデルである水平多関節ロボットの実用的ドライバソフトの開発

è 実モデルに設計した制御系を取り入れたときの各リンクモータの応答特性の考察

参考文献
1) 孫信偉、大久保重範：ロボットマニピュレータのロバスト制御、計測自動制御学会東北支部
第 184 回研究集会資料、184-7(1999)

2) 木村英紀、藤井隆雄、森武宏：ロバスト制御、コロナ社 (1994)

3) 加藤一郎、他：ロボット工学、日本ロボット学会、コロナ社 (1990)

4) 吉川恒夫：ロボット制御基礎論、コロナ社 (1988){5{