A unified dynamics-based motion planning algorithm for autonomous underwater vehicle-manipulator systems (UVMS)

A new unified motion planning algorithm for autonomous Underwater Vehicle-Manipulator Systems (UVMS) has been presented in this paper. Commonly, a UVMS consists of two sub-systems, a vehicle and a manipulator, having vastly different dynamic responses. The proposed algorithm considers the variability in dynamic bandwidth of the complex UVMS system and generates not only kinematically admissible but also dynamically feasible reference trajectories. Additionally, this motion planning algorithm exploits the inherent kinematic redundancy of the whole system and provides reference trajectories that accommodates other important criteria such as thruster/actuator faults and saturations, and also minimizes hydrodynamic drag. Effectiveness of the proposed unified motion planning algorithm has been verified by extensive computer simulation. The results are quite promising.


INTRODUCTION
In robotics, trajectory planning is one of the most challenging problems. 1 Traditionally, trajectory planning problem is formulated as a kinematic problem and therefore the dynamics of the robotic system is neglected. 2 Although the kinematic approach to the trajectory planning has yielded some very successful results, they are essentially incomplete as the planner does not consider the system's dynamics while generating the reference trajectory. As a result, the reference trajectory may be kinematically admissible but may not be dynamically feasible.
Researchers, in the past several years, have developed various trajectory planning methods for robotic systems considering different kinematic and dynamic criteria such as obstacle avoidance, singularity avoidance, time minimization, torque optimization, energy optimization, and other objective functions. [3][4][5][6][7][8][9][10][11][12][13][14][15][16][17][18][19][20][21] A robotic system that has more than 6 dofs (degrees-of-freedom) is termed as a kinematically redundant system. For a kinematically redundant system, the mapping between task-space trajectory and the joint-space trajectory is not unique. It admits infinite number of joint-space solutions for a given task-space trajectory. However, there are various mathematical tools such as Moore-Penrose Generalized Inverse, which map the desired Cartesian trajectory into the corresponding joint-space trajectory for a kinematically redundant system. Researchers have developed various trajectory planning methods for redundant systems. 1,[3][4][5]9 A kinematic approach of motion planning has been reported in references [1][2][3][4][5]. Zhou and Nguyen 3 formulated optimal joint-space trajectories for kinematically redundant manipulators by applying Pontryagin's Maximum Principle. Siciliano 4 has proposed an inverse kinematic approach for motion planning of a redundant spacecraft-manipulator system. Antonelli and Chiaverini 5 have used a pseudoinverse method for taskpriority redundancy resolution for an autonomous Underwater Vehicle-Manipulator System (UVMS) using a kinematic approach.
Several researchers, [6][7][8][9][10][11][12][13][14][15] on the other hand, have considered the dynamics of the system for trajectory planning. Vukobratovic and Kircanski 6 proposed an inverse problem solution to generate nominal joint-space trajectory considering the dynamics of the system. Bobrow 7 presented the Cartesian path of the manipulator with a B-spline polynomial and then optimized the total path traversal time satisfying the dynamic equations of motion. Shiller and Dubowsky 8 presented a time-optimal motion planning method considering the dynamics of the system. Shin and McKay 9 proposed a dynamic programming approach to minimize the cost of moving a robotic manipulator. Recently, Hirakawa and Kawamura 10 have proposed a method to solve trajectory generation problem for redundant robot manipulators using the variational approach with a Bspline function to minimize the consumed electrical energy. Saramago and Steffen 11 have formulated off-line joint-space trajectories to optimize traveling time and minimize mechanical energy of the actuators using spline functions. Zhu et al. 12 have formulated real-time collision free trajectory by minimizing an energy function. Faiz and Agrawal 13 have proposed a trajectory planning scheme that explicitly satisfy the dynamic equations and the inequality constraints prescribed in terms of joint variables. Recently, Macfarlane and Croft 14 have developed and implemented a jerk-bounded trajectory for an industrial robot using concatenated quintic polynomials. Motion planning of land-based mobile robotic systems has been reported in references [15][16][17][18]. Brock and Khatib 15 have proposed a global dynamic window approach that combines planning and realtime obstacle avoidance algorithms to generate motion for mobile robots. Huang et al. 16 have presented a coordinated motion planning approach for a mobile manipulator considering system stability and manipulation. Yamamoto and Fukuda 17 formulated trajectories considering kinematic and dynamic manipulability measures for two mobile robots carrying a common object while avoiding a collision by changing their configuration dynamically. Recently, Yamashita et al. 18 have proposed a motion planning method for multiple mobile robots for cooperative transportation of a large object in a 3D environment. To reduce the computational burden, they have divided the motion planner into a global path planner and a local manipulation planner then they have designed it and integrated it. All the previously mentioned researches [6][7][8][9][10][11][12][13][14][15][16][17][18] are performed for either space robotic or land-based robotic systems. On the other hand, very few works on motion/trajectory planning of underwater robotic systems have been reported. [19][20][21] Yoerger and Slotine 19 formulated a robust trajectory control approach for underwater robotic vehicles. Spangelo and Egeland 20 developed an energy-optimum trajectory for underwater vehicles by optimizing a performance index consisting of a weighted combination of energy and time consumption by the system. Recently, Kawano and Ura 21 have proposed a motion planning algorithm for nonholonomic autonomous underwater vehicle in disturbance using reinforcement learning (Q-learning) and teaching method. Sarkar and Podder 22 have presented a coordinated motion planning algorithm for a UVMS to minimize the hydrodynamic drag. Note that UVMS always implies an autonomous UVMS in this paper.
However, the majority of the trajectory planning methods available in the literature that considered the dynamics of the system are formulated for land-based robots. They have either optimized some objective functions related to trajectory planning satisfying dynamic equations or optimized energy functions. Moreover, for the land-based robotic system, the dynamics of the system is either homogeneous or very close to homogeneous. [6][7][8][9][10][11][12][13][14][15][16][17][18] On the other hand, most of the trajectory planning methods that have been developed for space and underwater robotic systems use the pseudoinverse 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, the traditional trajectory planning approaches may generate such reference trajectories that either the UVMS may not be able to track them or while tracking, it may consume exorbitant amount of energy which is extremely precious for autonomous operation in an oceanic environment.
Additionally, in this research, we exploit the kinematic redundancy of the UVMS to formulate a unified dynamically feasible reference trajectory generation algorithm that can accommodate thruster/actuator faults, saturations, and provide a minimum drag trajectory for a given task. All these performance criteria are very important for autonomous underwater operation. They provide a fault-tolerant and reduced energy consuming autonomous operation framework.

