Study on Visual Feedback Attitude Synchronizationibuki/paper/Ib_master.pdfAbstract In this paper we consider visual feedback attitude synchronization in leader-follower type visibility

修士論文

視覚フィードバックによる姿勢同期制御に関する研究

Study on Visual Feedback Attitude Synchronization

指導教員藤田政之教授

東京工業大学

理工学研究科機械制御システム専攻

08M51195

伊吹竜也

2010 年 8 月 4 日

Copyright c⃝ 2010 by Tatsuya Ibuki

要旨

　本論文では視覚情報に基づいた 3次元空間における姿勢同期問題を考察する．姿勢同期

問題は，複数存在する剛体の姿勢を分散的な制御則を用いてある共通の姿勢に収束させる

ことを目的とする．本問題では各剛体は自身の位置姿勢情報や近傍との通信による情報は

得られず，近傍の視覚情報のみが取得可能であるとし，必要な視覚情報は各剛体から視覚

センサで抽出可能な画像特徴量のみに限定する．先行研究では姿勢同期問題に対して視覚

情報を用いて制御則を決定する手法が提案されているが，提案された制御則では制御偏差

が残り，また理論的な保証が示されていない．そこで，本研究では新たな視覚情報に基づく

制御則を提案し，これを適用することにより姿勢同期が達成されることを証明する．まず，

制御対象として剛体の運動を表す運動学モデルと剛体間の情報構造を表すリーダ追尾型の

可視構造，および視覚による観測出力からなるビジュアルロボティックネットワークを提案

する．つぎに，視覚フィードバックによる姿勢同期を定義し，このネットワークに対して視

覚フィードバックによる姿勢同期を達成する制御則を提案する．提案する制御則では，各

剛体は非線形オブザーバを内蔵することにより可視剛体との相対位置姿勢を推定し，その

推定値を用いて姿勢同期を達成する入力を決定する．さらに，提案したオブザーバ併合型

の制御則を適用することにより，先頭の剛体 (リーダ) が回転運動をしないときにビジュア

ルロボティックネットワークが姿勢同期を達成することを証明する．また，リーダが回転運

動を行うときは，その角速度を外乱とみなし，ネットワークに制御則を適用した閉ループ

系に対して安定性解析を行う．安定性解析では，可視構造は一直線に並んだ鎖型のものを

考え，リーダの角速度を入力，他のすべての剛体の偏差を状態および出力とした系に対し

て，それぞれ入力状態安定性解析および入出力安定性解析を行う．これにより，リーダの回

転運動に対する各剛体の追従性能を評価することが可能となる．また，3 次元空間上におけ

る数値シミュレーションを行うことにより，提案する制御則による収束性解析および安定性

解析の有効性を示す．さらに，2 次元平面上における実機を用いた検証実験を行うことによ

り提案する制御則による収束性解析の妥当性を示す．

Abstract

In this paper we consider visual feedback attitude synchronization in leader-follower type

visibility structures in the Special Euclidean group SE(3). We first define visual robotic net-

works consisting of the dynamics describing rigid body motion, visibility structures among

bodies and visual measurements. We then propose a visual feedback attitude synchroniza-

tion law combining a vision-based observer with the attitude synchronization law presented

in our previous works. We then prove that when the leader doesn’t rotate, the robotic net-

work with the control law achieves visual feedback attitude synchronization in the absence

of communication and measurement of the states. Moreover, for a rotating leader, we eval-

uate the tracking performance of the other bodies. In this analysis, we regard the leader’s

angular velocity as an external disturbance and consider the stability of the closed-loop sys-

tem of visual robotic network dynamics with the presented law. Finally, the validity of the

proposed control law and the analysis is demonstrated through both numerical simulations

in 3-dimensional space and experiments on a planar testbed.

目次 i

目次

第 1章はじめに 1

1.1 研究背景 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1

1.2 研究目的 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

1.3 論文構成 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 4

第 2章問題設定 6

2.1 ビジュアルロボティックネットワーク . . . . . . . . . . . . . . . . . . . . . . 6

2.1.1 剛体の運動学モデル . . . . . . . . . . . . . . . . . . . . . . . . . . . 6

2.1.2 可視構造 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8

2.1.3 観測出力 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

2.2 ビジュアルロボティックネットワークの姿勢同期 . . . . . . . . . . . . . . . . 10

第 3章視覚フィードバックによる姿勢同期制御 12

3.1 制御則の提案 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 12

3.2 収束性解析 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14

第 4章視覚フィードバックによる姿勢同期制御の安定性解析 19

4.1 入力状態安定性解析 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4.1.1 入力状態安定性の定義 . . . . . . . . . . . . . . . . . . . . . . . . . . 19

4.1.2 2 台の剛体の入力状態安定性解析 . . . . . . . . . . . . . . . . . . . . 20

4.1.3 視覚フィードバックによる姿勢同期制御の入力状態安定性解析 . . . . 23

4.2 入出力安定性解析 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

4.2.1 入出力安定性の定義 . . . . . . . . . . . . . . . . . . . . . . . . . . . 26

4.2.2 視覚フィードバックによる姿勢同期制御の入出力安定性解析 . . . . . 26

第 5章数値シミュレーション 33

5.1 シミュレーション 1 (収束性の検証) . . . . . . . . . . . . . . . . . . . . . . . 33

5.2 シミュレーション 2 (入出力安定性解析の検証) . . . . . . . . . . . . . . . . . 34

目次 ii

第 6章検証実験 38

6.1 実験環境 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 38

6.2 姿勢同期制御の検証実験 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

第 7章まとめ 41

謝辞 43

参考文献 44

付録A 剛体の運動学モデルの受動性 50

A.1 剛体の運動学モデルの受動性 . . . . . . . . . . . . . . . . . . . . . . . . . . 50

A.2 相対運動モデルの受動性 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 51

付録B ビジュアルモーションオブザーバ 53

B.1 ビジュアルモーションオブザーバ . . . . . . . . . . . . . . . . . . . . . . . . 53

B.2 ビジュアルモーションオブザーバの性能解析 . . . . . . . . . . . . . . . . . . 57

付録C 数学的準備 60

C.1 ∧ に関する公式 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

C.1.1 aa = 0 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

C.1.2 12tr(ab) = −aT b . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 60

C.1.3 a2 = aaT − ∥a∥2I3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

C.1.4 a3 = −∥a∥2a . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

C.2 ロドリゲスの公式 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 61

C.3 数学的準備 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

C.3.1 eξθωe−ξθ = (eξθω)∧ . . . . . . . . . . . . . . . . . . . . . . . . . . . . 62

C.3.2(

ddt

eξθ)

e−ξθ の歪対称性 . . . . . . . . . . . . . . . . . . . . . . . . . 62

C.3.3 sk(eξθ) = ξ sin θ . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

C.3.4 eξθsk(eξθ)∨ = sk(eξθ)∨ . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

C.3.5 eξθ ≈ I3 + sk(eξθ) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 63

C.4 エネルギ関数 φ(eξθ) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 64

図目次 iii

図目次

1.1 Rigid Body Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 3

2.1 Rigid Body Motion . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 7

2.2 Leader-follower Type Visibility Structure (Directed Spanning Tree) . . . . . 9

2.3 Perspective Projection Model . . . . . . . . . . . . . . . . . . . . . . . . . . 10

2.4 Visual Feedback Attitude Synchronization . . . . . . . . . . . . . . . . . . . 11

3.1 Block Diagram of Visual Feedback Attitude Synchronization Law . . . . . . 13

3.2 Definition of Rigid Body Sets . . . . . . . . . . . . . . . . . . . . . . . . . . 14

5.1 Visibility Graph in Simulation . . . . . . . . . . . . . . . . . . . . . . . . . . 33

5.2 Positions in Σw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34

5.3 Relative Rotation Angles about x-axis . . . . . . . . . . . . . . . . . . . . . 34

5.4 Relative Rotation Angles about y-axis . . . . . . . . . . . . . . . . . . . . . 34

5.5 Relative Rotation Angles about z-axis . . . . . . . . . . . . . . . . . . . . . . 34

5.6 Positions in Σw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

5.7 Rotation Angles about x-axis . . . . . . . . . . . . . . . . . . . . . . . . . . 35

5.8 Rotation Angles about y-axis . . . . . . . . . . . . . . . . . . . . . . . . . . 35

5.9 Rotation Angles about z-axis . . . . . . . . . . . . . . . . . . . . . . . . . . 35

5.10 Positions in Σw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5.11 Rotation Angles about x-axis . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5.12 Rotation Angles about y-axis . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5.13 Rotation Angles about z-axis . . . . . . . . . . . . . . . . . . . . . . . . . . 36

5.14 Control Performance (γ = 1) . . . . . . . . . . . . . . . . . . . . . . . . . . . 37

5.15 Control Performance (γ = 0.373) . . . . . . . . . . . . . . . . . . . . . . . . 37

6.1 Experimental Environment . . . . . . . . . . . . . . . . . . . . . . . . . . . . 39

6.2 Visibility Graph in Experiment . . . . . . . . . . . . . . . . . . . . . . . . . 39

図目次 iv

6.3 Positions in Σw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

6.4 Rotation Angles in Σw . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 40

6.5 Estimation Error of Position between 2 and 1 . . . . . . . . . . . . . . . . . 40

6.6 Estimation Error of Rotation Angle between 2 and 1 . . . . . . . . . . . . . 40

6.7 Estimation Error of Position between 3 and 2 . . . . . . . . . . . . . . . . . 40

6.8 Estimation Error of Rotation Angle between 3 and 2 . . . . . . . . . . . . . 40

B.1 Block Diagram of Visual Motion Observer . . . . . . . . . . . . . . . . . . . 58

第 1 章はじめに 1

第 1章

はじめに

1.1 研究背景

近年，地球温暖化や有害排出物による公害などの環境問題，危険区域における監視，防

犯・防災，交通システムの社会インフラ分野などにおいて効率の良いモニタリングが求め

られている [1]-[3]．このモニタリングに対して，効率性や耐故障性の向上のためにセンサが

ネットワークを構築することが有用である．広大な領域における移動体 (生物の生息環境な

ど) の計測や環境の変化 (地球の大気調査に対する風や海洋調査に対する水流など) が存在

する状況におけるモニタリングには，固定されたセンサではなくその計測領域や環境の変

化に適応して移動するセンサが重要となる．そこで，複数のセンサを搭載した移動ロボット

やモバイルセンサがネットワークを構築するモバイルセンサネットワーク [4] が注目を集め

ている．

モバイルセンサネットワークの制御において，各モバイルセンサ間の協調が不可欠である．

これを達成する手法として，合意問題 [5, 6] や群れ問題 [7, 8]，被覆問題 [9]，cyclic pursuit

問題 [10]，同期問題 [11]-[17] などに代表される協調制御の研究が活発に行われている．協調

制御の目的は，複数のエージェントやセンサなどがお互いの情報を交換し，制御されるこ

とにより，システム全体が協調して制御目的を達成することである．協調制御は自然界にお

ける鳥や魚の群れなどの協調動作の解析を発端として始まった分野であるが，近年ではモ

バイルセンサネットワーク [4] のみではなく，ロボットネットワーク [18]，衛星や航空機の

フォーメーション制御 [19, 20]，ビークルネットワーク [21] などの工学的応用に用いられて

いる．実際のモニタリングでは，ネットワークを構築するセンサの集合はある秩序だった形

状を構成する必要があり，その中のひとつとしてすべてのセンサが同じ方向を向く状況が

考えられる．また，観測地を変更するためにネットワークが群を成して同じ方向へ移動す

る状況においても，すべてのモバイルセンサの姿勢を同一方向に揃える必要性が生じるこ


とが予想される．したがって，本研究では環境モニタリングのための協調制御問題の部分

問題として，複数の剛体の姿勢同期問題を考える．本研究で扱う姿勢同期問題では，すべ

ての剛体の姿勢を分散制御則を用いてある共通の姿勢に収束させることを目的とする．文

献 [2] では実際に環境モニタリングに協調制御を適用した例として，無人自動航行海底探査

機に姿勢協調制御則を実装した海洋調査を行っている．

協調制御問題を考える上で重要な問題はどのような情報をどのようにして取得すべきか

である．多くの研究ではGPSなどによる絶対的位置情報の取得や無線通信機器の搭載を仮

定している．しかし，無線通信機器などによるコストの削減や水中などの通信が難しい状

況を考えると，各ロボットが相対情報を取得するセンサを搭載し，近傍との相対情報のみ

を用いて分散的に制御されることが望ましい．実際に文献 [2] の海洋調査においても通信の

ために定期的に各センサが海面上に浮上する必要がある．これに対して，豊富な相対情報

を与えるセンサの一つとして視覚センサがあり，実装を考慮に入れても汎用のカメラを用

いるだけなので工学的に有用である．以上の理由から，本研究では相対姿勢を取得するセ

ンサとして視覚センサを用いる．

視覚情報を用いたロボットマニピュレータの制御に関する研究は既に数多く行われてきた

が [22]-[27]，近年は視覚情報を用いた協調制御問題が注目を集めている [28]-[33]．Moshtagh

らは複数の非ホロノミックロボットに対して単眼カメラにより視覚情報に基づいた群れ制

御則を提案している [28, 29]．ここでは，複数の形式のフォーメーションの形成を達成する

制御則を画像から抽出できる情報のみで実現できることを示している．Morbidi らは画像上

の特徴点から距離を推定する推定器を用いることにより，視覚情報に基づいたリーダ追尾

型のフォーメーション制御問題に取り組んでいる [30, 31]．ここでは，リアプノフの議論を

用いることで閉ループ系の安定性を示しているが，提案された制御則ではリーダとフォロ

ワの間に通信を仮定している．Giesbrecht らもまた同様の問題に取り組んでおり [32]，ここ

ではフォロワは一定時間遅れた仮想的なリーダの軌跡を追従する手法をとっている．また，

Vidal らは視覚サーボ問題での制御手法に基づいて同様の問題を考えている [33]．ここでは，

対象となる空間を複数の画像平面に変換することによりフォロワの視覚サーボ問題として

考え，フィードバック線形化を用いることにより制御則を提案している．以上述べたよう

に，視覚情報に基づいた協調制御問題に関する研究は多数あるが，通信を用いずに収束性

を保証するオブザーバ併合系の制御則は提案されていない．

文献 [30]-[33] で考察されているように，現在の視覚情報に基づく協調制御問題では，情

報構造がリーダ追尾型であるものが大半を占める．本研究でも視覚情報に基づく姿勢同期

問題において，剛体間の情報構造はリーダ追尾型のものを用いる．リーダ追尾型の協調制

御の研究は盛んに行われており [34]-[39]，Tanner ら [34] やChen ら [35] はリーダの存在す


World FrameFrame

Frame Frame

Fig. 1.1: Rigid Body Motion

るフォーメーション制御問題に対して，入力状態安定性という概念を用いて制御系の安定

性を評価している．一方で，文献 [36]-[39] では，従来の合意問題で扱われている系に対し

て，1 台および複数台のエージェントをリーダに選び，その制御系の可制御性および可観測

性を達成する通信構造を考察している．

我々の先行研究では，視覚情報に基づく協調制御則として，近傍との相対位置姿勢を用

いた視覚情報に基づく位置姿勢同期制御則を提案している [40]．ここでは，文献 [13, 14] で

提案されている受動性に基づく位置姿勢同期制御則を視覚情報のみを用いて実現している．

この制御則に必要な情報は近傍の剛体との相対位置姿勢であり，文献 [41] で提案されてい

る受動性に基づく視覚オブザーバを制御則に内蔵することで推定している．文献 [41] では，

情報の取得構造として観測対象から抽出可能な画像特徴量を利用し，3 次元空間における

観測対象との相対位置姿勢を推定する非線形オブザーバを提案している．文献 [40] では制

御と推定の機構が容易に併合できることを示しており，実験を通してその有効性を示して

いるが，収束性解析が行われていない．そこで，本研究の課題は視覚情報のみに基づいた 3

次元空間における姿勢同期問題を考え，リーダ追尾型の情報構造をもつ集団に対して理論

的な保証付きの制御則を提案することである．

1.2 研究目的

本研究では，Fig. 1.1 に示すように，3 次元空間上に複数の剛体が存在する状況を考え，

視覚情報のみに基づいた姿勢同期問題に対して制御則を提案し，理論的な解析を行うこと

を目的とする．各剛体は SE(3) 空間上でモデル化され，視覚により近傍の情報を取得する

とする．

まず，制御対象として剛体の運動を表す運動学モデルと剛体間の情報構造を表す可視構


造，および視覚による観測出力からなるビジュアルロボティックネットワークを提案する．

ここで考える可視構造はリーダ追尾型の構造を用いる．つぎに，このネットワークに対し

て視覚フィードバックによる姿勢同期を定義し，これを達成する制御則を提案する．本稿

で提案する制御則は姿勢同期制御則と近傍との相対位置姿勢を視覚情報から推定する視覚

オブザーバを併合した制御則となっている．ここで提案する制御則は収束性を理論的に保

