第 61 回 システム制御情報学会研究発表講演会 （2017 年 5 月 23 日〜 25 日， 京都）
’17
むだ時間を考慮したドローンの遠隔ビジュアルサーボ制御
The Visual Servo Control of Drone in Consideration of Dead Time
立命館大学 ○ 白井 淳平, 山口 崇司, 鷹羽 浄嗣
○ Junpei Shirai and Takashi Yamaguchi and Kiyotsugu Takaba Ritsumeikan University
Abstract Recently, the use of drones has been expected in many applications such as exploration in dangerous areas, security surveillance, and logistics. Accurate position control of the drones is required in these applications. This work is concerned with the remote position tracking control of a quadrotor-type drone via visual feedback control. The transient response or stability of the drone may be significantly deteriorated by the adverse effect of the time delays due to wireless communication and image processing. In this work, we construct the LQI optimal servo control system with state augmentation to cope with the time delays. The effectiveness of the proposed method is verified by an experiment.
1
はじめに
本稿では, 小型無人航空機の一種であるクアッドロー タ型ドローン (以下ドローン) の制御について考察する. ドローンは 4 つの回転翼を持ち, 各ロータの回転数のみ によって制御可能な小型無人航空機である．近年のセン サ技術や電子部品の発展により, 高性能なドローンが安 価で手に入るようになり研究が盛んである．ドローンの 応用として, 災害現場で人が立ち入ることができない場 所の探索, 人工知能とリンクさせた不審者の発見, 追跡 [1] や物流などが期待されている．これらの応用でドロー ンを使用する際, 位置の自動制御は必要不可欠である． 本稿では精密な位置制御として, ドローンに搭載されて いるカメラを用いたビジュアルフィードバック制御を考 察する. この際の問題点となるのが, 通信および画像処 理によって生じるむだ時間による制御性能の劣化および 不安定化である．最終的な目標は, ドローン単体で画像 処理を行い追従を成功させることであるが, 本稿で使用 するドローンは外部 PC と制御信号を授受するため, 通 信によるむだ時間も考慮する．これらのむだ時間を補償 し, 離散時間系における LQI 最適サーボ設計に帰着させ ることによって制御則を導出し, 実機実験を行いむだ時 間を考慮しないで設計した場合との比較検討を行う．ビ ジュアルフィードバックを用いた高さ制御に関しては, 永野 [8] による研究で成功しているため, 本稿では左右 方向の制御系設計を行う.2
モデリング
2.1 座標系と回転行列 地上座標系は地上に固定されたマーカーの重心に原点 を取り, 北向きを x 軸, 西向きを y 軸, 地球の中心から外 向きを z 軸とする. 機体座標系は機体の重心を原点とし た座標系であり, ドローンに固定されている前カメラ方 向を X 軸, 左向きを Y 軸, Z 軸を右手座標系とする. そ れぞれの座標系は図 1 に示す. 図 1: 座標系 図 2: 各方向の回転方向 回転角ロール, ピッチ, ヨー角をそれぞれ θ, ϕ, ψ とす る．各回転方向は図 2 のようになる. 機体の回転の順番 がロール, ピッチ, ヨーの順番であるとすると, 機体座標 系から地上座標系へ変換する行列は R(Θ) = R(ψ)R(ϕ)R(θ) となる．ここで R(·) はそれぞれの姿勢角に関する機体 座標系から地上座標系への回転行列である.2.2 運動方程式 よく知られているようにドローンの並進・回転運動は， 次式の非線形運動方程式で与えれる [2]. m ¨ξ + mgEz= Fξ (1) ˙ η = J−1(τ− C(η, ˙η) ˙η) (2) ここで Fξは機体座標系 Z 方向に生じる各ロータからの 推力の和 F = Ezu 及び回転行列 R(Θ) を用いて Fξ= R(Θ)F (3) 各物理量およびパラメータは表 1 に示す. 表 1: 物理量およびパラメータの定義 m 機体の質量 ξ = (x, y, z) 地上座標系における位置 g 重力加速度 g = 9.8[m/s2] Ez= [0 0 0]⊤ ｚ方向の単位ベクトル η = (ψ, ϕ, θ) ヨー, ピッチ, ロール方向の姿勢角 J = diag(Ix, Iy, Iz) 慣性モーメント行列 τ =(τψ, τϕ, τθ) 各ロータの推力によるトルク C 航空力学トルク u Z 方向の揚力 (各ロータの推力の和) 2.3 線形化モデル ホバー状態 (η = 0, ˙η = 0, u = mg) で運動方程式を線 形化する. m¨x =−mgϕ Ixϕ = τ¨ ϕ (4) m¨y = mgθ Iyθ = τ¨ θ (5) m¨z = u− mg (6) Izψ = τ¨ ψ (7) ここで Ix, Iy, Izは機体の重心周りの慣性モーメントで ある．式 (4)∼(7) は, ４つの非干渉化された部分システ ムとして見ることができる．本稿では, ドローンの左右 方向 (y 軸方向) のみの制御を行うため式 (5) を用いる． 2.4 内部コントローラ ここまで述べた非線形及び線形モデルにおいて入力量 は各ロータが発生する推力 u やトルク τ である．しかし本 研究の実験に用いたドローン (Parrot 社製 AR Drone2.0) では，PC から送信できる指令値は，ヨー，ピッチ，ロー ル角度目標値 ηd = (ψd, ϕd, θd) であり，推力 u やトル ク τ を指令値として直接送信できない．また，ドローン に元から装備されている内部コントローラは，ブラック ボックス化されており，変更することができない．そこ で本研究では，内部コントローラは式 (8) で表すように PD コントローラとなっていると仮定する． Iyθ(t) =¨ −b0(θ(t)− θd(t− L1))− a0θ(t)˙ m¨y(t) = mgθ− K0˙y(t) (8) ただし，式 (5) の第 2 式に空気抵抗の項を付加して式 (8) 第 2 式を得ている．K0は空気抵抗係数である．ドロー ンは WiFi 通信を介して指令値を送信するため，通信に よる入力むだ時間 L1が発生する．また観測においては， 内蔵カメラで撮影された画像をドローンから WiFi 経由 で PC に送り，ドローンと目標値の位置を計算するため， 通信と画像処理に起因する出力むだ時間 L2が存在する． 2.5 離散時間モデル 本研究では，式 (8) をサンプリング時間 ∆t で離散化し たモデルに基づいてデジタルコントローラを設計する． まず連続時間系の状態空間モデルとして式 (8) より， 次式を得る． ˙ x(t) = Acx(t) + Bcu(t− L1) (9a) y(t) = Ccx(t) (9b) x(t) = θ(t) ˙ θ(t) y(t) ˙y(t)
, u(t) = θd(t), y(t) = y(t),
Ac = 0 1 0 0 −b −a 0 0 0 0 0 1 g 0 0 −Kv , Bc= 0 b 0 0 , Cc= [ 0 0 1 0 ] ただし，a = a0/Iy, b = b0/Iy, Kv= K0/m である1． ここで，むだ時間に関して，次の仮定をおく． 仮定 1： むだ時間 L1, L2はサンプリング時間 ∆t の 整数倍である．すなわち，適当な非負整数 l1, l2に対し て，L1= l1∆t, L2= l2∆t が成り立つ． 1パラメータの値に関しては，sine sweep による周波数応答デー タから最小 2 乗推定により得た．
このモデルをサンプリング時間 ∆t[s] で 0 次ホールド により離散化する．制御入力を
u(k∆t + τ ) = u(k∆t), τ ∈ [0, ∆t), (k = 0, 1, 2, . . . ) として，離散時間空間モデルは次式で与えられる.
x[k + 1] = Ax[k] + Bu[k− l1] (10a)
y[k] = Cx[k] (10b) A = eAc∆t, B = eAc∆t ∫ ∆t 0 e−Acτdτ Bc, Cc = C ただし，連続時間信号 f (t) の t = k∆t, k = 0, 1, 2, ... に おけるサンプル値を f [k] := f (k∆t) と表記する． 後述の積分型制御系設計のために次の仮定をおく． 仮定 2： (i) (A, B) は可安定，かつ，(C, A) は可検出である． (ii) rank [ A− I B C 0 ] は，行フルランクである．
3
コントローラ設計
離散時間系において, 通信及び画像処理にかかるむだ 時間を補償し, 所望の性能を満たすコントローラを設計 する. 図 3 は, 制御対象であるドローンの伝達関数 G(z) 及び制御コントローラ全体のブロック線図である. ݖିమ ͳ ݖ െ ͳ ܭௐ ݑ ݖ ିభ ܩሺݖሻ ݎሾ݇ሿ ݖିమ ኇኳናዙክ ܭଡ଼ ݖିଵ ݖିଶ ڭ ݖିభ ܭ௫ ݕሾ݇ሿ ܺሾ݇ሿ ݑሾ݇ሿ ݔௗሾ݇ሿ ኦዊዙዐ ʶ െ 図 3: ブロック線図 3.1 問題設定 図 4 にシステム構成を示す. ドローン内蔵のカメラか ら得られる目標物体の画像情報に基づいて，機体に関し て左右方向 (y 軸方向) の目標値追従制御を行う．具体的 には，画像から得られる目標物体（図 4 では黄色のラン プ）の y 座標 r[k] に対して， (i) 定常偏差の無い目標値追従 lim k→∞|r[k] − y[k]| = 0 図 4: 問題設定 (ii) 速やかかつ滑らかな過渡応答 を実現する制御系を設計する．本研究では，(i) に対し て積分補償を，(ii) に対して LQ レギュレータを適用す る LQI 最適サーボ系を設計する． 3.2 拡大状態空間モデル 前述のように，本研究の制御系には，通信による入力 むだ時間 l1と通信と画像処理に起因する出力むだ時間 l2が存在する．むだ時間の補償には，むだ時間を反映し た拡大状態空間モデルを構成する手法が有効である [3]． まず，出力むだ時間を並べたベクトル xod[k] := y[k− l2] y[k− l2+ 1] .. . y[k− 1] (11) を定義し, X[k] := [ xod[k] x[k] ] により状態を拡大すれば， 状態方程式 (10) は, 次式のように書換えられる. X[k + 1] = AobX[k] + Bobu[k− l1] (12a) y[k− l2] = CobX[k] (12b) Aob= 0 Ip 0 0 0 0 0 . .. . .. ... .. . ... . .. Ip 0 0 . . . 0 0 C 0 . . . 0 0 A , Bob= 0 0 .. . 0 B , Cob= 1 0 .. . 0 T入力むだ時間についても，ベクトル xid[k] を xid[k] := u[k− l1] u[k− l1+ 1] .. . u[k− 1] として，状態を拡大すればよ いが，詳細は次節で述べる． 3.3 LQI 最適サーボ設計 前節で述べたように，制御目的 (i),(ii) を達成するた めに，LQI 最適サーボ系を設計する． ここで積分器を
W [k + 1] := W [k] + E[k], E[k] := r[k− l2]− y[k − l2]
で与え，Xm[k] = X[k] W [k] xid[k] と定義すれば, 拡大された状 態に関する状態空間モデルは次式となる. Xm[k + 1] = ˆAXm[k] + ˆBu[k] + [0 r 0 . . . 0 1]T (13a) y[k− l2] = ˆCXm[k] (13b) ˆ A = Aob 0 Bob 0 0 0 −Cob 1 0 0 0 0 0 0 0 1 0 0 0 0 0 0 . .. 0 0 0 0 0 0 1 0 0 0 0 0 0 , B =ˆ 0 0 0 .. . 0 1 ˆ C = [ 1 0 . . . 0 ] 本節では，全状態が観測可能であると仮定して，次の状 態フィードバック則を適用する． u[k] = KXX[k] + KWW [k] + Kxidxid[k] + u0 (14) KX, KW, Kxidはフィードバックゲインであり, u0は目 標値からのフィードフォワード要素である. (13a) 式には, r に関する項が含まれている．この制御 則を設計するために，誤差系を構成することによりこれ を消去する． 仮定 2(ii) の下で，制御目的 (i) が達成されたときの x, u の定常値 ¯x, ¯u は [ ¯ x ¯ u ] = [ A− I B C 0 ]†[ 0 r ] (15) で表される．積分器の定常値 ¯W は任意である．このと き，定義により拡大状態ベクトルの定常値は ¯ Xm= ¯ X ¯ W ¯ xid , ¯X = r .. . r ¯ x , x¯id= ¯ u .. . ¯ u (16) となる．さらに (14) 式を k→ ∞ で評価することにより u0=−KXX¯ − KWW¯ − Kxidx¯d+ ¯u (17) を得る． それぞれの状態の定常値との偏差を ˜ Xm[k] = Xm[k]− ¯Xm= ˜ X[k] ˜ W [k] ˜ xid[k] ˜ X[k] = X[k]− ¯X, W [k] = W [k]˜ − ¯W , ˜
xid[k] = xid[k]− ¯xid, u[k] = u[k]˜ − ¯u
と定義すれば，誤差系の拡大状態方程式を得る． ˜ Xm[k + 1] = ˆA ˜Xm[k] + ˆB ˜u[k] (18) 容易に確認できるように，(14), (17) 式は ˜u[k] = K ˜Xm[k], K = [KX KW Kxid] と同値である． 制御目的 (ii) を達成するために，評価関数 J を次のよ うに定義する． J = ∞ ∑ k=0 ( E[k]2+ QwW [k]˜ 2+ R˜u[k]2 ) = ∞ ∑ k=0 ( ˜ X[k] ˜ W [k] ˜ xd[k] T Q ˜ X[k] ˜ W [k] ˜ xd[k] + R˜u[k]2 ) (19) Q = diag(0, CTC, Qw, 0) 仮定 3： 重み Qwと R は正定数である． このとき，最適フィードバックゲインは K =−(R + ˆBTP ˆB)−1BˆTP ˆA (20) で与えられる．ここに P は，代数 Riccati 方程式 P = ˆATP ˆA− ˆATP ˆB(R + ˆBTP ˆB)−1BˆTP ˆA + Q (21) の非負定値安定化解である．そのような解の存在は，仮 定 2, 3 の下で保証されている．ここで, ¯W は任意であ
るが, 最適コスト J∗ = ˜XT m[0]P ˜Xm[0] をさらに最小化 するように, ¯ W = P T XWX[0] + P˜ W xidx˜id[0] PW W (22) とする．ここで PXW, PW W は代数 Riccati 程式の解 P の部分行列である. 3.4 最小次元オブザーバ 仮定 2(i) より (Cob, Aob) は可検出である．このとき， X[k] = [ x1[k] x2[k] ] = y[k− l2] y[k− l2+ 1] .. . y[k− 1] x[k] のように分割すれば，(12a) 式は x1[k + 1] = A12x2[k] (23) x2[k + 1] = A22x2[k] + B2u[k− l1] (24) となる．ただし， Aob= [ A11 A12 A21 A22 ] = 0 Ip 0 0 0 0 0 Ip 0 ... .. . ... . .. Ip 0 0 . . . 0 0 C 0 . . . 0 0 A , Bob= [ B1 B2 ] = 0 0 .. . 0 B である．状態変数のうち x1[k] = y[k− l2] は観測可能で あるが，x2[k] は観測できないので，最小次元オブザー バを設計して，x2[k] を推定する．離散時間系に対する 最小次元オブザーバの理論（例えば，文献 [5] 定理 4.3） によれば，式 (23), (24) に対する最小次元オブザーバは， 次式で与えられる． z[k + 1] = Azz[k] + AzLx1[k] + B2u[k− l1] ˆ x2[k] = z[k] + Lx1[k] ここで Az:= A22− LA12である．この式より, X[k] の 推定値 ˆX[k] = [ x1[k] ˆ x2[k] ] を得る．オブザーバゲイン L は, オブザーバの極 (Azの固有値) の絶対値が γ 未満になる ように次の線形行列不等式を解くことにより設計する． ただし，パラメータ γ は推定誤差の収束速度を表す． 命題 1: 与えられた γ ∈ (0, 1) に対して， [ γ2P L γQL γQT L γ2PL− AT22PLA22− AT12QL− QTLA12 ] > 0 を満たす対称行列 PLと行列 QLが存在すると仮定する． このとき，L = PL−1QLとすれば，Az= A22− LA12の すべての固有値の絶対値は γ 未満となる．
4
シミュレーションと実機実験
図 4 に示すように，yz-平面上に z = 1[m] の高さで y 軸方向に 1[m] 間隔で配置された電球の On-Off を切替 えることにより，ドローンを点灯中の電球に追従させる 実験を行う．なお，x, z 軸方向には，x = 2[m] および z = 1[m] を維持するようにフィードバック制御をかけて いる． 点灯を切替えるタイミングは，15[s], 30[s] とする．実 験に用いるドローンの仕様を表 2 に示す．内蔵カメラの フレームレートに合わせて，サンプリング時間を 1/30≃ 0.0333[s] とする．また，実測およびシステム同定による 各パラメータ値を表 3 に示す． さらに，いずれのシミュレーション・実験においても， LQ レギュレータおよびオブザーバ極配置のパラメータは Qw= 0.001, R = 30, γ = 0.86 とした．これらは，シミュレーションでオーバシュート 25% 以下かつ整定時間 10[s] 以下となるように設定した． 表 2: ドローン仕様 機種 AR Drone 2.0 (Parrot 社製) 大きさ 51.5[cm]×52.0[cm] 質量 472 [g] 入力制約 0.15[rad] 動力 ブラシレス・モーター 35,000[rpm] 内蔵センサー HD カメラ 720p 30[fps] 対地面超音波センサー（高度計） ジャイロセンサー 表 3: システムパラメータ 慣性モーメント Ix= 0.033, Iy= 0.033, Iz= 0.0062[kg·m2] むだ時間 L1= 0.133, L2= 0.566 [s] 内部コントローラ a = 10.2, b = 62.7, Kv= 2.85674.1 むだ時間補償なしの場合 0 2 4 6 8 10 12 14 16 18 20 0 0.2 0.4 0.6 0.8 図 5: シミュレーション結果 (むだ時間補償なし) まず，むだ時間補償の有効性を確かめるために, むだ 時間を無視 (l1= l2 = 0 と仮定) して設計して得られた コントローラをむだ時間を有する線形プラントモデルに てシミュレーションした結果を図 5 に示す．むだ時間補 償しないで設計した場合，オーバシュート及び整定時間 が大幅に増大していることがわかる．また，このコント ローラを実機に適用したが，オーバーシュートが大きい ためにカメラの視野から外れ，追従に失敗した． 4.2 むだ時間補償ありの場合 提案手法によりむだ時間補償を行った場合のシミュレー ション及び実験の結果を図 6 に示す． 0 5 10 15 20 25 30 -0.5 0 0.5 1 1.5 (a) 位置 (y 座標) 0 5 10 15 20 25 30 -0.2 -0.1 0 0.1 0.2 (b) 入力 図 6: 実験結果 (むだ時間補償あり) 図 6(a) より，シミュレーション結果・実験結果とも に，所期の応答が実現できていることがわかる．しかし ながら実験結果の応答は，若干振動している．この原因 として，気流による外乱や線形化で無視した非線形性の 影響が挙げられる．また図 6(b) では，入力が飽和しワ インドアップ現象が発生している事がわかる．オーバー シュートをさらに低減するためには，入力飽和に対する 対策が必要である．
5
おわりに
本稿では, ビジュアルフィードバックを利用したドロー ンの目標追従制御系の設計及び実機検証を行った．この 際, 画像処理や通信によって生じるむだ時間を状態の拡 大によって補償を行い LQI 最適サーボ系を設計した．実 機検証の結果より，むだ時間補償手法の有効性を確認 した．今後の課題としてオーバシュート低減のための anti-windup 制御 [6] や非線形要素を考慮した設計があ げられる.参考文献
[1] T. Pobkrut, T. Eamsa-ard, T. Kerdcharoen: “Sen-sor drone for aerial odor mapping for agriculture and security services,” Proc. of 13th ECTI-CON, 2016.
[2] L.R. Garcia Carrillo et al.: Quad Rotorcraft Control: Vision-based Hovering and Navigation, Springer-Verlag, 2013
[3] F. Liao, K. Takaba, T. Katayama: “Design of an optimal preview servomechanism for discrete-time systems in a multirate setting,” Dynamics of Con-tinuous, Discrete and Impulsive Systems: Series B, vol. 10, pp. 727-744, 2003.
[4] L.V. Santana, A.S. Brandao, M. Sarcinelli-Filho: “Outdoor waypoint navigation with the AR.Drone quadrotor,” Proc. of Int. Conf. on Unmanned Air-craft Systems (ICUAS), pp. 303-311, 2015.
[5] 萩原：ディジタル制御入門，コロナ社，1999. [6] 達見，鷹羽：“制御性能を考慮した Anti-Windup 制 御,” 第 32 回制御理論シンポジウム, pp.139-144, 2003. [7] 藤田 : ロバスト制御, コロナ社, 2001. [8] 永野，鷹羽 : “ビジュアルフィードバックによるク アッドコプターの高度制御” , 第 59 回システム制御 情報学会研究発表講演会講演論文集, 4p, 2015
