Remote Visual Servo Tracking Control of Drone Taking Account of Time Delays

Transcription

1 Proceedings of the SICE Annual Conference 217 September 19-22, 217, Kanazawa University, Kanazawa, Japan Remote Visual Servo Tracking Control of Drone Taking Account of Time Delays Junpei Shirai 1, Takashi Yamaguchi 1 and Kiyotsugu Takaba 1 1 Department Electrical and Electronic Engineering, Ritsumeikan University ( ktakaba@fcritsumeiacjp) Abstract: The use of drones has recently 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 for the linearized drone model with state augmentation to cope with the time delays The effectiveness of the proposed method is verified by a simulation and an experiment Keywords: LQI optimal servomechanism, visual feedback, time delay, state augmentation 1 INTRODUCTION We consider the control of a quadrotor-type drone which is a kind of small unmanned aerial vehicle The drone is controlled only by the angular speeds of four electric rotors Due to the recent development of electronic devices and sensors, high spec drones have come to be available at inexpensive costs The drones are expected in many applications such as exploration at disaster sites, security surveillance[2], and logistics Accurate automatic position control of the drones is essential in these applications In this paper, we will consider the remote position tracking control of the drone via visual feedback using a built-in camera 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 Several researches have been reported on how to cope with these time-delays For instance, Kiset al[4] employed the state prediction control technique to compensate the delay due to image processing under the situation that the position of the drone is measured bya camerafixed on the ground In this paper, we will consider the remote position tracking control based on the visual information obtained from a built-in camera rather than a ground-fixed camera We will design the LQI optimal servomechanism for the augmented linear state space model which includes the information about the time delays [5-7] An experimental result will also be presented in order to verify the effectiveness of the proposed method 2 MODELING 21 Coordinate systems and rotation matrices Fig1 shows the coordinate systems of the drone The inertial fixed frame is a system of coordinates whose origin is located at the center of the marker fixed on the ground The x-axis points towards the north, the y-axis points towards the west, and the z-axis points towards the Junpei Sirai is the presenter of this paper outward direction from the center of the earth The Body fixed frame is a system of coordinates whose origin is fixed at the center of gravity of the drone The X-axis of the body frame is the optic axis of the front camera fixed to the drone The Y-axis points towards the left direction, and Z-axis points towards the up direction The rotation angles, namely roll, pitch, and yaw, are denoted by θ, φ, and, ψ respectively The orientation of the drone is defined in the order of the roll, pitch, and the yaw angles Hence, the rotation matrix R(Θ) which converts the orientation in the body frame to that in the inertial frame is expressed by R(Θ) = R(ψ)R(φ)R(θ), R(ψ), R(φ), and R(θ) are the rotation matrices of the rotations around the x, y, and z-axes, respectively Fig 1 Coordinate systems 22 Equations of motion This section introduce the modeling of the drone [?, 3] The translational and rotational motions of the drone are expressed by the nonlinear differential equations as m ξ + mge z = R(Θ)E z u (1) η = J 1 (τ C(η, η) η) (2) PR1/ SICE 1589