証するために先行研究 [40] とは異なる構造をもつ．つぎに，提案した制御則による収束性

解析および制御性能解析を行う．収束性解析では，まず姿勢同期の達成が制御偏差システ

ムと推定偏差システムを統合したシステムの平衡点の漸近安定性としてとらえることがで

きることを示す．そして，リアプノフ関数の候補として制御偏差と推定偏差をともに評価

できるポテンシャル関数を用いて収束性を証明する．収束性の証明ではリーダが回転運動

をしないという仮定を用いる．しかし，実際に工学的応用を考えたとき，ネットワークが所

望の方向を向いたり所望の位置に移動するためにリーダが回転運動を行う状況を考える必

要がある．そこで，本稿ではリーダが回転運動するときのネットワーク全体の制御偏差お

よび推定偏差を評価することで提案する制御則の制御性能解析を行う．具体的には，すべ

ての剛体が一直線に並んだ鎖型の可視構造をもつロボティックネットワークに対して，リー

ダの角速度を入力，ネットワーク全体の偏差を状態および出力とした系を考え，入力状態

安定性解析および入出力安定性解析を行う．この解析により，リーダが任意に回転すると

きの各剛体の追従性能を評価することが可能となると考えられる．この追従性能を上げる

ことで，ネットワークを構成する各剛体はリーダの回転運動に対しても十分な精度・速さ

で姿勢を追従させることが可能となる．また，この制御手法は自然界の群れ行動の模倣や

フォーメーション制御などにも応用することが期待される．最後に，3 次元空間上の数値シ

ミュレーションおよび 2 次元平面上での実機を用いた検証実験を行うことにより，本稿で

提案する制御則および収束性解析，制御性能解析の有効性を確認する．

本研究は他の視覚情報に基づいた協調制御に関する研究 [28]-[33] とは異なり，オブザー

バ併合系で姿勢同期を示している．また，視覚情報から相対位置姿勢を推定するオブザー

バを組み込むことにより，相対位置姿勢情報のみからなる協調制御則は同様の手法により

通信などを一切用いることなくすべて実現できると考えられる．

1.3 論文構成

本論文の構成は以下の通りである．

2 章では，制御対象として剛体の運動を表す運動学モデルと剛体間の情報構造を表す可視

構造，および視覚による観測出力からなるビジュアルロボティックネットワークを提案し，


本稿の目的である視覚フィードバックによる姿勢同期を定義する．

3 章では，ビジュアルロボティックネットワークに対して視覚フィードバックによる姿勢

同期を達成する制御則を提案する．また，提案した制御則をネットワークに適用すること

により，リーダが回転運動しない場合において，あるゲイン条件の下で視覚フィードバック

による姿勢同期が達成されることを示す．

4 章では，リーダが回転運動をする場合において，制御則を適用した系の入力状態安定性

解析および入出力安定性解析を行うことによりその制御性能を評価する．

5 章では，3 次元空間上の数値シミュレーションを行うことにより，本稿で提案する制御

則および収束性解析，制御性能解析の有効性を確認する．

6 章では，2 次元平面上での実機を用いた検証実験を行うことにより，本稿で提案する制

御則および収束性解析の妥当性を確認する．

7 章でまとめと今後の課題について述べる．

第 2 章問題設定 6

第 2章

問題設定

本章では，剛体の運動を表す運動学モデルと剛体間の情報構造を表す可視構造，および視

覚による観測出力からなるビジュアルロボティックネットワークを提案する．また，本稿の

目的である視覚フィードバックによる姿勢同期を定義する．

2.1 ビジュアルロボティックネットワーク

2.1.1 剛体の運動学モデル

3 次元空間上に n 台の剛体を考える (Fig. 2.1)．3 次元空間上に固定された慣性系Σw か

ら見た剛体 i の位置，姿勢を (pwi, eξwiθwi) ∈ SE(3) または同次表現を用いて

gwi =

eξwiθwi pwi

0 1

∈ R4×4, i ∈ {1, · · · , n}

と表す．ここで，ξwi ∈ R3 は回転軸，θwi ∈ R は回転軸周りの回転角度を表す．∧ は任意

の 3 次元ベクトル a, b ∈ R3 に対して ab = a× b となり，6 次元ベクトルに対しては以下の

ボディ速度の定義に示される演算子である．また∨ は∧ の逆演算を表すものとする．以降，

表記の簡単化のために ξwiθwi を ξθwi と表すこととする．さらに，回転行列 eξθwi の歪対称

成分を

sk(eξθwi) :=1

2(eξθwi − e−ξθwi) ∈ R3×3

と定義する．


World FrameFrame

Frame Frame

Fig. 2.1: Rigid Body Motion

つぎに，各剛体のボディ速度を以下に定義する．

V bwi := g−1

wi gwi =

ωbwi vb

wi

0 0

∈ R4×4,

V bwi =

vbwi

ωbwi

=

e−ξθwi pwi

(e−ξθwi eξθwi)∨

∈ R6.

ここで，vbwi ∈ R3 は剛体 i のボディ並進速度，ωb

wi ∈ R3 はボディ角速度である．このとき，

各剛体の運動は以下の運動学モデルで記述される．

gwi = gwiVbwi, i ∈ {1, · · · , n}. (2.1)

本稿ではボディ速度 V bwi を制御入力として考え，すべての剛体のボディ並進速度は等しい，

すなわち vbwi = vb

wj∀i, jであるとする．

つぎに，剛体の相対運動に対する運動学モデルについて述べる．座標系Σi から見た座標

系Σj の相対位置姿勢を gij = (pij, eξθij) ∈ SE(3) と表すと，

gij = g−1wi gwj (∵ gwigij = gwj)

=

e−ξθwi −e−ξθwipwi

0 1

eξθwj pwj

0 1

=

e−ξθwieξθwj e−ξθwi(pwj − pwi)

0 1

. (2.2)


と導出される．(2.2) 式を時間微分すると，

gij = g−1wi gwj + g−1

wi gwj

= −g−1wi gwig

−1wi gwj + g−1

wi gwjg−1wj gwj (∵ gwig

−1wi = I4より，g−1

wi = −g−1wi gwig

−1wi )

= −g−1wi gwigij + gijg

−1wj gwj

= −V bwigij + gijV

bwj

となる (In は n × n の単位行列を表すものとする)．さらに，上式の両辺に左から g−1ij を乗

じると，

g−1ij gij = −g−1

ij V bwigij + V b

wj

となる．これより，V bij := g−1

ij gij とし，上式の両辺に∨ をとると以下の座標系Σi から見た

座標系Σj の相対運動に対する運動学モデルが求められる．

V bij = −Ad(g−1

ij )Vbwi + V b

wj. (2.3)

ここで，Ad(gij) は随伴写像表現であり，次式で定義される [42]-[44]．

Ad(gij) :=

eξθij pijeξθij

0 eξθij

∈ R6×6.

また，(2.3) 式の導出の際，以下の性質を用いている．

V ′ij = gijVijg

−1ij → V ′

ij = Ad(gij)Vij.

本稿では (2.3) 式を相対運動モデルと呼ぶこととする．また，この随伴写像表現において，

pij ∈ R3 の随伴写像Ad(pij)，eξθij の随伴写像Ad(eξθij )

はそれぞれ以下に定義される．

Ad(pij) :=

I3 pij

0 I3

, Ad(eξθij )

:=

eξθij 0

0 eξθij

.

2.1.2 可視構造

本節では，各剛体間の情報交換の構造を表す可視構造について述べる．まず，本稿では各

剛体は視覚を有し，他の可視剛体の視覚情報を抽出できると仮定する．剛体の集合を V :=

{1, · · · , n} と定義し，剛体 j が剛体 i にとって可視であるという事象を (j, i) ∈ E ⊂ V × V

と表す．さらに，剛体 i の可視剛体Ni を次式で定義する．

Ni := {j ∈ V | (j, i) ∈ E}.

本稿では剛体間の可視構造に対して，以下の仮定をおく．


1

2

4

3

5 6

Fig. 2.2: Leader-follower Type Visibility Structure (Directed Spanning Tree)

仮定 1. 　

• N1 = ∅

• |Ni| = 1 and Ni is fixed for all i ∈ V \ {1}.

• ∀i ∈ V , ∃v1, · · · , vr ∈ V s.t. v1 = 1, vr = i and (vk, vk+1) ∈ E ∀k ∈ {1, · · · , r − 1}.

ここで，|Ni| はNi の要素数を表す．この可視構造では，可視剛体をもたないリーダ (剛体

1) が 1 台存在し，他のすべての剛体は常に同一の可視剛体を 1 台もち，さらに任意の剛体

からリーダへの可視性の経路が存在する．したがって，これはリーダ追尾型の可視構造で

ある．この剛体間の可視構造は V を頂点の集合，E を辺の集合とみなすと有向グラフとみ

なすことができる (Fig. 2.2)．このとき，仮定 1 は可視構造が固定で，さらに可視剛体をも

たないリーダを根とした根付き全域木であることを示している．

2.1.3 観測出力

本稿では，各剛体 j はm (m ≥ 4) 点の特徴点が取り付けられていると仮定し，剛体 j の

座標系 Σj の原点から各特徴点までのベクトルを pjjk∈ R3, k ∈ {1, · · · ,m} とする．この

とき，剛体 i から見た特徴点 pijk:= [xijk

yijkzijk

]T ∈ R3, k ∈ {1, · · · ,m} は

pijk= gijpjjk

(2.4)

と表される．ただし，剛体 j の座標系Σj における特徴点の位置 pjjkは剛体 i にとってすべ

て既知であるとする．また，(2.4) 式の形式で表現したときに限り pijkは [pT

ijk1]T を表す

ものとする．これは，位置ベクトルの同次表現である [42]-[44]．

つぎに，各剛体の観測出力を与える．本稿では各剛体が Fig. 2.3 に示す透視投影モデ

ルをもつとする．このとき，相対的な特徴点 pijkは 3 次元情報であるため透視投影モデル

から直接得ることが不可能な情報である．そこで，剛体 i の可視剛体 j に対する観測出力


World Frame

Frame

Frame

Image Plane

Fig. 2.3: Perspective Projection Model

として透視投影モデルから得ることのできる pijkを 2 次元平面に射影した画像特徴量を

fijk:= [fxijk

fyijk]T ∈ R2, k ∈ {1, · · · ,m} と定義すると，これは

fijk=

λ

zijk

xijk

yijk

(2.5)

と表される [45]．ここで，λ ∈ R は焦点距離である．この画像平面上の fijkを縦に特徴点の

数m 個分並べたベクトルを

fij :=

fij1

...

fijm

∈ R2m.

と定義する．本稿では各剛体は画像情報から可視剛体の特徴点を抽出できると仮定する．す

なわち，剛体 i の観測出力は

fi := {fij}j∈Ni, i ∈ V (2.6)

と表される．

本稿では，運動学モデル (2.1) を有する n 台の剛体，仮定 1 を満たすリーダ追尾型可視構

造 {Ni}i∈V および観測出力 (2.6) を併せてビジュアルロボティックネットワークと呼ぶ．

2.2 ビジュアルロボティックネットワークの姿勢同期

本稿の目的はビジュアルロボティックネットワークが以下に定義される視覚フィードバッ

クによる姿勢同期を達成するボディ速度 V bwi を設計することである．


Fig. 2.4: Visual Feedback Attitude Synchronization

定義 1. 　ビジュアルロボティックネットワークが視覚フィードバックによる姿勢同期を達

成するとは，ネットワークを構成する剛体が次式を満たすことである．vbwi = vb

wj

limt→∞

φ(e−ξθwieξθwj) = 0, ∀i, j ∈ {1, · · · , n}. (2.7)

ここで，φ(eξθwi) := 12tr(I3 − eξθwi) ≥ 0 は姿勢誤差に関するエネルギ関数であり，φ(eξθwi) =

0 ⇔ eξθwi = I3 という性質をもつ (付録 C.4)．(2.7) 式はすべての剛体のボディ並進速度が

等しく，姿勢が一定値に収束することを意味する (Fig. 2.4)．

姿勢同期の定義は先行研究である文献 [13, 14] と同様のものである．本研究の目的は，文

献 [13, 14] で各剛体が e−ξθwieξθwj を利用できることを仮定しているのに対して，観測出力

(2.6) のみを用いて姿勢同期を達成する速度入力を提案することである．

第 3 章視覚フィードバックによる姿勢同期制御 12

第 3章

視覚フィードバックによる姿勢同期制御

本章では前章で定義した視覚フィードバックによる姿勢同期を達成する制御入力を提案し，

その制御入力が姿勢同期を達成することを示す．

3.1 制御則の提案

本稿では以下の制御入力を提案する．

速度入力

オブザーバ

vbwi = v (3.1a)

ωbwi = Kijsk(e

ˆξθij)∨ (3.1b)

V bij = (g−1

ij˙gij)

∨ = −Ad(g−1ij )V

bwi + uij (3.1c)

uij = Keij

eij −

− 1Keij

eξθeeijvbwi

sk(eˆξθij)∨

(3.1d)

j ∈ Ni, i ∈ V, .

ここで，Kij, Keij ∈ R は正のゲインであり，v ∈ R3 はすべての剛体に共通なボディ並進

速度である．角速度入力 (3.1b) は文献 [13] と同様の制御則であるが，3 次元情報である gij

が直接取得できないため，これを (3.1c)，(3.1d) 式で構成されるある非線形オブザーバ（以

降，視覚オブザーバと呼ぶ）から得られる推定値 gij で置き換えている．そこで，以下では

この視覚オブザーバ (3.1c)，(3.1d) の構造を説明する．これは，先行研究 [41] で提案されて

いる非線形オブザーバ (本稿ではビジュアルモーションオブザーバと呼ぶ) に修正を加えた

ものとなっている (付録B.1)．

(3.1c) 式は (2.3) 式の運動学モデルに基づき推定値 gij の運動学モデルを構成したもので

ある．ここで，gij = (pij, eˆξθij) ∈ SE(3) は相対位置姿勢 gij の推定値，V b

ij := (g−1ij

˙gij)∨ =


Relative Rigid Body MotionRelative Rigid Body Motion

Perspective Projection

Nonlinear Observer

Control Input

Fig. 3.1: Block Diagram of Visual Feedback Attitude Synchronization Law

[(vbij)

T (ωbij)

T ]T ∈ R6 はそのボディ速度である．また，uij := [uTvij uT

ωij]T ∈ R6 は推定値

gij, V bij を真値に収束させるための外部入力である．(3.1d) 式はこの外部入力 uij を決定す

る．相対位置姿勢の真値 gij と推定値 gij の偏差 geeij = (peeij, eξθeeij) ∈ SE(3) を

geeij := g−1ij gij =

e−ˆξθijeξθij e−

ˆξθij(pij − pij)

0 1

(3.2)

と定義し，推定偏差と呼ぶ．さらに，この推定偏差 geeij に対する推定偏差ベクトルを eij と

して次式で定義する．

eij :=

peeij

sk(eξθeeij)∨

∈ R6.

eij は |θeeij| < π のとき，eij = 0 ⇔ geeij = I4 という性質をもち，このとき gij = gij となり

相対位置姿勢の推定値が真値と一致する．(3.1d) 式において，eξθeeij は eij から，eij は画像

特徴量 fij から導出可能である．また，推定偏差の運動学モデルは

V beeij := (g−1

eeij geeij)∨ = −Ad(g−1

eeij)uij + V b

wj. (3.3)

と導出される (付録B.1)．本稿ではこれを推定偏差システムと呼ぶ．

速度入力 (3.1b) は剛体の運動学モデル (2.1) が V bwi を入力，[pT

wi (sk(eξθwi)∨)T ]T を出力

としたときに受動性を有する (付録A.1) ことに基づいて構成されている．また，推定偏差

の運動学モデル (3.3) は，観測対象が運動しない，すなわちV bwj = 0 のとき，入力を uij，出

力を−eij とみなすと受動性を有する (付録 B.1)．この性質を用いることにより，外部入力

(3.1d) の第一項 (uij = Keijeij) を入力とする相対位置姿勢を推定するビジュアルモーション

オブザーバが文献 [41] で提案されている (付録B.1)．これに対して，本稿で用いる視覚オブ

ザーバは視覚フィードバックによる姿勢同期を理論的に保証するため，観測対象が一定並

進速度 v で移動することを考慮に入れた文献 [41] の結果に修正を加えたものとなっている．


1

Fig. 3.2: Definition of Rigid Body Sets

制御則 (3.1) の構造をFig. 3.1 に示す．制御則 (3.1) では，観測出力である画像特徴量 fij

とその推定値 fij を用いて推定偏差ベクトル eij を構成し，運動学モデル (3.3) にフィード

バックするオブザーバ機構を内蔵している (付録B.1 および Fig. B.1 参照)．さらに，この

視覚オブザーバの推定値 gij を用いて角速度入力を構成している．したがって，本研究で提

案する制御入力は視覚情報のみに基づいて構成される．

3.2 収束性解析

