A Unified Dynamics-Based Motion Planning Algorithm for Autonomous

A uniﬁed dynamics-based motion planning algorithm for autonomous underwater vehicle-manipulator systems (UVMS) Tarun Kanti Podder* and Nilanjan Sarkar† (Received in Final Form: May 19, 2003)

SUMMARY space trajectory is not unique. It admits infinite number of A new unified motion planning algorithm for autonomous joint-space solutions for a given task-space trajectory. Underwater Vehicle-Manipulator Systems (UVMS) has However, there are various mathematical tools such as been presented in this paper. Commonly, a UVMS consists Moore-Penrose Generalized Inverse, which map the desired of two sub-systems, a vehicle and a manipulator, having Cartesian trajectory into the corresponding joint-space vastly different dynamic responses. The proposed algorithm trajectory for a kinematically redundant system. Research- considers the variability in dynamic bandwidth of the ers have developed various trajectory planning methods for complex UVMS system and generates not only kine- redundant systems.1,3–5,9 A kinematic approach of motion matically admissible but also dynamically feasible reference planning has been reported in references [1–5]. Zhou and trajectories. Additionally, this motion planning algorithm Nguyen3 formulated optimal joint-space trajectories for exploits the inherent kinematic redundancy of the whole kinematically redundant manipulators by applying Pon- system and provides reference trajectories that accom- tryagin’s Maximum Principle. Siciliano4 has proposed modates other important criteria such as thruster/actuator an inverse kinematic approach for motion planning of a faults and saturations, and also minimizes hydrodynamic redundant spacecraft-manipulator system. Antonelli and drag. Effectiveness of the proposed unified motion planning Chiaverini5 have used a pseudoinverse method for task- algorithm has been verified by extensive computer simula- priority redundancy resolution for an autonomous tion. The results are quite promising. Underwater Vehicle-Manipulator System (UVMS) using a kinematic approach. Several researchers,6–15 on the other hand, have con- KEYWORDS: Underwater vehicle-manipulator system; Under- sidered the dynamics of the system for trajectory planning. water robots; Dynamics-based motion planning; Heterogeneous Vukobratovic and Kircanski6 proposed an inverse problem dynamic system; Trajectory planning. solution to generate nominal joint-space trajectory considering the dynamics of the system. Bobrow7 presented the 1. INTRODUCTION Cartesian path of the manipulator with a B-spline polyno- In robotics, trajectory planning is one of the most challeng- mial and then optimized the total path traversal time ing problems.1 Traditionally, trajectory planning problem is satisfying the dynamic equations of motion. Shiller and formulated as a kinematic problem and therefore the Dubowsky8 presented a time-optimal motion planning dynamics of the robotic system is neglected.2 Although the method considering the dynamics of the system. Shin and kinematic approach to the trajectory planning has yielded McKay9 proposed a dynamic programming approach to some very successful results, they are essentially incomplete minimize the cost of moving a robotic manipulator. as the planner does not consider the system’s dynamics Recently, Hirakawa and Kawamura10 have proposed a while generating the reference trajectory. As a result, the method to solve trajectory generation problem for redundant reference trajectory may be kinematically admissible but robot manipulators using the variational approach with a B- may not be dynamically feasible. spline function to minimize the consumed electrical energy. Researchers, in the past several years, have developed Saramago and Steffen11 have formulated off-line joint-space various trajectory planning methods for robotic systems trajectories to optimize traveling time and minimize considering different kinematic and dynamic criteria such as mechanical energy of the actuators using spline functions. obstacle avoidance, singularity avoidance, time minimiza- Zhu et al.12 have formulated real-time collision free tion, torque optimization, energy optimization, and other trajectory by minimizing an energy function. Faiz and objective functions.3–21 A robotic system that has more than Agrawal13 have proposed a trajectory planning scheme that 6 dofs (degrees-of-freedom) is termed as a kinematically explicitly satisfy the dynamic equations and the inequality redundant system. For a kinematically redundant system, constraints prescribed in terms of joint variables. Recently, the mapping between task-space trajectory and the joint- Macfarlane and Croft14 have developed and implemented a jerk-bounded trajectory for an industrial robot using concat- * Monterey Bay Aquarium Research Institute, 7700 Sandholdt enated quintic polynomials. Motion planning of land-based Road, Moss Landing, California, CA 95039 (USA) mobile robotic systems has been reported in references E-mail: [email protected] 15 † Department of Mechanical Engineering, Vanderbilt University, [15–18]. Brock and Khatib have proposed a global Nashville, Tennessee, TN 37235 (USA) dynamic window approach that combines planning and real- E-mail: [email protected] time obstacle avoidance algorithms to generate motion for 118 Underwater vehicle mobile robots. Huang et al.16 have presented a coordinated such reference trajectories that either the UVMS may not be motion planning approach for a mobile manipulator con- able to track them or while tracking, it may consume sidering system stability and manipulation. Yamamoto and exorbitant amount of energy which is extremely precious Fukuda17 formulated trajectories considering kinematic and for autonomous operation in an oceanic environment. dynamic manipulability measures for two mobile robots Additionally, in this research, we exploit the kinematic carrying a common object while avoiding a collision by redundancy of the UVMS to formulate a unified dynam- changing their configuration dynamically. Recently, Yama- ically feasible reference trajectory generation algorithm that shita et al.18 have proposed a motion planning method for can accommodate thruster/actuator faults, saturations, and multiple mobile robots for cooperative transportation of a provide a minimum drag trajectory for a given task. All large object in a 3D environment. To reduce the computa- these performance criteria are very important for autono- tional burden, they have divided the motion planner into a mous underwater operation. They provide a fault-tolerant global path planner and a local manipulation planner then and reduced energy consuming autonomous operation they have designed it and integrated it. All the previously framework. mentioned researches6–18 are performed for either space robotic or land-based robotic systems. On the other hand, 2. THEORETICAL DEVELOPMENT very few works on motion/trajectory planning of underwater robotic systems have been reported.19–21 Yoerger and Slo- tine19 formulated a robust trajectory control approach for 2.1. UVMS dynamics underwater robotic vehicles. Spangelo and Egeland20 devel- For convenience, we commonly use two reference frames to oped an energy-optimum trajectory for underwater vehicles describe underwater robotic systems. These two frames are by optimizing a performance index consisting of a weighted namely the earth-fixed frame (denoted by XYZ) and the combination of energy and time consumption by the system. body-fixed frame (denoted by XvYvZv), as shown in Figure 1. Recently, Kawano and Ura21 have proposed a motion planning algorithm for nonholonomic autonomous under- The dynamic equations of motion of a UVMS can be water vehicle in disturbance using reinforcement learning expressed as follows: 22 (Q-learning) and teaching method. Sarkar and Podder have Mb(qm )˙w+Cb(qm , w)w+Db(qm , w)w+Gb(q)= b (1) presented a coordinated motion planning algorithm for a where the subscript ‘b’ denotes the corresponding parame- UVMS to minimize the hydrodynamic drag. Note that ters in the body-fixed frames of the vehicle and the UVMS always implies an autonomous UVMS in this (6+n) (6+n) manipulator. Mb(qm ) is the inertia matrix paper. (6+n) (6+n) including the added mass and Cb(qm ,w) is the However, the majority of the trajectory planning methods centrifugal and Coriolis matrix including terms due to added available in the literature that considered the dynamics of (6+n) (6+n) mass. Db(qm ,w) is the drag matrix, the system are formulated for land-based robots. They have G(q) (6+n) is the vector of restoring forces and either optimized some objective functions related to trajec- (6+n) b is the vector of forces and moments acting on tory planning satisfying dynamic equations or optimized T the UVMS. The displacement vector q=[qv,qm ] , where energy functions. Moreover, for the land-based robotic q =[q ,....,q]T, and q =q ,....,q ]T ·q,q and q system, the dynamics of the system is either homogeneous v 1 6 m 7 6+n 1 2 3 6–18 are the linear (surge, sway, and heave) displacements of the or very close to homogeneous. On the other hand, most vehicle along X, Y, and Z axes, respectively, expressed in of the trajectory planning methods that have been developed the earth-fixed frame. q4, q5 and q6 are the angular (roll, for space and underwater robotic systems use the pseu- pitch, and yaw) displacements of the vehicle about X, Y and doinverse approach that neglects the dynamics of the system.4–5,22 In this research, we propose a new trajectory planning methodology that generates a kinematically admissible and dynamically feasible trajectory for kinematically redundant systems whose subsystems have greatly different dynamic responses. We consider the trajectory planning of underwater robotic systems as an application to the proposed theoretical development. In general, a UVMS is composed of a 6 dof Autonomous Underwater Vehicles (AUV) and one (or more) n dof robotic manipulator(s). Commonly, the dynamic response of the AUV is an order of magnitude slower than that of the manipulator(s). Therefore, a UVMS is a kinematically redundant heterogeneous dynamic system for which the trajectory planning methods available in the literature are not directly applicable. For example, when the joint-space description of a robotic system is determined using pseudoinverse, all joints are implicitly assumed to have the same or similar dynamic characteristics. Therefore, Fig. 1. Coordinate frames for an underwater vehicle-manipulator the traditional trajectory planning approaches may generate system. Underwater vehicle 119

