日本ロボット学会誌

No. xx, pp.1〜4, 202xVol. xx

1レター

階層化最適制御を用いたクアッドコプタのフォーメーションフライトの検証
赤 井直紀∗嵐和 也∗

原進∗

Demonstration of Formation Flight for Quadcopters Using Hierarchical Optimal Control
Naoki Akai∗ , Kazuya Arashi∗ and Susumu Hara∗
This letter presents a formation flight method for multirotor unmanned aerial vehicles using hierarchical optimal
control (HOC). HOC enables to tune trade off relationships between individual and cooperative targets existing information flight via simple weight parameters tuning. The aim of this letter is implementing the formation flightmethod for actual quadcopters and discussing its possibility and effectiveness for real applications through demonstrations. Demonstrations show that relative state errors between quadcopters can be accurately controlled by HOC
more than general linear quadratic regulator. The software used in this work is publicly available.
Key Words: Hierarchical Optimal Control, Linear Quadratic Regulator, Formation Flight, Quadcopter

2. 関 連 研 究

1. は じ め に
Unmanned aerial vehicle（UAV）の応用先は多く，フォー

本章では UAV の FFC 関連する研究に言及し，HOC を用いメーションを組んだ荷物の運搬や検査などもある．フォーメー

た FFC との違いを考察する．ただし紙面の都合上，代表的なションの形成にあたっては，(1) 各 UAV がそれぞれの目標状関連研究についてのみ言及する．

態に即座に向かうこと，(2) それぞれの UAV の相対状態が即

文献 [3] にて，航空機の FFC の実施例が報告されている．こ座に目標状態になることの，2 つの目標が考慮されるべきであの例では，各機体が目標状態へ向かうための制御と，各機体のる．ただし，これらの目標にはトレードオフの関係があり，ど相対状態を考慮した軌道修正のための FFC が，階層的に実装ちらの目標を優先すべきかはタスクにより異なる．これに対しされている．なお，各機体の目標状態は，フォーメーション制て Kamitani らは，椿野らが文献 [1] で述べている階層的な線形御器により決定される．

2 次レギュレータ（linear quadratic regulator: LQR）を用いた，フォーメーションフライト制御（formation flight control:FFC）を提案している [2]．本稿では，この階層的な構造を持つLQR を用いた制御を，階層化最適制御（hierarchical optimalcontrol: HOC）と呼ぶ．
HOC は最適レギュレータであるため，ロバスト性を持ちつつ，かつ上記 (1)，(2) のトレードオフの調整を，シンプルなパラメータ調整で達成させる．すなわち HOC は，実システムに他のフォーメーション形成方法として，バーチャルストラク
チャ [4] を用いるものがある．バーチャルストラクチャは，複数の機体を 1 つの仮想剛体と見なし，その構造を変化させる手法である．Rosa らは，バーチャルストラクチャを用い，その際に得られる追従誤差と相対状態をコスト関数に組み込んだモデル予測制御（model predictive control: MPC）を実装し，FFCを達成している [5]．また，フォーメーション形成達成のための軌道を，MPC を用いて生成する方法も提案されている [6]．

適用するにあたり，性能保証があるという観点で好ましい手法

Mercado らは，リーダフォロワーアプローチによるクアッドである．ただし Kamitani らの研究では，シミュレーションにコプタの FFC を達成している [7]．この手法では，リーダの経よる検証が主であり，実機を用いた検証までは実現されていな路追従制御，およびフォロワーの相対位置制御のためにそれぞれい．本稿では，Kamitani らの先行研究を参考に，トイドロースライディングモード制御が利用されている．そのため，フォン（Tello EDU ）を用いた FFC の実機検証を行う．そしてこロワーには個別の目標が与えられていないフォーメーションフの結果を基に，HOC の実応用の可能性検証と，その有効性をライトとなっている．類似する研究は [8] にも見られる．

議論する．なお，使用したソフトウェアは一般公開している†† ．

