SICE 標準ロボットアームによる視覚フィードバック制御○河 合 宏 之 ∗ · 村 尾 俊 幸 ∗ · 藤 田 政 之 ∗∗金 沢 大 学

Visual Feedback Control on SICE Direct Drive Arm○ Hiroyuki Kawai∗, Toshiyuki Murao∗ and Masayuki Fujita∗∗ Kanazawa University
In this paper, we consider the vision–based robot control for the three dimensional target tracking. The construction and the implementation of the visual feedback system are shown by combining SICE–DD arm and a visionsystem with software for control design. Experimental results are reported to conﬁrm the eﬀectiveness of the proposed
visual feedback control law.
1れ g = (p, eξ̂θ ) とする. 視覚フィードバックシステムの制御

はじめに視覚フィードバック制御とは, ロボットマニピュレータをはじめとするダイナミカルシステムに対して, 視覚情報をフィードバックループに組み込んだ制御のことである 1) . 視覚システムは豊富で多様な情報を有し, かつ手軽であるため, センサとしてその重要性が増してきており, 近年では DNA を細胞へ注入するロボットの自動化のために, 視覚フィードバック制御をシステムに組み込んだ研究も進められている 2) . しかしながら, ロボットの視覚フィードバック制御における多くの従来研究では, ロボットはゆっくりと動作すると仮定されておりマニピュレータダイナミクスは考慮されていない 3) .

目的は, マニピュレータの手先を運動する観測対象に追従させることである. したがって, 相対位置姿勢 g = (p, eξ̂θ ) を目標
値 gd = (pd , eξ̂θd ) と一致させられれば, 制御目的は達成される. しかしながら, 真値 g はカメラから直接得られないため,
ˆオブザーバを構成する. そして, 推定値 ḡ = (p̄, eξ̄θ̄ ) と目標値

gd = (pd , eξ̂θd ) との偏差 ec と真値 g と推定値 ḡ の偏差 ee を 0にすることで制御目的を達成させる. ただし, マニピュレータダイナミクスを考慮しているため, ec と ee を 0 にするためには,ξ も 0 にする必要がある.
ここで新たな制御入力を uξ として, 次の入力トルクを考える.

τ = M (q)q̈d + C(q, q̇)q̇d + g(q)本稿では, マニピュレータダイナミクスを考慮した Eye-in-

hand 構造の 3 次元視覚フィードバック制御 4) を考え, SICE–DD アーム5)+JbT (q)AdT(g−1 ) Wc ec + uξ(2)

dでの視覚フィードバックシステムの構成と実装について述べる. そして, 実験により提案する制御則の有効性を検Jb (q) はマニピュレータヤコビアン, Ad は随伴写像と呼ばれる証する.

演算子である 6) . Wc は制御偏差 ec と推定偏差 ee のバランスをとるための重み行列のひとつである.

2

視覚フィードバックシステム(1)(2) 式を含む視覚フィードバックシステムに対して, つぎの視覚フィードバック制御則を考える 4) .

マニピュレータダイナミクスは次式で表される 5) .

M (q)q̈ + C(q, q̇)q̇ + g(q) = τ(1)232 I0Kξ 0 0T6u = − 4 0 Kc 0 5 4 0 −Ad(g ) Wc0q は関節角度, τ は入力トルク, M (q) は正定な慣性行列, C(q, q̇)q̇は遠心力・コリオリ力項, g(q) は重力項を表す. ここで, 角速度の目標値を q̇d として, その偏差ベクトルを ξ := q̇ − q̇d と定義する.

^)(p,eNG列のひとつである. この制御則の安定性と L2 ゲイン制御性能解

3.1
^ wc)
(pwc,eNGee析については文献 4) で議論している.

35c−1d0 Ad(e−ξ̂θec ) Wc −We32 3
ξ75 4 ec 5 (3)

ここで Kξ , Kc, Ke はゲインである. We は Wc と同様に重み行

CameraImage0 Ke00

SICE–DD アームによる実験

実験環境

提案する制御則を SICE–DD アームに実装し検証実験をおこなう. 本実験では SICE–DD アームの手先にカメラを取り付