Z axes, respectively, expressed in the earth-fixed frame. q7, We approximate the task-space trajectories using Fourier q8,....,q6+n are the angular displacements of joint 1, joint series and represent it in terms of the summation of several 2,...... , joint n of the manipulator in link-fixed frames. frequencies in ascending order. The quasi velocity vector w=[w ,...... ,w ]T, where 1 6+n w , w and w are the linear velocities of the vehicle along 1 2 3 x (t)=f (t)=a + a cos(r t/L)+ b sin(r t/L) Xv, Yv, and Zv axes respectively, expressed in the body-fixed d6 1 6 1 0 r r frame. and are the angular velocities of the vehicle r=1 r=1 w4, w5 w6 (3) about Xv, Yv, and Zv axes, respectively, expressed in the body-fixed frame. w7, w8,...... ,w6+n are the angular where a0, ar, br are the coefficients of Fourier series and are velocities of manipulator joint 1, joint 2,....., joint n, represented as 6 1 column vectors, r/2L is the frequency of expressed in the link-fixed frame. A detailed derivation of the series and 2L is the time period. Eq. (1) is given in reference [23]. Now we truncate the series at a certain value of r Equation (1) is represented in the body-fixed frame of the (assuming r=p1 to be sufficiently large) so that it can UVMS because it is convenient to measure and control the represent the task-space trajectories reasonably. We rewrite motion of the UVMS with respect to the moving frame. the task-space trajectory in the following form: However, the integration of the angular velocity vector does x (t)=f (t)=f (t)+f (t)+...... +f (t) (4) not lead to the generalized coordinates denoting the d6 1 6 1 1 2 p1 orientation of the UVMS. In general, we can relate the where f1(t)=a0 +a1 cos( t/L)+b1 sin( t/L), and fj (t)=aj derivative of the generalized coordinates and the velocity cos( j t/L)+bj sin( j t/L) for j=2,3,...,p1. vector in the body-fixed frame by the following linear We then use these truncated series as the reference task- transformation: space trajectories and map them into the desired (reference) joint-space trajectories by using weighted pseudoinverse q=Bw˙ (2) method as follows: + The transformation matrix B in Equation (2) is given by: q˙ dj =JWj x˙dj (5) + ˙ q¨ dj =JWj(¨xdj J˙qdj ) (6)