2 that the internal controller has the PD control structure described by θ(t) = b(θ(t) θ(t L 1 )) a θ(t), mÿ(t) = mgθ K ẏ(t), (7a) (7b) Fig 2 Rotations of the drone The physical parameters of the drone are defined in Table 1 In particular, the drone of (1), (2) is driven by the torque τ and the thrust u in the Z-direction Table 1 Physical parameters m the mass ξ=(x, y, z) T the position in the inertial frame g the acceleration of gravity 98[m/s 2 ] E z = [,, 1] T the unit vector in the z-direction η=(ψ, φ, θ) T (yaw, pitch, roll) J = diag(i x, I y, I z ) the inertia matrix τ=(τ ψ,τ φ,τ θ ) T the vector of torques C(η, η) the Coriolis term u the thrust in the Z-direction Linearizing (1) and (2) about the hovering states, ie η =, η =, u = mg, yields the following four decoupled sub-systems: mẍ = mgφ, (3) I x φ = τ φ, mÿ = mgθ, (4) I y θ = τ θ, m z = u mg, (5) I z ψ = τ ψ, (6) I x, I y,andi z are the moments of inertia around the center of gravity in the body frame In this paper, we are interested in the tracking control in the crosswise direction, equivalently in the y-axis Since the sub-systems (3) (6) are decoupled each other, we have only to consider the equations in (4) 23 Internal control law In this work, we will use the quadrotor-type drone AR Drone 2 (Parrot Inc) for the experiment in Section 4 Although the inputs in the model (1), (2) are the torque τ and the thrust u, the AR Drone 2 incorporates the internal controller which converts the command angles η d = (ψ d,φ d,θ d ) and the vertical velocity command ż d into (τ, u) Since the internal controller is a black box, it is impossible to modify the controller, and we need to estimate its structure and feedback gains Thus, we assume the term K y is added in order to account for air resistance Since the drone is remotely controlled by the PC through wireless communication, there exist an input delay L 1 and an measurement delay L 2 The input delay L 1 is due to wireless communication, and the measurement delay L 2 is caused by both wireless communication and image processing The detail of the system configuration and problem setting will be described in Section discrete-time modeling We discretize (7a) at sampling period Δt to design a digital controller for the drone The state space model of the continuous time system (7a) is expressed as ẋ(t) = A c x(t) + B c u(t L 1 ), y(t) = C c x(t), θ θ x(t) =, u(t) = θ y d (t), y(t) = y(t), ẏ 1 b a A c = 1 g K v C c = [ 1 ],, B c = a = a /I y, b = b /I y, K v = K /m b, (8a) (8b) We make the following assumption about the time delays Assumption 1: The time delays L 1 and L 2 can be divided by the sampling period Δt Namely, there exist positive integers l 1 and l 2 such that L 1 = l 1 Δt and L 2 = l 2 Δt By the zeroth-order hold discretization of (8), we obtain the discrete-time state space model as x[k + 1] = Ax[k] + Bu[k l 1 ], y[k] = Cx[k], A = e A cδt, B = e A cδt Δt e A cτ dτb c, C c = C, (9a) (9b) a discrete-time or sampled-data signal is denoted by f [k] := f (kδt), k =, 1, 2,, and the control input is given through the zeroth-order hold as u(kδt + τ) = u[k], τ [, 1), k =, 1, 2, We put the following assumption for the later discussion on the servomechanism design 159

3 Assumption 2: (i) (A, B) is stabilizable, and (C, A) is detectable A I B (ii) has full row rank C 3 CONTROLLER DESIGN In this section, we will design a controller which achieves the desired tracking performance as well as compensating the time delays due to image processing and wireless communication, in the discrete-time setting 31 Problem Statement The schematic picture of the system configuration for our tracking control is shown in Fig3 By the in-front built-in camera, the drone captures the image of the target object expressed by the electric light balls, and send it to the PC The PC processes the image to detect the position of the target, and send back the control command to the drone All the communications between the PC and the drone are done by WiFi Since we are interested in the position tracking in the horizontal direction, the target object is constrained on the yz-plane Let the reference signal r[k] denote the y-position of the target object at step k regulator theory is applied to realize a good transient response (the objective (ii)) Fig4 illustrates the block diagram of the overall LQI servo control system, in which G(z) denotes the transfer function of the drone observer Drone Fig 4 LQI optimal servo control system with delays 32 Augmented state space model Recall that our control system contains the input delay l 1 due to wireless communication and the measurement delay l 2 due to wireless communication and image processing As is well known, the state augmentation [5-7] is an effective technique to cope with the time delays Define the augmented state vector including the measurement delays by X[k] xod [k], x x[k] od [k] = y[k l 2 ] y[k l 2 + 1] y[k 1], (1) It then follows from (9) that X[k + 1] = A ob X[k] + B ob u[k l 1 ], y[k l 2 ] = C ob X[k], (11a) (11b) Fig 3 System Configuration We wish to design the feedback controller which achieves the following control objectives (i) Robust servomechanism The output y[k] shouldtrackthe referencesignalr[k] without steady-state errors, namely lim r[k] y[k] = k for any step reference r[k] and for any initial conditions (ii) Rapid and smooth transient response For this purpose, we employ the LQI optimal servomechanism design [7], the integral action is incorporated in the controller to fulfill the objective (i) based on the internal model principle, and the LQ I p I p A ob =, B I p ob =, C A B C ob = [ 1 ] Furthermore, we define vector of delayed inputs by input u[k l 1 ] u[k l 1 + 1] x id [k] = u[k 1] This will be used in the next section 33 LQI optimal servomechanism We design the LQI optimal servo system to achieve the control objectives (i) and (ii) 1591