FFC の多くは，リーダー機の経路追従，フォロワー機の相対位置制御を行うことが主といえる．一方で HOC による FFC で†原稿受付
名古屋大学大学院工学研究科∗Graduate School of Engineering, Nagoya University■ 要素分野†https://www.ryzerobotics.com/jp/tello-edu††
https://github.com/NaokiAkai/tellos_ffc.git∗

日本ロボット学会誌 xx 巻 xx 号

は，各機体に個別の目標を与えながら，複数機間の協調目標を定めることができる．ここでいう協調目標が，相対状態を任意のものにするなどであり，この目標達成により FFC が達成される．そして HOC では，両目標のどちらを優先するかを，重みパラメータの調整により達成できる．もちろんこの調整は，—1—

202x 年 xx 月

赤2

井直紀嵐和

MPC 内で使用されるコスト関数内の重みを調整することでも達成可能である．しかし HOC は，扱うシステムが線形システ
ムに限られるものの，制御入力に対する最適性を保証すること也

原進⊤ ⊤⊤ ⊤
ここで，x̂ = (x1⊤ ... xN) ，û = (u1⊤ ... uN) であり，IN はRN ×N の単位行列，⊗ はクロネッカー積を表す．このシステムに対し，式 (5) の目標を達成することを考える．

ができる．さらに階層性を上手く設計できると，システム間でとは例えば，全機体が互いの状態を知る必要がなく，グループの平均状態だけ知れれば良いなどというものである．

3. 手（5）

lim |xi (t) − xj (t)|共有する情報量を削減することも可能となる [1]．この情報削減

t→∞

式 (5) は，i 番目の機体がその目標状態へ向かうための個別目
標，および i 番目と j 番目の機体の相対状態を任意のものとす

法

る協調目標が含まれた目標を表す．

3. 1 変数定義と状態方程式
本稿で扱う UAV としては，クアッドコプタを想定する．クアッドコプタの状態 x は，3 次元位置 (x y z)⊤ と姿勢 (φ θ ψ)⊤で表されるものとする．φ，θ ，ψ はそれぞれ世界座標系におけるクアッドコプタのロール，ピッチ，ヨー角である．また，クアッドコプタは速度制御で制御できるものとし，制御入力 u は⊤(vx vy vz ωx ωy ωz ) とする．vx ，vy ，vz と ωx ，ωy ，ωz は式 (5) の目標を達成するための評価出力の定量化について考える．まず個別の目標，すなわち xi を零にするために，それぞれの状態を評価出力とする．

zind = (IN ⊗ Cind ) x̂,（6）

Cind = I4

次に協調目標の達成のために，i = 1 の機体との相対状態をゼロとする目標を与える．

それぞれ機体座標系における並進速度と角速度である．ただし，zcp = (Cstr ⊗ Ccp ) x̂

ロール角とピッチ角の制御に関しては，クアッドコプタ内部のCstr = (1N −1 | − IN −1 ) ,フライトコントローラによる制御則をそのまま用いることとす（7）

Ccp = I4

ここで，1N −1 は RN −1 の要素がすべて 1 のベクトル，IN −1 はるため，x = (x y z ψ)⊤ ，u = (vx vy vz ωz )⊤ とする．
本稿では，N 機のクアッドコプタのフォーメーションフライ
トについて考える．これにあたり，i 番目の機体の状態，制御入力をそれぞれ xi ，ui とする．各クアッドコプタは，それぞれRN −1×N −1 の単位行列である．式 (6)，(7) より，評価出力を()⊤ ⊤z = z⊤と定める．
ind zcp
評価出力 z を用いた評価関数 J を式 (8) として定める．

任意の目標状態に向かうものとし，その状態を d xi とする．まJ=た，目標状態周辺での制御入力として d ui を定める．なお，目∫ ∞0d標状態で静止することを考えると，この制御入力は， ui = 0( ⊤z (t)Qz(t) + û⊤ (t)Rû(t) dt)（8）となる．ここで，クアッドコプタの状態と制御入力を以下の様

ここで Q ∈ R4(N −1)×4(N −1) ，R ∈ R4N ×4N は重み行列であに定義しなおす．