UVMS dynamics
For convenience, we commonly use two reference frames to describe underwater robotic systems. These two frames are namely the earth-fixed frame (denoted by XYZ) and the body-fixed frame (denoted by X v Y v Z v ), as shown in Figure 1.
The dynamic equations of motion of a UVMS can be expressed as follows: where the subscript 'b' denotes the corresponding parameters in the body-fixed frames of the vehicle and the manipulator. M b (q m ) ᑬ (6+n)ϫ (6+n) is the inertia matrix including the added mass and C b (q m , w) ᑬ (6+n)ϫ (6+n) is the centrifugal and Coriolis matrix including terms due to added mass. D b (q m , w) ᑬ (6+n)ϫ (6+n) is the drag matrix, G(q) ᑬ (6+n) is the vector of restoring forces and b ᑬ (6+n) is the vector of forces and moments acting on the UVMS. The displacement vector q = [q v , q m ] T , where q v = [q 1 , . . . . , q 6 ] T , and q m = q 7 , . . . . , q 6+n ] T · q 1 , q 2 and q 3 are the linear (surge, sway, and heave) displacements of the vehicle along X, Y, and Z axes, respectively, expressed in the earth-fixed frame. q 4 , q 5 and q 6 are the angular (roll, pitch, and yaw) displacements of the vehicle about X, Y and Z axes, respectively, expressed in the earth-fixed frame. q 7 , q 8 , . . . . , q 6+n are the angular displacements of joint 1, joint 2, . . . . . . . , joint n of the manipulator in link-fixed frames. The quasi velocity vector w = [w 1 , . . . . . . . , w 6+n ] T , where w 1 , w 2 and w 3 are the linear velocities of the vehicle along X v , Y v , and Z v axes respectively, expressed in the body-fixed frame. w 4 , w 5 and w 6 are the angular velocities of the vehicle about X v , Y v , and Z v axes, respectively, expressed in the body-fixed frame. w 7 , w 8 , . . . . . . . , w 6+n are the angular velocities of manipulator joint 1, joint 2, . . . . . , joint n, expressed in the link-fixed frame. A detailed derivation of Eq. (1) is given in reference [23].
Equation (1) is represented in the body-fixed frame of the UVMS because it is convenient to measure and control the motion of the UVMS with respect to the moving frame. However, the integration of the angular velocity vector does not lead to the generalized coordinates denoting the orientation of the UVMS. In general, we can relate the derivative of the generalized coordinates and the velocity vector in the body-fixed frame by the following linear transformation:q