4 Firstly, we introduce the discrete-time integrator as k 1 W[k] = E[i 1] + W[], i=1 E[k] := E[k] = r[k l 2 ] y[k l 2 ] is the tracking error It is easily seen from the above equation that W[k + 1] = W[k] + E[k] (12) Furthermore, define to obtain X[k] X m [k] W[k], x id [k] r R 1 X m [k + 1] = ÂX m [k] + ˆBu[k] + R, y[k l 2 ] = ĈX m [k], A ob B ob C ob 1 1 Â =, ˆB =, 1 1 Ĉ = [ 1 ] (13a) (13b) For the moment, we assume that all the state variables can be accurately measured Then, we will design the feedback controller in the form of u[k] = K X X[k] + K W W[k] + K xid x id [k] + u, (14) K X, K W, K xd,andu are the feedback gains and the feedforward term to be designed In order to eliminate the constant term R on the righthand side of (13a), we introduce the steady-state values x,ū of the state x and the input u By Assumption 2 (ii), when the control objective (i) is achieved, the steady-state values x and ū satisfy [ x = ū] [ A I B (15) C ] r Note that the steady-state value W of the integrator is arbitrary Thus, the steady-state value of the augmented state vector is given by r X[k] ū X m [k] = W[k], X =, x id = x id [k] r (16) ū x Evaluating the equation (14) at k leads to u = K X X + K W W + K xid x id + ū (17) Consequently, by defining X[k] X m [k] = X m [k] X m = W[k], x id [k] X[k] = X[k] X, W[k] = W[k] W, x id [k] = x id [k] x id, ũ[k] = u[k] ū we get the state space model without any constant terms of the error system by X m [k + 1] = Â X m [k] + ˆBũ[k] (18) For the control objective (ii), we introduce the performance index ) J = (E[k] 2 + Q w W[k] 2 + Rũ[k] 2 (19) k= Minimization of J means that the tracking error should rapidly converge to zero without excessive variations of the control input and integrator Assumption 3: The weights Q w, R are positive constants Using the augmented state vector X m [k], the performance index J is rewritten as ( J = X m [k] T Q X m [k] + Rũ[k] 2), (2) k= Q = diag(, C T C, Q w, ) From the above observation, we can apply the LQ optimal regulator theory to (18),(2) in order to obtain the desired controller The optimal feedback gain is given by K = [ K X K W K xid ] = (R + ˆB T P ˆB) 1 ˆB T PÂ, (21) P is the positive semi-definite stabilizing solution to the algebraic Riccati equation (ARE) P = Â T PÂ Â T P ˆB(R + ˆB T P ˆB) 1 ˆB T PÂ + Q (22) The existence and uniqueness of such a solution P is guaranteed by the following proposition Proposition 1: Under Assumptions 2 and 3, (Â, ˆB) is stabilizable, and (Q, Â) is detectable Hence, there exists a unique positive semi-definite solution to the ARE (22), and it is also a stabilizing solution Proof: Omitted It should be noted that, since W is arbitrary, the optimal cost J = X m[]p T X m [] can be further minimized by choosing W as W = PT X[] XW + P Wxd x d [], (23) P WW P XW and P WW are the sub-matrices of P 1592

5 34 Minimal-order observer Since some components of X[k] andx xid [k] cannot be directly measured, we introduce the minimal order observer for the state space model of (11) Note that (C ob, A ob ) is detectable from Assumption 2(i) We partition the state equation (11a) as X[k] = x 1 [k + 1] = A 12 x 2 [k], (24) x 2 [k + 1] = A 22 x 2 [k] + B 2 u[k l 1 ], (25) [ x1 [k] x 2 [k] ] = y[k l 2 ] y[k l 2 + 1], y[k 1] x[k] I p I A11 A p A ob = 12 = A 21 A 22, I p C A B1, B ob = = B 2, B We need to estimate x 2 [k] by a minimal-order observer, while x 1 [k] is directly measurable By applying the minimal-order observer theory (eg Th 43 [4]) to (24) and (25), we obtain z[k + 1] = A z z[k] + A z Lx 1 [k] + B 2 u[k l 1 ] (26) ˆx 2 [k] = z[k] + Lx 1 (27) 4 SIMULATION AND EXPERIMENT We perform the experiment based on the system configuration of Fig3 In this experiment, two electric light balls are located at x = [m], z = 1[m], and y =, 1[m], respectively Either one of the light balls is turned on, and the other is turned off The bright light ball is the target object The switching times of the light balls are [s] and 15[s] Table2 shows the specifications of the drone used in this experiment The sampling period is set to Δt = 1/3[s], which is equal to the frame rate of the built-in camera The system parameter values are summarized in Table3 These values are obtained by direct measurement, or by system identification The drone is stabilized in the x- and z-directions to x = 2[m] and z = 1[m] by the other decoupled controllers For the LQ regulator and observer design, we choose Q w = 1, R = 3, γ = 86 so that the overshoot is less than 25[%] and the settling time is less than 1[s] Table 2 Specification of the drone model size mass constraint of input power sensor AR Drone 2 (Parrot) 515[cm] 52[cm] 472 [g] 15[rad] Brushless motor 35,[rpm] HD camera 72p 3[fps] Ultrasonic sensor Altimeter Gyro sensor Table 3 system parameter Inertia moment I x = 33, I y = 33, I z = 62[kg m 2 ] Time delay L 1 = 133, L 2 = 566 [s] Internal controller a = 12, b = 627, K v = A z A 22 LA 12, and the state estimate ˆX[k] is given by x1 [k] ˆX[k] = (28) ˆx 2 [k] We design the observer gain L so that the absolute value of every eigenvalue of A z is less γ, theparameter γ (, 1) prescribes the convergence rate of the state estimation errors With the aid of the regional pole placement technique [9], it turns out that such a gain L can be obtained by solving the linear matrix inequality (LMI) described below Proposition 2: For a given constant γ (, 1), assume that there exist a symmetric matrix P L and a matrix Q L satisfying γ 2 P L γq L γq T L γ 2 P L A T 22 P LA 22 A T 12 Q L Q T L A > 12 Then, the absolute values of all the eigenvalues of A z = A 22 LA 12 are less than γ for L = P 1 L Q L Simulation result We perform a numerical simulation to verify the effectiveness of the proposed method for time delay compensation Fig 5 illustrates the following three responses Red line: The LQI optimal controller is designed and applied to the plant model without any time delays (ideal response) Blue line: The LQI optimal controller is designed ignoring the time delays, ie assuming L 1 = L 2 =, and is applied to the plant model with the delays Green line: The proposed servo controller taking account of the delays is applied to the plant model with the time delays It is seen that the proposed controller significantly improves the transient response in comparison with the controller without time delay compensation which has a large overshoot and a long settling time In the experiment applying the controller without time delay compensation, the drone went out of the field of view of the built-in camera because of the large overshoot, and failed to track the target object

6 position[m] 1 5 w/o delay compensation ideal response with delay compensation reference time[s] Fig 5 Simulation result 42 Experimental result Fig6 shows the experimental result of the proposed controller with time delay compensation position[m] angle[rad] experiment simulation reference time[s] (a) position (y-axis) experiment simulation time[s] (b) input Fig 6 Experimental result with time delay compensation It is seen from Fig 6(a) that the proposed controller achieves a good transient response close to the simulation result, though the output is slightly oscillating This oscillationiscausedbydisturbancesdue toairflow and/or neglected nonlinear dynamics As observed from Fig 6(b), the control input is saturated when the reference position is switched, and this cause the overshoot in the output We need to compensate the input saturation in order to reduce the overshoot REFERENCES [1] M-D Hua, T Hamel, P Morin, and C Samson: Introduction to feedback control of underactuated VTOL vehicles: A review of basice control design ideas and principles, IEEE Control Systems Magazine, vol 33, no 1, pp 61-75, 213 [2] T Pobkrut, T Eamsa-ard, and T Kerdcharoen: Sensor drone for aerial odor mapping for agriculture and security services, Proc of 13th ECTI-CON,216 [3] LRG Carrillo, AED López, R Lozano, and C Pégard: Quad Rotorcraft Control: Vision-Based Hovering and Navigation, Springer-Verlag, 213 [4] L Kis and B Lantos, Time-delay extended state estimation and control of a quadrotor helicopter, Proc 2th Mediterranean Conf on Control & Automation (MED212), 212 [5] T Fujinaka and M Araki: Discrete-time optimal control of systems with unilateral time-delays, Automatica, vol 23, no 6, pp , 1987 [6] J Chu: Application of a discrete optimal tracking controller to an industrial elecric heater with pure delays, J Process Control, vol 5, no 1, pp 3-8, 1995 [7] F Liao, K Takaba, and 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 , 23 [8] THagiwara: Introduction to Digital Control, Corona Publishing CoLtd, 1999 (in Japanese) [9] M Chiali, P Gahinet, and P Apkarian: Robust pole placement in LMI regions, IEEE Trans Automat Contr, vol 44, no 12, pp , 1999 [1] L Zaccarian and AR Teel: Modern Anti-windup Synthesis: Control Augmentation for Actuator Saturation, Princeton Univ Press, CONCLUSION We have studied the deign and experiment of the remote tracking control system of the drone via visual feedback To compensate the time delays due to wireless communication and image processing, we have designed the tracking control system by combining the LQI optimal servomechanism and the state augmentation technique The experimental result demonstrates the effectiveness of the proposed method As a future work, we will need to study the control theory for nonlinear time delay systems in order to realize the tracking control of the drone in a broader operating region It also remains to apply anti-wind up control [1] in order to compensate the input saturation as pointed out in the previous section 1594