Target Object 5o^ wo)(pwo,eNG

5wFig. 1:視覚フィードバックシステム

けており 1 軸が 0.2 [m], 2 軸が 0.3 [m] であり, 動作範囲は,

q1 = ±90◦ , q2 = ±120◦ である. カメラは PULNiX 社 TM7EX の CCD カメラ (30 [fps]) を使用する. また, 画像入力ボードは, PCI バスモノクロ画像入力ボードの PicPort-Stereo-H4D

Eye-in-hand 構造の視覚フィードバックシステムを Fig. 1 にを使用し, 特徴量は画像処理ソフト HALCON を利用して観測対示す. 基準座標系を Σw , カメラ座標系を Σc , 観測対象座標系

象物上にある 4 点の中心点をそれぞれ算出する. 画像処理とデーを Σo とする. また Σc から見た Σo の位置と姿勢をそれぞタ解析をおこなう Host PC の CPU は Pemtium 4, 2.53GHz で300

6time (s)12zo
5o-40-100xo

Object Frame yo

Fig. 2:SICE–DD アームの座標系
メモリは 512MB である. 制御則を実装するディジタル制御装置は dSPACE 社の DS1005(PowerPC 750 processor, 480MHz)を用いており, サンプリング周期は 1 [ms] である. また, 制御則

fy3 (pixel)

fy1 (pixel)

Camera Frame

06time (s)12成する. Fig. 2 に SICE–DD アームにおける視覚フィードバッ
クシステムの座標系を示す.

3.2

実験結果

本実験において, 目標位置姿勢は pd = [0 0 −0.9]T , eξ̂θd = I

6time (s)301206time (s)1206time (s)12-40-100-7006time (s)1206time (s)1210020500-4506time (s)12500Fig. 6:特徴量の推定値(重み行列なし)(重み行列あり)norm || z || (Gain B)である ControlDesk により DSP に実装する. 観測対象は, 位置050

Fig. 5:特徴量の推定値

0.30.20.10は Matlab と Simulink により設計し, dSPCE のソフトウェアと姿勢を決定可能にするために Matlab と VRML を用いて構

-70
10020

fx3 (pixel)-45

fy3 (pixel)

5c50

fx1 (pixel)

5w
yw

-2070

fy1 (pixel)

xw

-20

70

fx3 (pixel)zw

norm || z || (Gain A)

yc

World Frame

fx1 (pixel)

xc

zc

02

468

10

1202

4

6time (s)

810

120.3
0.20.10Fig. 7:被制御出力のユークリッドノルムとし, マニピュレータの初期角度は q1 = 30◦ , q2 = −30◦ とし安定であることが確認できる. また, Gain A は γ = 0.62 , Gain

た. 観測対象は Fig. 3, 4 に示すように, 0 ≤ t ≤ 4 の間は直線運B は γ = 0.23 であり, Fig. 7 の実験結果より γ が外乱抑制レ動させ, 4 ≤ t ≤ 9.6 の間は 8 の字運動をさせる.

ベルを表す指標となっていることが確認できる. ただし, 観測対象が静止した後に定常偏差が残ってしまっており, これについて-0.1-0.1は今後の課題である.

40

yo (m)

yo (m)

0

本稿では, マニピュレータダイナミクスを考慮した Eye-in-hand 構造の 3 次元視覚フィードバック制御を考え, SICE–DD0.1

0.1

おわりにアームでの視覚フィードバックシステムの構成と実装について述べた. また, 実験により提案する制御則の安定性と制御性能解0.20.60.45
xo (m)

Fig. 3: 直線運動

0.3

0.2

0.6

0.45
xo (m)

0.3

Fig. 4: 8 の字運動

また重み行列を Wc = 0.1I, We = I とした. ゲインはKξ = diag{10, 10} とし, Kc , Ke は以下の 2 種類を用いる.
Gain A : Kc = diag{60, 60, 30, 30, 30, 60}, Ke = 20I6
Gain B : Kc = diag{120, 120, 60, 60, 60, 120}, Ke = 30I6
まず, 重み行列 Wc と We の有効性を示すために, 重み行列なし (Wc = We = I) とした場合と重み行列を考えた場合について, 観測対象の特徴点のうちの 2 点の x, y 座標のカメラからの測定値 (破線) と推定値（実線）をそれぞれ Fig. 5, 6 に示す.
重み行列を含む場合のほうが, 同じゲインを用いても推定偏差が少ないことから, 重み行列 Wc と We が推定のための設計パラメータである考えられる.

Fig. 7 に状態 ξ, ec, ee を含む被制御出力のユークリッドノル

析および重み行列の有効性を検証した.
参考文献

1) S. Hutchinson, G. D. Hager and P. I. Corke, “A Tutorial on Visual Servo Control,” IEEE Trans. Roboticsand Automation, vol. 12, no. 5, pp. 651–670, 1996.
2) S. Yu and B. J. Nelson, “Autonomous Injection of Biological Cells Using Visual Servoing,” In:D. Rus andS. Singh (Eds), Experimental Robotics VII, Springer–Verlag, pp. 169–178, 2001.
3) R. Kelly, R.Carelli, O. Nasisi, B. Kuchen and F.
Reyes, “Stable Visual Servoing of Camera-in-Hand
Robotic Systems,” IEEE/ASME Trans. Mechatronics, vol. 5, no. 1, pp. 39-48, 2000.
4) 河合, 村尾, 藤田, “受動性に基づく動的視覚フィード
バック制御の安定性と L2 ゲイン制御性能解析,” 第 32回制御理論シンポジウム資料 (掲載予定), 2003.

ムを示す. ゲイン A のグラフは上部に, ゲイン B のグラフを下

5) 小林ら, ロボット制御の実際, コロナ社, 1997.

部に示す. Fig. 7 の実験結果において, 観測対象が静止した後,

6) R. Murray, Z. Li and S. S. Sastry, A Mathematical Introduction to Robotic Manipulation, CRC Press, 1994.

偏差が 0 付近に収束している. この結果によりシステムが漸近