前章で提案した制御則 (3.1) によってビジュアルロボティックネットワークが視覚フィー

ドバックによる姿勢同期を達成することを示す．まず，表記の簡単化のために制御および推

定に関する姿勢の偏差ベクトルとして，ecij, eeij ∈ R3 を以下に定義する．

ecij := sk(eˆξθij)∨, eeij := sk(eξθeeij)∨.

さらに，リーダ追尾型の可視構造を有する各剛体に対し，以下の表記を用いる (Fig. 3.2)．

Vp := {i ∈ V | 1 ∈ Ni}

Vq := {i ∈ V | i /∈ Nj∀j ∈ V}

Vr := {V \ ({1} ∪ Vp ∪ Vq)}

Vi := {j ∈ Vq | ∃v1, · · · , vr ∈ V s.t. v1 = j, vr = i and (vk, vk+1) ∈ E ∀k ∈ {1, · · · , r − 1}}

.

ここで，Vp ⊂ V はリーダである剛体 1 を可視剛体とする剛体の集合，Vq ⊂ V は全域木の

葉となる（自身を可視剛体にもつ剛体が存在しない）剛体の集合，Vr ⊂ V はそのどちらに

も属さない剛体の集合である．このとき，{1} ∪ Vp ∪ Vq ∪ Vr = V となることに注意する．

また，Vi ⊂ V は Vq の中で，剛体 i までの可視性の経路を有する剛体の集合を表す．


つぎに，制御偏差システムと推定偏差システムを結合した以下の偏差システムを考える． ωbij

V beeij

=

−e−ˆξθij I

0 −Ad(g−1eeij)

ωbwi

uij

+

0

V bwj

. (3.4)

ここで，I := [0 I3] ∈ R3×6 である．制御偏差システムとは相対位置姿勢の推定値 gij の運

動学モデル (3.1c) の姿勢に関する部分を抜き出したものであり，次式で表される．

ωbij = −e−

ˆξθijωbwi + uωij.

本稿ではこの偏差システム (3.4)をネットワーク間のすべての可視性でつながれる組{Ni}i∈V

について結合したシステムを集合偏差システムと呼ぶ．また，その状態は制御および推定

に関する偏差ベクトル [eTcij eT

ij]T , j ∈ Ni, i ∈ V を {Ni}i∈V に関して縦に並べたベクトル

とし，これを xe と表す．このとき，集合偏差システムに対してつぎの補題が成り立つ．

補題 1. リーダが回転運動をせず (ωbw1 = 0) ，可視構造が仮定 1 を満たすとき，制御則 (3.1)

を適用した集合偏差システムは自律システムであり，平衡点 xe = 0 をもつ．

証明. 制御則 (3.1) を (3.4) 式に代入すると，

ωbij

V beeij

=

−e−ˆξθij I

0 −Ad(g−1eeij)

Kijecij

Keij

eij −

0

ecij

+

0

ωbw1

, j ∈ Ni, i ∈ Vp,

ωbij

V beeij

=

−e−ˆξθij I

0 −Ad(g−1eeij)

Kijecij

Keij

eij −

0

ecij

+

0

Kjkecjk

,

k ∈ Nj, j ∈ Ni, i ∈ Vq ∪ Vr (3.5)

が得られる．eˆξθij およびgeeij がそれぞれecij およびeij の関数であり，さらに ωb

ij = (e−ˆξθij e

ˆξθij)∨，

V beeij = (g−1

eeij geeij)∨ であることに注意する．これは，制御則 (3.1) を適用した集合偏差シス

テムが自律システムであることを示している．また，(3.5) 式より xe = 0 が集合偏差システ

ムの平衡点であることは明らかである．

つぎに，平衡点 xe = 0 が漸近安定であることを示す．これは，本稿の主要な結果であり，

制御則 (3.1) を適用したビジュアルロボティックネットワークが視覚フィードバックによる

姿勢同期を達成することを意味する．


定理 1. リーダが回転運動をしない (ωbw1 = 0) とする．このとき，可視構造が仮定 1 を満た

し，さらに各剛体のゲインが以下の条件を満たすとき，制御則 (3.1) を適用した集合偏差シ

ステムの平衡点 xe = 0 は漸近安定である．Kjk <2KijKeij

Kij+Keij, k ∈ Nj, j ∈ Ni, i ∈ Vq

Kjk <2KijKeij

Kij+2Keij, k ∈ Nj, j ∈ Ni, i ∈ Vr

. (3.6)

証明. ポテンシャル関数としてE ≥ 0 を以下に定義する．

E :=n∑

i=2

∑j∈Ni

qi

(φ(e

ˆξθij) +1

2∥peeij∥2

2 + φ(eξθeeij)

).

ここで，qi ∈ {1, 2, 3, · · · } は i ∈ V \ Vq に関しては qi = |Vi| であり，i ∈ Vq に関しては

qi = 1 である (Fig. 3.2)．また，∥ · ∥2 はユークリッドノルムを表す．E = 0 の必要十分条

件は eˆξθij = I3, peeij = 0, eξθeeij = I3 (ecij = 0, eij = 0), j ∈ Ni

∀i ∈ V であり，それ以外の

ときはE > 0 であることに注意する．

E2 を偏差システム (3.4) の解軌道に沿って微分すると，

E =n∑

i=2

∑j∈Ni

qi

(eT

cijωbij + eT

ijAd(eξθeeij)V beeij

)(∵付録B.1補題 7の計算過程参照)

=n∑

i=2

∑j∈Ni

qi

(−eT

cij

(ωb

wi − uωij

)− eT

ij

(uij − Ad(eξθeeij)V b

wj

))(∵ (3.5)式)

となる．上式に制御入力 (3.1b)，(3.1d) を代入し，平方完成を用いてまとめると，

E = −n∑

i=2

∑j∈Ni

qi

(Kij∥ecij∥2

2 + Keij∥peeij∥22 + Keij∥ecij − eeij∥2

2 − eTeijω

bwj

)が導かれる．上式において，

∑j∈Ni

eTeijω

bwj は剛体 i ∈ Vp に対しては ωb

w1 = 0 より 0 とな

り，それ以外の剛体に対しては平方完成を用いることにより

eTeijω

bwj = Kjke

Teijecjk

=1

2Kjk

(∥eeij∥2

2 + ∥ecjk∥22 − ∥eeij − ecjk∥2

2

)(3.7)

と表せる．ここで，k ∈ Nj である．このとき，

ψi :=

−12Ki1∥eci1∥2

2 − Kei1∥eci1 − eei1∥22 − Kei1∥peei1∥2

2, i ∈ Vp

−Kij∥ecij∥22 − Keij∥ecij − eeij∥2

2 − Keij∥peeij∥22 − 1

2Kjk∥eeij − ecjk∥2

2 + 12Kjk∥eeij∥2

2,

k ∈ Nj, j ∈ Ni, i ∈ Vq

−12Kij∥ecij∥2

2 − Keij∥ecij − eeij∥22 − Keij∥peeij∥2

2 − 12Kjk∥eeij − ecjk∥2

2 + 12Kjk∥eeij∥2

2,

k ∈ Nj, j ∈ Ni, i ∈ Vr


と定義すると，E は

E =n∑

i=2

qiψi

と表される．ここで，ψi, i ∈ Vp ∪ Vr は (3.7) 式の第 2 項を含んでいることに注意する．

個々の ψi について考える．まず，剛体 i ∈ Vp に関しては ψi ≤ 0 は明らかであり，ψi =

0 ⇔ eci1 = 0, ei1 = 0, i ∈ Vp が成り立つ．また，剛体 i ∈ Vq に関しては，まず ecij に関し

て平方完成し，つぎに eeij に関して平方完成することにより，ψi は

ψi = −(Kij + Keij)

∥∥∥∥ecij −Keij

Kij + Keij

eeij

∥∥∥∥2

2

− Keij∥peeij∥22

− KijKeij

Kij + Keij

∥∥∥∥eeij −Kjk(Kij + Keij)

2KijKeij

ecjk

∥∥∥∥2

2

−Kjk(2KijKeij − Kjk(Kij + Keij))

4KijKeij

∥ecjk∥22

と変形できる．したがって，ゲイン条件 (3.6) を満たすときψi ≤ 0 となり，ψi = 0 ⇔ ecij =

0, eij = 0, ecjk = 0, k ∈ Nj, j ∈ Ni, i ∈ Vq が成り立つ．同様にして，ψi, i ∈ Vr は平方完

成することにより，

ψi = −(Kij + 2Keij)

2

∥∥∥∥ecij −2Keij

Kij + 2Keij

eeij

∥∥∥∥2

2

− Keij∥peeij∥22

− KijKeij

Kij + 2Keij

∥∥∥∥eeij −Kjk(Kij + 2Keij)

2KijKeij

ecjk

∥∥∥∥2

2

−Kjk(2KijKeij − Kjk(Kij + 2Keij))

4KijKeij

∥ecjk∥22

と変形され，ゲイン条件 (3.6) を満たせば ψi ≤ 0 かつ ψi = 0 ⇔ ecij = 0, eij = 0, ecjk =

0, k ∈ Nj, j ∈ Ni, i ∈ Vr を導くことができる．したがって，E ≤ 0 となり，制御則 (3.1)

を適用した集合偏差システムの平衡点 ecij = 0, eij = 0, j ∈ Ni∀i ∈ V，すなわち xe = 0 は

漸近安定である．

eij = 0 より gij = gij となり，また ecij = 0 より，eˆξθij = I3 となる．したがって，これら

を併せると eξθij = e−ξθwjeξθwi = I3 が導かれる．さらに，可視構造が全域木の構造をもつこ

とを考慮すると，eξθwi = eξθwj ∀i, j が得られる．以上のことから，定理 1 はビジュアルロボ

ティックネットワークが制御則 (3.1) により視覚フィードバックによる姿勢同期を達成する

ことを示していることがわかる．制御則 (3.1) では，未知情報である V bwj の並進速度がすべ

ての剛体で共通であると仮定していることを用いて，オブザーバの入力 (3.1d) の並進速度

入力部分に第二項を加えることによりその並進速度を相殺している．


定理 1 におけるゲイン条件 (3.6) について考察する．(3.6) 式においてKeij = Kij と設定

すると， Kjk < Kij, k ∈ Nj, j ∈ Ni, i ∈ Vq

Kjk < 23Kij, k ∈ Nj, j ∈ Ni, i ∈ Vr

が得られる．これは，Kij の値によらず，後方の剛体は前方の剛体よりも速く移動しなけれ

ばならないことを示している．一方で，推定に関するゲインKeij をすべての剛体で共通と

し，これをK とおくと，(3.6) 式より以下の不等式が得られる．Kij − Kjk > Kij − 2KijK

Kij+K=

Kij(Kij−K)

Kij+K, k ∈ Nj, j ∈ Ni, i ∈ Vq

Kij − Kjk > Kij − 2KijK

Kij+2K=

K2ij

Kij+2K, k ∈ Nj, j ∈ Ni, i ∈ Vr

.

したがって，剛体 i ∈ Vq に関しては，K > Kij のとき前方の剛体よりも遅く移動すること

が可能であるが，剛体 i ∈ Vr に関してはK の値によらず前方の剛体よりも速く移動しなけ

ればならないことを示している．これらの結果は，前方の剛体は後方の剛体の集団に与え

る影響が大きく，一方で後方の剛体はそれほど影響が大きくないと考えられるため，妥当

な結果であるといえる．

ロボット制御において，受動性を有するシステムに対するオブザーバ併合系の安定性の

証明は個々の制御問題や推定問題よりも非常に難しいことが知られている [46]-[48]．また，

複数の剛体を扱う同期問題ではさらに困難であると考えられる [49, 50]．これに対して，本

稿では複数の剛体の同期問題に対して通信などを一切用いず，オブザーバ併合系の安定性

を証明している．

定理 1 では ωbw1 = 0 と仮定しているが，先頭の剛体が回転運動をする場合においても後

方の剛体が群れ行動のように追従することが期待される．したがって，次章ではωbw1 = 0 で

あるときの制御性能解析を行う．

上記の証明では各剛体の ψi が ψi ≤ 0 であるという条件を用いているため，ゲイン条件

(3.6) は保守性の強いものとなっている．ゲイン条件の緩和は今後の課題とする．

第 4 章視覚フィードバックによる姿勢同期制御の安定性解析 19

第 4章

視覚フィードバックによる姿勢同期制御の

安定性解析

前章ではビジュアルロボティックネットワークが視覚フィードバックによる姿勢同期を達成

するオブザーバ併合型の制御則を提案した．しかし，収束性の証明においてリーダは回転

運動をしないという仮定を用いている．これに対して，本研究で提案する制御則は先頭の

剛体が角速度を有する場合においても後方の剛体が十分な精度・速さで姿勢を追従させる

ことが期待される．そこで，本章では先頭の剛体が角速度を有するときの制御性能解析と

して，集合偏差システムの入力状態安定性解析および入出力安定性解析を行う．

4.1 入力状態安定性解析

本節では，ビジュアルフィードバックによる姿勢同期制御問題に対して，先頭の剛体が角

速度を有するときの全体の制御および推定偏差を評価する制御性能解析の一つとして，入

力状態安定性解析を行う．

4.1.1 入力状態安定性の定義

本項では，本研究で扱う入力状態安定性の定義を与える．入力状態安定性とは，入出力

写像表現におけるシステムの安定性である入出力安定性に状態空間でのリアプノフの意味

での安定性を取り入れたものとなっており，従来の入出力安定性よりも状態空間での挙動

を厳密に表現できる．

以降，∥ · ∥L∞ は信号のL∞ ノルムを表すものとする．すなわち，

∥z∥L∞ := supt≥0

∥z(t)∥2


である．本節で取り扱う信号の空間はL∞e とする．

つぎの状態空間表現をもつ非線形システムΣxo: U → Yを考える．

Σxo

:

x = f(x, u), x(0) = xo

y = h(x, u). (4.1)

ここで，x ∈ Rn, u ∈ Rm, y ∈ Rp はそれぞれシステムの状態，入力，出力を表し，f :

Rn ×Rm 7→ Rn, h : Rn ×Rm 7→ Rp は f(0, 0) = 0, h(0, 0) = 0 を満たす十分なめらかな

関数であるとする．このようなシステムに対して，入力状態安定性を以下に導入する [51]．

定義 2. (4.1) 式の状態空間表現をもつシステムは，任意の初期状態 xo および有界な入力

u(t) に対して次式を満たす class-KL 関数 β および class-K 関数α が存在するとき入力状態

安定であるという．

∥x(t)∥2 ≤ β(∥xo∥2, t) + α(∥u∥L∞). (4.2)

ここで，α(·) が class-K 関数であるとはそれが連続な単調増加関数で α(0) = 0 であるとき

をいい，さらに limt→∞ α(t) = ∞ を満たすとき class-K∞ 関数であるという．また，α(·) が

class-L 関数であるとはそれが連続な単調減少関数で limt→∞ α(t) = 0 であるときをいい，

β(·, ·) が class-KL 関数であるとは任意の固定した s > 0, t > 0 に対して β(·, t), β(s, ·) が

それぞれ class-K，class-L 関数であるときをいうものとする．

つぎに，本節の解析で用いる入力状態安定性に関する 2 つの性質を述べる．

補題 2. システム (4.1) が入力状態安定であるための必要十分条件は，任意の初期状態 xo お

よび入力 u(t) に対して次式を満たすK∞ 関数 α0, α1, α2 が存在することである．∫ t

0

α0(∥x(s)∥2)ds ≤ α1(∥xo∥2) +

∫ t

0

α2(∥u(s)∥2)ds, ∀t ≥ 0. (4.3)

証明. 　文献 [52] を参照．

補題 3. 2 つの入力状態安定なシステムΣxo

1 : U → Y とΣξo

2 : Y → Z を直列結合したシス

テムΣ2 ◦ Σ1 は入力状態安定である．

証明. 　文献 [53] を参照．

4.1.2 2 台の剛体の入力状態安定性解析

本項では，まず 1 台のリーダ (剛体 1) と 1 台のフォロワ (剛体 2) が存在する状況を考え，

リーダの角速度を入力，フォロワのリーダに対する制御偏差および推定偏差を状態とした

系が入力状態安定であることを示す．


偏差システム (3.4) 式に制御則 (3.1) を適用した以下のシステムを考える．

ωb21

V bee21

=

−e−ˆξθ21 I

0 −Ad(g−1ee21)

K21ec21

Ke21

e21 −

0

ec21

+

0

ωbw1

. (4.4)

このシステムの入力をリーダの角速度ωbw1，状態を制御および推定に関する偏差ベクトルを

並べた [eTc21 eT

21]T とし，これを xe2 ∈ R9 と表す．このとき，制御則 (3.1) を適用した偏差

システム (4.4) に対してつぎの補題が成り立つ．

補題 4. 剛体1が回転運動をする (ωbw1 = 0)状況を考える．任意の時刻で |θ21| < π

2, |θee21| < π

2

が成り立ち，さらにある正の定数 γe2 に対して，次式を満たすとする．このとき，制御則