= Bw
(2) The transformation matrix B in Equation (2) is given by: where the linear velocity transformation matrix, J 1 , and the angular velocity transformation matrix, J 2 , are given as: Ϫ S 6 C 4 + S 4 S 5 C 6 C 4 C 6 + S 4 S 5 S 6 S 4 C 5 S 4 S 6 + S 5 C 4 C 6 Ϫ S 4 C 6 + S 5 S 6 C 4 C 4 C 5 Here S i , C i and T i represent sin(q i ), cos(q i ) and tan(q i ), respectively, and I is the identity matrix. Note that there is a Euler angle (roll, pitch, yaw) singularity in J 2 when the pitch angle (q 5 )is an odd multiple of ± 90°. Generally, the pitch angle in practical operation is restricted to ͉ q 5 ͉ < 90°. However, if we need to avoid singularity altogether, unit quarternions can be used to represent orientation. 24

Dynamics-based trajectory planning
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 general s-bandwidth dynamic system. Then we improvise this algorithm for the application to a UVMS.
Let us assume that we know the natural frequency of each subsystem of the heterogeneous dynamic system. This will give us a measure of the dynamic response of each subsystem. Let these frequencies be i , i=1,2, . . . . . . , s.
We approximate the task-space trajectories using Fourier series and represent it in terms of the summation of several frequencies in ascending order.
where a 0 , a r , b r are the coefficients of Fourier series and are represented as 6ϫ 1 column vectors, r/2L is the frequency of the series and 2L is the time period. Now we truncate the series at a certain value of r (assuming r = p 1 to be sufficiently large) so that it can represent the task-space trajectories reasonably. We rewrite the task-space trajectory in the following form: where f 1 (t) = a 0 + a 1 cos( t/L) + b 1 sin( t/L), and f j (t) = a j cos( j t/L) + b j sin( j t/L) for j=2,3, . . . , p 1 .
We then use these truncated series as the reference taskspace trajectories and map them into the desired (reference) joint-space trajectories by using weighted pseudoinverse method as follows:q where q d j are the joint-space velocities and q d j are the joint-space accelerations corresponding to the task-space velocities ẋ d j = d(f j ((t))/dt and task-space accelerations Ϫ 1 are the weighted pseudoinverse of Jacobians and W j = diag(h 1 j , . . . . , h (6+n) j ) are diagonal weight matrices.
In our proposed scheme we use weighted pseudoinverse technique in such a way that it can act as a filter to remove the propagation of undesirable frequency components from the task-space trajectories to the corresponding joint-space trajectories for a particular subsystem. This we do by putting suitable zeros in the diagonal entries of the W Ϫ 1 j matrices in Eq. (5) and Eq. (6). We leave the other elements of W Ϫ 1 j as unity. We have developed two cases for such a frequency-wise decomposition as follows: Case I -Partial decomposition. In this case, the segments of the task-space trajectories having frequencies t ( t ≤ i ) will be allocated to all subsystems that have natural frequencies greater than t up to the maximum bandwidth subsystem. To give an example, for a UVMS, the lower frequencies will be shared by both the AUV and the manipulator, whereas the higher frequencies will be solely taken care of by the manipulator.
Case II -Total decomposition. In this case, we partition the total system into several frequency domains, starting from the low frequency subsystem to the very high frequency subsystem. We then allocate a particular frequency component of the task-space trajectories to only those subsystems that belong to the frequency domain just higher than the task-space component to generate joint-space trajectories. For a UVMS, this means that the lower frequencies will be taken care of by the vehicle alone and the higher frequencies by the manipulator alone.
To improvise the general algorithm for a (6 + n) dof UVMS, we decompose the task-space trajectories into two components as follows: where f 11 (t) = a 0 + r 1 r=1 a r cos(r t/L) + b n sin(r t/L), r 1 and r 2 (r 2 = p 1 ) are suitable finite positive integers. Here, f 11 (t) consists of lower frequency terms and f 22 (t) has the higher frequency terms. Now, the mapping between the task-space variables and the joint-space variables are performed as . We have considered the weight matrices for two types of decompositions as follows: For Case I -Partial decomposition: W 2 = diag(0, . . . . , 0, h 7 , . . . . , h (6+n) ) (12) For Case II -Total decomposition: The weight design is further improved by incorporating the system's damping into the trajectory generation for UVMS. A significant amount of energy is consumed by the damping in the underwater environment. Hydrodynamic drag is one of the main components of such damping. Thus, if we decompose the motion in the joint-space in such a way that it is allocated in an inverse ratio to some measure of damping, the resultant trajectory is expected to consume less energy while tracking the same task-space trajectory. Thus, we incorporate the damping into the trajectory generation by designing the diagonal elements of the weight matrix as 6+n) is the damping ratio of the particular dynamic subsystem which can be found out using multi-body vibration analysis techniques. 25 It is to be noted that in the proposed dynamics-based method we have decomposed the task-space trajectory into two domains where the lower frequency segments of the task-space trajectories are directed to either the heavier subsystem, i.e. the vehicle in Case II, or to both the heavier and lighter subsystems, i.e. the vehicle and the manipulator as in Case I. The high frequency segments of the task-space trajectories, on the other hand, are always allocated to the lighter subsystem, i.e. the manipulator. These allocations of task-space trajectories have been mapped to corresponding joint-space trajectories by utilizing weighted pseudoinverse technique where the heterogeneous dynamics of the UVMS have been taken into consideration. Then, these reference joint-space trajectories are followed by the individual joint/ dof to execute the end-effector's trajectories.

Fault tolerant decomposition
A UVMS is expected to function in a hazardous and unstructured underwater environment. A thruster/actuator fault can occur due to various reasons. There are different methods to detect and isolate these faults. Without going into the details of the possible nature of thruster/actuator faults and how they can be detected and isolated, we assume in this work that we can detect and isolate thruster/actuator faults when they occur. In general, there are more thrusters and actuators than what is minimally required for the specific dof that a UVMS is designed for. Here, we develop an algorithm to exploit the thruster and actuator redundancy to accommodate thruster/actuator faults during operation.
In order to relate the generalized force vector b with the individual thruster/actuator force/torque, let us consider a UVMS which has p thrusters and actuators where, in general, p ≥ (6 + n). In such a case, we can write b = EF t (15) where E ᑬ (6+n)ϫ p thruster configuration matrix and F t ᑬ p is the reference thruster and actuator forces and torques. The thruster configuration matrix is a constant matrix that depends on the geometric locations of the thrusters and actuators. Substituting Eq. (15) into Eq. (1) and performing algebraic manipulation we get.
where b = C b (q m , w)w + D b (q m , w)w + G b (q). Differentiation of Eq. (2) leads to the following acceleration relationship:q = Bẇ +Ḃw (17) Now, from Eq. (16) and Eq. (17) we can writë where (6+n)ϫ p = BM Ϫ 1 b E and -(6+n)ϫ 1 =Ḃw Ϫ BM Ϫ 1 b b . From Eq. (18), using weighted pseudoinverse technique we obtain a least-norm solution to thruster and actuator forces and torques as where + W = W Ϫ 1 T ( W Ϫ 1 T ) Ϫ 1 is the weighted pseudoinverse of and W = diag(h 1 , h 2 , . . . . . , h p ) is the weight matrix. Now, we construct a thruster fault matrix, pϫ p = W Ϫ 1 , with diagonal entries either 1 or 0 to capture the fault information of each individual thruster/actuator. If there is any thruster/actuator fault we introduce 0 into the corresponding diagonal element of , otherwise it will be 1. We can also rewrite Eq. (19) in terms of thruster fault matrix, , as Equation (20) provides us the fault tolerant allocation of thruster/actuator force/torque, F t . More detailed discussion on this topic can be found in references [26,27].

Saturation limit
In the previous section, we have derived Eq. (20) for desired thruster/actuator force/torque allocation that allows the operation of the UVMS with faults. However, it cannot guarantee that the desired allocated forces/torques will remain within the saturation limit of the thrusters/actuators. As a result, if some of the forces and torques determined from those equations are beyond the capacity of the corresponding thrusters and actuators, the performance of the controller will suffer because of saturation effect. The saturation problem cannot be solved based on the formulation given by Eq. (20). In order to avoid the saturation effect, the thruster/actuator force/torque must be controlled so that it cannot reach the saturation limit. However, in such a case, since the input to the controller and the output of the controller will be algebraically related, static state feedback technique will not be ablel to control the thruster and actuator forces and torques. We, therefore, propose to use the dynamic state feedback technique [28][29] to generate thruster forces that are within the saturation limit. The basic idea of dynamic state feedback is to introduce integrators at the input channel to enlarge the state space, and then apply the static state feedback on the enlarged system.
We differentiate the output Eq. (18) to obtain a new input as follows: where ␥ = F t +and =Ḟ t . Now, we consider the following control law = + WF [ត q d + K 1 (q d Ϫq) + K 2 (q d Ϫq) + K 3 (q d Ϫ q)] Ϫ ␥ (22) and integration of Eq. (22) yields the desired thruster and actuator forces and torques as F td = ͵ dt (23) where + WF = W Ϫ 1 F T ( W Ϫ 1 F T ) Ϫ 1 , K 1 is the acceleration gain, K 2 is the velocity gain and K 3 is the position gain; q d and F td are desired parameters of q and F t , respectively. The diagonal elements of the weight matrix, W F = diag(h -1 , h -2 . . . . . , hp ) are computed from the thruster/ actuator saturation limits as follows: We define a function of thruster and actuator force and torque variables as where F t i,max and F t i,min are the upper and lower limits of thrust/ torque of the i-th thruster/actuator, and C i is a positive quantity which is determined from the damping property of the dynamic system. Then, differentiating Eq. (24), we obtain Then, the diagonal elements of the weight matrix are defined as From the above expression (25), we notice that ѨH(F t )/ѨF t i is equal to zero when the i-th thruster/actuator is at the middle of its range, and becomes infinity at either limits. Thus, hi varies from 1 to infinity if the i-th thrust goes from middle of the range to its limit. If the i-th thrust/torque approaches its limit, then hi becomes very large and the corresponding element in W Ϫ 1 F goes to zero and the i-th thruster/actuator avoids saturation. Depending upon whether the thruster/actuator is approaching toward or departing from its saturation limit, hi can be redefined as hi = 1 + ͉ ѨH(F i )/ѨF i ͉ when ⌬ ͉ ѨH(F i )/ѨF i ͉ ≥ 0 (i.e. the thruster/actuator force/torque is approaching toward its limit), and hi = 1 when ⌬ ͉ ѨH(F i )/ѨF i ͉ ≤ 0 (i.e. the thruster/ actuator force/torque is departing from its limit). Finally, the desired thruster/actuator force/torque vector, F td , that is guaranteed to stay within the saturation limit.
Substituting Eq. (22) into Eq. (21) and denoting e = q d Ϫ q, we obtain the following error equation in jointspace ត e +K 1 ë + K 2ė + K 3 e = 0 (27) Thus, for positive values of the gains, K 1 , K 2 and K 3 , the joint-space errors reduce to zero asymptotically, as time goes to infinity. Now, to incorporate both the fault and the saturation information in control Eq. (22), we define a fault-saturation matrix, ⌫ pϫ p = W Ϫ 1 F , having diagonal elements either 1/hi or zero. Whenever there is any thruster/actuator fault, we put that corresponding diagonal element in ⌫ matrix as zero. If there is no fault in any thruster/actuator, that corresponding diagonal entry of the fault-saturation matrix will be 1/hi . Thus, it accounts for the force/torque saturation limits along with the fault information. We can rewrite Eq. (22) in terms of fault-saturation matrix, ⌫, as

Drag minimization
Drag is a dissipative force that does not contribute to the desired motion. Actually, a UVMS will require a significant amount of energy to overcome the hydrodynamic drag.
Reduction of drag can also be useful from another perspective. The UVMS can experience a large reaction force because of the drag. High reaction force can saturate the controller and thus degrade the performance of the autonomous system. This problem is less severe when human operators are involved (as in case of ROV), because they can adjust their strength and coordination according to the situations. However, for an autonomous system, it is desirable to reduce such a large force especially when the force is detrimental to the task. Thus, we want to design a motion planning algorithm that generates trajectories in such a way that the UVMS not only reaches to its goal position and orientation from an initial position and orientation, but also minimizes the drag on the UVMS, while it follows the generated trajectories. Hence, we exploit the kinematic redundancy of the system not only to coordinate the motion of the UVMS but also to satisfy a secondary objective criterion that we believe will be useful in underwater applications. The secondary objective criterion that we choose to satisfy in this work is hydrodynamic drag optimization. Recalling Eq. (8) and Eq. (9), we can write the complete solution to the joint-space acceleration as: 30 where the null-space vectors (I Ϫ J + W 1 J ) 1 (for i = 1,2) will be utilized to minimize the drag effects on the UVMS.
We define a positive definite scalar potential function p(q,q), which is a quadratic function of drag forces as p(q,q) = D T (q,q) W D D(q,q) (31) where D(q,q) ᑬ (6+n) is the vector of drag forces and W D ᑬ (6+n)ϫ (6+n) is a positive definite weight matrix. Note that a proper choice of this W D matrix can enable us to design the influence of drag on individual components of the UVMS. Generally, W D is chosen to be a diagonal matrix so that the cross-coupling terms can be avoided. If it is chosen to be an identity, then the drag experienced on all dof of the combined system is equally weighted. However, by increasing or decreasing the values of the diagonal elements of the W D matrix, the corresponding drag contribution of each dof can be regulated. The potential function, p(q,q), captures the total hydrodynamic drag on the whole vehiclemanipulator system. Therefore, the minimization of this function will lead to the reduction of drag on the whole system. Now, taking the gradient of the potential function, p(q,q), we obtain We take the gradient ٌp(q,q) as the arbitrary vector, i , of Eq. (29) and Eq. (30) to minimize the hydrodynamic drag in the following form: where i are arbitrary positive quantities, and the negative sign implies minimization of the performance criteria. A detailed discussion on this drag minimization can be found in reference [22]. Thus, Eqs. (29) and (30) along with Eqs. (31)-(33) provide us with the reference joint-space trajectories considering the dynamics-based planning method as well as the drag minimization scheme. Now, we can obtain all the desired joint-space variables required for the control law (Eq. (22) or Eq. (28)) by integrating Eq. (29) and Eq. (30) and making use of Eq. (10). Then by differentiating it we can obtain the desired third derivative for the joint-space variables. Thus, we have formulated a unified motion planning algorithm by integrating the dynamics-based planning algorithm (Eq.

Thruster dynamics
The desired thruster force allocation as obtained from Equation (23) can be directly applied to the dynamic model of the UVMS given by Eq. (1) (using Eq. (15)) to generate the actual motion of the system. However, in such a case the dynamics of the thrusters will be neglected and the results will not accurately reflect the reality. Yoerger et al. 31 pointed 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 controller with limit cycle instability. There are several dynamic models of marine thrusters 31-33 that can reliably account for thruster dynamics.
In this work we use the model proposed by Healey et al. 32 that included a four-quadrant mapping of the lifts and drag forces of the propeller blades and was coupled with the motor and fluid system dynamics. This model is given by the following equations: where ⍀ and ⍀ r are the actual and the desired/reference propeller angular velocity, respectively, and i m is the motor current. The other parameters are: ␣ 2 = Ar 2 2 tan 2 (␥), where is the density of the water, r is the radius of the 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, K t is the motor torque constant, K f is the motor viscous friction constant, I is the motor and proportion inertia matrix, K f b is the motor feedback gain, and ᑤ is the propeller shaft torque. Neglecting the motor inductance, 32 the motor input voltage can be written as where V m is the motor input voltage, R m is the motor resistance and K emf is the motor back emf constant.
The propeller torque and the axial thrust are related to the blade lift, L and the drag, D as follows: where F t,act is the propeller shaft thrust, = ␥ Ϫ ␣, and ␣ is the angle of attack. A schematic diagram of the faulttolerant control system is given in Figure 2.

Trajectory
We have chosen a square path in xy (horizontal) plane for the computer simulation. We have assumed that each side of the square path is tracked in equal time. The geometric path and the task-space trajectories are given in Figure 3. The task-space trajectories can be represented as     where 'j' implies the coefficients for x, y or z; k is a constant and 2L is the time period. The Fourier coefficients are: a ox = Ϫ a 0y = k, a rx = Ϫ a ry = 4k/(r) 2 (cos r Ϫ 1) and b rx = Ϫ b ry = 8k/(r) 2 (sin r/2).
For this simulation, we have taken k = 1m, i.e. the path is 2m square, L = 5 and maximum frequency at which the Fourier series is truncated is r = p 1 = 30. Total time of simulation = 10sec. The frequency of the manipulator is 10 times higher than that of the vehicle. We have taken the natural frequency of the vehicle as 0.15 cycles per second and the manipulator to be 10 time faster than the vehicle. We have segmented the task-space trajectories as f j 1 (t) = a 0j + a 1j cos(t /L )+b 1j sin(t /L )

Results and discussion
We have performed extensive computer simulation to investigate the efficacy of the proposed Unified Dynamicsbased Motion Planning (UDMP) algorithm. To verify the effectiveness of the proposed method, we have compared the results of UDMP approach with that of Conventional Motion Planning (CMP) method. In conventional method, the trajectory is designed in three sections: the main section (intermediate section) that is a straight line is preceded and followed by two short parabolic sections. The UVMS used for these simulations consists of a 6 dof vehicle and a 3 dof planar manipulator working in the vertical plane. The vehicle is ellipsoidal in shape with length, width and height 2m, 1m and 1m, respectively. The mass of the vehicle is 1073kg. The links are cylindrical and each link is 1m long. 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 16kg, respectively. The simulation time is 10sec that is required to complete the square path. The total length of the path is 8m, thus the average speed is about 1.6knot. 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. It is to be noted that both the thrusters are located at the same bracket of the UVMS, which is one of the worst thruster fault situations. In our simulation, we have considered the following thruster/actuator thrust/torque saturation limits: ± 400N for horizontal thrusters (Thruster 1-4), ± 200N for vertical thrusters (Thruster 5-8), ± 200N.m for actuator 1, ± 100N.m for actuator 2 and ± 50N.m for actuator 3. The thruster configuration matrix for the simulated UVMS is as follows: where R t1 = 1.25m, R t2 = 1.75m, R t3 = 0.75m, and R t4 = 1.25m are the perpendicular distances from the center of the vehicle to the axes of the side and the front horizontal thrusters, and the side and the front vertical thrusters, respectively.
To make the simulation close to reality, we have introduced sensory noise in the measurements of positions and its derivatives. We have considered Gaussian noise of 1.0 mean and 1.0 standard deviation in the measurement of linear quantities (in mm unit), and 0.01 mean and 0.05 standard deviation in measurement of angular quantities (in deg unit). We have considered 10% modeling inaccuracy during computer simulation to reflect the uncertainties that are present in underwater environment.
We have presented results from the computer simulations in Figure 4 through Figure 8. The results we have provided here are from Case I: Partial decomposition of the proposed UDMP method. The task-space geometric paths are plotted in Figure 4, where we can see that the path tracking errors in our proposed UDMP method are much smaller as compared to that of CMP method. We have also plotted task-space trajectories in Figure 5. It is also observed from plots in Figure 5 that the end-effector tracks the task-space trajectories quite accurately in UDMP method. The errors are less in proposed UDMP method as compared to the CMP method. The joint-space trajectories are plotted in Figure 6. From these plots it is observed that the proposed UDMP method effectively reduces the motions of the heavy subsystem (the vehicle) and allows greater and sharper motions to the lighter subsystem (the manipulator) while tracking the same task-space trajectories. It is also noticed that the motion of the heavy subsystem is smoother in the proposed method. We find that these sharper and larger motions of the heavy subsystem in case of CMP method demand higher driving force that we see in Figure 7. From the plots in this figure (Figure 7) it is also observed that in case of UDMP method thrusters 4, 7, 8 and actuator 1 have reached the saturation limits, but they have not exceeded the limits. On the other hand, in case of CMP method all the thrusters and actuators have reached the saturation limits, however the saturation scheme was able to keep them to within the specified limits. Because of this, the path and trajectory tracking performance in CMP method has been degraded, as we can see in Figure 4 and Figure 5. Thus, the conventional planning method demands more powerful actuation system to track the same trajectories with reasonable accuracy. We also observe that the thrust 1 and thrust 5 are zero from 6sec as marked by "A" and "B", respectively (see Fig. 7). These imply they have developed faults at 6th second and remain non-functional for rest of the time. At this moment we observe some perturbations in trajectories and paths, however, the proposed UDMP scheme gradually brings the system to the desired directions and reduces the tracking errors. On the other hand, after the occurrence of faults the paths and the trajectories are tacked poorly in case of CMP method, because this algorithm cannot account for the dynamics of the system while generating the reference trajectories.
We have also plotted the simulation results for surgesway motion, power requirement and energy consumption of the UVMS in case of CMP method (in the left column) and that of in case of proposed UDMP method (in the right 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 changes the motion sharply and moves more as compared to the motion generated from the UDMP method. It may so happen that, in practice, this type of sharp and fast movements may be beyond the capability of the heavy dynamic subsystem and consequently large errors in trajectory tracking will occur. Additionally, this may cause saturation of the thrusters and the actuators resulting in degradation in performance. Moreover, the vehicle will experience large velocity and acceleration in CMP method that result in higher power requirement and energy consumption, as we observe it in next two sets of plots in Figure  8. Thus, this investigation reveals that our proposed Unified Dynamics-Based Motion Planning method is very promising for autonomous operation of dynamic system composed of several subsystems having variable dynamic responses.

CONCLUSIONS
We have proposed a new unified dynamics-based motion planning algorithm that can generate both kinematically admissible and dynamically feasible joint-space trajectories for systems composed of heterogeneous dynamics. We have then extended this algorithm for an autonomous underwater vehicle-manipulator system, where the dynamic response of the vehicle is much slower than that of the manipulator. We have also exploited the kinemetic redundancy to accommodate the thruster/actuator faults and saturation and also to minimize hydrodynamic drag. We have incorporated thruster dynamics when modeling the UVMS. Although, some researchers have exploited kinematic redundancy for optimizing various criteria, but those works have mainly been addressed to problems with land-based robotics or spacerobotics. Hardly any motion planning algorithm has been developed for autonomous underwater vehicle-manipulator system. In this research, work we have formulated a new unified motion planning algorithm for a heterogeneous underwater robotic system that has a vastly different dynamic bandwidth. The results from computer simulation demonstrate the effectiveness of the proposed method. It shows that the proposed algorithm not only improves the trajectory tracking performance but also significantly reduces the energy consumption and the power requirements for the operation of an autonomous UVMS. We have not presented results from Case II (Total decomposition) because of the length of the paper. However, these results are comparable to the conventional motion planning approach.
There are a few drawbacks of this paper as well. We used a model-based control technique to evaluate our planning algorithm. However, the underwater environment is uncertain and we need to use adaptive control techniques in future. Finally, the proposed scheme needs to be validated by experiments.