B O J O where q˙ d are the joint-space velocities and q¨ d are the B(q)= 166 6 n ,B= 1 , B =[I] j j 1 2 joint-space accelerations corresponding to the task-space O B O J n 6 2nn (6+n) (6+n) 2 velocities and task-space accelerations x˙dj =d(f j ((t))/dt x¨ =d2(f ((t))/dt 2 for j=1,2,..., p. J+ =W1 JT (JW1JT)1 where the linear velocity transformation matrix, , and the dj j 1 wj j j J1 are the weighted pseudoinverse of Jacobians and angular velocity transformation matrix, , are given as: J2 are diagonal weight matrices. Wj =diag(h1j ,....,h(6+n)j ) In our proposed scheme we use weighted pseudoinverse C C S C +S S C S S +S C C 5 6 6 4 4 5 6 4 6 5 4 6 technique in such a way that it can act as a ﬁlter to remove J1 = S6C5 C4C6 +S4S5S6 S4C6 +S5S6C4 the propagation of undesirable frequency components from S5 S4C5 C4C5 the task-space trajectories to the corresponding joint-space trajectories for a particular subsystem. This we do by 1 1 S4T5 C4T5 putting suitable zeros in the diagonal entries of the Wj matrices in Eq. (5) and Eq. (6). We leave the other elements J2 = 0 C4 S4 1 of Wj as unity. We have developed two cases for such a 0 S4 /C5 C4 /C5 frequency-wise decomposition as follows:

Here Si, Ci and Ti represent sin(qi ), cos(qi ) and tan(qi ), respectively, and I is the identity matrix. Note that there is a Case I – Partial decomposition. In this case, the segments Euler angle (roll, pitch, yaw) singularity in J when the pitch 2 of the task-space trajectories having frequencies ( ≤ ) angle (q )is an odd multiple of ±90°. Generally, the pitch t t i 5 will be allocated to all subsystems that have natural angle in practical operation is restricted to q <90°. 5 frequencies greater than up to the maximum bandwidth However, if we need to avoid singularity altogether, unit t subsystem. To give an example, for a UVMS, the lower quarternions can be used to represent orientation.24 frequencies will be shared by both the AUV and the manipulator, whereas the higher frequencies will be solely 2.2. Dynamics-based trajectory planning taken care of by the manipulator. In this section, we present a trajectory planning algorithm that accounts for different bandwidth characteristic of a dynamic system. First, we present the algorithm for a Case II – Total decomposition. In this case, we partition general s-bandwidth dynamic system. Then we improvise the total system into several frequency domains, starting this algorithm for the application to a UVMS. from the low frequency subsystem to the very high Let us assume that we know the natural frequency of each frequency subsystem. We then allocate a particular fre- subsystem of the heterogeneous dynamic system. This will quency component of the task-space trajectories to only give us a measure of the dynamic response of each those subsystems that belong to the frequency domain just subsystem. Let these frequencies be i, i=1,2,...... ,s. higher than the task-space component to generate joint- 120 Underwater vehicle space trajectories. For a UVMS, this means that the lower subsystem, i.e. the vehicle in Case II, or to both the heavier frequencies will be taken care of by the vehicle alone and and lighter subsystems, i.e. the vehicle and the manipulator the higher frequencies by the manipulator alone. as in Case I. The high frequency segments of the task-space To improvise the general algorithm for a (6+n) dof trajectories, on the other hand, are always allocated to the UVMS, we decompose the task-space trajectories into two lighter subsystem, i.e. the manipulator. These allocations of components as follows: task-space trajectories have been mapped to corresponding joint-space trajectories by utilizing weighted pseudoinverse f(t)=f (t)+f (t) (7) 11 22 technique where the heterogeneous dynamics of the UVMS where have been taken into consideration. Then, these reference joint-space trajectories are followed by the individual joint/ r1 r1 dof to execute the end-effector’s trajectories. f (t)=a + a cos(r t/L)+ b sin(r t/L), 11 0 r r r=1 r=1 2.3. Fault tolerant decomposition A UVMS is expected to function in a hazardous and r r 2 2 unstructured underwater environment. A thruster/actuator f (t)= a cos(r t/L)+ b sin(r t/L), 22 r n fault can occur due to various reasons. There are different r=r1 +1 r=r1 +1 methods to detect and isolate these faults. Without going r and r (r =p ) are suitable finite positive integers. Here, into the details of the possible nature of thruster/actuator 1 2 2 1 faults and how they can be detected and isolated, we assume f11(t) consists of lower frequency terms and f22(t) has the higher frequency terms. in this work that we can detect and isolate thruster/actuator Now, the mapping between the task-space variables and faults when they occur. In general, there are more thrusters the joint-space variables are performed as and actuators than what is minimally required for the specific dof that a UVMS is designed for. Here, we develop + ˙ q¨ d1 =JW1(¨xd1 J˙xd1 ) (8) an algorithm to exploit the thruster and actuator redundancy q¨ =J+ (¨x J˙˙x ) (9) to accommodate thruster/actuator faults during operation. d2 W2 d2 d2 In order to relate the generalized force vector b with the q¨ =¨q +¨q (10) d d1 d2 individual thruster/actuator force/torque, let us consider a (6+n) (6+n) (6+n) UVMS which has p thrusters and actuators where, in where Wi are the weight matrices, q¨ d + 1 T general, p≥(6+n). In such a case, we can write are the joint-space accelerations and Jwi =Wi J (JW1 JT )1 for (i=1,2). We have considered the weight i b =EFt (15) matrices for two types of decompositions as follows: where E (6+n) p thruster configuration matrix and For Case I – Partial decomposition: p Ft is the reference thruster and actuator forces and torques. The thruster configuration matrix is a constant W1 =diag(h1,h2,....,h(6+n)) (11) matrix that depends on the geometric locations of the W2 =diag(0,....,0,h7,....,h(6+n)) (12) thrusters and actuators. For Case II – Total decomposition: Substituting Eq. (15) into Eq. (1) and performing algebraic manipulation we get. W1 =diag(h1,....,h6,0,....,0) (13) w=M˙ 1(EF ) (16) W =diag(0,....,0,h,....,h ) (14) b td b 2 7 (6+n) where b =Cb(qm, w)w+Db(qm, w)w+Gb(q). The weight design is further improved by incorporating the Differentiation of Eq. (2) leads to the following accelera- system’s damping into the trajectory generation for UVMS. tion relationship: A significant amount of energy is consumed by the damping in the underwater environment. Hydrodynamic drag is one q=B˙¨ w+Bw˙ (17) of the main components of such damping. Thus, if we Now, from Eq. (16) and Eq. (17) we can write decompose the motion in the joint-space in such a way that - it is allocated in an inverse ratio to some measure of q=¨ Ft + (18) 1 - ˙ 1 damping, the resultant trajectory is expected to consume where (6+n) p =BMb E and (6+n) 1 =Bw BMb b. less energy while tracking the same task-space trajectory. From Eq. (18), using weighted pseudoinverse technique Thus, we incorporate the damping into the trajectory we obtain a least-norm solution to thruster and actuator generation by designing the diagonal elements of the weight forces and torques as matrix as h =f( ), where (i=1,...... ,6+n) is the i i i F =+ (q) (19) damping ratio of the particular dynamic subsystem which t W + 1 T 1 T 1 can be found out using multi-body vibration analysis where W =W ( W ) is the weighted pseudo- 25 techniques. inverse of and W=diag(h1,h2,.....,hp ) is the weight It is to be noted that in the proposed dynamics-based matrix. 1 method we have decomposed the task-space trajectory into Now, we construct a thruster fault matrix, p p =W , two domains where the lower frequency segments of the with diagonal entries either 1 or 0 to capture the fault task-space trajectories are directed to either the heavier information of each individual thruster/actuator. If there is Underwater vehicle 121 any thruster/actuator fault we introduce 0 into the corre- where Ft and Ft are the upper and lower limits of thrust/ i,max i,min sponding diagonal element of , otherwise it will be 1. We torque of the i-th thruster/actuator, and Ci is a positive can also rewrite Eq. (19) in terms of thruster fault matrix, , quantity which is determined from the damping property of as the dynamic system. Then, differentiating Eq. (24), we T T 1 obtain Ft = ( ) (¨q ) (20) )2 ( Equation (20) provides us the fault tolerant allocation of H(Ft ) (Ft max Ft min 2Ft Ft max Ft min) = i, i, i i, i, (25) thruster/actuator force/torque, F . More detailed discussion 2 )2 t Fti Ci (Fti,max Fti) (Fti Fti,min on this topic can be found in references [26,27]. Then, the diagonal elements of the weight matrix are defined as 2.4. Saturation limit In the previous section, we have derived Eq. (20) for desired hi =1+ H(Fi )/ Fi (26) thruster/actuator force/torque allocation that allows the From the above expression (25), we notice that H(Ft )/ Fti operation of the UVMS with faults. However, it cannot is equal to zero when the i-th thruster/actuator is at the guarantee that the desired allocated forces/torques will middle of its range, and becomes infinity at either limits. remain within the saturation limit of the thrusters/actuators. - Thus, h i varies from 1 to infinity if the i-th thrust goes from As a result, if some of the forces and torques determined middle of the range to its limit. If the i-th thrust/torque from those equations are beyond the capacity of the - approaches its limit, then h i becomes very large and the corresponding thrusters and actuators, the performance of 1 corresponding element in WF goes to zero and the i-th the controller will suffer because of saturation effect. thruster/actuator avoids saturation. Depending upon The saturation problem cannot be solved based on the whether the thruster/actuator is approaching toward or formulation given by Eq. (20). In order to avoid the - departing from its saturation limit, h i can be redefined as saturation effect, the thruster/actuator force/torque must be - ≥ h i =1+ H(Fi )/ Fi when H(Fi )/ Fi 0 (i.e. the controlled so that it cannot reach the saturation limit. thruster/actuator force/torque is approaching toward its However, in such a case, since the input to the controller and - limit), and h i =1 when H(Fi )/ Fi ≤0 (i.e. the thruster/ the output of the controller will be algebraically related, actuator force/torque is departing from its limit). Finally, the static state feedback technique will not be ablel to control desired thruster/actuator force/torque vector, Ftd, that is the thruster and actuator forces and torques. We, therefore, guaranteed to stay within the saturation limit. 28–29 propose to use the dynamic state feedback technique to Substituting Eq. (22) into Eq. (21) and denoting generate thruster forces that are within the saturation limit. e=qd q, we obtain the following error equation in joint- The basic idea of dynamic state feedback is to introduce space integrators at the input channel to enlarge the state space, and then apply the static state feedback on the enlarged e+K1ë+K2e+K˙ 3e=0 (27) system. Thus, for positive values of the gains, K1, K2 and K3, the We differentiate the output Eq. (18) to obtain a new input joint-space errors reduce to zero asymptotically, as time as follows: goes to infinity. q=F˙ +˙F +-˙ =+ (21) Now, to incorporate both the fault and the saturation t t information in control Eq. (22), we define a fault-saturation -˙ ˙ 1 - where =˙ Ft + and =Ft. matrix, p p =WF , having diagonal elements either 1/h i or Now, we consider the following control law zero. Whenever there is any thruster/actuator fault, we put =+ [q +K (¨q q)+K¨ (˙q q)+K˙ (q q)] (22) that corresponding diagonal element in matrix as zero. If WF d 1 d 2 d 3 d there is no fault in any thruster/actuator, that corresponding - and integration of Eq. (22) yields the desired thruster and diagonal entry of the fault-saturation matrix will be 1/h i . actuator forces and torques as Thus, it accounts for the force/torque saturation limits along with the fault information. We can rewrite Eq. (22) in terms Ftd = dt (23) of fault-saturation matrix, , as T T 1 + 1 T 1 T 1 = ( ) [ qd +K1(¨qd q)+K¨ 2(˙qd q)˙ where WF =WF ( WF ) , K1 is the acceleration gain, K2 is the velocity gain and K3 is the position gain; +K3(qd q)] (28) qd and Ftd are desired parameters of q and Ft , respectively. The diagonal elements of the weight matrix, 2.5. Drag minimization W =diag(h- ,h- .....,h- ) are computed from the thruster/ F 1 2 p Drag is a dissipative force that does not contribute to the actuator saturation limits as follows: desired motion. Actually, a UVMS will require a significant We define a function of thruster and actuator force and amount of energy to overcome the hydrodynamic drag. torque variables as Reduction of drag can also be useful from another p perspective. The UVMS can experience a large reaction 1 (F F ) H(F )= ti,max ti,min (24) force because of the drag. High reaction force can saturate t C (F F )(F F ) the controller and thus degrade the performance of the i=1 i ti,max ti ti ti,min autonomous system. This problem is less severe when 122 Underwater vehicle human operators are involved (as in case of ROV), because where i are arbitrary positive quantities, and the negative they can adjust their strength and coordination according to sign implies minimization of the performance criteria. A the situations. However, for an autonomous system, it is detailed discussion on this drag minimization can be found desirable to reduce such a large force especially when the in reference [22]. force is detrimental to the task. Thus, we want to design a Thus, Eqs. (29) and (30) along with Eqs. (31)–(33) motion planning algorithm that generates trajectories in provide us with the reference joint-space trajectories such a way that the UVMS not only reaches to its goal considering the dynamics-based planning method as well as position and orientation from an initial position and the drag minimization scheme. Now, we can obtain all the orientation, but also minimizes the drag on the UVMS, desired joint-space variables required for the control law while it follows the generated trajectories. Hence, we (Eq. (22) or Eq. (28)) by integrating Eq. (29) and Eq. (30) exploit the kinematic redundancy of the system not only to and making use of Eq. (10). Then by differentiating it we coordinate the motion of the UVMS but also to satisfy a can obtain the desired third derivative for the joint-space secondary objective criterion that we believe will be useful variables. Thus, we have formulated a unified motion in underwater applications. The secondary objective crite- planning algorithm by integrating the dynamics-based rion that we choose to satisfy in this work is hydrodynamic drag optimization. planning algorithm (Eq. (8)–(10)) with fault-tolerant algo- Recalling Eq. (8) and Eq. (9), we can write the complete rithm (Eq. (20)), saturation algorithm (Eq. (21)–(22)), and solution to the joint-space acceleration as:30 drag minimization algorithm (Eq. (29)–(30)).

q¨ =J + (¨x J˙˙ q )+(IJ+ J)¨ (29) d1 W1 d1 d1 W1 1 2.6. Thruster dynamics The desired thruster force allocation as obtained from + ˙ + ¨ (30) Equation (23) can be directly applied to the dynamic model q¨ d2 =JW2 (¨xd2 J˙qd2)+(I JW2 J) 2 of the UVMS given by Eq. (1) (using Eq. (15)) to generate where the null-space vectors + ¨ (for ) will the actual motion of the system. However, in such a case the (I JW1 J) 1 i=1,2 be utilized to minimize the drag effects on the UVMS. dynamics of the thrusters will be neglected and the results 31 We define a positive definite scalar potential function will not accurately reflect the reality. Yoerger et al. pointed p(q,q) ˙ , which is a quadratic function of drag forces as out that the system dynamics of an underwater vehicle can be greatly influenced by the dynamics of the thrusters, and neglecting this dynamics may result in a limited bandwidth p(q,q)=D ˙ T(q,q) ˙ W D(q,q) ˙ (31) D controller with limit cycle instability. There are several dynamic models of marine thrusters31–33 that can reliably (6+n) where D(q,q) ˙ is the vector of drag forces and account for thruster dynamics. (6+n) (6+n) WD is a positive definite weight matrix. Note In this work we use the model proposed by Healey et al.32 that a proper choice of this WD matrix can enable us to that included a four-quadrant mapping of the lifts and drag design the influence of drag on individual components of forces of the propeller blades and was coupled with the the UVMS. Generally, W is chosen to be a diagonal matrix D motor and fluid system dynamics. This model is given by so that the cross-coupling terms can be avoided. If it is the following equations: chosen to be an identity, then the drag experienced on all dof of the combined system is equally weighted. However, by 0.5 0.5 increasing or decreasing the values of the diagonal elements r = 2 sign(Ftd) Ftd (34) of the WD matrix, the corresponding drag contribution of each dof can be regulated. The potential function, p(q,q) ˙ , 1 1 captures the total hydrodynamic drag on the whole vehicle- im =Kt 1Ftd +Kf Kfb( r) (35) manipulator system. Therefore, the minimization of this ˙ 1 function will lead to the reduction of drag on the whole =I [Ktim Kf ] (36) system. Now, taking the gradient of the potential function, p(q,q) ˙ , we obtain where and r are the actual and the desired/reference propeller angular velocity, respectively, and im is the motor 22 2 current. The other parameters are: 2 = Ar tan ( ), p(q,q) ˙ p(q,q) ˙ where is the density of the water, r is the radius of the p(q,q)= ˙ + (32) q q˙ propeller, A is the thruster duct area, is the propeller efficiency, is the average pitch of the propeller blade, 1 is an experimentally determined constant, Kt is the motor We take the gradient p(q,q) ˙ as the arbitrary vector, ¨ , of i torque constant, Kf is the motor viscous friction constant, I Eq. (29) and Eq. (30) to minimize the hydrodynamic drag in is the motor and proportion inertia matrix, Kfb is the motor the following form: feedback gain, and is the propeller shaft torque. Neglecting the motor inductance,32 the motor input ¨ T i = i p for i=1, 2. (33) voltage can be written as Underwater vehicle 123

Fig. 2. Uniﬁed dynamics-based motion planning scheme.

Fig. 3. Task-space geometric path and trajectories.

Fig. 4. Task-space geometric paths: Conventional Motion Planning (CMP) method on the left and Uniﬁed Dynamics-based Motion Planning (UDMP) method on the right. Dotted line denote the desired paths and solid lines denote actual paths.

Vm =imRm +Kemf (37) the square path is tracked in equal time. The geometric path and the task-space trajectories are given in Figure 3. where Vm is the motor input voltage, Rm is the motor The task-space trajectories can be represented as resistance and Kemf is the motor back emf constant. The propeller torque and the axial thrust are related to the 4kt/Lk if 0

z(t)=fz(t)=0 (42)

3. SIMULATION RESULTS The Fourier series for the above trajectories are as follows:

3.1. Trajectory f (t)= + a cos(rt/L)+ b sin(rt /L ) (43) We have chosen a square path in xy (horizontal) plane for j 0j rj rj r=1 r=1 the computer simulation. We have assumed that each side of 124 Underwater vehicle

Fig. 5. Task-space trajectories: Conventional Motion Planning method (left column) and Uniﬁed Dynamics-based Motion Planning method (right column). Actual trajectories (solid lines) are superimposed on desired trajectories (dotted lines).

Fig. 6. Joint-space trajectories: Uniﬁed Dynamics-based Motion Planning method (solid lines) and Conventional Motion Planning Method (dashed lines). Underwater vehicle 125

Fig. 7. Thruster and actuator forces and torques of the UVMS. Uniﬁed Dynamics-based Motion Planning method (solid lines) and Conventional Motion Planning method (dashed lines). Thruster faults are marked by “A” (Thruster 1) and “B” (Thruster 5).

where ‘j’ implies the coefﬁcients for x, y or z; k is a constant effectiveness of the proposed method, we have compared and 2L is the time period. The Fourier coefﬁcients are: the results of UDMP approach with that of Conventional 2 aox = a0y =k, arx = ary =4k/(r ) (cos r 1) and Motion Planning (CMP) method. In conventional method, 2 brx = bry =8k/(r ) (sin r /2). the trajectory is designed in three sections: the main section For this simulation, we have taken k=1m, i.e. the path is (intermediate section) that is a straight line is preceded and 2m square, L=5 and maximum frequency at which the followed by two short parabolic sections. The UVMS used

Fourier series is truncated is r=p1 =30. Total time of for these simulations consists of a 6 dof vehicle and a 3 dof simulation=10sec. The frequency of the manipulator is 10 planar manipulator working in the vertical plane. The times higher than that of the vehicle. We have taken the vehicle is ellipsoidal in shape with length, width and height natural frequency of the vehicle as 0.15 cycles per second 2m, 1m and 1m, respectively. The mass of the vehicle is and the manipulator to be 10 time faster than the vehicle. We 1073kg. The links are cylindrical and each link is 1m long. have segmented the task-space trajectories as The radii of link 1, 2 and 3 are 0.1m, 0.08m and 0.07m, respectively. The link masses (oil filled) are 32kg, 21kg and f j1(t)=a0j +a1j cos( t/L)+b1j sin( t /L ) (44) 16kg, respectively. The simulation time is 10sec that is 30 30 required to complete the square path. The total length of the f (t)= a cos(rt/L) b sin(rt/L) (45) path is 8m, thus the average speed is about 1.6knot. j2 rj rj r=2 r=2 We have simulated two thruster faults: one horizontal thruster (Thruster 1) and the other one vertical thruster (Thruster 5). Both the thrusters stop functioning from 6sec. 3.2. Results and discussion It is to be noted that both the thrusters are located at the We have performed extensive computer simulation to same bracket of the UVMS, which is one of the worst investigate the efficacy of the proposed Unified Dynamics- thruster fault situations. In our simulation, we have based Motion Planning (UDMP) algorithm. To verify the considered the following thruster/actuator thrust/torque 126 Underwater vehicle saturation limits: ±400N for horizontal thrusters (Thruster during computer simulation to reflect the uncertainties that 1–4), ±200N for vertical thrusters (Thruster 5–8), ±200N.m are present in underwater environment. for actuator 1, ±100N.m for actuator 2 and ±50N.m for We have presented results from the computer simulations actuator 3. The thruster configuration matrix for the in Figure 4 through Figure 8. The results we have provided simulated UVMS is as follows: here are from Case I: Partial decomposition of the proposed UDMP method. The task-space geometric paths are plotted 1 0 1 0 0 0 0 0 0 0 0 in Figure 4, where we can see that the path tracking errors 0 1 0 1 0 0 0 0 0 0 0 in our proposed UDMP method are much smaller as 0 0 0 0 1 1 1 1 0 0 0 compared to that of CMP method. We have also plotted 0 0 0 0 Rt3 0 Rt3 0 0 0 0 task-space trajectories in Figure 5. It is also observed from E= 0 0 0 0 0 Rt4 0 Rt4 0 0 0 plots in Figure 5 that the end-effector tracks the task-space Rt1 Rt2 Rt1 Rt2 0 0 0 0 0 0 0 trajectories quite accurately in UDMP method. The errors 0 0 0 0 0 0 0 0 1 0 0 are less in proposed UDMP method as compared to the 0 0 0 0 0 0 0 0 0 1 0 CMP method. The joint-space trajectories are plotted in 0 0 0 0 0 0 0 0 0 0 1 Figure 6. From these plots it is observed that the proposed UDMP method effectively reduces the motions of the heavy where Rt1 =1.25m, Rt2 =1.75m, Rt3 =0.75m, and Rt4 =1.25m subsystem (the vehicle) and allows greater and sharper are the perpendicular distances from the center of the motions to the lighter subsystem (the manipulator) while vehicle to the axes of the side and the front horizontal tracking the same task-space trajectories. It is also noticed thrusters, and the side and the front vertical thrusters, that the motion of the heavy subsystem is smoother in the respectively. proposed method. We find that these sharper and larger To make the simulation close to reality, we have motions of the heavy subsystem in case of CMP method introduced sensory noise in the measurements of positions demand higher driving force that we see in Figure 7. From and its derivatives. We have considered Gaussian noise of the plots in this figure (Figure 7) it is also observed that in 1.0 mean and 1.0 standard deviation in the measurement of case of UDMP method thrusters 4, 7, 8 and actuator 1 have linear quantities (in mm unit), and 0.01 mean and 0.05 reached the saturation limits, but they have not exceeded the standard deviation in measurement of angular quantities (in limits. On the other hand, in case of CMP method all the deg unit). We have considered 10% modeling inaccuracy thrusters and actuators have reached the saturation limits,

Fig. 8. Surge-sway motion of the vehicle, power requirement and energy consumption of the UVMS. Results from Conventional Motion Planning method are on the left and that of Unified Dynamics-based Motion Planning method are on the right. Underwater vehicle 127 however the saturation scheme was able to keep them to underwater robotic system that has a vastly different within the specified limits. Because of this, the path and dynamic bandwidth. The results from computer simulation trajectory tracking performance in CMP method has been demonstrate the effectiveness of the proposed method. It degraded, as we can see in Figure 4 and Figure 5. Thus, the shows that the proposed algorithm not only improves the conventional planning method demands more powerful trajectory tracking performance but also significantly actuation system to track the same trajectories with reduces the energy consumption and the power require- reasonable accuracy. We also observe that the thrust 1 and ments for the operation of an autonomous UVMS. We have thrust 5 are zero from 6sec as marked by “A” and “B”, not presented results from Case II (Total decomposition) respectively (see Fig. 7). These imply they have developed because of the length of the paper. However, these results faults at 6th second and remain non-functional for rest of the are comparable to the conventional motion planning time. At this moment we observe some perturbations in approach. trajectories and paths, however, the proposed UDMP There are a few drawbacks of this paper as well. We used scheme gradually brings the system to the desired directions a model-based control technique to evaluate our planning and reduces the tracking errors. On the other hand, after the algorithm. However, the underwater environment is uncer- occurrence of faults the paths and the trajectories are tacked tain and we need to use adaptive control techniques in poorly in case of CMP method, because this algorithm future. Finally, the proposed scheme needs to be validated cannot account for the dynamics of the system while by experiments. generating the reference trajectories. We have also plotted the simulation results for surge- ACKNOWLEDGMENTS sway motion, power requirement and energy consumption This work was partially supported by the National Science of the UVMS in case of CMP method (in the left column) Foundation under grant BES97-01614 and the Office of and that of in case of proposed UDMP method (in the right Naval Research under grant N00014-97-1-0961. column) in Figure 8. Top two plots in this figure show the profile of the surge-sway movements of the vehicle in the said two methods. In case of the CMP method, the vehicle References changes the motion sharply and moves more as compared to 1. C.A. Klein and C.H. Huang, “Review of Pseudoinverse the motion generated from the UDMP method. It may so Control for Use with Kinematically Redundant Manip- ulators,” IEEE Transactions on Systems, Man and Cybernetics happen that, in practice, this type of sharp and fast SMC-13, No. 3, 245–250 (1983). movements may be beyond the capability of the heavy 2. R. Paul, “Manipulator Cartesian Path Planning,” IEEE dynamic subsystem and consequently large errors in Transactions on Systems, Man and Cybernetics SMC-9, No. trajectory tracking will occur. Additionally, this may cause 11, 702–711 (1979). saturation of the thrusters and the actuators resulting in 3. Z.L. Zhou and C.C. Nguyen, “Globally Optimal Trajectory Planning for Redundant Manipulators using State Space degradation in performance. Moreover, the vehicle will Augmentation Method,” Journal of Intelligent and Robotic experience large velocity and acceleration in CMP method Systems 19, No. 1, 105–117 (1997). that result in higher power requirement and energy con- 4. B. Siciliano, “Closed-Loop Inverse Kinematics Algorithms sumption, as we observe it in next two sets of plots in Figure for Redundant Spacecraft/Manipulator Systems,” Proceed- 8. Thus, this investigation reveals that our proposed Unified ings of IEEE International Conference on Robotics and Automation, Atlanta, GA (1993) pp. 95–100. Dynamics-Based Motion Planning method is very promis- 5. G. Antonelli and S. Chiaverini, “Task-Priority Redundancy ing for autonomous operation of dynamic system composed Resolution for Underwater Vehicle-Manipulator Systems,” of several subsystems having variable dynamic responses. Proceedings of IEEE International Conference on Robotics and Automation, Leuven, Belgium (May, 1998) pp. 756–761. 4. CONCLUSIONS 6. M. Vukobratovic and M. Kircanski, “A Dynamic Approach to Nominal Trajectory Synthesis for Redundant Manipulators,” We have proposed a new unified dynamics-based motion IEEE Transactions on Systems, Man, and Cybernetics SMC planning algorithm that can generate both kinematically 14, No. 4, 580–586 (July, 1984). admissible and dynamically feasible joint-space trajectories 7. J.E. Bobrow, “Optimal Robot Path Planning Using the for systems composed of heterogeneous dynamics. We have Minimum-Time Criterion,” IEEE Journal of Robotics and then extended this algorithm for an autonomous underwater Automation 4, No. 4, 443–450 (August, 1988). 8. Z. Shiller and S. Dubowsky, “Robot Path Planning with vehicle-manipulator system, where the dynamic response of Obstacles, Actuator, Gripper, and Payload Constraints,” Int. J. the vehicle is much slower than that of the manipulator. We Robotics Research 8, No. 6, 3–18 (December, 1989). have also exploited the kinemetic redundancy to accom- 9. K. Shin and N. McKay, “A Dynamic programming Approach modate the thruster/actuator faults and saturation and also to to Trajectory Planning of Robot Manipulators,” IEEE Trans- minimize hydrodynamic drag. We have incorporated thrus- actions on Automatic Control AC-31, No. 6, 491–500 (June, 1986). ter dynamics when modeling the UVMS. Although, some 10. A.R. Hirakawa and A. Kawamura, “Trajectory Planning of researchers have exploited kinematic redundancy for opti- Redundant Manipulators for Minimum Energy Consumption mizing various criteria, but those works have mainly been without Matrix Inversion,” Proceedings of the IEEE Inter- addressed to problems with land-based robotics or space- national Conference on Robotics and Automation, robotics. Hardly any motion planning algorithm has been Albuquerque, New Mexico (April, 1997) pp. 2415–2420. 11. S.F.P. Saramago and V. Steffen Jr., “Optimization of the developed for autonomous underwater vehicle-manipulator Trajectory Planning of Robot Manipulators Taking into system. In this research, work we have formulated a new Account the Dynamics of the System,” Journal of Mechanism unified motion planning algorithm for a heterogeneous and Machine Theory 33, No. 7, 883–894 (1998). 128 Underwater vehicle 12. Z.H. Zhu, R.V. Mayorga and A.K.C. Wong, “Dynamic Robot Subject to Drag Optimization,” IEEE Journal of Oceanic Manipulator Trajectory Planning for Obstacle Avoidance,” Engineering 26, No. 2, 228–239 (2001). Journal of Mechanics Research Communications 26, No. 2, 23. T.K. Podder, “Motion Planning of Autonomous Underwater 139–144 (1999). Vehicle-Manipulator Systems”, Ph.D. Thesis (Department of 13. N. Faiz and S.K. Agrawal, “Trajectory Planning of Robots Mechanical Engineering, University of Hawaii at Manoa, with Dynamics and Inequalities,” the Proceedings of IEEE Honolulu, December, 2000). International Conference on Robotics and Automation, San 24. T.I. Fossen, Guidance and Control of Ocean Vehicles (John Francisco, CA (April 2000) pp. 3977–3983. Wiley & Sons, 1994). 14. S. Macfarlane and E.A. Croft, “Jerk-bounded Manipulator 25. M.L. James, G.M. Smith, J.C. Wolf and P.W. Whaley, Trajectory Planning: Design for Real-time Applications,” Vibration of Mechanical and Structural Systems (Harper and IEEE Transactions on Robotics and Automation 19, No. 1, Row Publishers, New York, 1989). 42–52 (February, 2003). 15. O. Brock and O. Khatib, “High-Speed Navigation Using the 26. T.K. Podder and N. Sarkar, “Fault Tolerant Control of an Global Dynamic Window Approach,” Proceedings of the Autonomous Underwater Vehicle Under Thruster Redun- IEEE International Conference on Robotics and Automation, dancy: Simulation and Experiments,” Proceedings of IEEE Detroit, MI (May, 1999) pp. 341–346. International Conference on Robotics and Automation, San 16. Q. Huang, K. Tanie and S. Sugano, “Coordinated Motion Francisco (April, 2000) pp. 1251–1256. Planning for a Mobile Manipulator Considering Stability and 27. T.K. Podder, G. Antonelli and N. Sarkar, “An Experimental Manipulation,” Int. J. Robotics Research 19, No. 8, 732–742 Investigation into the Fault-tolerant Control of an Autono- (August, 2000). mous Underwater Vehicle,” Journal of Advanced Robotics 15, 17. Y. Yamamoto and S. Fukuda, “Trajectory Planning of No. 5, 501–520 (2001). Multiple Mobile Manipulators with Collision Avoidance 28. A. Isidori, C.H. Moog and A. De Luca, “A sufficient Capability,” Proceedings of the IEEE International Con- Condition for Full Linearization via Dynamic State Feed- ference on Robotics and Automation, Washington D.C. (May, back,” Proceedings of IEEE Conference on Decision and 2002) pp. 3565–3570. Control, Athens, Greece (December, 1986) pp. 203–208. 18. A. Yamashita, T. Arai, Ota Jun and H. Asama, “Motion 29. X. Yun, “Dynamic State Feedback Control of Constrained Planning of Multiple Mobile Robots for Cooperative Manip- Robot Manipulators,” Proceedings of IEEE Conference on ulation and Transportation,” IEEE Transactions on Robotics Decision and Control, Austin, Texas (December, 1988) and Automation 19, No. 2, 223–237 (April, 2003). pp. 622–626. 19. D.R. Yoerger and J.J.E. Slotine, “Robust Trajectory Control of 30. A. Ben-Israel and T.N.E. Greville, Generalized Inverse: Underwater Vehicles,” IEEE Journal of Oceanic Engineering Theory and Applications (New York, Wiley, 1974). 10 , No. 4, 462–470 (October, 1985). 31. D. Yoerger, J. Cooke and J.J.E. Slotine, “The Influence of 20. I. Spangelo and O. Egeland, “Trajectory Planning and Thruster Dynamics on Underwater Vehicle Behavior and Collision Avoidance for Underwater Vehicle using Optimal Control,” IEEE Journal of Oceanic Engineering 19, No. 4, Their Incorporation into Control System Design,” IEEE 502–511 (1994). Journal of Oceanic Engineering 15, No. 3, 167–178 (1990). 21. K. Kawano and T. Ura, “Motion Planning Algorithm for 32. A.J. Healey, S.M. Rock, S. Cody, D. Miles and J.P. Brown, Nonholonomic Autonomous Underwater Vehicle in Dis- “Toward an Improved Understanding of Thruster Dynamics turbance using Reinforced Learning and Teaching Method,” for Underwater Vehicles,” IEEE Journal of Oceanic Engi- Proceedings of the IEEE International Conference on Robot- neering 20, No. 4, 354–361 (1995). ics and Automation, Washington D.C. (May, 2002) 33. L.L. Whitcomb and D.R. Yoerger, “Development, Compar- pp. 4032–4038. ison, and Preliminary Validation of Non-linear Dynamic 22. N. Sarkar and T.K. Podder, “Coordinated Motion Planning Thruster Models,” IEEE Journal of Oceanic Engineering 24, and Control of Underwater Vehicle-Manipulator systems No. 4, 481–493 (1999).