(3.1) を適用した偏差システム (4.4) は入力状態安定である．Ke21 > 12

Ke21(2K21−1)2K21+2Ke21−1

> 1+γe2

2γe2

.

証明. 　ポテンシャル関数としてE2 ≥ 0 を以下に定義する．

E2 := φ(eˆξθ21) +

1

2∥pee21∥2

2 + φ(eξθee21). (4.5)

(4.5) 式をシステム (4.4) の解軌道に沿って微分すると，

E2 = eTc21ω

b21 + eT

21Ad(eξθee21 )

V bee21

= −eTc21(ω

bw2 − uω21) − eT

21u21 + eT21Ad

(eξθee21 )V b

w1 (∵ (3.4)式を適用)

= −K21∥ec21∥2 − Ke21∥ec21 − ee21∥22 − Ke21∥pee21∥2

2 + eTe21ω

bw1 (∵制御入力 (3.1)を代入)

となる．ここで，eTe21ω

bw1 は正のスカラー γe2 を用いて平方完成を行うと，

eTe21ω

bw1 = −1

2γe2

∥∥∥∥ωbw1 −

1

γe2

ee21

∥∥∥∥2

2

+1

2γe2∥ωb

w1∥22 +

1

2γe2

∥ee21∥22

と表される．したがって，上式の右辺第一項は非正であるので，

E2 ≤ −K21∥ec21∥22 − Ke21∥ec21 − ee21∥2

2 − Ke21∥pee21∥22 +

γe2

2∥ωb

w1∥22 +

1

2γe2

∥ee21∥22

= −[eT

c21 pTee21 eT

e21

]I3 0 I3

0 I3 0

0 0 −I3

K21I3 0 0

0 Ke21I3 0

0 0 Ke21I3

I3 0 0

0 I3 0

I3 0 −I3

ec21

pee21

ee21

+

γe2

2∥ωb

w1∥22 +

1

2γe2

∥ee21∥22

= −xTe2Qe2xe2 +

γe2

2∥ωb

w1∥22 +

1

2γe2

∥ee21∥22 (4.6)


が得られる．ただし，

Qe2 :=

I3 0 I3

0 I3 0

0 0 −I3

K21I3 0 0

0 Ke21I3 0

0 0 Ke21I3

I3 0 0

0 I3 0

I3 0 −I3

と定義する．また，(4.6) 式の右辺第三項は

We2 :=

0 0 0

0 0 0

0 0 12γe2

I3

を用いると xT

e2We2xe2 と表せる．したがって，これを適用することにより，

Ee2 ≤ −xTe2Qe2xe2 +

γe2

2∥ωb

w1∥22 + xT

e2We2xe2 +1

2∥xe2∥2

2 −1

2∥xe2∥2

2

= −xTe2

(Qe2 − We2 −

1

2I9

)xe2 +

γe2

2∥ωb

w1∥22 −

1

2∥xe2∥2

2

が得られる．さらに，Pe2 := Qe2 − We2 − 12I9 と定義すると，Pe2 > 0 のとき上式より

Ee2 ≤γe2

2∥ωb

w1∥22 −

1

2∥xe2∥2

2

が導かれる．さらに両辺を時刻 t = 0 から T まで積分すると

Ee2(T ) − Ee2(0) ≤ 1

2γe2

∫ T

0

∥ωbw1(t)∥2

2dt − 1

2

∫ T

0

∥xe2(t)∥22dt

が得られる．したがって，∫ T

0

∥xe2(t)∥22dt ≤ 2Ee2(0) +

∫ T

0

γe2∥ωbw1(t)∥2

2dt

となる．ここで，|θ21| < π2, |θee21| < π

2のとき，

E2 = φ(eˆξθ21) +

1

2∥pee21∥2

2 + φ(eξθee21)

= 1 − cos θ21 +1

2∥pee21∥2

2 + 1 − cos θee21 (∵ φ(eξθ) = 1 − cos θ) (付録C.4))

≤ sin2 θ21 + ∥pee21∥22 + sin2 θee21

(∵ |θ21| <

π

2, |θee21| <

π

2

)= ∥ξ21 sin θ21∥2

2 + ∥pee21∥22 + ∥ξeeij sin θee21∥ (∵ ξT ξ = 1)

= ∥sk(eˆξθ21)∨∥2

2 + ∥pee21∥22 + ∥sk(eξθee21)∨∥2

2 (∵ sk(eξθ)∨ = ξ sin θ (付録C.3.3))

= ∥xe2∥22

が成り立つので，これを用いると，∫ T

0

∥xe2(t)∥22dt ≤ 2∥xe2(0)∥2

2 +

∫ T

0

γe2∥ωbw1(t)∥2

2dt


が得られる．したがって，補題 2 より，偏差システム (4.4) は入力状態安定である．

最後に，Pe2 > 0 となるときのゲインの条件を導出する．Pe2 は

Pe2 =

(K21 + Ke21 − 1

2

)I3 0 −Ke21I3

0(Ke21 − 1

2

)I3 0

−Ke21I3 0(Ke21 − 1

2γe2− 1

2

)I3

と表され，Pe2 が正定となる条件はそのシューア補元を計算することによりKe21 > 1

2

Ke21(2K21−1)2K21+2Ke21−1

> 1+γe2

2γe2

と求まる．

4.1.3 視覚フィードバックによる姿勢同期制御の入力状態安定性解析

前項では，1 台のリーダと 1 台のフォロワからなる視覚フィードバックによる姿勢同期制

御問題に対して，偏差システムが入力状態安定であることを示した．本項では，これを複

数台のフォロワに拡張する．ただし，ここで扱う可視構造のクラスは複数の剛体が一直線

上に並んだ鎖型可視構造のみを扱う．したがって，全域木となるクラスの可視構造に対し

ては，リーダから各末端までの剛体の集合を評価するものとする．

Fig. 2.2 に示されるクラスの可視構造において，先頭からある末端までのm (≤ n) 台の

剛体を抜き出し，新たに剛体 1 から剛体m と名付ける．このとき，m 台の剛体の制御およ

び推定に関する偏差システムを結合した以下のシステムを考える．

ωb21

V bee21

...

ωbm(m−1)

V beem(m−1)

=

−e−ξθec21 I

0 −Ad(g−1ee21)

. . .

−e−ξθecm(m−1) I

0 −Ad(g−1eem(m−1)

)

ωbw2

u21

...

ωbwm

um(m−1)

+

0

V bw1

...

0

V bw(m−1)

.

(4.7)

システム (4.7) は 2 台目からm 台目までの偏差システム (3.4) を結合したシステムとなって

おり，本稿ではこれを鎖型集合偏差システムと呼ぶ．


鎖型集合偏差システム (4.7) 式に制御則 (3.1) を適用すると以下のシステムが得られる．

ωb21

V bee21

...

ωbm(m−1)

V beem(m−1)

=

−e−ξθec21 I

0 −Ad(g−1ee21)

. . .

−e−ξθecm(m−1) I

0 −Ad(g−1eem(m−1)

)

=

K21ec21

Ke21

e21 −

0

ee21

...

Km(m−1)ecm(m−1)

Kem(m−1)

em(m−1) −

0

eem(m−1)

+

0

ωbw1

0...

0

. (4.8)

このシステムの入力を先頭のボディ角速度ωbw1，状態を次式に示すように2台目からm台目の

制御偏差 eci(i−1) ∈ R3 および推定偏差 ei(i−1) ∈ R6 を並べたものとし，これをxem ∈ R9(m−1)

と表す．すなわち，

xem :=

ec21

e21

...

ecm(m−1)

em(m−1)

(4.9)

とする．

システム (4.8) に対して，つぎの定理が成り立つ．

定理 2. 先頭の剛体が回転運動をする (ωbw1 = 0) 状況を考える．任意の時刻で |θi(i−1)| <

π2, |θeei(i−1)| < π

2, i ∈ {2, · · · ,m} が成り立ち，さらにある正の定数 γei, i ∈ {2, · · · ,m} に

対して，次式を満たすとき，制御則 (3.1) を適用した鎖型集合偏差システム (4.8) は入力状

態安定である． Kei(i−1) > 12

Kei(i−1)(2Ki(i−1)−1)

2Ki(i−1)+2Kei(i−1)−1> 1+γei

2γei

, i ∈ {2, · · · ,m}.


証明. 　 (4.4) 式と同様にして，剛体 i, i ∈ {2, · · · ,m} の制御則 (3.1) を適用したつぎの偏

差システムを考える．

ωbi(i−1)

V beei(i−1)

=

−e−ˆξθi(i−1) I

0 −Ad(g−1eei(i−1)

)

Ki(i−1)eci(i−1)

Kei(i−1)

ei(i−1) −

0

eci(i−1)

+

0

ωbw(i−1)

.

このとき，補題 4 より，剛体 i の偏差システムは剛体 i − 1 の角速度 ωbi−1を入力，xei :=

[eTci(i−1) eT

i(i−1)]T を状態としたとき入力状態安定である．ここで，この偏差システムの出力

を yei := ωbwi と考える．このとき，剛体 i の偏差システムの出力は剛体 i + 1 の偏差システ

ムの入力となる．したがって，補題 3 より，剛体 i の偏差システムと剛体 i + 1 の偏差シス

テムを直列結合したシステムは入力状態安定である．また，この結合されたシステムの出

力 ye(i+1)を剛体 i + 1 の角速度ωbw(i+1) とすると，これは剛体 i + 2 の偏差システムの入力と

なる．したがって，補題 3 よりこれを結合したシステムもまた入力状態安定である．同様

の操作を剛体 2 から剛体m の偏差システムまで行うと，鎖型集合偏差システム (3.5) は入

力状態安定であることが示される．

(4.2) 式より，定理 2は入力 ωbw1，状態 xem に対して

∥xem(t)∥2 ≤ β(∥xem(0), t∥) + α(∥ωbw1∥L∞)

を満たす class-KL 関数 β(·, ·) および class-K 関数 α(·) が存在することを示している．本問

題の場合ではリーダの角速度に対する制御および推定偏差を評価しているため，これは各

剛体の先頭の剛体に対する追従性能を示す指標とみなすことができる．すなわち，状態xem

をより小さく抑えられるα(·), β(·, ·) が得られる制御則を構成することで，追従性能の高い

設計が可能となる．この設計により，ネットワークを構成する各剛体はリーダの回転運動

に対しても十分な精度・速さで姿勢を追従させることが可能となる．また，この制御手法は

自然界の群れ行動の模倣やフォーメーション制御などにも応用することが期待される．具

体的なα(·), β(·, ·) の導出は今後の課題であるが，文献 [54]-[57] などを参考にすればあるゲ

イン条件のもとでは導出可能であると期待される．

前章で考察されている可視構造が全域木となるクラスの問題に対しては，すべての末端

の剛体までの鎖型集合偏差システムを考えることにより評価することが可能であると考え

られる．


4.2 入出力安定性解析

本節では，ビジュアルフィードバックによる姿勢同期制御問題に対して，先頭の剛体が角

速度を有するときの全体の制御および推定偏差を評価する制御性能解析の一つとして，入

出力安定性解析を行う．入出力安定性解析では，L2 ゲイン制御性能解析を行う．

4.2.1 入出力安定性の定義

本項では，本稿で扱う入出力安定性の評価指標である有限ゲイン L2 安定性の定義を与

える．

以降，∥ · ∥L2 は信号のL2 ノルムを表すものとする．すなわち，

∥z∥L2 :=

√∫ ∞

0

uT (t)u(t)dt

である．本節で取り扱う信号の空間はL2e とする．

つぎの入出力間の写像H : U → Y を考える．

y = Hu.

ここで，u ∈ Rm, y ∈ Rp はそれぞれ入力，出力を表す．このような写像に対して，有限ゲ

インL2 安定性を以下に導入する [51]．

定義 3. もし以下の不等式を満たす非負の定数γ0, β0 が存在するならば，写像H : Lm2e 7→ Lp

2e

は有限ゲインL2 安定であるという．

∥yτ∥L2 ≤ γ0∥uτ∥L2 + β0.

定義 3 はシステムが γ0 以下のL2 ゲインを有するということもでき，本稿ではこちらの

表現を非線形システムに対して用いる．

4.2.2 視覚フィードバックによる姿勢同期制御の入出力安定性解析

前節と同様にして鎖型可視構造をもつ剛体の集合に対して，制御則 (3.1) を適用した鎖型

集合偏差システム (4.8) を考える．この鎖状集合偏差システムの入力を先頭のボディ角速度

ωbw1, 出力を前節で状態とした xem(4.9) とする．このとき，本稿では先頭の剛体が角速度を

有するときの追従性能の指標として，L2 ゲイン制御性能解析を行う．ここでは，定義 3 を


用いて，鎖型集合偏差システム (3.5) が γ 以下のL2 ゲインを有することを示す．すなわち，

正のスカラー γ，非負のスカラー β に関してつぎの式を満たすことを示す．

∥xem∥L2 ≤ γ∥ωbw1∥L2 + β.

このL2 ゲイン制御性能解析では，入力を外乱として捉えることで，先行研究 [58]-[60] で考

察されている外乱抑制問題に関係の深い問題と考えることができる．また，本問題の場合

では先頭の剛体の角速度を入力，他の剛体の制御および推定に関する偏差を出力とするこ

とで，得られるL2 ゲインは各剛体の先頭の剛体に対する追従性能を示す指標とみなすこと

ができる．すなわち，より小さいL2 ゲインが得られる制御則を構成することで，追従性能

の高い制御則を設計することが可能となる．これにより，ネットワークを構成する各剛体

はリーダの回転運動に対しても十分な精度・速さで姿勢を追従させることが可能となる．ま

た，この制御手法は自然界の群れ行動の模倣やフォーメーション制御などにも応用するこ

とが期待される．以降，前節と同様に鎖状集合偏差システムに対してL2 ゲイン制御性能解

析を行う．

ある実対称行列 P ∈ R9(m−1)×9(m−1) を以下に定義する．

P := J − W − 1

2I9(m−1). (4.10)

ここで，W ∈ R9(m−1)×9(m−1) はある正のスカラー γ について (7, 7), (8, 8), (9, 9) 要素が 12γ2

であり，それ以外は 0 である対角行列である．また，J ∈ R9(m−1)×9(m−1) は 3× 3 ブロック

に対して対角要素 J(i, i) および非対角要素 J(i, j)(j > i) がそれぞれ以下に定義される実対

称行列である．

J(i, i) =

(K(i+1)i + Ke(i+1)i)I3 i = 3k − 2 (k = 1, · · · ,m − 1)

Ke(i+1)iI3 otherwize

J(i, j) =

−Ke(i+1)iI3 i = 3k − 2, j = i + 2 (k = 1, · · · ,m − 1)

−12K(i+1)iI3 i = 3k − 2, j = i + 5 (k = 1, · · · ,m − 1)

0 otherwize

.

例として，m = 3 のときの P を次式に挙げる．

P =

(K21 + Ke21 − 1

2

)I3 0 −Ke21I3 0 0 − 1

2K21I3

0(Ke21 − 1

2

)I3 0 0 0 0

−Ke21I3 0(Ke21 − 1

2 − 12γ2

)I3 0 0 0

0 0 0(K32 + Ke32 − 1

2

)I3 0 −Ke32I3

0 0 0 0(Ke32 − 1

2

)I3 0

−12K21I3 0 0 −Ke32I3 0

(Ke32 − 1

2

)I3

.

このとき，鎖状集合偏差システム (3.5) に対して，つぎの定理が成り立つ．


定理 3. 先頭の剛体が角速度を有する (ωbw1 = 0) 場合，Ki(i−1), Kei(i−1), i ∈ {2, · · · ,m} に

関する線形行列不等式 P > 0 が可解であるとき，制御則 (3.1) を適用した鎖型集合偏差シ

ステム (4.8) は γ 以下のL2 ゲインを有する．

まず，簡単のために 3 台の剛体からなる集合偏差システムについて証明を行う．2 台の剛

体からなるシステムについては，先行研究 [41] の部分結果であるため省略する (推定のみの

場合は付録B.2 参照)．

証明 (m=3 のとき).　ポテンシャル関数としてE3 ≥ 0 を以下に定義する．

E3 := φ(eˆξθ21) +

1

2∥pee21∥2

2 + φ(eξθee21) + φ(eˆξθ32) +

1

2∥pee32∥2

2 + φ(eξθee32).

E3をシステム (4.8) の解軌道に沿って微分すると，

E3 = eTc21ω

b21 + eT

21Ad(eξθee21 )

V bee21 + eT

c32ωb32 + eT

32Ad(eξθee32 )

V bee32

= −eTc21(ω

bw2 − uω21) − eT

21u21 + eT21Ad

(eξθee21 )V b

w1 − eTc32(ω

bw3 − uω32) − eT

32u32

+eT32Ad

(eξθee32 )V b

w2 (∵ (4.8)式を適用)

= −K21∥ec21∥22 − Ke21∥ec21 − ee21∥2

2 − Ke21∥pee21∥22 + eT

