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 21, x, y, z, X, Y, Z 1 1: 2:,, θ, ϕ, ψ 2,,, R(Θ) = R(ψ)R(ϕ)R(θ) R( ) 17 第 61 回システム制御情報学会研究発表講演会 (217 年 5 月 23 日 25 日, 京都 )
22 2] m ξ + mge z = F ξ (1) η = J 1 (τ C(η, η) η) (2) F ξ Z F = E z u R(Θ) F ξ = R(Θ)F (3) 1 1: m ξ = (x, y, z) g g = 98m/s 2 ] E z = ] η = (ψ, ϕ, θ),, J = diag(i x, I y, I z ) τ=(τ ψ, τ ϕ, τ θ ) C u Z () 23 (η =, η =, u = mg) mẍ = mgϕ (4) I x ϕ = τϕ mÿ = mgθ (5) I y θ = τθ m z = u mg (6) I z ψ = τψ (7) I x, I y, I z (4) (7),, (y ) (5) 24 u τ (Parrot AR Drone2) PC η d = (ψ d, ϕ d, θ d ) u τ (8) PD I y θ(t) = b (θ(t) θ d (t L 1 )) a θ(t) mÿ(t) = mgθ K ẏ(t) (8) (5) 2 (8) 2 K WiFi L 1 WiFi PC L 2 25 (8) t (8) ẋ(t) = A c x(t) + B c u(t L 1 ) y(t) = C c x(t) θ(t) x(t) = θ(t) y(t), u(t) = θ d(t), y(t) = y(t), ẏ(t) 1 A c = b a 1, B c = b, g K v ] C c = 1 (9a) (9b) a = a /I y, b = b /I y, K v = K /m 1 1 L 1, L 2 t l 1, l 2 L 1 = l 1 t, L 2 = l 2 t 1 sine sweep 2
ts] u(k t + τ) = u(k t), τ, t), (k =, 1, 2, ) xk + 1] = Axk] + Buk l 1 ] yk] = Cxk] (1a) (1b) t A = e Ac t, B = e Ac t e Acτ dτb c, C c = C f(t) t = k t, k =, 1, 2, fk] := f(k t) 2 (i) (A, B) (C, ] A) A I B (ii) rank C 3,, 3, G(z) 31 3: 4 (y ) 4 y rk] (i) lim rk] yk] = k 4: (ii) (i) (ii) LQ LQI 32 l 1 l 2 3] yk l 2 ] yk l 2 + 1] x od k] := (11) yk 1] ] x od k], Xk] := xk] (1), Xk + 1] = A ob Xk] + B ob uk l 1 ] yk l 2 ] = C ob Xk] (12a) (12b) I p 1 A ob = I p, B ob =, C ob = C A B T
x id k] uk l 1 ] uk l 1 + 1] x id k] := uk 1] 33 LQI (i),(ii) LQI W k + 1] := W k] + Ek], Ek] := rk l 2 ] yk l 2 ] Xk] X m k] = W k], x id k] X m k + 1] = ÂX mk] + ˆBuk] + r 1] T yk l 2 ] = ĈX mk] A ob B ob C ob 1 1 Â =, ˆB = 1 ] Ĉ = 1 1 (13a) (13b) uk] = K X Xk] + K W W k] + K xid x id k] + u (14) K X, K W, K xid, u (13a), r 2(ii) (i) x, u x, ū ] x = ū A I C ] ] B r (15) W r X ū X m = W, X = r, x id = (16) x id ū x (14) k u = K X X KW W Kxid x d + ū (17) Xk] X m k] = X m k] X m = W k] x id k] Xk] = Xk] X, W k] = W k] W, x id k] = x id k] x id, ũk] = uk] ū X m k + 1] = Â X m k] + ˆBũk] (18) (14), (17) ũk] = K X m k], K = K X K W K xid ] (ii) J J = = (Ek] 2 + Q w W k] 2 + Rũk] 2) k= ( Xk] W k] k= x d k] T Xk] ) Q W k] + Rũk] 2 x d k] Q = diag(, C T C, Q w, ) 3 Q w R (19) K = (R + ˆB T P ˆB) 1 ˆBT P Â (2) P Riccati P = ÂT P Â ÂT P ˆB(R + ˆB T P ˆB) 1 ˆBT P Â + Q (21) 2, 3, W
, J = X T m]p X m ], W = P XW T X] + P W xid x id ] (22) P W W P XW, P W W Riccati P 34 2(i) (C ob, A ob ) yk l 2 ] ] yk l x1 k] 2 + 1] Xk] = = x 2 k] yk 1] xk] (12a) x 1 k + 1] = A 12 x 2 k] (23) x 2 k + 1] = A 22 x 2 k] + B 2 uk l 1 ] (24) I p ] I A11 A 12 p A ob = = A 21 A 22 Ip, C A ] B1 B ob = = B 2 B x 1 k] = yk l 2 ] x 2 k] x 2 k] 5] 43 (23), (24) (A z ) γ γ 1: γ 2 P L γ (, 1) γq L γq T L γ 2 P L A T 22P L A 22 A T 12Q L Q T L A 12 ] > P L Q L L = P 1 L Q L A z = A 22 LA 12 γ 4 4 yz- z = 1m] y 1m] On-Off x, z x = 2m] z = 1m] 15s], 3s] 2 1/3 333s] 3 LQ Q w = 1, R = 3, γ = 86 25% 1s] 2: AR Drone 2 (Parrot ) 515cm] 52cm] 472 g] 15rad] 35,rpm] HD 72p 3fps] zk + 1] = A z zk] + A z Lx 1 k] + B 2 uk l 1 ] ˆx 2 k] = zk] + Lx 1 k] A z := A 22 LA ] 12, Xk] ˆXk] x 1 k] = L, ˆx 2 k] 3: I x = 33, I y = 33, I z = 62kg m 2 ] L 1 = 133, L 2 = 566 s] a = 12, b = 627, K v = 28567
41 8 6 4 2 2 4 6 8 1 12 14 16 18 2 5: (), (l 1 = l 2 = ) 5 42 6 15 1 5-5 5 1 15 2 25 3 2 1-1 (a) (y ) -2 5 1 15 2 25 3 (b) 6: () 6(a) 6(b) 5,, LQI anti-windup 6] 1] T Pobkrut, T Eamsa-ard, T Kerdcharoen: Sensor drone for aerial odor mapping for agriculture and security services, Proc of 13th ECTI-CON, 216 2] LR Garcia Carrillo et al: Quad Rotorcraft Control: Vision-based Hovering and Navigation, Springer-Verlag, 213 3] F Liao, K Takaba, T Katayama: Design of an optimal preview servomechanism for discrete-time systems in a multirate setting, Dynamics of Continuous, Discrete and Impulsive Systems: Series B, vol 1, pp 727-744, 23 4] LV Santana, AS Brandao, M Sarcinelli-Filho: Outdoor waypoint navigation with the ARDrone quadrotor, Proc of Int Conf on Unmanned Aircraft Systems (ICUAS), pp 33-311, 215 5] 1999 6] Anti-Windup, 32, pp139-144, 23 7] :,, 21 8] :, 59, 4p, 215