り，特に Q は式 (9) の様に定める．

xi := xi − d xi ,ui := ui − d ui

（1）

Q=

式 (1) の xi をゼロとする制御を実現することで，FFC が達成されるものと考える．そしてこれらを用いて，各クアッドコプタの運動方程式を式 (2) により定める．

A = O4,4cos(ψi ) sin(ψi )
B=

00

qind I4N00
qcp I4(N −1))（9）

ここで qind と qcp は正の定数であり，それぞれ個別，協調目標に対する重みとなる．
今，式 (10) に示す Riccati 形代数方程式を考える．

ẋi = Axi + Bui

(P (IN ⊗ A) + P IN ⊗ A⊤ P− sin(ψi )cos(ψi )0000100（2）0 01(− P IN ⊗ BR−1 B ⊤(()))⊤P + qind IN ⊗ CindCind （10）(⊤⊤+ qcp CstrCstr ⊗ Ccp
Ccp = 0())

式 (8) に示す評価関数を最小にする制御入力 û は，式 (11) に

なお O4,4 は R4×4 のゼロ行列である．式 (2) より，x˙i =より定まる．

vxi cos(ψi ) − vyi sin(ψi )，y˙i = vxi sin(ψi ) + vyi cos(ψi ) となる．ここで，これらを仮想入力とみなしてそれぞれ uxi ，uyi とû = −R−1 IN ⊗ B ⊤ P x̂おくことで，線形システムとしてみなせるようにする．つまり，最終的に，式 (11) により定まった制御入力を，式 (3) を用いて4×4

B := I4 と定義しなおす．I4 は Rの単位行列である．なお，実際の制御入力 vxi ，vyi は，求めた仮想入力を用いて，式 (3)(vxivyi)=(cos(ψi )− sin(ψi )sin(ψi )cos(ψi ))(uxiuyi)（3）

3. 2 階層化最適制御を用いた制御入力の決定
N 機のクアッドコプタの FFC を考えるにあたり，N 機のクアッドコプタをまとめた運動方程式を，式 (4) の様に記述する．
x̂˙ = (IN ⊗ A) x̂ + (IN ⊗ B) û

JRSJ Vol. xx No. xx（11）

変換して各クアッドコプタに印加することで，FFC を達成する．

4. 実により計算される．

()

験

4. 1 実験装置
実験には，Tello EDU を用いる．開発環境には ROS を用い，tello_driver† を用いて，Tello EDU の制御を行う．tello_driverを用いることで，Tello EDU を速度制御で制御することができる．また，Tello EDU の状態を得るにあたっては，Acuity 社製のモーションキャプチャ†† を用いる．
†（4）

††—2—

http://wiki.ros.org/tello_driver
https://www.acuity-inc.co.jp/xx, 202x

階層化最適制御を用いたクアッドコプタのフォーメーションフライトの検証
123
456Y [m]

1

1
2
3
4562
1Y [m]

20

30-1-1-2-2

2.5-1012-2-1

X [m]

0

1

2

Error [m]

-2

X [m]

(a) LQR

1234562(b) HOC

1.510.5Fig. 1: Trajectories during the hovering experiments.

00244. 2 比較手法・実験条件

68101214Time [second]

実験では，個別目標のみに対して制御を行う LQR との比較を行う．なおこの LQR は，式 (9) に示す qcp をゼロとするこ(a) Hovering errors when LQR was usedとで実装できる．HOC に関する重みパラメータは，実験的にqind = 0.3，qcp = 0.2，R = I4 とした．
実験では，6 機の Tello EDU を用いる．そして，(1) それぞれの機体が目的状態で静止するケース（ホバリング），(2) それ2.5
123456Error [m]2

ぞれの機体の目標状態を時間で変化させるケース（経路追従）の2 種類の目標達成に関する実験を行う．なお，実験に関する動†画は一般公開している ．

1.5
10.5

4. 3 実験結果
4. 3. 1 ホバリング
Fig. 1 に，ホバリング実験を行った際の 6 機の Tello EDUの飛行軌跡を示す．この実験では，1 から 6 の Tello EDU
が，それぞれ状態 (x, y, z, ψ) の (1, 0, 1.5, 0)，(0, 0.5, 1.5, 0)，(0, −0.5, 1.5, 0)，(−1, 1, 1.5, 0)，(−1, 0, 1.5, 0)，(−1, −1, 1.5, 0)を維持する様ホバリングを行う．なお 1 がリーダー機であり，1 との相対位置を維持することを，本実験でのフォーメーショ

002468101214

Time [second](b) Hovering errors when HOC was used
2.5234562Error [m]ン維持と定め，この相対位置と飛行時の相対位置の差をフォーメーション誤差（相対誤差）とする．また，z と ψ に関してはすべて同じであるため，以下では x と y に関して主に議論を行

1.5
10.5う．なお，LQR と HOC を使用した際の各機の初期位置はわず0かに異なるが，これはそれぞれが離陸して制御を行うまでに生024じたずれである．Fig. 1 からは，LQR と HOC を用いたどち

68101214Time [second]

らの場合でも，それぞれの機体が目標地点に向かっていることが確認できる．ただしどちらの場合も，完全に目標状態で静止(c) Relative errors when LQR was used

できていないことも確認できる．これは Tello EDU の制御性能による問題であると考えられる．

2.5Fig. 2 には，LQR と HOC を用いた際の，ホバリング時の目標地点と各機の誤差（hovering error）と，リーダー機と各機の相対位置誤差（relative error）をそれぞれ示す．どちらの制御

23456Error [m]2則を用いても，ホバリング誤差はあまり差が見られない結果と1.51なった．それぞれのホバリング誤差が収束して以降（約 5 秒後）0.5の平均ホバリング誤差は，LQR を用いた場合が 0.170 m，HOC00を用いた場合が 0.163 m であった．一方で相対誤差に関しては2468101214Time [second]異なる結果を得ることができた．Fig. 2c，2d からは，HOC を用いた場合の方が若干相対誤差の収束が早くなることが確認できる．さらに，ホバリング誤差収束時以降の相対誤差の平均は，(d) Relative errors when HOC was used

LQR を用いた場合が 0.231 m，HOC を用いた場合が 0.156 m

Fig. 2: Hovering and relative errors during the hoveringexperiments.となり，HOC を用いた場合の方が良い結果が得られた．

4. 3. 2 経路追従
Fig. 3 に，経路追従実験を行った際のそれぞれの機体の飛行†

https://www.youtube.com/watch?v=pfaknRdTN-A

日本ロボット学会誌 xx 巻 xx 号—3—

202x 年 xx 月

赤4123456Y [m]1-1-2-201

嵐和2

原進

いることを考えると当然の結果であるともいえる．一方で，相対状態まで考慮して UAV を制御することに計算コストの観点から見るとほとんど差はなく，その意味で HOC を FFC に用いることは有効であるといえる．
しかしながら，HOC は線形システムを要求するため，障害-2

X [m]-101

物回避などを含む複雑な制御への拡張は難しい．これに対して，2MPC などを用いることで，障害物回避を達成しながらの FFCの実現例も報告されている [5]．そのため，複雑なタスク実行を目指すための FFC には，HOC が適しているとは言い難いものの，比較的容易なタスクを達成するための FFC には，性能保証の観点から見ても HOC は適した手法であるといえる．ただし，HOC が行う保証は，式 (8) に示す評価関数を最小化する制X [m](a) LQR(b) HOC

Fig. 3: Trajectories during the tracking experiments.

御入力が定まるという最適性の保証であり，実システムの安定

1234560.8
Error [m]也による定式化において，相対状態が考慮された制御則となって0-1-1

紀12345610-2

直2Y [m]2

井0.6性保証まではできないことに留意されたい．

5. お わ り に

0.4

本稿では，階層化最適制御を用いたクアッドコプタの FFC の0.2実機実証を行った．実験では，ホバリングと経路追従実験を行001020304050

い，それぞれ線形 2 次レギュレータを用いた制御方法と比較し60Time [second]た．これらの結果から，各クアッドコプタの相対位置誤差が，階層化最適制御を用いた場合の方が小さくなることを確認した．
本稿で行った検証は，モーションキャプチャを用いたもので(a) LQRあるため，今後は，この様な外部位置計測システムに頼らない自己位置推定システム（例えば [9] [10]）を用いた検証を行う．

1234
56Error [m]0.80.6謝 辞

参0.4
0.200102030405060

Time [second](b) HOC

Fig. 4: Relative errors during the tracking experiments.
軌跡を示す．なおこの実験では，前項で述べたホバリング制御が収束した状態から，そのフォーメーションを維持しつつ，式(1) に示す d xi を時間変化させることで経路追従を行う様にしている．実験では，d xi が円軌道を描くように変化させているが，LQR と HOC のどちらの手法を用いた場合でも，正確に円軌道を追従するには至らなかった．なお，前項でホバリング実験と異なり，目標状態となる d xi が時間変化するため，誤差は絶対に 0 にはならない．そのため，目標状態に対する誤差の評価は省く．

Fig. 4 には，経路追従実験時の相対誤差を示す．経路追従実験時の相対誤差には，LQR と HOC の間に大きな差が出る結果となった．なおそれぞれの平均相対誤差は，LQR を用いた場合が 0.200 m，HOC を用いた場合が 0.072 m であった．前項で述べた実験結果と合わせて，HOC を用いることで相対誤差をより小さくすることが可能になることが確認できた．

4. 4 考察HOC による定式化の導入により，相対状態の誤差を小さくする制御が実現できることが確認できた．ただしこれは，HOC

JRSJ Vol. xx No. xx

本研究は，科研費 18K13727 の支援により行われた．

—4—考文

献

[ 1 ] 椿野，吉岡，原. 代数的特徴付けに基づく大規模動的システムの階層化最適制御. 計測自動制御学会論文集, 49(12):1154–1163, 2013.
[ 2 ] Y. Kamitani and D. Tsubakino. Formation control for multiple quadrotors based on optimal regulation of individual andcooperative performance outputs. In The 3rd International
Symposium on Swarm Behavior and Bio-Inspired Robotics,
pages 121–127, 2019.
[ 3 ] F. Giulietti, L. Pollini, and M. Innocenti. Autonomous formation flight. IEEE Control Systems Magazine, 20(6):34–44,2000.
[ 4 ] W. Ren and R.W. Beard. Decentralized scheme for spacecraft
formation flying via the virtual structure approach. Journal of
Guidance, Control, and Dynamics, 27(1):73–82, 2004.
[ 5 ] V.S.M. Rosa and E.M. Belo. Virtual structure formation flightcontrol based on nonlinear MPC. In 2021 International Conference on Unmanned Aircraft Systems (ICUAS), pages 1383–1390, 2021.
[ 6 ] W. Zhao and T.H. Go. Quadcopter formation flight control
combining MPC and robust feedback linearization. Journal ofthe Franklin Institute, 351(3):1335–1355, 2014.
[ 7 ] D. A. Mercado, R. Castro, and R. Lozano. Quadrotors flightformation control using a leader-follower approach. In 2013
European Control Conference (ECC), pages 3858–3863, 2013.
[ 8 ] A. Davidi, N. Berman, and S. Arogeti. Formation flight using multiple integral backstepping controllers. In 2011 IEEE
5th International Conference on Cybernetics and Intelligent
Systems (CIS), pages 317–322, 2011.
[ 9 ] N. Akai, K. Yasui, K. Arashi, K. Saliou, D. Tsubakino, andS. Hara. Bayesian filtering fusion of optimization-based monocular visual localization and autonomous quadcopter navigation.
In 2022 IEEE/SICE International Symposium on System Integration (SII), pages 754–759, 2022.
[10] N. Akai. Mobile robot localization considering uncertainty ofdepth regression from camera images. IEEE Robotics and Automation Letters (RA-L), 7(2):1431–1438, 2022.

xx, 202x