e21ωbw1 − K32∥ec32∥2

2

−Ke32∥ec32 − ee32∥22 − Ke32∥pee32∥2

2 + eTe32ω

bw2 (∵制御入力 (3.1)を代入)

となる．ここで，

eTe32ω

bw2 = K21e

Te32ec21 =

1

2K21

(∥ec21∥2

2 + ∥ee32∥22 − ∥ee32 − ec21∥2

2

)であり，また eT

e21ωbw1 は正のスカラー γ を用いて平方完成を行うと

eTe21ω

bw1 = −1

2γ2

∥∥∥∥ωbw1 −

1

γ2ee21

∥∥∥∥2

2

+1

2γ2∥ωb

w1∥22 +

1

2γ2∥ee21∥2

2

と表される．したがって，上式の右辺第一項は非正であるので，

E3 ≤ −1

2K21∥ec21∥2

2 − Ke21∥ec21 − ee21∥22 − Ke21∥pee21∥2

2 − K32∥ec32∥22 − Ke32∥ec32 − ee32∥2

2

−Ke32∥pee32∥22 +

1

2K21∥ee32∥2

2 −1

2K21∥ee32 − ec21∥2

2 +γ2

2∥ωb

w1∥22 +

1

2γ2∥ee21∥2

2

が得られる．また，上式の下線部は 13 := [1 1 1] とし，K3 ∈ R24×24, N3 ∈ R24×18, xe3 を


それぞれ

K3 := diag

{1

2K2113, Ke2113, Ke2113,

1

2K3213, Ke3213, Ke3213,−

1

2K2113,

1

2K2113

},

N3 :=

I3 0 0 0 0 0

0 I3 0 0 0 0

−I3 0 I3 0 0 0

0 0 0 I3 0 0

0 0 0 0 I3 0

0 0 0 −I3 0 I3

0 0 0 0 0 I3

−I3 0 0 0 0 I3

, xe3 :=

ec21

pee21

ee21

ec32

pee32

ee32

と定義すると−xTe3N

T3 K3N3xe3 と表せる．したがって，NT

3 K3N3 を J3 とおくと，

E3 ≤ −xTe3Jxe3 +

γ2

2∥ωb

w1∥22 +

1

2γ2∥ee21∥2

2

が導かれる．また，上式の右辺第三項は (7, 7), (8, 8), (9, 9) 要素が 12γ2 でそれ以外の要素が

0 である対角行列W3 ∈ R18×18 を用いると xTe3W3xe3 と表せる．したがって，

E3 ≤ −xTe3J3xe3 +

γ2

2∥ωb

w1∥22 + xT

e3W3xe3 +1

2∥xe3∥2

2 −1

2∥xe3∥2

2

= −xTe3

(J3 − W3 −

1

2I18

)xe3 +

γ2

2∥ωb

w1∥22 −

1

2∥xe3∥2

2

が得られる．ここで，P3 := J3 − W3 − 12I18 とすると，P3 > 0 であるとき，上式の右辺第

一項は非正となり，

E3 ≤γ2

2∥ωb

w1∥22 −

1

2∥xe3∥2

2

が導かれる．さらに両辺を時刻 t = 0 から T まで積分すると

E3(T ) − E3(0) ≤ 1

2γ2

∫ T

0

∥ωbw1(t)∥2

2dt − 1

2

∫ T

0

∥xe3(t)∥2dt

が得られ，これより

∥xe3∥L2 ≤ γ∥ωbw1∥L2 +

√2E3(0)

が導出される．したがって，定義 3 より，P3 > 0 のとき集合偏差システムは γ 以下の L2

ゲインを有することが示される．


つぎに，P3 > 0 となるときのゲイン条件について考察する．P3 は

P3 =

2

6

6

6

6

6

6

6

6

6

4

`

K21 + Ke21 − 12

´

I3 0 −Ke21I3 0 0 − 12K21I3

0`

Ke21 − 12

´

I3 0 0 0 0

−Ke21I3 0“

Ke21 − 12− 1

2γ2

”

I3 0 0 0

0 0 0`

K32 + Ke32 − 12

´

I3 0 −Ke32I3

0 0 0 0`

Ke32 − 12

´

I3 0

− 12K21I3 0 0 −Ke32I3 0

