On-vehicle visual tracker with disturbance rejection

ON-VEHICLE VISUAL TRACKER WITH DISTURBANCE REJECTION F. G´omez-Estern ∗F.R. Rubio ∗J. Aracil ∗ ∗Dept. Ingenier´ıa de Sistemas y Autom´atica Escuela Superior de Ingenieros. Universidad de Sevilla Camino de los Descubrimientos s/n. 41092-Sevilla. SPAIN e-mails: [email protected],{rubio, aracil}@esi.us.es TEL: +34 954487346 FAX: +34 954487340 Abstract: This paper presents the synthesis and implementation of a passivity based controller designed for visual tracking of moving objects in a disturbance rejection scheme. The purpose of this application is to control the line of sight of a 2-DOF sensor platform in a moving coordinate frame, when the movement of the vehicle carrying the platform acts as a disturbance of finite energy. External torques due to vehicle movement downgrade the visual tracking performance and can cause instability and total loss of sight of the objective. These disturbances can be compensated for in an effectively manner introducing a classical L2-gain disturbance rejection scheme, widely deployed in inertial frame robotic systems. The proposed controller has been tested in a real 2-DOF platform mounted on a moving testbench that emulates different types of non-inertial vehicle surfaces. Keywords: Nonlinear systems, Visual tracking, Mobile robotics, Passivity–based control 1. INTRODUCTION In recent works, the theories of passive and port Hamiltonian systems have fruitfully given rise to a set of tools that succesfully tackle the disturbance suppression problem in electromechanical systems. In (Van der Schaft, 1989) the properties of port-controlled Hamiltonian systems (PCH) have been stated (see also more specific approaches (Wang and Li, Internal report 2002) for multimachine power systems and (Slotine and Li, 1988) for robotic systems). These tools provide a boundeddisturbance bounded-output control in the sense of the L2gain. On the other hand, on-vehicle sensors like satellite antennas and visual tracking systems are inherently affected by undesired perturbations due to movements in the coordinate frame where they live. These perturbations appear as Coriolis and centrifugal forces typical of noninertial frames. Consequently, the problem of keeping a static line of sight in this type of sensor is a disturbance rejection problem, where the disturbance can be caused by vehicle movements or by displacements of the target point (e.g. non-geostationary satellite). When tracking objectives are far, the former case is the one that more severely affects the stability of the line of sight and these will be the ones studied in this work. In vessels, the aforementioned disturbance is mainly due to sea waves and are usually approximated with sinusoidal functions. This type of signals, when amplitudes are low, are well handled in the L2disturbance attenuation framework. We will study a 2-DOF platform designed for vessel guidance and hence perturbed by wave-induced movements of the ship surface. With a laboratory setup that physically emulates different types of perturbation, we will present and analyze a visual servoing system based on the Slotine and Li controller (Slotine and Li, 1988) that guarantees L2disturbance rejection properties. IAV2004 - PREPRINTS 5th IFAC/EURON Symposium on Intelligent Autonomous Vehicles Instituto Superior Técnico, Lisboa, Portugal July 5-7, 2004 316 With the aim of studying the properties and effects of on-vehicle perturbations, a two degree of freedom platform has been set up on a destabilizing 2-DOF desk. A visual servoing system has been implemented with a CCD camera. When analyzing experimentally the effect of the real disturbances on the visual tracking loop, a substantial improvement of the overall behavior has been achieved with the proposed controller. These results are presented grraphically. This paper is organized as follows: in Section 2, the simplified system model of the platform is given. In Section 3, the new control structure is presented, divided in visual estimator and trajectory tracker. In Section 4, technical details of the physical setup and controller implementation are given. Section 5 presents some experimental results. Finally, a set of conclusive remarks are given in Section 6. 2. SYSTEM MODEL The system consists of a platform with two degrees of freedom immersed in a non inertial reference system emulated by a destabilizing desk. The synthesis of controllers for this platform raises important challenges like the robust stabilization rejecting disturbances, visual tracking of moving objects and gyroscopic stabilization to compensate for the coordinate frame movements. The model used (G´omez-Estern et al., 2000) corresponds to the 2-DOF platform shown in Fig.1. It is composed of two main bodies: the base, whose position is determined by the azimuth angle ϕand the head body whose coordinate is the elevation angle θ. In our experimental setup, this system is mounted on a mechanical desk capable of turning in two axes. The whole system has four degrees of freedom, but only the two local coordinates (those of the tracker platform) are available from the built-in encoders. Therefore only the 2-DOF platform kinematics are considered, while any coupling effect due to the moving surface will be treated as a disturbance (G´omez- Estern et al., 2003) . The load of the elevation axis is concentrated along the shaft, hence the potential energy is invariant and therefore discarded from the equations. The origin of the azimuth angle is arbitrary. The variable θis zero when both bodies are perpendicular. The generalized coordinates of the system are (ϕ,θ). The equations of motion are easily obtained in Hamiltonian form. Under the assumption stated above, the total energy of the plant is reduced to the kinetic energy: 1 2˙ϕ˙ θΓ(θ)0 0Iyy2˙ϕ ˙ θ(1) The term Γ(θ)standsfor Γ(θ)Izz1+Ixx2sin2(θ)+Izz2cos2(θ)(2) Fig. 1. Two degree-of-freedom platform. where Izz1is the zaxis inertia of the base, and Ixx2,Iyy2 and Izz2are the second body (elevation) inertia momenta with respect to the x,yand zaxes respectively. 3. VISUAL TRACKING IN NON INERTIAL VEHICLES The visual tracking problem in this platform can reduced in some cases to the stabilization of equilibria. Indeed, if the object to be tracked (e.g. stationary satellite) is fixed or has relatively slow motion, and the platform is installed on a static surface, an equilibrium stabilization controller with slowly varying set point will do the job. The set point is estimated by an image processing algorithm. However if the system is immersed in a fast rotating coordinate frame, as is the case of a vehicle antenna, the performance of a set point stabilization controller with varying reference is quickly downgraded even for slow motions, as small line of sight errors due to disturbances areamplifiedwhenthetrackedobjectsaredistant.The latter case is a far more challenging tracking problem. Summarizing, the angles of the line of sight of the target with respect to the camera move by one of two causes •Visual tracking of mobile objects from a platform on a fixed ground. This problem can be tackled as aparticular case of the following. •Fixed or mobile object tracking from a platform mounted on board of a ground vehicle or a vessel. The objective is to take to zero the angular velocity vector of the line-to-target when it is referred to the camera coordinate frame. This vector is actually the composition of the motion of the vehicle (or sea waves) and the reaction of the 2-DOF platforms in order to compensate it. As the second of these situations is more general, this will be the one studied in this paper. 317 Radar Video Encoder/gyro Passivity based trajectory tracker Visual algorithm for trajectory computation D(t) x(t),y(t) q(t),q'(t) qd(t),q'd(t) Disturbance (wave motion) + + u Sensor Platform Fig. 2. Passive visual tracking scheme. The control problem will be divided in two parts (G´omez-Estern, 2003). First, a passivity-based trajectory tracking system with disturbance suppression is designed exploiting detailed knowledge of the mechanical structure of the platform. Then, a visual tracking algorithm estimates the deviation of the tracked object from the line of sight and, based on geometric considerations, it generates the reference trajectory to be tracked by the passive system. The proposed scheme is illustrated in Fig. 2. 3.1 Visual tracking The design of the first block consist of calculating the desired trajectory for the platform coordinates, qd(t)= (ϕd(t),θ d(t)) in such a way that the image of the moving object stabilizes at the center of the screen. The input of this block is a 2D vector with the (x, y)coordinatesin the screen, referred to the origin at the center point. A simple image scanning algorithm computes this vector at a 10ms rate. We will assume that there is a unique trajectory qd(t)∈C 2in the local platform coordinates such that q(t)≡qd(t)⇒(x(t),y(t)) = (0,0),∀t>0 This trajectory is obviously unknown, as it depends on the motion of the coordinate frame of the vehicle and the mobiletarget.However,aswillbeseen,theknowledge of the projection of the target on the screen is enough to compute the desired trajectory qd(t) and an estimate of the derivatives at any instant t.Onceqd(t)isknown, a passivity-based controller will be designed in order to ensure asymptotic stability of the trajectory and, henceforth lim t→∞ (x(t),y(t)) = (0,0) The screen coordinates of the tracked object (x,y)canbe viewed as a measure of the tracking error, since these are zero if and only if the object is perfectly tracked. However it is necessary to express this error in terms of the angles of the platform joints, (ϕ, θ). These can be easily obtained from (x,y)asfollows.Assume(X, Y, Z) are the 3D coordinates of the target in the camera coordinate frame (see Fig. 3). By triangle equivalence, we have By triangle equivalence, we have x d=X Z y d=Y Z where dis the distance of the plane of the image to the viewer (this parameter can be calibrated following camera instructions). After some manipulations, the 3D coordinates are isolated X=xD d2+x2+y2 Y=yD d2+x2+y2 The azimuth and elevation errors of the camera in terms of xand yare given by eϕ= arctan X Z=arctanx d=ϕd−ϕ eθ= arctan Y Z=arctany d=θd−θ Deriving this expression with respect to time, we have ˙eϕ=d dt arctan X Z=d dt arctan x d =˙xd d2+x2=˙ϕd−˙ϕ ˙eθ=d dt arctan Y Z=d dt arctan y d =˙yd d2+y2=˙ θd−˙ θ, From where we obtain the desired trajectory for the platform angles at any instant t qd(t)=ϕ(t) θ(t)+eϕ(t) eθ(t)(3) ˙qd(t)=˙ϕ(t) ˙ θ(t)+˙eϕ(t) ˙eθ(t)(4) 318 Y Z d Side view Upper view Z d X y x Image projection plane Image projection plane Fig. 3. Screen and real coordinates. The generalized coordinates (ϕ(t),θ(t)) are measured in real time with the built-in encoders of the platform independently of the Euler angles of the vehicle. This real time computed target trajectory will be the reference of the passivivity-based trajectory tracking controller whose design will be illustrated in the following. 3.2 Trajectory tracking subsystem In order to get the trajectory tracking we will start from the Euler–Lagrange equations, and use the celebrated results of Slotine and Li (Slotine and Li, 1988). Given the nolinear inertia matrix of the platform M=Γ(θ)0 0Iyy2(5) we obtain the dynamic equations in open loop using the Euler-Lagrange description for robotic systems M(q)¨q+C(q, ˙q)˙q=τ, (6) where no potential energy terms are being considered because the head of the platform is mechanically balanced by construction (in order to reduce energy demand). If they were nonzero the treatment would be equivalent except in the fact that an inclinometer would be necessary to measure the real angles of the platform with respect to an inertial frame, in order to properly measure and compensate gravity effects. In order to asymptotically track the reference trajectory qd(t) we will apply the control law τ=M(q)˙ ξ+C(q, ˙q)ξ+ν(7) where ξ=˙qd−Λ(q−qd)(8) and Λ = ΛT>0. Substituting in (7) yields M(q)˙s+C(q, ˙q)s=ν(9) where s˙q−ζ. Defining the energy function as H(s, q)=1 2sTM(q)s(10) then, along the trajectories (9) d dtH=sTM(q)˙s+1 2sT˙ Ms =−sTCs +1 2sT˙ Ms+sTν=sTν(11) The last expression is due to the skew-symmetry of the matrix ( ˙ M−2C). The system is dissipative with respect to (sTν) and represents a passive ν→ sbecause H≥0, for any initial condition. If, additionally, we define ˆν=Ks (12) where K=KT>0 is a positive definite matrix that Fig. 4. Control structure for trajectory tracking appears in the feedback block of Fig.4. Then we have that Proposition 1. The control law 319 τ=M(q)[¨qd−Λ( ˙q−˙qd)] +C(q, ˙q)( ˙qd−Λ(q−qd)) +Kd[˙q−˙qd+Λ(q−qd)] where Kd=KT d>0andΛ=Λ T>0 are tuning parameters, and qd(t) is the computed trajectory that must be tracked, asymptotically stabilizes the error e(t)=q(t)− qd(t)intheorigin For more details on the proof we refer the reader to (Van der Schaft, 1989). 3.3 Disturbance rejection in trajectory tracking TheproposedschemeisbasedonthewellknownSlotine and Li controller (Slotine and Li, 1988), which in principle lacks of quantifiable disturbance suppression properties when the position error of the robot is taken as the output of the system. A known fact from the literature (see (Sadegh and Horowith, 1990)) is that the addition of a term proportional to the position error to the Slotine and Li controller produces a disturbance suppression behavoir in the L2-gain sense from the input to the output e(t)=qd(t)−q(t) with a attenuation level γarbitrarily small in the case of fully actuated systems. Hence the previous proposition is transformed into Proposition 2. Consider the 2-DOF platform with the control law τ=M(q)[¨qd−Λ( ˙q−˙qd)] +C(q, ˙q)( ˙qd−Λ(q−qd)) +Kd[˙q−˙qd+Λ(q−qd)] + Kp(q−qd) with the same constants defined in proposition 1, and suppose, additionally, that the following conditions hold: Kd=1 21 γ2+1+λ Kp=1 2γ2+1+λ. Then, the L2-gain from the input τto the output (qd−q) is less or equal to the arbitrarily defined gain γ,and the transient behavior can be adjusted using the positive scalar λ. The proof of this result is based in that of (Sadegh and Horowith, 1990). We will use it for its implementation in the real 2-DOF platform inside the visual feedback loop. But firstly we will describe, in the following section, how the information is conveyed among the different controller blocks. 4. IMPLEMENTATION Figure 5 shows the experimental setup. The platform turns in two axis powered by two brushless DC motors. The destabilizing desk is also 2-DOF and is powered by three-phase motors controlled with a velocity variator. Themeasurementsofthepositionsensors(differential encoders for the desk and inclinometers for the desk) and velocities (gyroscopes) are fed back into a PC that contains two dSpaceTM DSP cards (one for each axis) based on the DSP TMS320C31 processor and equipped with a control development environment ControlDesk. Fig. 5. Experimental setup. For the visual tracker we have developed and simple algorithm for image scanning running on a spare PC. Such algorithm is a C++ routine that determines the screen coordinates of the tracking target (x,y), in our experiment just a black circle printed on a white background. This image is first used to calibrate the parameters of the camera. The acquired data is packed into an UDP datagram with a time stamp and sent via Ethernet to the peer computer, where the dSpace cards are running the tracking controller in connection with the platform sensors and actuators. In order to check the validity of the proposed scheme, it has been observed that the total time in acquiring an image, identifying the angles of the target, and sending the datagram, does not exceed a total time of 20ms. Thus, with a sample period of 100ms the jitter due to unmodeled delays is not larger than 20% of the sampling time. The PC acting as controller receives the UDP packets (no packet losses have been accounted for), finds out the error angles from the received (x,y) coordinates, and conveys the data to the DSP cards, where the value of qdis computed according to encoder measurements and equation (3). In these processors, a SimulinkTM 320 RTW 1algorithm applies the voltage the torque to the DC servomotor of the corresponding axis . The physical constants of the model have been obtained through least squares identification over several records on the step response captured by the DSP boards. As friction will be to some extent compensated for in the final controller. the LuGre model parameters have been identified as indicated in (Astrom K.J., 1995). 5. EXPERIMENTAL RESULTS The quality of the experimental results significantly increases when our controller is enhanced with a feedforward compensator of the friction phenomena based on the LuGre model. Theelementsdepictedin(6)aretheslowsinusoidal reference provided by the visual tracker when the destabilizing desk oscillates in sinusoidal slow motion (marine vehicle emulation), together with the position given by the encoders. The tracking controller is also artificially perturbedwithafastsinusoidalsignal(includedinthe figure) directly added to the DC motor inputs. The small deviations between the position and the reference are due to this disturbance, which is the one to be attenuated with the robust scheme. The L2-gainofthefast disturbance-to-output map has been adjusted according to proposition 2. Although the value of γcan be made arbitrarily small, in experiments we have observed that it should not be chosen below a certain value due to the appearance of a sustained oscillation, possibly due to communication delays inducing a limit cycle. As a matter of fact any unmodeled oscillation is amplified through the derivative term of the controller, which is in turn increased as parameter γgoes smaller. Fig. 6. Visual tracking of a fixed target with sinusoidal desk motion and additive white noise disturbance. 6. CONCLUSIONS In this work we have presented the design and practicalimplementationofacontrollerbasedontheSlotine 1Real Time WorkshopTM. and Li scheme immersed in a visual feedback loop with Ethernet data transmission. The controller is split between image processing, target trajectory computation and trajectory tracking. The latter block is a passive controller specifically designed for suppression of undesired disturbances up to a level that, in theory, can be made arbitrarily small. The main contribution lies in the use of passivity and robustness techniques in a visual servoing framework. In the experimental results we have checked that well known control techniques designed for other domains have proved to be effective in this challenging type of application. The extension of the techniques to visual servoing systems is natural and only relies on the computation of a trajectory to be tracked as a result of a fast image analysis C++ algorithm. The powerful experimental setup consisting of a 2-DOF sensor platform mounted on a 2-DOF destabilizing desk has allowed us to verify experimental results in a most realistic environment, and this makes us believe that the controller can be easily upgraded into the real ground and water vehicle world. Acknowledgements. This work has been partially funded by the Spanish Ministry of Science and Technology under grants DPI2001-2424, DPI2003-00429 and HF2001-0126. REFERENCES Astrom K.J., Canudas de Wit C., Olsson H. Lischinscky P. (1995). A new model for control on systems with friction. IEEE Tansactions of Automatic Control 40(3), 419–425. G´omez-Estern, F. (2003). PhD Thesis: Control de Sistemas no Lineales basado en la Estructura Hamiltoniana.. Dept. Ing de Sistemas y Autom´atica. Universidad de Sevilla. G´omez-Estern, F., J. Aracil and F.R. Rubio (2000). Practical implementation of a hamiltonian–based 2– DOF platform. In: Proc. IFAC Symposium on Robot Control. Vienna, AU. G´omez-Estern, F., M.G. Cordones and F.R. Rubio (2003). Seguimiento visual basado en pasividad en sistemas no inerciales. In: Proc. of the XXIV Jornadas de Autom´atica. Leon, Spain. Sadegh, N. and R. Horowith (1990). Stability and robustness analysis of a class of adaptive controllers for robot manipulators. Int. Journal. rob. Res. 9, 74–92. Slotine, J.J. and W. Li (1988). Adaptive manipulator control: a case study. IEEE Trans. on Automatic Control 13(5), 995–1003. Van der Schaft, A.J. (1989). L2-Gain and Passivity Techniques in Nonlinear Control. Springer-Verlag. Wang, Y., Cheng D. and Ge Y. Li, C. (Internal report 2002). Dissipative hamiltonian realization and energy-based l2-disturbance attenuation control of multimachine power systems. 321