Trajectory generation for a remotely operated vehicle

TRAJECTORY GENERATION FOR A REMOTELY OPERATED VEHICLE Sérgio Loureiro Fraga, João Borges Sousa, Fernando Lobo Pereira {slfraga,jtasso, fl p}@fe.up.pt FaculdadedeEngenhariadaUniversidadedoPorto Rua Dr. Roberto Frias, s/n 4200-465 Porto PORTUGAL Tel: +351225081539 Fax: +351225081443 Keywords: Trajectory generation, Remotely operated underwater vehicles, Differential fl atness, Nonlinear Systems, Modeling. Abstract This paper addresses the problem of trajectory generation for a remotely operated vehicle (ROV). The ROV is a nonholonomic vehicle and has limited actuator capabilities. This means that the task of trajectory generation for the inspection of underwater structures is not a trivial one, and that it cannot be done without computer aided design tools. The approach is based on techniques developed for differential fl at systems. The ROV model is presented and it is shown that it satisfies the differential fl atness property. The paper details the architecture of the computer aided trajectory generation tool. 1Introduction In this paper we present our approach to the design of the trajectory specification, generation and tracking modules for the Inspection of Underwater Structures (IES) system (figure 1). IES is a collaborative project that involves the Administração dos Portos de Douro e Leixões (APDL), Faculdade de Engenharia do Porto and Instituto de Sistemas e Robótica - Pólo do Porto. Except for the ROV frame, hull and thrusters, all the other components and systems were designed and implemented at LSTS (Underwater Systems and Technology Laboratory). The IES system comprises the following subsystems: power, computer, motor control, navigation and image. The computer system consists of a PC-104 stack running the realtime operating system QNX, and a Windows based PC connected through an Ethernet cable. The PC-104 stack is housed in the main cylinder of the ROV, and controls the ROV systems through a CAN bus. The PC runs the operator console. The PC-104 computer system runs the command, control and navigation software. Basically, this computer accepts high-level commands from the console and informs the console about the state of the system. The navigation system includes the onboard sensors and an external acoustic navigation network. The on-board sensors are: magnetic compass, inclinometers, Inertial Motion Unit (IMU), Doppler Velocity Logger(DVL), Long Base Line(LBL)acoustic system, magnetic compass and depth cell [2][4]. One of the major innovations of the IES system with respect to commercially available ROV solutions is the consideration of two modes of operation: tele-operation and tele-programming. The IES ROV is able to receive motion commands through an umbilical cable from the console, which is mainly composed of a PC, a TV/Video set, and a joystick. In the tele-operation mode the ROV’s pilot uses the joystick to control the vehicle’s velocity. In this mode the trajectory generation module interprets, in real time, the joystick commands and transform them into feasible trajectories. In the tele-programming the pilot specifies with the aid of a graphical user interface a trajectory for the vehicle to follow. There are several trajectory specification models including for example waypoints. The trajectory generation module is able to convert the specification into feasible trajectories for ROV to follow. The computation of the trajectory is made at the console and it is sent to the on board computer, where the controller is implemented. Trajectory generation plays an important role in defining the overall performance of a ROV control system. A properly designed trajectory generation system facilitates the task of the controllers since it should prevent the system from saturating, andthus operatinginopen-loop. Oneway to avoid input saturation is to generate feasible trajectories. The design of trajectory generation modules has to take into account the cinematic and dynamic constraints of the ROV in order to generate feasible trajectories. Theproblemof trajectory generationfor nonlinear systemswith nonholonomicconstraints [6][5] representsa considerablechallenge to control design. In fact, there is no universal technique that can be readily applied to this end. In a differential fl at system we can take advantage of techniques from differential geometry for thegeneration of feasible trajectories. But one thing is trajectory generation, and the other is control design and performance. These are intimately related. We use a three level architecture for trajectory generation and tracking [11] (figure 2): • User interface for output specification. This module has a graphical user interface to assist the operator in the specification of the desired values for the ROV outputs. Upon validation, the pilot’s objectives (represented as system outputs) are sent to the trajectory generation module. • Trajectory generation. This module computes a feasible trajectory for the ROV, which may be optimized over some Figure 1: The remotely operated vehicle factor or variable to improve the overall system performance. The output of this module is a complete description of the desired state and input of the system. • Controller. This module controls the actuators to track the trajectory defined by the trajectory generation module. The output of this module is a control signal overlaying the input signal, defined by the trajectory generation module, in order to eliminate drifts or perturbations on the structure. The main focus of this article is the trajectory generation module. For a thorough discussion of the other modules see [4][2]. This paper is organized as follows. Section 2presents the dynamical and kinematical model of the ROV. Section 3introducesdifferential fl atsystems. Section4presentstwo approaches for trajectory generation for ROVs. Finally, section 5presents some concluding remarks. 2Modelization of the ROV The ROV is a rigid body with six degrees of freedom. Its motion can be described using either an inertial coordinate system XYZ or a body fixed coordinate system X 0 Y 0 Z 0 (figure 3). Since the ROV moves slowly in the water, a earth fixed coordinate system can be defined to be inertial. On the other hand, the body fixed coordinate system has its origin in the ROV’s centre of mass and its axis’ directions coincide with the vehicle’s principal inertial axis: • X o - longitudinal axis (from aft to fore) • Y o - transverse axis (from port side to starboard) • Z o - vertical axis (from top to bottom). The vehicle’s position and velocity is represented in the inertial and body fixed coordinate system. We adopted the notation form ”The Society of Naval Architects and Marine Engineers (SNAME)” (1950). According to SNAME, the motion of underwater vehicles in six degrees of freedom can be described by the following vectors: η =  η T 1 ,η T 2  T ; η 1 =[ x,y,z ] T ; η 2 =[ φ,θ,ψ ] T ; v =  v T 1 ,v T 2  T ; v 1 =[ u,v,w ] T ; v 2 =[ p,q,r ] T ; τ =  τ T 1 ,τ T 2  T ; τ 1 =[ X,Y,Z ] T ; τ 2 =[ K,M,N ] T . Outputs definition Controller + + - Perturbations ROV Trajectory Generation η d η τ d τ c Figure 2: Modules of the trajectory generation system Thus, η represents the inertial coordinates of the vehicle, ν represents the linear and angular velocities in body fixed coordinates and, finally, τ are the forces and moments applied on the vehicle in body fixed coordinates. The kinematical equations relating the state variables between the inertial and body fixed coordinate system are given by:  ˙ η 1 ˙ η 2  =  J 1 ( η 2 )0 3 × 3 0 3 × 3 J 2 ( η 2 )  v 1 v 2  ⇔ ˙ η = J ( η ) v (1) with ( t. =tan( . ) M , s. =sin( . ) and c. =cos( . ) ) J 1 ( η 2 )=   cψcθ − sψcφ + cψsθsφ sψcθ cψcφ + sφsθsψ − sθ cθsφ sψsφ + cψcφsθ − cψsφ + sθsψcφ cθcφ   (2) and J 2 ( η 2 )=   1 sφtθ cφtθ 0 cφ − sφ 0 sφ cθ cφ cθ   . (3) The matrix of equation (3) has a singularity for θ = ± π 2 and is not a rotational matrix, i.e. it does not belong to SO (3) . This singularity does not bring problems for the ROV’s control since it never reaches a state where θ = ± π 2 . On the other hand, J 1 matrix is a rotational matrix, i.e. J 1 ∈ SO (3) . The dynamical model of a rigid body moving in six degrees of freedom is a well known model [7] and may be represented by the following equation (body fixed coordinates): M RB ˙ v + C RB ( v ) v = τ RB . (4) In the preceding equation, v =[ u,v,w,p,q,r ] T , C RB ( ν ) represents the Coriolis forces and centripetal terms, then τ RB = [ X,Y,Z,K,M,N ] T and, finally, M RB represents the inertial matrix of the rigid body. Taking into account the previous definitions about body fixed coordinate system, M RB is given by: M RB =         m 00000 0 m 0000 00 m 000 000 I x 00 0000 I y 0 00000 I z         (5) Z 0 X 0 Y 0 u (surge) p (roll) w (heave) r (yaw) v (sway) q (pitch) O Z X Y Inertial frame ro Body fixed frame Figure 3: Body fixed and inertial coordinates systems where m is the vehicle’s massand I x ,I y ,I z are the inertial moments around the axis x,y,z , respectively. The term C RB ( v ) .v is given by  ω b × mv b ω b × Iω b  with v b =  uvw  T and w b =  pqr  T and τ RB are exterior actuations. The external forces acting on the ROV are included in the right side of equation (4). These forces can be classified in the following categories [1][4]: radiation-induced forces τ H (added inertia, hydrodynamic damping, restoring forces), environmental forces τ E (underwater currents, waves, wind) and propulsion forces τ (thrusters and propeller forces, control surfaces and rudder forces). Considering the previous forces, the resulting force acting on the vehicle is given by: τ RB = τ H + τ E + τ. (6) The hydrodynamic forces τ H has the following expression: τ H = − M A ˙ v − C A ( v ) v − D ( v ) v − g ( η ) . (7) The term − M A ˙ v − C A ( v ) v models the added mass due to the inertia of the surrounding fl uid, while the term − D ( v ) v represents the total hydrodynamic damping [1]. Finally, the component − g ( η ) represents the restoring forces due to the vehicles’ weight and buoyancy and has the following expression: g ( η )=col(0 , 0 , 0 , − z B B cos θ sin φ, − z B B sin θ, 0) , (8) where z B represents the distance between the application point of the weight and the buoyancy force and B is the impulsion force, which is equal to the weight W for this ROV. Substituting equation (6) into (4) together with equation (7), we obtain the vehicle’s dynamic model: M ˙ v + C ( v ) v + D ( v ) v + g ( η )= τ E + τ (9) where M = M A + M RB and C ( v )= C RB ( v )+ C A ( v ) .A full description of these matrices can be found in [1]. It wasnot considered themodel of ROV’s actuators, namely the model of the thrusters and the electric motors. It was not also considered the model of environment forces, such as aquatic currents, waves and wind. Despite their in fl uence in the vehicle’s behaviour, not including these models doesn’t affect the desired results for this work. On the other hand, this simplification reduces considerably the implementation of software y F 2 F3 F 4 F 3 x F1y F4y F 4 F 1 x F2y F1=F2 F4 F3 F 3 x F3z F1z=F2z F4x z x Figure 4: Applied forces on the ROV by the thrusters equations allowing more control on the trajectory generation results. The dynamical model of the ROV is presented in equation (9) in body fixed coordinates and can also be represented in the inertial coordinate system by using the kinematical equation (1): M η ( η )¨ η + C η ( v,η )˙ η + D η ( v,η )˙ η + g η ( η )= τ η . (10) where M η ( η )= J − T ( η ) MJ − 1 ( η ) C η ( v,η )= J − T ( η )  C ( v ) − MJ − 1 ( η )˙ J ( η )  J − 1 ( η ) D η ( v,η )= J − T ( η ) D ( v ) J − 1 ( η ) g η ( η )= J − T ( η ) g ( η ) τ η ( η )= J − T ( η ) τ. (11) After presentation of dynamic and cinematic equations it is convenient to show how the ROV’s thrusters affect the variable τ . In figure 4are represented the forces applied in the ROV by the thrusters. The ROV does not have rudders so its motion is only affected by thrusters. The relationship between the forces applied by the thrustersand its effect invariable τ isgivenby:         X Y Z K M N         =         1100 0010 0001 00 − F 3 zF 4 y F 1 zF 2 z 0 − F 4 x − F 1 y − F 2 yF 3 x 0         .     F 1 F 2 F 3 F 4     . (12) 3Differential flatness In this section, we show the utility of differential fl atness proprieties for trajectory generation [9][10]. A system is differentially fl at if it is possible to find a set of variables (equal in number of the inputs), called fl at outputs, such that all state variables and input become defined from them without integration. Let η ∈ R n be the system state and τ ∈ R m its input, then the system is fl at if it is possible to find fl at outputs z ∈ R m in the form: z = Z ( η,τ, ˙ τ,...,τ ( l ) ) (13) such that η = η ( z, ˙ z,...,z ( q ) ) (14) τ = τ ( z, ˙ z,...,z ( q ) ) . where Z , η , τ are mapping functions between fl at outputs, state variables and input. Differentially fl at systems are interesting when fl at outputs represent the desired behaviour of the system and trajectory planning can be made from these variables. This approach is especially interesting when the fl at outputs have a physical meaning and are not an arbitrary combination of state variables and input. The problem is that there is no systematic methodto determine the fl at outputs. Hence, finding fl at outputs with physical meaning for a particular system can be a hard task [8]. Usually, the dimension of the fl at outputs is lower than system dimension, which means an improvement on the steering efficiency. For differentially fl at systems we are able to transform the system such that the equations of motion for the fl at outputs variables become trivial. For instance, in the case of a rigid body with six degrees of freedom and three inputs, if the fl at outputs can be found then it would be possible to steer the body just as if it was a point. Since fl at outputs trajectories are completely free, the only constraints that should be imposed refer to the desired initial and final configuration of the system and bounds on the derivatives of the trajectories. Other constraints, such as, input saturation can be converted to the fl at outputs space by imposing limitations on the curvature of the trajectories or bounds on the higher order derivatives. Next, we show thatROV’s model is differentially fl at. Asstated before, its model in inertial coordinates is given by: M η ( η )¨ η + C η ( v,η )˙ η + D η ( v,η )˙ η + g η ( η )= τ η . (15) The system state η represents the ROV’s inertial positions. We can easily check that the state variables are also the fl at outputs since the system input ( τ η ) becomes defined from those variables without integration [8][3]. As pointed out before the number of fl at outputs must be equal to the dimension of the input space. This is not the case of our ROV. The IES ROV is under-actuated. We have actuation in four degrees of freedom. In this situation, the trajectory generation is based on the fact that roll an pitch are stable (due to the large distance between the application point of the weight and the buoyancy forces) and assuming that it is not a requisite to define a trajectory for these degrees of freedom, so their trajectories are always assumed to be zero. The actuation on these two degrees of freedom is also negligible since the components F ij in equation (12) affecting the variables K,M are smaller when compared to the actuation on the others degrees of freedom. Hence, we compute the desired actuationin all degrees of freedom but we only apply four forces on the ROV. We expect the actuation on roll and pitch to be negligible. We confirmed this hypothesis from the simulation runs. We do not consider 0 5 10 15 20 0 5 10 x [m] time [s] 0 5 10 15 20 0 2 4 6 z [m] time [s] Figure 5: Flat outputs parametrization K,M in the computation of these forces. This may result in some performance degradation. The computation of the forces to apply to each propeller is given by:     X Y Z N     =     1100 0010 0001 − F 1 y − F 2 yF 3 x 0     .     F 1 F 2 F 3 F 4     . (16) The effect of the actuators on φ,θ is small but we will present a technique that minimize the actuation in these two degrees of freedom. We expect a better performance since the equation (16) becomes a better approximation of the physical reality. To sum up, the ROV’s fl at outputs are the state variables and an approximation is made when computing the input from those fl at outputs. 4Trajectory generation for the ROV In this section, we present two trajectory generation methods based on differential fl atness [10]. The problems addressed in this paper are finite horizon and anti-causal, i.e. some information about the trajectory’s future is necessary. This information can be the complete form of the trajectory or waypoints. The temporal scheme of the trajectory generation architecture defines the desired trajectory for all states and inputs at a lower rate than that of the controllers. These, in turn, track the defined trajectory at a higher rate ( 10 ms ). We implemented the algorithms in ANSI C and used the libraries from the software package ”C-Routines for Trajectory Generation for Flat and Approximately Flat Systems” [9] developed at ”California Institute of Technology”. In this paper we consider the problem of finding a trajectory for the state variables and the system inputs from the fl at outputs trajectory, in a finite time horizon [ t 0 ,t f ] . The trajectories of the fl at outputs will be approximated by polynomials to compute its derivatives symbolically to enhance the computational efficiency. The model used for this problem is the one presented in equation (15) since, in this case, it is convenient to define the trajectory in the inertial coordinate system to facilitate the task of the pilot. The first method is the simplest one since it refers to guide the vehicle from a point of its state space to another one. In this problem we are given the entire state at t 0 and at t f . Assuming the inputs and its derivatives are also defined at both times, we can compute the fl at outputs for these instants (equation (14)). After determination of the initial and final values of the 0 5 10 15 20 -20 0 20 40 60 Xb [N] time [s] 0 5 10 15 20 -20 0 20 40 Zb [N] time [s] 0 5 10 15 20 -20 -15 -10 -5 0 Kb [N.m] time [s] 0 5 10 15 20 -20 -15 -10 -5 0 Mb [N.m] time [s] Figure 6: Computed inputs fl at outputs it is possible to parameterize them for an interval [ t 0 ,t f ] as follows: z i ( t )=  j A ij φ j ( t ) (17) where φ j ( t ) represents a basis of polynomial functions, which through a linear combination given by the matrix A allows the computation of the fl at outputs at [ t 0 ,t f ] . The values of the A ij coefficients can be obtained by solving the following system of equations: z i ( t 0 )=  j A ij φ j ( t 0 ) z i ( t f )=  j A ij φ j ( t f ) ... ... z ( l ) i ( t 0 )=  j A ij φ ( l ) j ( t 0 ) z ( l ) i ( t f )=  j A ij φ ( l ) j ( t f ) . (18) Let p be the number of polynomials of basis φ j .Thenitisnecessary to specify the initial and final values for the fl at outputs until the l derivative ( p =2( l +1) ) in order to determine all A ij coefficients. Thus, the number of coefficients to be computed in matrix A will be 2( m ( l +1)) . Since a trajectory for the fl at outputs has already been defined between the instants t 0 ,t f by the previous parameterization, now it is necessary to compute the state and input of the system from that trajectory. This can be done by choosing several points along the fl at outputs trajectory in order to compute the corresponding state and input. Increasing the number of computed points N , means an improvement on the system accuracy since the discretization is made with a higher frequency. In the ROV, the fl at outputs coincide with vehicle’s state so it is only necessary to compute its input using equation (15). However, it should be noted that a trade-off has to be resolved because increasing the number of computed points will increase the computation time, which is also determined by the time necessary to perform the fl at outputs parameterization. The parameterization time is much smaller on systems where nonlinear systems of equations must be resolved to compute the state and the input. In the ROV system, it is not necessary to resolve systems of equations since analytical expressions exist to obtain the sys0 5 10 15 20 0 1 2 3 4 5 6 7x 10 -3 phi [rad] time [s] 0 5 10 15 20 0 0.05 0.1 0.15 0.2 0.25 theta [rad] time [s] Figure 7: ROV’s roll and pitch behaviour tem input. In figure 5 we show a parameterization (with p =6 )ofthe fl at outputs from two given points: an initial point ( x,y,z,φ,θ,ψ )= (0 , 0 , 0 , 0 , 0 , 0) at t 0 =0 s and a final point ( x,y,z,φ,θ,ψ )= (10 , 0 , 5 , 0 , 0 , 0) at t f =20 s . At these points the initial and final velocity is defined to be zero. It is only shown the trajectories for x,z because the others variables are always zero. After performing the fl at outputs parameterization, it is now possible to compute the input, which is able to implement the parameterized trajectory. The number of points used to perform that task were N =100 and the computed inputs are depicted in figure 6 (in body coordinates). Again, Y,N is not presented since these inputs are always zero. It should be noted that this trajectory will be the input of the control module mentioned in section 1. As it is possible to verify in figure 6, the desired values for K,M are slightly different of zero. This means a degradation of performance as discussed in section 4. As a result of computing thrusters forces using equation (16), the ROV’s roll and pitch will have the shape depicted in figure 7. As expected, the approach brings loose of performance in roll and pitch, which is a consequence of ROV’s under actuation. In order to minimize this drawback, it is possible to generate trajectories minimizing the actuation effort on K,M . To implement this concept, trajectories will be generated through the following minimization cost function: min A  t 1 t 0 ( z d ( s ) − Aφ ( s )) T ∗ ( z d ( s ) − Aφ ( s ))+ λP ( K,M ) ds (19) where z d ( t ) represents the desired trajectory for the fl at outputs, for instance the trajectory defined by the previous algorithm, z ( t )= Aφ ( s ) is the output trajectory ( z ( t ) still be a linear combination of a polynomial basis with p =10 polynomials) and P represents a function which penalizes the actuation effort on the variables K e M (in this example P = K 2 + M 2 ). The trajectory z is a trade-off solutionbetween trajectory tracking of z d ( t ) and the actuation effort in K,M , which is obtained byasuitablechoiceof λ .If λ is chosen to be close to zero, this means that we give more preference to trajectory tracking. Otherwise, we prefer to reduce the actuation effort. The main design issue is the choice of a suitable λ . This choice is left to the pilot who will be able to go through an iterative 0 5 10 15 20 -2 -1 0 1 2 Kb [N.m] time [s] 0 5 10 15 20 -2 -1 0 1 2 Mb [N.m] time [s] Figure 8: Results of input minimization on K,M procedure with the user interface. Detailed information about the output trajectory with a chosen λ should be given to the pilot before applying the trajectory on the vehicle. This way the ROV’s pilot can refine as much as we want the value λ ,and obtain a better overall performance. The results of trajectory computation with λ =0 , 05 ,with z d ( t ) being the same trajectory presented in figure 5, are depicted in figure 8, where it is possible to see asignificant reduction on K,M values. This reduction causes a slightly degradation in trajectory tracking, namely in x,z , but, as expected, results in a lower perturbation on θ (figure 9). The main drawback of the previous approach is, definitively, the computational power required to perform the minimization presented in equation (19). This fact may prevent the application of this technique in real-time applications. 5Conclusions This paper has reported an approach to the problem of trajectory generation for a ROV. The architecture of the trajectory generation and control system consists of three modules: 1) output specification ; 2) trajectory generation ; 3) controller. The main advantage of this architecture is its modularity: we developed the three modules separately, and further improvements can be implemented independently. The main focus of this article is the design of the trajectory generation module. This module takes into account ROV’s dynamical and kinematical model tosteer the vehicle without violating its constraints. It is shown that ROV’s model is differentially fl at under some mild assumptions: the stability of ROV in the pith and roll modes and the negligible in fl uence of the thrusters on these degrees of freedom. We presented two trajectory generation techniques that take advantage of the differential fl atness of the ROV model. The first one steers the vehicle from an initial state configuration to a final one, while the second one steers the vehicle minimizing the effect on K,M variables. We present simulation results that confirm the validity of our assumptions, namely in what concerns the low sensitivity of the ROV’s roll and pitch when we use the second technique. However, this improvement requires more computational time, which may not be feasible for real time applications. 0 5 10 15 20 0 0.05 0.1 0.15 0.2 theta [rad] time [s] Figure 9: Pitch behaviour after minimizing actuation on K,M 6Acknowledgments Thismaterialis based upon work funded bythe Programa PRAXIS XXI - Medida 3.1b) (IES project) and by Ministério da Defesa, Portugal. Sérgio Fraga was funded by the Fundação para a Ciência e Tecnologia (FCT) and the European Social Fund. References [1] Thor I. Fossen. GuidanceandControlof OceanVehicles. John Wiley and Sons, 1995. [2] Sérgio Loureiro Fraga, João Sousa, Anouck Girard, and AlfredoMartins. An automated maneuver control framework for a remotely operated vehicle. In MTS/IEEE Oceans 2001. IEEE, 2001. [3] Sérgio Loureiro Fraga, João Borges Sousa, and Fernando Lobo Pereira. A framework for the automation of a remotely operated vehicle. In 10th IEEE Mediterranean Conference on Control and Automation, Junho, 2002. [4] Rui Manuel Ferreira Gomes. Modeling and control of underwater vehicles. Master’s thesis, Faculdade de Engenharia da Universidade do Porto, 2002. [5] Jean-Claude Latombe. Robot Motion Planning. KAP, 1993. [6] Jean-Paul Laumond. Robot Motion Planning and Control. Springer, 1998. [7] Richard Murray, Zexiang Li, and S. Sastry. Mathematical Introduction to Robotic Manipulation. CRC Press, 1994. [8] Richard Murray, Muruhan Rathinam, and Willem Sluis. Differential fl atness of mechanical control systems: A catalog of prototype systems. In ASME International MechanicalEngineeringCongressandExposition, 1995. [9] Michiel J. Van Nieuwstadt. Trajectory Generation for Nonlinear Control Systems. PhD thesis, California Institute of Technology, Pasadena, California, 1997. [10] Michiel Van Nieuwstadt and Richard Murray. Approximate trajectory generation for differentially fl at systems with zero dynamics. In IEEE Conference on Decision and Control, New Orleans, 1995. [11] Pravin Varaiya. Towards a layered view of control. In 36th IEEE Conference on Decision and Control. IEEE, 1997.