`

Ke32 − 12

´

I3

3

7

7

7

7

7

7

7

7

7

5

と表され，この P3 が正定となる条件はそのシューア補元を計算することで

Ke21 > 12

γ2 4K21Ke21−2K21−4Ke21+12K21+2Ke21−1

> 1

Ke32 > 12

2K32Ke32−Ke32

2K32+2Ke32−1− K2

21

4K21+4Ke21−2

− 2γ2K221K2

e21

(2K21+2Ke21−1)(γ2(4K21Ke21−2K21−4Ke21+1)−(2K21+2Ke21−1))> 1

2

として与えられる．上式より，各剛体間のゲインの関係やゲインと γ の値の関係を導出す

ることは非常に困難であり，今後の課題とする．

つぎに，m 台の剛体について定理 3 の証明を与える．

証明. ポテンシャル関数としてEm ≥ 0 を以下に定義する．

Em :=m∑

i=2

(φ(e

ˆξθi(i−1)) +1

2∥peei(i−1)∥2

2 + φ(eξθeei(i−1))

).

Em をシステム (4.8) の解軌道に沿って微分すると，3 台のときの証明と同様にして

Em =m∑

i=2

(eT

ci(i−1)ωbi(i−1) + eT

i(i−1)Ad(eξθeei(i−1))V beei(i−1)

)= −

m∑i=2

(Ki(i−1)∥eci(i−1)∥2 + Kei(i−1)∥peei(i−1)∥2 + Kei(i−1)∥eci(i−1) − eei(i−1)∥2

−eTei(i−1)ω

bw(i−1)

)≤ −xT

emPxem +γ2

2∥ωb

w1∥22 −

1

2∥xem∥2

2

が導かれる．ここで，P ∈ R9(m−1)×9(m−1) は (4.10) 式で与えられ，γ は正のスカラーであ

る．P > 0 であるとき，上式の両辺を時刻 t = 0 から T まで積分することで，

Em(T ) − Em(0) ≤ 1

2γ2

∫ T

0

∥ωbw1(t)∥2

2dt − 1

2

∫ T

0

∥xem(t)∥2dt

が得られ，これより

∥xem∥L2 ≤ γ∥ωbw1∥L2 +

√2Em(0)

が導出される．したがって，P > 0 のとき集合偏差システムは γ 以下の L2 ゲインを有す

る．


Ki(i−1), Kei(i−1) に関する線形行列不等式P > 0 の可解性や可解のときの各剛体間のゲイ

ンの関係については今後の課題である．以降，様々な状況における線形行列不等式 P > 0

の解を紹介する．

まず，γ = 1 と設定したときのP > 0 の解を剛体が 4 台，5 台，6 台の場合について以下

に示す．

4台 :

K21 = 4.49, K32 = 7.93, K43 = 11.1

Ke21 = 7.01, Ke32 = 9.97, Ke43 = 10.54

5台 :

K21 = 4.42, K32 = 7.64, K43 = 12.16, K54 = 16.21

Ke21 = 8.64, Ke32 = 13.07, Ke43 = 15.63, Ke54 = 15.29

6台 :

K21 = 4.38, K32 = 7.32, K43 = 11.16, K54 = 16.59, K65 = 21.53

Ke21 = 10.63, Ke32 = 16.22, Ke43 = 20.25, Ke54 = 22.29, Ke65 = 21.06.

ここで，解の導出の基準として，∑m

i=2

(Ki(i−1) + Kei(i−1)

)を最小化することを目的とした．

これより，台数が増加するにつれてゲインが増加していることがわかる．また，後方の制御

に関するゲインは前方のゲインよりも大きいことがわかる．

つぎに，4 台，5 台，6 台の場合にそれぞれゲイン制約Ki(i−1), Kei(i−1) < 20 を課したと

きの γ を最小化することを目的とした解を以下に示す．

4台 :

γ = 0.373

K21 = 11.28, K32 = 18.99, K43 = 19.99

Ke21 = 19.98, Ke32 = 19.99, Ke43 = 19.99

5台 :

γ = 0.4793

K21 = 7.18, K32 = 11.28, K43 = 18.89, K54 = 19.99

Ke21 = 19.98, Ke32 = 19.99, Ke43 = 19.99, Ke54 = 19.99

6台 :

γ = 0.691

K21 = 4.35, K32 = 7.18, K43 = 11.29, K54 = 18.74, K65 = 19.99

Ke21 = 19.94, Ke32 = 19.97, Ke43 = 19.98, Ke54 = 19.99, Ke65 = 19.99

.

上記の結果から，推定に関するゲインは γ を小さくするためにほとんど制約 (20未満) の限

界に近い値を用いていることがわかる．制御に関するゲインに着目すると，最後方の剛体

は制約に近い値を用いているが，前方にいくにしたがって剛体のゲインの値は徐々に小さ

くなっている．したがって，収束性解析のときと同様に，追従性能を良くするためには前方

の制御に関するゲインは後方のゲインよりもある程度小さくしなければならないことがわ

かる．また，台数が増加するにつれて γ の最小値が大きくなっていることがわかる．これ


は，台数が増加することにより出力である推定偏差と制御偏差が増加することから妥当な

結果であると考えられる．

本節のL2 ゲイン制御性能解析では γ を追従性能の指標として考えている．本節では，こ

の問題を先頭の角速度 ωbw1を外乱とみなした外乱抑制問題のひとつとして考えられている

が，これは外乱 ωbw1 から制御出力 xe までのL2 ゲインを有するH∞ 最適問題などに拡張す

ることが期待される．

前章で考察されている可視構造が全域木となるクラスの問題に対しては，すべての末端

の剛体までの鎖型集合偏差システムを考えることにより評価することが可能であると考え

られる．

第 5 章数値シミュレーション 33

第 5章

数値シミュレーション

本章では前章までに得られた結果に対して，数値シミュレーションにより妥当性と有効性

を確認する．本章では特に収束性解析と入出力安定性解析について検証する．

5.1 シミュレーション1 (収束性の検証)

1

2

4

3

5

Fig. 5.1: Visibility Graph in Simulation

本節では数値シミュレーションによりリーダが回転運動をしない (ωbw1 = 0 [rad/s]) とき

にビジュアルロボティックネットワークが視覚フィードバックによる姿勢同期を達成するこ

とを示す．

Fig. 5.1 の可視構造をもつ 5 台の剛体を考え，初期状態を以下のように設定する．

pw1(0) = [5 − 5 5]T [m], ξθw1(0) = [0 π4

0]T [rad],

pw2(0) = [0 0 0]T [m], ξθw2(0) = [0 0 0]T [rad],

pw3(0) = [0 0 − 5]T [m], ξθw3(0) = [0 − π4

0]T [rad],

pw4(0) = [−5 − 5 − 5]T [m], ξθw4(0) = [0 π3

0]T [rad],

pw5(0) = [−5 0 − 10]T [m], ξθw5(0) = [0 0 0]T [rad].


-10 -5 05 10 15 20

-5

0

5-10

-5

0

5

10

15

xw

[m]yw

[m]

z w [m

]

pw1

pw2

pw3

pw4

pw5

Fig. 5.2: Positions in Σw

0 5 10 15

-0.1

-0.05

0

0.05

0.1

0.15

-0.15Time [s]

ξθx

ξθx21

ξθx32

ξθx42

ξθx54

[rad

]

Fig. 5.3: Relative Rotation Angles about

x-axis

0 5 10 15-1.5

-1

-0.5

0

0.5

1

Time [s]

ξθy

ξθy21

ξθy32

ξθy42

ξθy54

[rad

]


y-axis

0 5 10 15-0.1

-0.05

0

0.05

0.1

Time [s]

ξθz

ξθz21

ξθz32

ξθz42

ξθz54

[rad

]


z-axis

すべての剛体に共通なボディ並進速度は v = [0 0 1]T [m/s] で固定，各剛体の角速度入力

に関するゲインはゲイン条件 (3.6) を満たすようにK21 = 1.8, K42 = 2.3, K32 = K54 = 3

と設定し，オブザーバの入力に関するゲインはすべて 5 とする．

数値シミュレーションの結果を Figs. 5.2-5.5 に示す．Fig. 5.2 は各剛体の慣性系から見

た位置を示したものであり，Figs. 5.3-5.5 の横，縦軸はそれぞれシミュレーション時刻と

各剛体間の相対姿勢 ξθij の x, y, z 要素を示している．Fig. 5.2 より，初め任意の方向に進

んでいた剛体が最終的にリーダである剛体 1 と同じ方向に進んでいることがわかる．また，

Figs. 5.3-5.5より，およそ 10 秒ですべての剛体の姿勢がリーダの姿勢と同じ値に収束して

いることがわかる．以上のことから，提案した制御則 (3.1) によりビジュアルロボティック

ネットワークが視覚フィードバックによる姿勢同期を達成することが確認される．

5.2 シミュレーション2 (入出力安定性解析の検証)

本節では前章で示した入出力安定性解析の有効性を数値シミュレーションにより確認する．


-100

1020

3040

-10

0

10

20

0

20

40

xw

[m]yw

[m]

z w [m

]

pw1

pw2

pw3

pw4

pw5


0 10 20 30 40 50 60-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

time [s]

ξθx

ξθ

wx1ξθ

wx2ξθ

wx3ξθ

wx4ξθ

wx5[rad

]

Fig. 5.7: Rotation Angles about x-axis

0 10 20 30 40 50 60-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

time [s]

ξθy

ξθ

wy1ξθ

wy2ξθ

wy3ξθ

wy4ξθ

wy5[rad

]

Fig. 5.8: Rotation Angles about y-axis

0 10 20 30 40 50 60-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

time [s]

ξθz

ξθ

wz1ξθ

wz2ξθ

wz3ξθ

wz4ξθ

wz5[rad

]

Fig. 5.9: Rotation Angles about z-axis

シミュレーション 1 と同様の可視構造，ボディ並進速度をもつ 5 台の剛体を考え，初期

状態を以下のように設定する．

pw1(0) = [5 − 5 5]T [m], ξθw1(0) = [0 π4

0]T [rad],

pw2(0) = [0 0 0]T [m], ξθw2(0) = [0 0 0]T [rad],

pw3(0) = [0 0 − 5]T [m], ξθw3(0) = [0 − π4

0]T [rad],

pw4(0) = [−5 − 5 − 5]T [m], ξθw4(0) = [0 π3

0]T [rad],

pw5(0) = [−5 0 − 10]T [m], ξθw5(0) = [0 0 0]T [rad].

また，リーダとなる剛体 1 の角速度入力は以下のように設定する．

ωb1(t) =

[0.05 0 0]T t ∈ [0, 10)

[0 − 0.05 0]T t ∈ [10, 20)

[−0.05 0 0]T t ∈ [20, 40)

[0 0 0]T t ∈ [40, 60)

[rad/s].

数値シミュレーションの結果を Figs. 5.6-5.15 に示す．Figs. 5.6-5.9 は各剛体のゲイン

をそれぞれ K21 = 4.49, K32 = K42 = 7.93, K54 = 11.1, Ke21 = 7.01, Ke32 = Ke42 =


-100

1020

3040

-10

0

10

20

0

20

40

xw

[m]yw

[m]

z w [m

]

pw1

pw2

pw3

pw4

pw5


0 10 20 30 40 50 60-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

time [s]

ξθx

ξθ

wx1ξθ

wx2ξθ

wx3ξθ

wx4ξθ

wx5[rad

]

Fig. 5.11: Rotation Angles about x-axis

0 10 20 30 40 50 60-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

time [s]

ξθy

ξθ

wy1ξθ

wy2xq

wy3ξθ

wy4ξθ

wy5[rad

]

Fig. 5.12: Rotation Angles about y-axis

0 10 20 30 40 50 60-2

-1.5

-1

-0.5

0

0.5

1

1.5

2

time [s]

ξθz

ξθ

wz1ξθ

wz2ξθ

wz3ξθ

wz4ξθ

wz5[rad

]

Fig. 5.13: Rotation Angles about z-axis

9.97, Ke54 = 10.54 と設定したときのものである．これは，剛体 1 から剛体 5 までの 4 台の

剛体の集合に対し，先頭の角速度 ωbw1 から集合全体の偏差 xe までが γ = 1 のL2 ゲイン性

能を有する設定となっている．Figs. 5.6-5.9 より，各剛体はリーダの動きに十分追従してい

ることがわかる．

一方で，Figs. 5.10-5.13 は各剛体のゲインをそれぞれ K21 = 11.28, K32 = K42 =

18.99, K54 = 19.99, Ke21 = 19.98, Ke32 = Ke42 = 19.99, Ke54 = 19.99 と設定したと

きのものである．これは，剛体 1 から剛体 5 までの 4 台の剛体の集合に対し，先頭の角速

度 ωbw1 から集合全体の偏差 xe までが γ = 0.373 のL2 ゲイン性能を有する設定となってい

る．Figs. 5.10-5.13 より，先ほどのシミュレーション結果と同様に各剛体はリーダの動きに

十分追従していることがわかる．また，Figs. 5.7-5.9 とFigs. 5.11-5.13 を比較すると，リー

ダとの姿勢誤差が 2 つめのシミュレーションの方が小さくなっていることがわかる．これ

を明らかにするために Fig. 5.14 および Fig. 5.15 にそれぞれ γ = 1 および γ = 0.373 のと

きの ∥xe∥2 を示す．これらの結果から，γ = 0.373 の場合の方が γ = 1 の場合と比べると追

従性能が改善されていることがわかる．以上のことから，L2 ゲイン制御性能解析における

γ が追従性能を表す指標となっていることが確認される．


0 10 20 30 40 50 600

0.2

0.4

0.6

0.8

1

time [s]

||xe||

(=1

)2

Fig. 5.14: Control Performance (γ = 1)

0 10 20 30 40 50 600

0.2

0.4

0.6

0.8

1

time [s]

||xe||

(=0

.373

)2

Fig. 5.15: Control Performance (γ = 0.373)

第 6 章検証実験 38

第 6章

検証実験

本章では 2 次元平面上を動く 3 台の剛体を用いた検証実験を行うことにより制御入力 (3.1)

の妥当性を確認する．本章では特に収束性解析について検証する．

6.1 実験環境

検証実験では剛体として 3 台の車輪型倒立振子 e-nuvo WHEEL(ZMP 社) を用いる．ま

た，各剛体に搭載するカメラとして 30[fps] のフレームレートをもつ小型無線カメラ RC-

12(RF SYSTEM社) を用いる．小型無線カメラからの画像信号は無線チューターを介し

て画像ボード PICOLO DILLIGENT(Euresys 社) より PC に取り込まれ，画像処理ソフ

ト HALCON(MVTec 社) を用いてカメラ画像上での観測対象の剛体の特徴点の座標を抽

出する．そして Simulink(The Math Works 社)，DSP ボード DS1104(dSPACE 社) によ

り各剛体への速度制御入力を計算し，無線通信機器WiPort(LANTRONIX 社) を介して

各剛体に速度入力信号を送信する (Fig. 6.1)．この操作を約 33[ms] 周期で繰り返し実験

を行う．また，実験データの収集のため，フィールド全体を観測するカメラとしてMTV-

7310(komoto 社) を実験フィールド上空に設置する．このカメラの画像信号は画像ボード

Picpot-Color(Leutron Vision 社) よりPC に取り込まれ，前述の制御系の実験システムと同

様にHALCON, Simulink, DS1104 により解析され，実験データとして保存する．

本検証実験で用いる可視構造は Fig. 6.2 に示すものとする．これは 3 台で構成される全

域木のなかで最も簡単なもののひとつである．また，各剛体の初期位置姿勢は以下のよう

に設定する．

pw1(0) = [0.823 0.682 0]T [m], ξθw1(0) = [0 0 2.563]T [rad],

pw2(0) = [1.315 0.572 0]T [m], ξθw2(0) = [0 0 2.978]T [rad],

pw3(0) = [1.663 0.421 0]T [m], ξθw3(0) = [0 0 2.800]T [rad].


Fig. 6.1: Experimental Environment

123

Fig. 6.2: Visibility Graph in Experiment

さらに，すべての剛体に共通なボディ並進速度は [0 0.04 0]T [m/s] で固定とし，各剛体の

ゲインはKe21 = Ke32 = 15, K21 = K32 = 1 とする．これはゲイン条件 (3.6) を満たす設定

となっている．

6.2 姿勢同期制御の検証実験

実験結果を Figs. 6.3-6.8 に示す．Figs. 6.3, 6.4 は各剛体の慣性系における位置と姿勢角

を，Figs. 6.5, 6.6 は剛体 2 の座標系から見た剛体 1 の，Figs. 6.7, 6.8 は剛体 3 の座標系か

ら見た剛体 2 の相対位置姿勢の真値（天井カメラによる観測値）とオブザーバによる推定

値を示している．Figs. 6.5-6.8 より，相対位置姿勢を十分推定できていることが確認でき

る．また，この相対姿勢の推定値を用いて角速度入力を決定した結果，Fig. 6.4 より初期状

態では任意の姿勢であった剛体が時間が経過するにつれて姿勢同期を達成しようとしてい

ることが確認される．実験では多少の誤差が生じているが，これは倒立振子の振動や画像

認識の精度の低さ，無線カメラのノイズによるものであると考えられ，さらに実験システ

ムを改良することにより誤差は軽減できると期待される．以上のことから，制御入力 (3.1)

の妥当性が確認される．


0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6 1.80

0.2

0.4

0.6

0.8

1

1.2

1.4

xw

[m]

y w [m

]

Agent 1Agent 2Agent 3


0 5 10 15 20 25 302.2

2.4

2.6

2.8

3

3.2

Time [s]

Ori

enta

tion

[rad

]

Agent 1Agent 2Agent 3

Fig. 6.4: Rotation Angles in Σw

0 5 10 15 20 25 30-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

Time [s]

Rel

ativ

e Po

sitio

n [m

]

px21

py21

pesx21

pesy21

Fig. 6.5: Estimation Error of Position be-

tween 2 and 1

0 5 10 15 20 25 30-0.5

-0.4

-0.3

-0.2

-0.1

0

0.1

0.2

Time [s]

Rel

ativ

e O

rien

tatio

n [r

ad]

ξ θz21

ξ θesz21

Fig. 6.6: Estimation Error of Rotation

Angle between 2 and 1

0 5 10 15 20 25 30-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

0.6

Time [s]

Rel

ativ

e Po

sitio

n [m

] px32

py32

pesx32

pesy32

Fig. 6.7: Estimation Error of Position be-

tween 3 and 2

0 5 10 15 20 25 30-0.2

-0.1

0

0.1

0.2

0.3

0.4

0.5

Time [s]

Rel

ativ

e O

rien

tatio

n [r

ad]

ξ θz32

ξ θesz32

Fig. 6.8: Estimation Error of Rotation

Angle between 3 and 2

第 7 章まとめ 41

第 7章

まとめ

本稿では運動学モデルをもつ剛体による視覚情報に基づいた姿勢同期問題について考察し

た．まず，制御対象として剛体の運動を表す運動学モデルと剛体間の情報構造を表すリー

ダ追尾型可視構造，および視覚による観測出力からなるビジュアルロボティックネットワー

クを提案した．つぎに，このネットワークに対して視覚フィードバックによる姿勢同期を定

義し，これを達成する制御則を提案した．本問題では前提として各剛体は自身の位置姿勢

情報や近傍との通信による情報は得られず，近傍の視覚情報のみが取得可能であるとして

いる．必要な視覚情報は各剛体から視覚センサで抽出可能な画像特徴量のみに限定し，各

剛体は非線形オブザーバを内蔵することにより可視剛体との相対位置姿勢を推定し，その

推定値を用いて姿勢同期を達成する入力を決定する．この制御則を適用することで，リー

ダが回転運動をしない場合に，ビジュアルロボティックネットワークが姿勢同期を達成する

ことを示した．また，リーダが回転運動をする状況に対おいては，鎖型の可視構造を有す

る剛体の集合に対して，入力状態安定性解析および入出力安定性解析を行うことにより各

剛体の集合がある程度の追従性能を有することを示した．また，本稿で得られた結果に対

して数値シミュレーションおよび実機による検証実験によりその有効性を確認した．

今後の課題として以下のものが挙げられる．

• リーダが角速度をもつときの入力状態安定性に関するさらなる解析．

• リーダが角速度をもつときの入出力安定性に関するさらなる解析．具体的には，得ら

れた線形行列不等式の可解性やゲイン条件の考察など．

• より広いクラスの可視構造における制御則の提案および収束性解析．具体的には，互

いに可視であるとき（双方向グラフ）や複数台可視であるとき（閉路を有するグラフ）

の考察．

第 7 章まとめ 42

• 可視性維持問題の考察や収束の速さの解析．

• 位置も考慮に入れた視覚フィードバックによる位置姿勢同期問題．

謝辞 43

謝辞

本研究を進めるにあたり，終始的確なご指導をいただいた藤田政之教授に深く感謝の意を

表します．また，本研究に留まらず，共同研究を行うにあたりましても，現場に根付いた考

え方や研究の取り組み方について明確なご指導をいただき，大変勉強になりました．さら

に，TA や研究室の運営に関しましても，社会人として必要な多くのことを教えていただき

ました．

また，本研究を行うにあたり，数多くの具体的なご指導を受け賜わりました畑中健志助

教に深く感謝致します．理論的知識と問題解決に対する多彩な視点は大変参考になりまし

た．また，研究室での生活においても大変お世話になりました．

研究室における様々な事務を処理していただいた秘書の松石敦子氏，西山陽子氏に深く

感謝致します．

研究活動にとどまらず，研究室運営から研究生活面でいろいろと相談に乗っていただい

た村尾俊幸氏に深く感謝致します．

同種の研究内容の先輩として，様々な指導をいただいた小林尚斗氏に深く感謝いたしま

す. また，研究室での生活でお世話になり, 研究面において的確な助言をいただいた斎藤護

氏，宮野竜也氏，岩渕教郎氏に感謝致します．

自分の拙い英語に苦労されながらも，共に研究生活を送り，研究以外でも国際的視点から

いろいろなことを教えていただいた Jay Wagenpfeil 氏，Clemens Schaefer 氏，Jan Eikens

氏，Andreas Ulrich 氏，Stefan Berger 氏に感謝致します．

そして，同期として共に学んだ後藤高英氏，後藤達彦氏，森岡博史氏，星陽介氏に深

く感謝します．

また，研究室の後輩として大変お世話になりました伊藤広矩氏，高木拓人氏，西敬之

氏，野村彰浩氏，岡崎陽平氏，須永智氏，花井諭司氏，Risvan Dirza 氏に感謝致します．

最後に, 自分の生活を暖かく支え, 見守ってくださった両親に深く感謝致します.

参考文献 44

参考文献

　

[1] F. Zhang and N. Leonard, “Cooperative Filters and Control for Cooperative Explo-

ration,” IEEE Trans. on Automatic Control, Vol. 55, No. 3, pp. 650–663, 2010.

[2] N. E. Leonard, D. A. Paley, F. Lekien, R. Sepulchre, D. M. Fratatoni and R. E. Davis,

“Collective Motion, Sensor Networks and Ocean Sampling,” Proc. of the IEEE, Vol.

95, No. 1, pp. 48–74, 2007.

[3] 飯野穣，畑中健志，藤田政之，センサネットワークと制御理論，計測と制御，Vol. 47,

No. 8, pp. 649–656, 2008.

[4] P. Ogren, E. Fiorelli and N. E. Leonard, “Cooperative Control of Mobile Sensor Net-

works: Adaptive Gradient Climbing in A Distributed Environment,” IEEE Trans. on

Automatic Control, Vol. 49, No. 8, pp. 1292–1302, 2004.

[5] R. Olfati-Saber, J. A. Fax and R. M. Murray, “Consensus and Cooperation in Net-

worked Multi-Agent Systems,” Proc. of the IEEE, Vol. 95, No. 1, pp. 215–233, 2007.

[6] W. Ren and R. W. Beard, Distributed Consensus in Multi-vehicle Cooperative Control,

Springer, 2008.

[7] A. Jadbabaie, J. Lin and A. S. Morse, “Coordination of Groups of Mobile Autonomous

Agents Using Nearest Neighbor Rules,” IEEE Trans. on Automatic Control, Vol. 48,

No. 6, pp. 988–1001, 2003.

[8] H. Tanner, A. Jadbabaie and G. J. Pappas, “Flocking in Fixed and Switching Net-

works,” IEEE Control System Magazine, Vol. 27, No. 4, pp. 75–88, 2007.

[9] J. Cortes, S. Martinez, T. Karastas and F. Bullo, “Coverage Control for Mobili Sensing

Networks,” IEEE Trans. on Robotics and Automation, Vol. 20, No. 2, pp. 243–255,

2004.

参考文献 45

[10] J. A. Marshall, M. E. Broucke and B. A. Francis, “Formations of Vehicles in Cyclic

Pursuit,” IEEE Trans. Automatic Control , Vol. 49, No. 11, pp. 1963–1974, 2004.

[11] A. Jadbabaie, N. Motee and M. Barahona, “On the Stability of the Kuramoto Model

of Coupled Nonlinear Oscillators,” Proc. of the 2004 American Control Conference,

pp. 4296–4301, 2004.

[12] N. Chopra and M. W. Spong, “Passivity-based Control of Multi-agent Systems,” in

Advances in Robot Control: From Everyday Physics to Human-Like Movements, S.

Kawamura and M. Svnin, eds., pp. 107–134, Springer, 2006.

[13] Y. Igarashi, T. Hatanaka, M. Fujita and M. W. Spong, “Passivity-based Attitude

Synchronization in SE(3),” IEEE Trans. on Control Systems Technology, Vol. 17, No.

5, pp. 1119–1134, 2009.

[14] Y. Igarashi, T. Hatanaka, M. Fujita and M. W. Spong, “Passivity-based Output Syn-

chronization and Flocking Algorithm in SE(3),” Proc. of the 47th IEEE Conference on

Decision and Control, pp. 1024–1029, 2008.

[15] A. Sarlette, R. Sepulchre and N. E. Leonard, “Autonomous Rigid Body Attitude Syn-

chronization,” Automatica, Vol. 45, No. 2, pp. 572–577, 2009.

[16] H. Bai, M. Arcak and J. T. Wen “Rigid Body Attitude Coordination without Inertial

Frame Information,” Automatica, Vol. 44, No. 12, pp. 3170–3175, 2008.

[17] M. Arcak, “Passivity as a Design Tool for Group Coordination,” IEEE Trans. on

Automatic Control, Vol. 52, No. 8, pp. 1380–1390, 2007.

[18] F. Bullo, J. Cortes and S. Martinez, Distributed Control of Robotic Networks, Princeton

Series in Applied Mathematics, 2009.

[19] R. M. Murray, “Recent Research in Cooperative Control of Multivehicle Systems,”

Journal of Dynamic Systems Measurement and Control- Trans. of the Asme, Vol. 129,

No. 5, pp. 571–583, 2007.

[20] J. A. Fax and R. M. Murray, “Information Flow and Cooperative Control of Vehicle

Formations,” IEEE Trans. on Automatic Control, Vol. 49, No. 9, pp. 1465–1476, 2004.

参考文献 46

[21] P. Varaiya, “Smart Cars on Smart Roads: Problems of Control,” IEEE Transactions

on Automatic Control, Vol. 38, No. 2, pp. 195–207, 1993.

[22] F. Chaumette and S. A. Hutchinson, “Visual Servoing and Visual Tracking,” in

Springer Handbook of Robotics, B. Siciliano and O. Khatib, eds., Springer-Verlag, pp.

563–583, 2008.

[23] V. Lippiello, B. Siciliano and L. Villani, “Position-based Visual Servoing in Industrial

Multirobot Cells Using A Hybrid Camera Configuration,” IEEE Trans. on Robotics,

Vol. 23, No. 1, pp. 73–86, 2007.

[24] N. R. Gans and S. A. Hutchinson, “Stable Visual Servoing through Hybrid Switched-

system Control,” IEEE Trans. on Robotics, Vol. 23, No. 3, pp. 530–540, 2007.

[25] G. Hu, W. MacKunis, N. Gans, W. E. Dixonm, J. Chen, A. Behal and D. Dawson,

“Homography-based Visual Servo Control with Imperfect Camera Calibration,” IEEE

Trans. on Automatic Control, Vol. 54, No. 6, pp. 1318–1324, 2009.

[26] T. Ding, M. Sznaier and O. Camps, “Receding Horizon Rank Minimization Based

Estimation with Applications to Visual Tracking,” Proc. of the 47th IEEE Conference

on Decision and Control, pp. 3446–3451, 2008.

[27] J. Saunders and R. W. Beard, “Reactive Vision Based Obstacle Avoidance with Camera

Field of View Constraints,” AIAA Guidance, Navigation and Control Conference and

Exhibit, 2008.

[28] N. Moshtagh, N. Michael, A. Jadbabaie and K. Daniilidis, “Vision-based, Distributed

Control Laws for Motion Coordination of Nonholonomic Robots,” IEEE Trans. on

Robotics, Vol. 25, No. 4, pp. 851–860, 2009.

[29] N. Moshtagh, A. Jadbabaie and K. Daniilidis, “Vision-based, Distributed Coordination

and Flocking of Multi-agent Systems,” in Robotics: Science and Systems I, S. Thrun,

G. S. Sukhatme, S. Schaal and O. Brock, eds., The MIT Press, pp. 41–48, 2005.

[30] F. Morbidi, F. Bullo and D. Prattichizzo, “On Leader-follower Visibility Maintenance

for Dubins-like Vehicles via Controlled Invariance,” Proc. of the 47th IEEE Conference

on Decision and Control, pp. 1821–1826, 2008.

参考文献 47

[31] F. Morbidi, G. L. Mariottini and D. Prattichizzo, “Observer Design via Immersion and

Invariance for Vision-based Leader-follower Formation Control,” Automatica, Vol. 46,

No. 1, pp. 148–154, 2010.

[32] J. L. Giesbrecht, H. K. Goi, T. D. Barfoot and B. A. Francis, “A Vision-based Robotic

Follower Vehicle,” Proc. of the SPIE Defence, Security and Sensing, Vol. 7332, pp.

14-17, 2009.

[33] R. Vidal, O. Shakernia and S. Sastry, “Following The Flock,” IEEE Robotics & Au-

tomation Magazine, Vol. 11, No. 4, pp. 14–20, 2004.

[34] H. G. Tanner, G. J. Pappas and V. Kumar, “Leader-to-Formation Stability,” IEEE

Trans. on Robotics and Automation, Vol. 20, No. 3, pp. 443–455, 2004.

[35] X. Chen and A. Serrani, “ISS-Based Robust Leader/Follower Trailing Control,” Group

Coordination and Cooperative Control, K. Y. Pettersen, J. T. Gravdahl and H. Nijmei-

jer, eds, Springer-Verlag, pp. 75–91, 2006.

[36] R. Lozano, M. W. Spong, J. A. Guerrero and N. Chopra, “Controllability and Observ-

ability of Leader-Based Multi-agent Systems,” Proc. of the 47th IEEE Conference on

Decision and Control, pp. 3713–3718, 2008.

[37] M. Ji and M. Egerstedt, “A Graph-Theoretic Characterization of Controllability for

Multi-agent Systems,” Proc. of the 2007 American Control Conference, pp. 4588–4593,

2007.

[38] A. Rahmani and M. Mesbahi, “On the Controlled Agreement Problem,” Proc. of the

2006 American Control Conference, pp. 1376–1381, 2006.

[39] H. G. Tanner, “On the Controllability of Nearest Neighbor Interconnections,” Proc. of

the 43rd IEEE Conference on Decision and Control, pp. 2467–2472, 2004.

[40] M. Fujita, T. Hatanaka, N. Kobayashi, T. Ibuki and M. Spong, “Visual Motion

Observer-based Pose Synchronization: A Passivity Approach,” Proc. of the 48th IEEE

Conference on Decision and Control and 28th Chinese Control Conference, pp. 2402–

2407, 2009.

[41] M. Fujita, H. Kawai and M. W. Spong, “Passivity-based Dynamic Visual Feedback

Control for Three Dimensional Target Tracking: Stability and L2-gain Performance

参考文献 48

Analysis,” IEEE Trans. on Control Systems Technology, Vol. 15, No. 1, pp. 40–52,

2007.

[42] R. Murray, Z. Li and S. S. Sastry, A Mathematical Introduction to Robotic Manipula-

tion, CRC Press, 1994.

[43] F. Bullo and A. D. Lewis, Geometric Control of Mechanical Systems, Springer, Chapter

5, 2004.

[44] Y. Ma, S. Soatto, J. Kosecka and S. S. Sastry, An Invitation to 3-D Vision, Springer,

Chapter 2, 2003.

[45] M. W. Spong, S. Hutchinson and M. Vidyasagar, Robot Modeling and Control, John

Wiley & Sons, Inc., Chapter 11, 2006.

[46] B. Brogliato, R. Lozano, B. Maschke and O. Egeland, Dissipative Systems Analysis

and Control: Theory and Applications, Second Edition, Springer, 2006.

[47] S. P. M. Noijen, P. F. Lambrechts and H. Nijmeijer, “An Observer-controller Combi-

nation for A Unicycle Mobile Robot,” International Journal of Control, Vol. 78, No.

2, pp. 81–87, 2005.

[48] H. Berghuis and H. Nijmeijer, “A Passivity Approach to Controller-Observer Design

for Robots,” IEEE Trans. on Robotics and Automation, Vol. 9, No. 6, pp. 740–754,

1993.

[49] P. Yang, R. A. Freeman and K. M. Lynch, “A General Stability Condition for Multi-

Agent Coordination by Coupled Estimation and Control,” Proc. of the 2007 American

Control Conference, pp. 723–728, 2007.

[50] A. R-Angeles and H. Nijmeijer, “Mutual Synchronization of Robots via Estimated State

Feedback: A Cooperative Approach,” IEEE Trans. on Control System Technology, Vol.

12, No. 4, pp. 542–554, 2004.

[51] H. K. Khalil, Nonlinear Systems, Third Edition, Prentice Hall, 2002.

[52] E. D. Sontag, “Comments on Integral Variants of ISS,” Systems & Control Letters, No.

34, pp. 93–100, 1998.

参考文献 49

[53] 藤本健治，杉江俊治，入力状態安定性に基づいた非線形系のYoulaパラメトリゼーショ

ン，計測自動制御学会論文集，Vol. 35, No. 9, pp. 1162–1169, 1999.

[54] E. D. Sontag, “Input to State Stability: Basic Concepts and Results,” In Nonlinear and

Optimal Control Theory, P. Nistri and G. Stefani, eds., Springer-Verlag, pp. 163-220,

2007.

[55] E. D. Sontag and Y. Wang, “Output-to-state Stability and Detectability of Nonlinear

Systems,” Systems & Control Letters, No. 29, pp. 279–290, 1997.

[56] E. D. Sontag and Y. Wang, “New Characterization of Input-to-state Stability,” IEEE

Trans. on Automatic Control, Vol. 41, No. 9, pp. 1283–1294, 1996.

[57] E. D. Sontag and Y. Wang, “On Characterizations of The Input-to-state Stability

Property,” Systems & Control Letters, No. 24, pp. 351–359, 1995.

[58] S. Arimoto and T. Nakayama, “Another Language for Describing Motions of Mecha-

tronics Systems: A Nonlinear Position-Dependent Circuit Theory,” IEEE/ASME

Trans. Mechatronics, Vol. 1, No. 2, pp. 168–180, 1996.

[59] J. M. A. Scherpen and R. Ortega, “On Nonlinear Control of Euler- Lagrange Systems:

Disturbance Attenuation Properties,” Systems & Control Letters, Vol. 30, No. 1, pp.

490–56, 1997.

[60] S. Battilotti and L. Lanari, “Adaptive Disturbance Attenuation with Global Stability

for Rigid and Elastic Joint Robots,” Automatica, Vol. 33, No. 2, pp. 239–243, 1997.

本研究に関する発表文献

[1] T. Ibuki, T. Hatanaka, M. Fujita and M. W. Spong, “Visual Feedback Attitude Synchro-

nization in Leader-follower Type Visibility Structures,” Proc. of the 49th IEEE Conference

on Decision and Control, 2010 (to appear).

[2] M. Fujita, T. Hatanaka, N. Kobayashi, T. Ibuki and M. Spong, “Visual Motion Observer-

based Pose Synchronization: A Passivity Approach,” Proc. of the 48th IEEE Conference

on Decision and Control and 28th Chinese Control Conference, pp. 2402–2407, 2009.

[3] 伊吹竜也，畑中健志，藤田政之，ビジュアルフィードバックによる姿勢同期制御の収束

性解析，第 10 回制御部門大会，熊本，2010．

付録 A 剛体の運動学モデルの受動性 50

付録A

剛体の運動学モデルの受動性

本章では剛体の運動学モデル (2.1) および相対運動モデル (2.3) が受動性を有することを

示す．

A.1 剛体の運動学モデルの受動性

慣性座標系Σw から見た剛体 i の位置姿勢 gwi を剛体 i の座標系Σi から見たときの偏差

ベクトルとして

ewi :=

e−ξθwipwi

e−ξθwisk(eξθwi)∨

と定義する．また，ポテンシャル関数としてEwi ≥ 0を以下に定義する．

Ewi :=1

2∥pwi∥2

2 + φ(eξθwi).

ここで，φ(eξθwi) := 12tr(I3 − eξθwi) ≥ 0 は姿勢誤差に関するエネルギ関数であり，φ(eξθwi) =

(sk(eξθwi)∨)T ωbwi という性質をもつ (付録C.4)．このとき，剛体の運動学モデル (2.1) に対し

てつぎの補題が成り立つ．

補題 5. 剛体の運動学モデル (2.1) は以下の不等式を満たす．∫ T

0

(V bwi(t))

T ewi(t)dt ≥ −βwi.

ここで，βwi は正の定数である．


証明. Ewi を運動学モデル (2.1) の解軌道に沿って微分すると

Ewi =d

dt

(1

2∥pwi∥2

2 + φ(eξθwi)

)= pT

wipwi + (sk(eξθwi)∨)T ωbwi

= pTwipwi + (e−ξθwisk(eξθwi)∨)T ωb

wi (∵ e−ξθsk(eξθ)∨ = sk(eξθ)∨ (付録C.3.4))

= pTwie

ξθwivbwi + (e−ξθwisk(eξθwi)∨)T ωb

wi (∵ vbwi = e−ξθwi pwi)

=[(e−ξθwipwi)

T (e−ξθwisk(eξθwi)∨)T

] vbwi

ωbwi

= eT

wiVbwi

= (V bwi)

T ewi (∵ Ewi ∈ R) (A.1)

となる．したがって，(A.1) 式の両辺を時刻 t = 0 から T まで積分すると∫ T

0

(V bwi(t))

T ewi(t)dt = Ewi(T ) − Ewi(0) ≥ −Ewi(0)

が導かれる．

補題 5 は V bwi を入力，ewi を出力とみなすと剛体の運動学モデル (2.1) が受動性を有する

ことを示している．先行研究 [13, 14] ではこの性質に基づいて姿勢同期を達成する制御則を

提案しており，本研究においても同様の構造を用いている．

A.2 相対運動モデルの受動性

剛体 j が運動していない (V bwj = 0)とき，相対運動モデル (2.3) が受動性を有することを

示す．相対位置姿勢 gij の偏差ベクトルとして

erij :=

pij

sk(eξθij)∨

と定義すると，つぎの補題が成り立つ．

補題 6. 剛体 j が運動していない (V bwj = 0) 場合，相対運動モデル (2.3) は以下の不等式を

満たす． ∫ T

0

(V bwi(t))

T (−erij(t))dt ≥ −βij.

ここで，βij は正の定数である．


証明. ポテンシャル関数としてEij ≥ 0を以下に定義する．

Eij :=1

2∥pij∥2

2 + φ(eξθij).

Eijを相対運動モデル (2.3) の解軌道に沿って微分すると

Eij = pTij pij + (sk(eξθij)∨)T ωb

ij

= pTije

ξθije−ξθij pij + (sk(eξθij)∨)T eξθijωbij (∵ e−ξθsk(eξθ)∨ = sk(eξθ)∨)

=[pT

ij (sk(eξθij)∨)T

] eξθij 0

0 eξθij

vbij

ωbij

(∵ vbij = e−ξθij pij)

= eTrijAd

(eξθij )V b

ij

= −eTrijAd

(eξθij )Ad(g−1

ij )Vbwi (∵ (2.3)式)

= −eTrijAd(−pij)V

bwi

= (−erij)T V b

wi (∵ pTij pijω

bwi = −pT

ijωbwipij = 0)

= (V bwi)

T (−erij) (A.2)

となる．したがって，(A.2) 式の両辺を時刻 t = 0 から T まで積分すると∫ T

0

(V bwi(t))

T (−erij(t))dt = Eij(T ) − Eij(0) ≥ −Eij(0)

が導かれる．

補題 6 は V bwi を入力，−erij を出力とみなすと相対運動モデル (2.3) が受動性を有するこ

とを示している．本稿で用いる視覚オブザーバや次章で紹介する先行研究 [41] で提案され

ているビジュアルモーションオブザーバはこの性質に基づいて制御則を提案している．

付録 B ビジュアルモーションオブザーバ 53

付録B

ビジュアルモーションオブザーバ

本章では本稿で用いられる視覚オブザーバの基となる先行研究 [41] で提案されている視覚

情報から対象の相対位置姿勢を推定する非線形オブザーバ (本稿ではビジュアルモーション

オブザーバと呼ぶ) について述べる．

B.1 ビジュアルモーションオブザーバ

剛体 i が剛体 j との相対位置姿勢情報 gijを取得する状況を考える．まず，座標系Σi から

見た座標系Σj の相対運動モデルが (2.3) 式で与えられることは既に導出した．しかし，剛

体 i にとって剛体 j のボディ速度 V bwj は未知情報であるので，(2.3) 式から直接 gij を求め

ることは不可能である．そこで，本節では相対運動モデルに基づいた推定器を用いて gij を

推定することを考える．

相対運動モデル (2.3) に基づき，推定値 gij の運動学モデルを次式に構成する．

V bij = −Ad(g−1

ij )Vbwi + uij.

ここで，uij := [uTvij uT

ωij]T ∈ R6 は推定値 gij, V

bij を真値に収束させるためのオブザーバの

入力である．(2.4) 式と同様にして，相対位置姿勢の推定値 gij を用いて剛体 i から見た剛

体 j の特徴点 pijk:= [xijk

yijkzijk

]T ∈ R3, k ∈ {1, · · · ,m} を

pijk= gijpjjk

(B.1)

と表す．さらに，透視投影モデルから得ることのできる pijkを 2 次元平面に射影した画像

特徴量 fijk∈ R2, k ∈ {1, · · · ,m} を (2.5) 式と同様にして

fijk=

λ

zijk

xijk

yijk


と表す．また，fijkを縦に特徴点の数m 個分並べたベクトルを

fij =

fij1

...

fijn

∈ R2m

とする．

つぎに，画像特徴量 fij, fij と推定偏差ベクトル eij との間の関係式を導出する．観測対

象の真値 (2.4) と推定値 (B.1) との偏差は

pijk− pijk

= (pij − pij) + (eξθij − eˆξθij)pjjk

= eˆξθijpeeij + e

ˆξθij(e−ˆξθijeξθij − I3)pjjk

(∵ peeij = e−ˆξθij(pij − pij))

≈ eˆξθijpeeij + e

ˆξθijsk(eξθeeij)pjjk(∵ eξθ ≈ I3 + sk(eξθ) (付録C.3.5))

= eˆξθijpeeij + (e

ˆξθijsk(eξθeeij)e−ˆξθij)e

ˆξθijpjjk

= eˆξθijpeeij + (e

ˆξθijsk(eξθeeij)∨)∧eˆξθijpjjk

(∵ eξθωe−ξθ = (eξθω)∧ (付録C.3.1))

= eˆξθijpeeij − (e

ˆξθijpjjk)∧e

ˆξθijsk(eξθeeij)∨ (∵ ab = −ba)

= eˆξθijpeeij − (e

ˆξθij pjjke−

ˆξθij)eˆξθijsk(eξθeeij)∨ (∵ eξθωe−ξθ = (eξθω)∧)

= eˆξθij(peeij − pjjk

sk(eξθeeij)∨)

= eˆξθij

[I3 −pjjk

] peeij

sk(eξθeeij)∨

= e

ˆξθij

[I3 −pjjk

]eij (B.2)

と導出される．上式における近似では，オブザーバの姿勢推定偏差の回転量 θeeij が十分に

小さいという仮定を用いている．また，画像特徴量 fij は推定値 pijkの周りで 1 次テイラー

展開することで

fijk=

λ

zijk

xijk

yijk

+∂fijk

∂xijk

∣∣∣∣pijk

=pijk

(xijk− xijk

) +∂fijk

∂yijk

∣∣∣∣pijk

=pijk

(yijk− yijk

)

+∂fijk

∂zijk

∣∣∣∣pijk

=pijk

(zijk− zijk

) + · · ·

≈ fijk+

λzijk

0

(xijk− xijk

) +

0

λzijk

(yijk− yijk

) +

−λxijk

z2ijk

−λyijk

z2ijk

(zijk− zijk

)

= fijk+

λzijk

0 −λxijk

z2ijk

0 λzijk

−λyijk

z2ijk

(pijk− pijk

) (B.3)


と表される．したがって，(B.3) 式に (B.2) 式を代入すると，

fijk− fijk

=

λzijk

0 −λxijk

z2ijk

0 λzijk

−λyijk

z2ijk

eˆξθij

[I3 −pjjk

]eij

が得られる．ここで，k 番目の特徴点に対する画像ヤコビアンとして

Jijk(gij) :=

λzijk

0 −λxijk

z2ijk

0 λzijk

−λyijk

z2ijk

eˆξθij

[I3 −pjjk

]と定義する．このとき，すべての特徴点に対する画像ヤコビアンは

Jij(gij) =

Jij1(gij)

...

Jijm(gij)

=

λzij1

0 −λxij1

z2ij1

0 λzij1

−λyij1

z2ij1

0 0

0. . . 0

0 0

λzijm

0 −λxijm

z2ijm

0 λzijm

−λyijm

z2ijm

e

ˆξθij 0 0

0. . . 0

0 0 eˆξθij

I3 −pij1

......

I3 −pijm

となり，この画像ヤコビアンを用いることで

fij − fij = Jij(gij)eij

が得られる．さらに，Jij(gij) が列フルランクを満たすように 4 点以上の特徴点をとると，

その疑似逆行列が次式で得られる．

J†ij(gij) = (JT

ijJij)−1JT

ij .

この疑似逆行列を用いることで推定偏差ベクトル eij は

eij = J†ij(gij)(fij − fij)

と表される．以上のことから，推定偏差ベクトル eij は透視投影モデルから得られる画像特

徴量 fij と推定モデルより得られる推定値の画像特徴量 fij および相対位置姿勢の推定値 gij

より求まることが示される．

また，|θeeij| < π2のとき，次式より eij から eξθeeij が構成できる [42]．

ξθeeij =sin−1 ∥sk(eξθeeij)∨∥

∥sk(eξθeeij)∨∥sk(eξθeeij)∨,

eξθeeij = I3 + ξ sin θeeij + ξ2(1 − sin θeeij).


つぎに，相対運動モデルおよび推定値の運動学モデルを用いて剛体 i と剛体 j の間の相

対位置姿勢の推定偏差システム (3.3) を構成する．(2.3) 式の導出と同様にして (3.2) 式を時

間微分すると

geeij = ˙g−1ij gij + g−1

ij gij

= −(−g−1ij V b

wigij + uij)geeij + geeij(−g−1ij V b

wigij + V bwj)

が得られる．ここで，V beeij := g−1

eeij geeij とし，上式の左から g−1eeij を乗じると

Veeij = −g−1eeijuijgeeij + V b

wj

が導かれる．したがって，上式の両辺に∨ をとると以下の推定偏差システムが導出される．

V beeij = −Ad(g−1

eeij)uij + V b

wj.

この推定偏差システムに対してつぎの補題が成り立つ．

補題 7. 剛体 j が運動していない (V bwj = 0) 場合，推定偏差システム (3.3) は以下の不等式

を満たす． ∫ T

0

uTij(t)(−eij(t))dt ≥ −βeeij.

ここで，βeeij は正の定数である．

証明. ポテンシャル関数としてEeeij ≥ 0 を以下に定義する．

Eeeij :=1

2∥peeij∥2

2 + φ(eξθeeij).

Eeeij を推定偏差システム (3.3) の解軌道に沿って微分すると

Eeeij = pTeeij peeij + (sk(eξθeeij)∨)T ωb

eeij (∵付録C.4)

= pTeeije

ξθeeije−ξθeeij peeij + (sk(eξθeeij)∨)T eξθeeijωbeeij (∵ sk(eξθ)∨ = e−ξθsk(eξθ)∨)

=[pT

eeij (sk(eξθeeij)∨)T

] eξθeeij 0

0 eξθeeij

vbeeij

ωbeeij

(∵ vbeeij = e−ξθeeij peeij)

= eTijAd

(eξθeeij )V b

eeij

= −eTijAd

(eξθeeij )Ad(g−1

eeij)uij (∵ (3.3)式)

= −eTijAd(−peeij)uij

= (−eij)T uij (∵ pT

eeij peeijuωij = −pTeeijuωijpeeij = 0)

= uTij(−eij) (B.4)


となる．したがって，(B.4) 式の両辺を時刻 t = 0 から T まで積分すると∫ T

0

uTij(t)(−eij(t))dt = Eeeij(T ) − Eeeij(0) ≥ −Eeeij(0)

が導かれる．

補題 7 は uij を入力，−eij を出力とみなすと推定偏差システム (3.3) が受動性を有するこ

とを示している．

つぎに，補題 7 に基づき，推定偏差システム (3.3) に対して相対位置姿勢 gij を推定する

以下の入力を提案する．

uij = −Keij(−eij) = Keijeij. (B.5)

ここで，Keij ∈ R は正のゲインである (実際は正定行列であればよいが，ここでは簡単の

ためスカラーを用いる)．このとき，推定偏差システム (3.3) に入力 (B.5) を加えるとつぎ

の補題が成り立つ．

補題 8. 剛体 j が運動していない (V bwj = 0) 場合，(3.3) 式と (B.5) 式の入力で構成される閉

ループ系の平衡点 eij = 0 は漸近安定である．

証明. ポテンシャル関数 Eeeij を推定偏差システム (3.3) 式の解軌道に沿って微分すると，

(B.4) 式より

Eeeij = −uTijeij

= −KeijeTijeij

= −Keij∥eij∥22 ≤ 0

が得られる．したがって，Eeeij = 0 となるのは eij = 0 となるときのみであるので，平衡点

eij = 0 は漸近安定である．

|θeeij| < π のとき eij = 0 は peeij = 0, eξθeeij = I3 の必要十分条件である．すなわち，

geeij = g−1ij gij = I4 より，補題 8 は gij が gij に漸近的に収束することを示している．ビジュ

アルモーションオブザーバの構造を Fig. B.1 に示す．

B.2 ビジュアルモーションオブザーバの性能解析

本節では剛体 j が任意の速度で運動する場合について，その運動を外乱ととらえること

で前節で提案した制御入力によるビジュアルモーションオブザーバの性能解析を行う．


Fig. B.1: Block Diagram of Visual Motion Observer

まず，ある正の定数 γe を次式を満たすようにとる．

Keij >1

2γ2e

+1

2.

このとき，性能解析に関するつぎの補題が成り立つ．

補題 9. V bwj = 0 の場合，(3.3), (B.5) 式で構成される閉ループ系は γe 以下のL2 ゲインを

有する．

証明. V bwj = 0 のとき，ポテンシャル関数Eeeij を推定偏差システム (3.3) の解軌道に沿って

微分すると，


Eeeij = eTijAd

(eξθeeij )V b

eeij

= uTij(−eij) + eT

ijAd(eξθeeij )

V bwj (∵ (3.3)，(B.4)式)

= −uTijeij + eT

ij

eξθeeij 0

0 eξθeeij

V bwj

= −uTijeij −

1

2γ2

e

∥∥∥∥∥∥V bwj −

1

γ2e

e−ξθeeij 0

0 e−ξθeeij

eij

∥∥∥∥∥∥2

2

+1

2γ2e

∥∥∥∥∥∥e−ξθeeij 0

0 e−ξθeeij

eij

∥∥∥∥∥∥2

2

+1

2γ2

e∥V bwj∥2

2

≤ −uTijeij +

1

2γ2e

∥eij∥22 +

1

2γ2

e∥V bwj∥2

2 (∵ eξθeeijは直交行列)

= −eTijKeijeij +

1

2γ2e

∥eij∥22 +

1

2γ2

e∥V bwj∥2

2 +1

2∥eij∥2

2 −1

2∥eij∥2

2

= −(

Keij −1

2γ2e

− 1

2

)∥eij∥2

2 +1

2γ2

e∥V bwj∥2

2 −1

2∥eij∥2

2

≤ −1

2∥eij∥2

2 +1

2γ2

e∥V bwj∥2

2

(∵ Keij >

1

2γ2e

+1

2

)(B.6)

が導かれる．(B.6) 式の両辺を時刻 t = 0 から T まで積分すると

Eeeij(T ) − Eeeij(0) ≤ 1

2γ2

e

∫ T

0

∥V bwj(t)∥2

2dt − 1

2

∫ T

0

∥eij(t)∥22dt

より， ∫ T

0

∥eij(t)∥22dt ≤ γ2

e

∫ T

0

∥V bwj(t)∥2

2dt + 2Eeeij(0)

となる．したがって，非負のスカラー a1, a2 について√

a21 + a2

2 ≤ a1 + a2 が成り立つこと

を考慮すると，

∥eij∥L2 ≤ γe∥V bwj∥L2 +

√2Eeeij(0)

が導かれる．

付録 C 数学的準備 60

付録C

数学的準備

本章では，本稿で用いられるいくつかの数学的性質を述べる．

C.1 ∧ に関する公式

C.1.1 aa = 0

a, b ∈ R3 に対して，ab = a × b より，

aa = a × a = 0

となることから示される．

C.1.2 12tr(ab) = −aT b

a = [a1 a2 a3]T , b = [b1 b2 b3]

T ∈ R3とすると，直接計算することにより

1

2tr(ab) =

1

2tr

0 −a3 a2

a3 0 −a1

−a2 a1 0

0 −b3 b2

b3 0 −b1

−b2 b1 0

=1

2tr

−a2b2 − a3b3 ∗ ∗

∗ −a3b3 − a1b1 ∗

∗ ∗ −a1b1 − a2b2

= −(a1b1 + a2b2 + a3b3)

= −aT b



C.1.3 a2 = aaT − ∥a∥2I3

a2 を直接計算することにより

a2 =

0 −a3 a2

a3 0 −a1

−a2 a1 0

0 −a3 a2

a3 0 −a1

−a2 a1 0

=

−a2

2 − a23 a1a2 a3a1

a1a2 −a23 − a2

1 a2a3

a3a1 a2a3 −a21 − a2

2

=

a2

1 a1a2 a3a1

a1a2 a22 a2a3

a3a1 a2a3 a23

−

a2

1 + a22 + a2

3 0 0

0 a21 + a2

2 + a23 0

0 0 a21 + a2

2 + a23

= aaT − ∥a∥2I3


C.1.4 a3 = −∥a∥2a

前項の結果を用いると，a3 を直接計算することにより

a3 = a(a)2

= a(aaT − ∥a∥2I3) (∵ a2 = aaT − ∥a∥2I3)

= aaaT − ∥a∥2a

= −∥a∥2a (∵ aa = 0)


C.2 ロドリゲスの公式

ロドリゲスの公式 [42] は次式で表される．

eξθ = I3 + ξ sin θ + ξ2(1 − cos θ). (C.1)


これは，eξθ の定義と ∥ξ∥2 = 1 を考慮することにより

eξθ = I3 + ξθ +1

2!ξ2θ2 +

1

3!ξ3θ3 + · · ·

= I3 + ξ

(θ − 1

3!θ3 + · · ·

)+ ξ2

(1

2!θ2 − 1

4!θ4 + · · ·

)(∵ ξ3 = −∥ξ∥2ξ)

= I3 + ξ sin θ + ξ2(1 − cos θ)

と導出される．

C.3 数学的準備

C.3.1 eξθωe−ξθ = (eξθω)∧

eξθ ∈ SO(3), ω, v ∈ R3 として，eξθωe−ξθv を計算すると

eξθωe−ξθv = eξθω(e−ξθv)

= eξθ(ω × (e−ξθv)) (∵ ab = a × b)

= (eξθω) × (eξθe−ξθv)

= (eξθω) × v

= (eξθω)∧v (∵ ab = a × b)

となる．したがって，eξθωe−ξθ = (eξθω)∧ が示される．

C.3.2(

ddte

ξθ)

e−ξθ の歪対称性

eξθe−ξθ = I3

より，上式を時間微分すると(d

dteξθ

)e−ξθ + eξθ

(d

dte−ξθ

)= 0

となる．したがって，(d

dteξθ

)e−ξθ = −eξθ

(d

dte−ξθ

)= −eξθ

(d

dteξθ

)T

(∵ eξθ ∈ SO(3))

= −((

d

dteξθ

)e−ξθ

)T

(∵ eξθ ∈ SO(3))


が導かれる．よって，(

ddt

eξθ)

e−ξθ = −((

ddt

eξθ)

e−ξθ)T

が成り立つので(

ddt

eξθ)

e−ξθ は歪

対称行列である．

C.3.3 sk(eξθ) = ξ sin θ

ロドリゲスの公式 (C.1) を用いて計算すると

sk(eξθ) =1

2(eξθ − e−ξθ)

=1

2

(I3 + ξ sin θ + ξ2(1 − cos θ) − (I3 + ξ sin(−θ) + ξ2(1 − cos(−θ)))

)=

1

2

(I3 + ξ sin θ + ξ2(1 − cos θ) −

(I3 − ξ sin θ + ξ2(1 − cos θ)

))= ξ sin θ


C.3.4 eξθsk(eξθ)∨ = sk(eξθ)∨

∧ と ∨ が逆演算の関係であることを用いると

eξθsk(eξθ)∨ =

((eξθsk(eξθ)∨

)∧)∨

=(eξθsk(eξθ)e−ξθ

)∨(∵ eξθωe−ξθ = (eξθω)∧)

=

(eξθ 1

2(eξθ − e−ξθ)e−ξθ

)∨

=

(1

2(eξθ − e−ξθ)

)∨

= sk(eξθ)∨

となることから示される．また，同様にして e−ξθsk(eξθ)∨ = sk(eξθ)∨ を示すことができる．

C.3.5 eξθ ≈ I3 + sk(eξθ)

付録 C.3.3 より，sk(eξθ) = ξ sin θ が成り立つので，これを用いるとロドリゲスの公式

(C.1) は

eξθ = I3 + ξ sin θ + ξ2(1 − cos θ)

= I3 + sk(eξθ) + ξ2(1 − cos θ)


と表される．したがって，θ が十分に小さい場合，cos θ ≈ 1 より

eξθ ≈ I3 + sk(eξθ)

と近似できる．

C.4 エネルギ関数φ(eξθ)

姿勢誤差に関するエネルギ関数 φ(eξθ)は

φ(eξθ) :=1

2tr(I3 − eξθ)

と定義され，以下の性質をもつ．

1. φ(eξθ) ≥ 0であり，φ(eξθ) = 0 となるための必要十分条件は eξθ = I3 である．

2. φ(eξθ) = (sk(eξθ)∨)T ωb が成り立つ．

1 の証明.　ロドリゲスの公式 (C.1) を用いて計算すると

φ(eξθ) =1

2tr(I3 − eξθ)

=1

2tr(−ξ sin θ − ξ2(1 − cos θ))

= −1

2tr(ξ2)(1 − cos θ) (∵ tr(ξ) = 0)

= ξT ξ(1 − cos θ)

(∵ 1

2tr(ab) = −aT b

)= 1 − cos θ ≥ 0 (∵ ξT ξ = 1)

が導かれる．ここで，1 − cos θ は θ = 2πn, n ∈ Z (Z は整数の集合) の場合に 0 となり，

それ以外のときは正となる．θ = 2πn は eξθ = I3 と等価であり，これにより十分性が証明

される．また，必要性は明らかに成立する．

2の証明.　エネルギ関数 φ(eξθ) を時間微分すると，

φ(eξθ) = −1

2tr

(d

dteξθ

)= −1

2tr

((d

dteξθ

)e−ξθeξθ

)= −1

2tr

((d

dteξθ

)e−ξθsk(eξθ)

)− 1

2tr

((d

dteξθ

)e−ξθsym(eξθ)

)(C.2)

と表せる．ここで，sym(eξθ) は回転行列 eξθ の対称成分であり，

sym(eξθ) :=1

2

(eξθ − e−ξθ

).


と定義する．sym(·)と sk(·)を用いると，任意の正方行列A ∈ Rn×n に対してA = sym(A)+

sk(A) が成立し，(C.2) 式ではこれを用いた．(

ddt

eξθ)

e−ξθ が歪対称である (付録 C.3.2) こ

とを考慮すると，

tr

((d

dteξθ

)e−ξθsym(eξθ)

)= tr

(((d

dteξθ

)e−ξθsym(eξθ)

)T)

(∵ tr(A) = tr(AT ))

= tr

(sym(eξθ)T

((d

dteξθ

)e−ξθ

)T)

= tr

(sym(eξθ)

(−

(d

dteξθ

)e−ξθ

))(∵付録C.3.2)

= −tr

((d

dteξθ

)e−ξθsym(eξθ)

)(∵ tr(AB) = tr(BA))

が導かれる．したがって，tr((

ddt

eξθ)

e−ξθsym(eξθ))

= 0 が成り立つ．これを (C.2) 式に適

用すると，

φ(eξθ) = −1

2tr

((d

dteξθ

)e−ξθsk(eξθ)

)= −1

2tr

((((d

dteξθ

)e−ξθ

)∨)∧ (sk(eξθ)∨

)∧)

(∵ ∧と ∨は逆演算)

=

(((d

dteξθ

)e−ξθ

)∨)T

sk(eξθ)∨(

∵ 1

2tr(ab) = −aT b

)=

(sk(eξθ)∨

)T((

d

dteξθ

)e−ξθ

)∨

(∵ φ(eξθ) ∈ Rよりφ(eξθ) = φ(eξθ)T )

=(sk(eξθ)∨

)T

eξθe−ξθ

((d

dteξθ

)e−ξθ

)∨

=(sk(eξθ)∨

)T

eξθe−ξθ(eξθωbe−ξθ

)∨(∵ ωb = e−ξθeξθ)

=(sk(eξθ)∨

)T

eξθe−ξθeξθωb (∵ eξθωe−ξθ = (eξθω)∧)

=(sk(eξθ)∨

)T

eξθωb

=(sk(eξθ)∨

)T

ωb (∵ e−ξθijsk(eξθij)∨ = sk(eξθij)∨)


Study on Visual Feedback Attitude Synchronizationibuki/paper/Ib_master.pdfAbstract In this paper we consider visual feedback attitude synchronization in leader-follower type visibility

Documents