1 INTRODUCTION
The Unmanned Underwater Vehicles (UUVs) have many components that generally include mechanical, automatic controllers design and optimal path planning sections. Generating the optimal path is one of the main issues of UUVs and has attracted many researchers and scientists and lots of researches have been done on it. In this field, an algorithm for path planning based on fuzzy logic for Autonomous Underwater Vehicles (AUVs) in unknown three-dimensional space is provided by (^{Liu et al., 2012}). In this algorithm, the problem of three-dimensional path planning is decomposed to two independent planning problems in horizontal and vertical planes. In this way, avoiding collision with obstacles and finding the target in each plane takes place individually. In another study, in order to generate the UUV path in the horizontal plane, a new method based on quad tree and modified ant colony optimization algorithm, in which the horizontal plane is modeled using quad tree, was proposed by (^{Guang-Lei and He-Ming, 2012}). An autonomous method for path planning of a UUV of six degrees of freedom (DOF) was proposed by (^{Poppinga et al., 2011}) based on Rapidly-exploring Random Trees (RRT) and probabilistic roadmap method. Path planning for AUV in a dynamic environment by using fuzzy control-Particle Swarm Optimization (PSO) was provided by (^{Zhu et al., 2011}) in which the fuzzy controller was developed to create a collision-free path from the starting point to the target point. PSO was utilized to obtain the parameters of fuzzy controller. A multi-objective path planning was presented for vehicles by (^{Ahmed and Deb, 2013}), in which the length, safety and smoothness of the path were the objectives of the optimization. Furthermore, the non-dominant sorting method of elitist genetic algorithm was used for optimization. In another research, (^{Mashadi and Majidi, 2014}) have proposed the global optimal path planning of an autonomous vehicle for overtaking a moving obstacle, by performing a double lane-change maneuver after detecting it in a proper distance ahead. (^{Ali et al., 2015}) has proposed a method to prevent the underwater robots from colliding with obstacles. In this method, they used type-2 fuzzy and ontology-based semantic knowledge for simulation, identifying obstacles and avoiding collisions. They also presented a simulator software for the use of other researchers in order to prevent collisions with obstacles, which reduced the cost and lack of need for practical tests. An important issue that any UUV should benefit from in its way passing from starting point to target point is avoiding obstacles and also planning a short path to the target. In this study, this is considered as the goal in generating the path. Moreover, smoothness of the generated path is another feature considered in this method. Therefore, in order to generate a path from the starting point to the target point in 3D space, at first some points are selected in 3D space in a given interval, then the starting and target points are considered as the first and last points of the set of points on the path, respectively, and finally a curve is passed through these points by using the spline method. The position of these points should be selected in such a way that the final generated path does not collide with the obstacles in the environment. As in this case, the problem is converted to a constrained optimization problem.
In this study, in order to ensure accomplishing the answer, selection of spline control points is performed by using Imperialist Competitive Algorithm (ICA) (^{Atashpaz-Gargari and Lucas, 2007}). This algorithm, which was spired by the competition of colonizing countries on the other colonial countries, doesn't trap in local minima regarding its multiple search for the optimal answers, and due to this point it is used as a powerful tool for solving optimization problems in engineering problems (^{Hosseini-Moghari, 2015}; ^{Masoumi, 2015}).
After generating the optimal path, an appropriate controller is needed to guide the unmanned vehicle on the path. Selecting a suitable controller is of utmost importance, due to the remote control nature of unmanned vehicles (^{Zakeri et al., 2015}; ^{Zare et al., 2014}) or robots (^{Zakeri et al., 2012}; ^{Zakeri et al., 2013}; ^{Zakeri et al 2014}; ^{Moezi et al., 2014}). So far, researchers have presented different control methods for UUVs, including, control of a high-speed underwater robot using H_{∞} controller (^{Zhang et al., 2015}) or control of an underwater robot using dynamic sliding mode controller (^{Xu et al., 2015}). Among various controllers, the fuzzy controller, due to its nonlinear structure and also no need of the dynamic model of the system in its design, has been used a lot (^{Moezi et al., 2014}; ^{Moezi et al. 2016}) therefore, it has been used in much researches for controlling underwater vehicles, including, control of six DOF of an ROV or depth control of submarine robot using fuzzy controller (^{Ishaque, et al., 2010}; ^{Nag et al., 2013}).
According to mentioned points, in this research, the optimal Interval Type-2 Fuzzy Logic Controller (IT2FLC) is considered in order to guide the UUV in the optimal path, because it is more robust against uncertainties, compared to Type-1 Fuzzy Logic Controller (T1FLC), due to the existence of uncertainties in its Membership Functions (MFs) (^{Wu and Wan, 2006}). In addition, since this controller has more parameters, a better choice is achieved in optimization; in other words, it is more flexible (^{Fayek, 2014}). To optimize the parameters of this controller, Simplex Nelder-Mead optimization method has been used, due to its reasonable speed and accuracy (^{Nelder and Mead, 1965}). In addition, as far as we know, optimzal IT2FLC hasn't been applied on a UUV and not studied comparatively yet. This could be due to the fact that this controller is new; because using IT2FLC as a controller has been popular during the past decade (^{Mendel, 2009}). Therefore, utilizing this controller in this study can be a test of its performance on such systems.
According to what has been mentioned so far, the procedure of designing the optimal path and controlling the UUV on the generated path in this study is as follows: at first an optimal path without colliding with obstacles in 3D space is generated. Then, considering some conditions and in a process, the optimal path is converted to desirable signals of the underwater vehicle system. After that, again, these desirable signals are converted to desired inputs of the controller in a process. Finally, after converting the controller signals to the required forces, the vehicle is guided in the generated optimal path from the starting point to the target point. This process is shown in Fig 1.
In the following, modeling the UUV and designing of optimal IT2FLC using Nelder-Mead algorithm are performed in the second and third sections of the paper. In section four, the method of generating the optimal path without colliding the obstacles by using Spline and ICA is studied. A review of the simulations and comparing the results are presented in section five and finally the conclusions are proposed in section six.
2 MODELING THE 6-DOF UUV
In this section, in order to perform the modeling with more accuracy and closer to reality, an UUV called "mUUV-WJ-1" (^{Zakeri and Farahat, 2015}) is considered (Figure 2) and modeling is done regarding the physical parameters and its propulsion arrangement.
In order to simulate the UUV, at first the dynamical equations are obtained and then by using numerical methods in MATLAB/Simulink the UUV is simulated. To obtain the dynamical equations, robotic and dynamic methods, such as Euler-Lagrange method, are used (^{Craig, 1982}; ^{Baruh, 1999}). The coordinate system attached to the ground {E} (i.e., on the surface of the fluid (water)) is considered as the reference coordinate system and {B} attached to the UUV is considered as the body frame (which shows the position and state of the UUV). The origin of {B} is located in the center of the UUV volume (P^{E}_{B} = [x y z]^{T}), its longitudinal axis is along the length of the UUV, the transverse axis is across the vehicle width and the vertical axis is perpendicular to the upper surface of the UUV (Figure 3-a Figure 3-b). Three other frames, named as A_{1}, A_{2} and A_{3}, the state of which are shown in figures 3-a to 3-d, are defined to obtain the transfer matrix between coordinates {E} and {B}. The origins of {A_{3}} and {B} are the same, and The axes directions of {A_{3}} is the same as {E} directions. The origins of {A_{2}} and A_{3} are the same, and their Z axes coincide and rotate about Z axis by the value θ with respect to {A_{3}}. The origins of {A_{1}} and {A_{2}} are the same, and their Y axes coincide and rotate about Y axis by the value Φ with respect to {A_{2}}. The origins of {B} and {A_{1}} are the same, and their X axes coincide and rotate about X axis by the value ψ with respect to {A_{1}} (Refer to Figure 3-b toFigure 3-e).
At Figure 3-b, x, y, z , θ, ϕ and ψ show the position and orientation angles of the UUV. The UUV Center of Mass (CoM) is located at point P_{G} with respect to {B}.
In relation (1), x_{G}, y_{G} and z_{G} are the longitudinal, transverse and vertical components of P_{G} coordinate, respectively, in {B}. According to Figure 3, the relation between coordinate systems are obtained by means of matrix rotation and transfer rules (^{Craig, 1982}). Finally, R^{E}_{B} and T^{E}_{B} (3 × 3 rotation and transfer matrixes, respectively, of {B} relative to {E}) are obtained as relation (2):
The velocity of the origin of the {B} in the direction of the reference coordinate system is denoted by V_{E} and in the direction of itself is denoted by V_{B} (absolute velocity). Also, the velocity of the CoM of the UUV in the direction of the reference coordinate system is denoted by V_{G} (relation (3)). The angular velocity of the {B} in the direction of the reference coordinate system is denoted by Ω_{E} (relation (4)) and in the direction of itself is denoted by Ω_{B} (absolute angular velocity):
To obtain dynamical equations of the UUV, the Lagrange-Euler energy method is used. Denoting the total kinetic and potential energies of the system byE_{K} and E_{P} , respectively, they are obtained as relation (5):
In relation (5), g is the gravity acceleration, m is mass of the device, I_{G} is the matrix of moment of inertia around the center of mass and in the direction of {B} and h_{G} is the height position of the UUV Center of Mass (CoM) in {E}. The Lagrangian of the system is calculated as relation (6):
In the relation (6), L is the lagrangian of the system, q is a vector of generalized coordinate and Q is a vector of generalized forces, which are obtained by calculating the virtual works of the hydrodynamic and propulsive forces and torques. In the following, the procedure of obtaining the generalized forces (Q) is explained. External forces acting on the UUV, ignoring the gravity, include (^{Inzartsev, 2008}):
Forces due to propulsion (system input).
Hydrodynamic forces exerted on the vehicle.
Archimedean force due to the weight of the displaced fluid.
Since the propulsions installed on the UUV are fixed according to it, the directions of their forces are fixed according to the UUV. The locations and directions of the propulsion forces of the UUV, which are actually the input to the system, are presented in the relations (7) to (12) . Since the propulsions are considered in the form of water jet, they don't create any torque. The title and location of these propulsions are shown in Figure 3-a .
where, F_{T*}s and P_{T*}s are the forces produced by the propulsions and their installation positions on the vehicle, respectively. Therefore, the virtual work of the propulsions is calculated as relations (13) to (18):
In the above equations, (x) shows cross product operator, and δP^{E}_{B} and δA^{E}_{B} are the variation of the origin and angle of the {B} respect to {E}, respectively. Furthermore, δ_{W*} represent the resulting virtual works. The locations and directions of the hydrodynamic forces and torques are considered to be same as origin and coordinates of the B-frame. Hydrodynamic forces have many components, of which two important ones include the force of the added mass and the drag force (including force and torque of drag and friction of fluid on the body) as equation (19) .
In Equ. (19), T_{H} is the total hydrodynamic force, T_{R} is the hydrodynamic force of the added mass and T_{D} is the hydrodynamic drag force. The hydrodynamic force of the added mass is as follows (^{Inzartsev, 2008}):
Considering the symmetrical shape of the vehicle, M_{a} , C_{a} and D can be obtained as equation (21) and (22) (^{Inzartsev, 2008}):
The m_{a**}s are the values that depend on the vehicle shape. u_{B} is a vector of linear and angular velocities, presented as equation (23):
Hydrodynamic force, resulted from the friction of the body, fluid and drag, is as equation (24):
Considering symmetrical shape of the vehicle, D(u_{b}) is obtained as relation (25) (^{Inzartsev, 2008}):
Where Diag is a diagonal matrix function, therefore D(u_{B}) is a diagonal matrix. D_{**}s are friction and drag coefficients, which depend on the material and body shape. Virtual work of hydrodynamic force is as equation (26):
where, δP^{E}_{B} and δA^{E}_{B} are the variations of position and angles of reference coordinate B with respect to reference coordinate E, respectively. Archimedean force applied on the device is calculated as equation (27):
In equation (27), ρ_{W} is the water density and V_{w} is the UUV volume. The resulting virtual work is obtained as equation (28):
Consequently, the total virtual work is obtained as equation (29) (^{Baruh, 1999}):
Regarding the equation (29), it is obvious that the total virtual work (δw_{net}) is equal to the sum of the variation of generalized variables (δq_{i}) multiplied by generalized forces (Q_{i}). Therefore, after calculating the total virtual work, the generalized forces are easily calculated with factorization. According to what was presented for derivingQs and solving the Lagrange equation in relation (6), six dynamical equations are obtained for UUV. By sorting these equations, the matrix form of relation (30) is introduced for UUV.
where, M is the 6×6 inertia matrix, C is the 6×1 coriolis vector, G is the 6×1 gravity vector, H is the 6×6 propulsions arrangement matrix and U is the 6×1 input vector, which is as relation (31):
In this study, Equ. (30) is utilized in the simulation of the UUV.
3 DESIGN OF THE OPTIMAL INTERVAL TYPE-2 FUZZY LOGIC CONTROLLER FOR THE UUV
Fuzzy logic is one of the areas being discussed in mathematics, and has various applications in different fields, including humanities, medicine, management and engineering; however, it has a wide usage in the field of modeling and automatic control (^{Zimmermann, 1996}). In the last decade, the type-2 fuzzy controller has been used widely, due to its more compatibility with systems having uncertainties (^{Karnik and Mendel, 1998}). In type-2 fuzzy logic, the MFs are fuzzy sets, which are usually intended as fuzzy interval for simplicity and their borders are specified by the lower and upper limits of fuzzy functions. In addition, due to the type of MFs and its more parameters than type-1 fuzzy controller (i.e., the flexibility), its optimization results are more favorable and it achieves a much lower cost. The mentioned features make this controller suitable to be applied on UUV. Therefore, in this study, at first by using necessary transformations, the appropriate and compatible inputs and outputs of the controller are provided and then the controller is designed. Since the design procedure of IT2FLC and FLC controllers are the same and they differ only in their internal structures (^{Liang, and Mendel; 2000}), the design of type-1 fuzzy controller is introduced first and then the interval type-2 fuzzy controller is presented. The optimization procedure by using Nelder-Mead will be explained in the following.
3.1 Changing the System States for Applying the Controller to the UUV
In this study, in order to apply the specified controller on the UUV, the system inputs and outputs, which are the state variables and driving forces, are converted by using two appropriate conversion functions in a way such that the UVV state be controllable by the controllers individually; because the inputs and outputs of the UUV system depend UUV state variables. As a result, their dependence should be eliminated or reduced in such a way that they act as a 6 Single-Input/Single-Output (SISO) system, and each of these systems be controlled. The UUV system has 6 DOF, which has 6 inputs and 6 outputs (taking into account the generalized variables and their velocities, q and , it is 12 state variables). These outputs of the system state are converted to appropriate controller input for the controller, which are shown by OR and OT, by applying the following changes:
In the foregoing equation, R^{B}_{d} is obtained by using kinematic relationship and relations (32). In this equations, q_{d} is the vector of desired values, is a unit vector that if {B} rotates by the angle β around that it, it matches the desired orientation coordinate (^{Craig, 1982}). O_{T} and O_{R} are proportional to the error of position and rotation angles, respectively, of the UUV according to desired values and in the direction of the {B}. In other words, if the vectors O_{T} and O_{R} tend to zero, the system states tends to the desired states (the relationship (36)).
Also, another conversion is needed on the vehicle system inputs, for eliminating the dependency of UUV from inputs. As a result, a conversion function is introduced as relation (37):
In equation (37), M_{r} is considered in order to avoid the interaction of the forces and torques and is defined as follows:
Furthermore, T_{HU} is proportional to the forces and torques applied to the vehicle along and about the axes of the coordinate system attached to the vehicle ({B}). As a result, applying the proposed conversion functions causes the system to behave as follows:
Increasing and decreasing the components , and cause the positive and negative forces in the longitudinal and transverse directions and normal to the coordinate system connected to the vehicle, respectively. Moreover, O_{TxB}, O_{TyB} and O_{TzB} are proportional to the position error regarding the desired values in the direction of longitudinal, transverse and normal to the coordinate system connected to the vehicle, respectively (Figure 4-a to Figure 4-c).
Increasing and decreasing the components , and cause the positive and negative torques about the longitudinal and transverse directions and normal to the coordinate system connected to the vehicle, respectively. Furthermore, O_{RxB}, O_{RyB,} and O_{RzB}, are proportional to the rotation error of the device regarding the desired values in the direction of longitudinal, transverse and normal to the coordinate system connected to the vehicle, respectively (Figure 4-d to Figure 4-f).
Now, according to mentioned points, by applying the six controllers in the form shown in Figure 5, the UUV is controllable at every desired state.
3.2 Type-1 Fuzzy Logic Controller
The T1FLC is categorized as the set of controllers that don't need system dynamical model in their design and only general information about system behavior regarding different inputs will suffice. The design of T1FLC has the following steps:
Determining the type and number of controller inputs: As mentioned, in this study, the input of the fuzzy controller is considered a kind of signal proportional to the error of system state (o) and its rate of change versus time (ȯ). Therefore, the fuzzy controller used in this study has two real inputs.
Determining the number, type and parameters of MFs for each T1FLC input: In the design of T1FLC, 5 Gaussian and 2 Sigmoid MF are intended for each input, which are named for the very small and very big inputs, respectively, as follows:
The reasons for selecting this form of MF are simplicity, having few parameters and most important the continuity and smoothness of these functions. , and , are adopted from Sigmoid functions and to and to are adopted from Gaussian functions (the relationship (40)). Schematics of these MFs for different inputs are shown in Figure 6.
where, a, b, c and d are variables of Gaussian or Sigmoid MFs, the values of which are then determined by using optimization.
Determining the fuzzy rules, the type and number of controller output: Fuzzy rules are determined regarding the behavior of the system, i.e. how different inputs affect the output of the system (Figure 4). Thus, according to Figure 4 , and TSK zero order definition (^{Moezi et al., 2016}), the output of fuzzy controller t_{TH} is calculated as relation (41):
In the foregoing fuzzy rule, Ã and are fuzzy linguistics for inputs and T is a value of the consequences part of the fuzzy rule. Algebraic multiplication is used for logical operator "and" . The grade of any rule is shown by v (a value between 0 and 1) and is calculated by relation (42).
The corresponding fuzzy rules of the relation (42) are presented in Table 1.
3.3 Interval Type-2 Fuzzy Logic Controller
The design steps of IT2FLC are the same as type-1, and even their laws are similar; but they differ in terms of calculation and internal structure. The structure of the IT2FLC is such that after fuzzification of controller inputs with the aid of MFs (which are themselves fuzzy interval), the fuzzy rules are applied to them and they return some membership values for every law; as a result, these values are fuzzy interval. Since the output of this controller is itself fuzzy type-2, it is necessary to reduce the fuzzy type using a method such that the controller output can be obtained in order to be applied to the system by a defuzzification method (^{Karnik and Mendel,1998}). The mechanism of the IT2FLC inference is depicted in Figure 7.
The MFS of IT2FLC have upper (UMF) and lower (LMF) bonds, i.e. the MFs itself is a fuzzy interval. The area between UMF and LMF is called Footprint of Uncertainty (FOU) (refer to Figure 8-a) (^{Wu and Wan, 2006}). The same as MFs in the inputs, the MFs in the output are of interval type-2 fuzzy, the centroids of which are fuzzy interval in the range [T_{l} T_{r}]. If the MFs is symmetric, T_{r} and T_{l} are located with a distance α away from either sides of the center of symmetry (T_{m}) (refer to Figure 8-b).
In this study, the rules of IT2FLC are considered the same as T1FLC type-1, which was explained in the previous section. MFs are also considered the same as MFs type-1, but in the form of fuzzy interval (the relation. (43)). These functions are shown in Figure 8-c.
The grade of each rule is obtained using the relation (44). This is depicted in Figure 8-d.
As mentioned, the output of IT2FLC is a type-2 fuzzy interval; therefore, it requires to be reduced to type-1. In this study, in order to reduce type-2 to type-1, the method of center of sets is utilized (^{Liang and Mendel; 2000}). In this method, U_{r} and U_{l}, which are the right and left side of the range of the reduced type-1 fuzzy interval, are calculated as relation (45):
Since v is an interval and not a real number, the calculation of the relation (45) by the conventional method of multiplying the ranges is very time consuming; therefore, the Karnik and Mendel (KM) Algorithm is used for calculating U_{l} and U_{r}. The calculation procedure using this algorithm has been explained in reference (^{Karnik and Mendel, 1998}). After reducing the type and calculating U_{l} and U_{r}, fuzzy interval is converted to crisp output, which is done by the relation (46) (^{Karnik and Mendel, 1998}):
3.4 Optimization of Designed Interval Type-2 Fuzzy Logic Controller
The controller parameters are determined by various methods, including manual tuning or optimizing by the use of population-based or Simplex methods (^{Moezi et al., 2014}; Moezi et al. 2016; Tajjudin, 2001). Optimization by using population-based methods is difficult and requires lots of time to execute, due to the large number of optimization variables and the time consumed by each function evaluation. Optimization using simplex methods is done more quickly, but they require an initial point to start. According to what was mentioned, in this study, at first the controller parameters are adjusted manually using some simulation tests. Then these values are considered as an initial point for Simplex Nelder-Mead method (^{Nelder and Mead, 1965}), and optimization is performed until the optimum answer is obtained (Figure 9).
In order to simplify and reduce the number of optimization variables for fuzzes controllers, some assumptions are made and listed as follows:
For both T1FLC and IT2FLC controllers, all the parameters of the MFs, namely as, bs, ds and cs, are considered the same for every six fuzzy controller.
In the IT2FLC controller, BG (which is the FOU ratio of LMF and UMF), considered equal to upper boundary and this coefficient is the same for all MFs and every six controllers (relationship (47)). Moreover, the coefficient α of the output MFs (Figure 8-b) is considered the same in all the output MFs and every six controllers.
In addition to the mentioned assumptions, for maintaining the fuzzy rules and also the symmetries in the UUV system, some interfaces, as constraints, are considered for the parameters of the MFs and also the consequences part of fuzzy rules (which are designed for type-1 and type-2 fuzzy controllers) and are presented in relationship (47). Moreover, the ranges for determining the position of MFs in input and output is normally considered between -1 to 1 and instead, the input and output signals are amplified by gains. This will make ease the optimization process and selection the optimization variables ranges (Figure 10).
Regarding the assumptions mentioned above, relationships (47) and also Figure 10, the number of optimization variables for fuzzy controllers are determined 29 for type-1 and 31 for type-2. The objective function for optimizing the controller is weighted quadratic integral function including the input and errors of the UUV (^{Bolonkin and Sierakowski, 2003}), which is defined as relarion (48). The existence of quadratic terms of forces applied to the system, in addition to the UUV state error, in the cost function, reduces the applied input forces to the system as well as the system state errors, which is very important for controlling systems; because applying less force leads to selection of smaller actuator with less cost which causes saving energy.
In equation (48), e_{t} and U are 6×1 vectors of errors of states variables (e_{t} = q_{d} - q) and input forces of UUV, respectively, P and Q are positive and constant 6×6 weight matrices and T_{s} is the simulation time period of applying the controller on the UUV.
For the optimal controller to be able to track signals of different frequencies, desired signals in simulation should be obtained by summing several sinusoidal signals with different amplitudes and frequencies and also steps with different sizes.
3.4 Simplex Nelder Mead Algorithm
Nelder-Mead (N-M) algorithm, which was proposed by (^{Nelder and Mead, 1965}), is a searching method for multi-dimensional and unconstrained optimization problems. A simplex is a convex n-dimensional space (convex hull) of n + 1 points (like the triangle in two-dimensional space). To create a simplex, at first a point like P_{0} is chosen randomly in the search space and the n remaining points are selected using the relation (49):
In relation (49), e_{i} is a unit vector based on the n-dimensional search space and λ_{i} is a small constant number. The main idea of this method is to select the worst and best vertices of the simplex with the highest amount of the cost function and replace it with another point with the best function (with highest value) in each iteration. Thus, the created simplex moves from the worst point to the best one. One iteration in this method is as follows:
1. Each step of this method starts with sorting n + 1 vertices of the simplex in order to satisfy the condition of Equ. (50):
In Equ. (50), x_{1} and x_{n + 1} are the best and the worst points, respectively.
3. Regarding the relative value of f_{r}, obtained from the Equ. (53) , one of the following 4 modes may occur:
If the condition "a" is satisfied, the expansion point x_{e} is calculated using the relationship (55) in the same direction.
Finally, after calculating the expansion point, its value is obtained as f_{e} = f(x_{e}). However, if f_{e} < f_{r}, x_{n + 1} is replaced with x_{e} ; otherwise, x_{n + 1} is replaced with x_{r}.
If the condition "b" is satisfied, x_{n + 1} is replaced with x_{r}.
If the condition "c" is satisfied, the outside contraction is performed and f_{c} is calculated using the relation (56):
However, if f_{c} < f_{r}, x_{n + 1} is replaced with x_{c} and iteration ends; otherwise, it goes to step 4 and shrinking takes place.
If the condition "d" is satisfied, the inside contraction is performed and f_{c} is calculated using the relation (57):
However, if f_{cc} < f_{n+1}, x_{n+1} is replaced with x_{cc}; otherwise, it goes to step 4 and shrinking takes place.
Shrinking: The dividing action means that the search operation around x_{n+1} is useless and simplex should be crushed (Shrink) and go to near x_{1}. Now, the new V_{i} points are calculated as relation (58):
Then f is calculated at all n new points. Simplex unsorted vertices are x_{1}, V_{2},..., V_{n+1} at the next iteration.
4 GENERATING THE OPTIMAL PATH WITHOUT COLLISION USING SPLINE-ICA
In this section, a short path without collision is intended as the aim of generating the optimal path. Moreover, smoothness of the created path is another feature observed in this method. Therefore, to generate the path from the starting point P_{s} to the target point P_{g} in three-dimensional space, at first some points in the desired space and in the specified range is selected, which are known as control points. Furthermore, the starting and target points are considered as the first and last points of the path in the points set. Then, by using spline method, a curve is passed on these points (^{De Boor, 1978}). Determining the location of these points by the aid of ICA leads to a smooth and short path without colliding obstacles. Any point in the surrounding environment of the UUV that is owned to the obstacle, takes a numerical value. This value is used in the optimization in order to avoid collisions with obstacles (it is added to the objective function as penalty). Therefore, the objective function is defined in such a way that the resulting path has the shortest length and avoids colliding the obstacles.
4.1 Generating the Path by Using Spline
As mentioned, the locations of the control points are selected in such a way that a short path without collision is achieved. As a result, for selecting these points evolutionary optimization methods are used, which are suitable options. If n control points are considered to generate a path by spline method, the desired path is generated as relation (59) to (61) (^{Zakeri and Farahat, 2015} ; ^{Moezi et al., 2015}):
In relation (59), P_{n}s are control points of spline curve (^{De Boor, 1978}), (f_{x}, f_{y}, f_{z})s are path points that change from the starting point to the target point by changing k from k_{s} to k_{g}, i.e. they create the path. SPL is a function that passes a spline curve through the input points, and returns the desired point (^{Moezi et al., 2015}). The performance of this function is shown in Figure 11.
4.2 Determining the Control Points of the 3d Path by Using Optimization
In order to optimize any problem, the objective function and the optimization variables should be identified. In this problem, the optimization variables are control points , , . The objective function in this problem is specified such that its reduction means reduction of the path length and obstacle avoidance. Therefore, the function representing collision with obstacles in three-dimensional space is defined by the name OF which is a function of position.
The created path from the starting point to the target point is converted to nop discrete points as a set of points. As a result, the LP value, which indicates the path length, is obtained by summing the distances between any two consecutive points of the path. It is obvious that the more the number of points, the more accurate the path length is calculated. As a result, the path length is numerically calculated as relation (63), which is the goal and considered as an optimization cost function:
Lnt(j) indicates the distance between j th and j - 1th points, which is obtained as follows:
where,
As previously mentioned, OF represents the collision value and if collision happens, the cost function is provided with penalty. Therefore, the penalty function of collision is considered as relation (66).
Therefore, the cost function with the function of collision penalty is considered as relation (67):
By reducing the cost amount of relation (67), a short path without colliding with obstacles in the environment is achieved.
4.3 Creating Desired Signals from Optimal Generated Path
After the optimal path and the corresponding parameters are obtained, desired signals should be produced to control the vehicle. For this purpose, it is assumed that the UUV travels the path with a constant velocity V_{d}, and also the X-Y plane of {B} is parallel to the water level and the longitudinal direction of vehicle is tangent to the path projected on the X, Y plane of the {E}. Therefore, considering a constant velocity on the path, the relation (68) is obtained (^{Zakeri and Farahat, 2015} ; ^{Moezi et al., 2015}).
Also, According to mentioned condition, the desired signals for the UUV are obtained as below:
Where According to relation (86), k(t) is achieved as follows:
4.4 Imperialist Competitive Algorithm
Imperialist Competitive Algorithm (ICA) is originally inspired by the procedure of imprialist coutries in obtaining power and as a result the optimization method is relatively strong (^{Atashpaz and Lucas, 2007}). In this algorithm, all countries are divided into two categories of imprialist and colony countries. The imprialistic competition is the main part of this algorithm which leads to the convergence of the objective function toward the optimal point. In ICA, each of the population vectors is originally called a country and is considered as follows:
where, N_{var} is the number of variables or the number of dimension of the optimization problem. At the beginning of optimization process, N_{pop} countries are scattered randomly in optimization space and N_{imp} of the most powerful countries are considered as the empires. In order to divide and assign colonial countries to empires, the colonists cost values (cost and inverse of power are proportional with each other) should be normalized as relation (72):
where, c_{n} is the ith colonial cost and C_{n} is its normalized value. As a result, the normalized power of nth colonial is as relation (73):
From another point of view, the normalized power of each colonial specifies its contribution in colonizing the colony countries. As a result, the initial colonies of nth empire is as relation (74).
In the above equation, N.C._{n} is the initial number of colonies of each empire and N_{col} is the total colonial countries. As a result, this number of colonies are randomly selected for each empire. Then colonial countries move toward their colonizing countries. Any country moves toward its colonizing country by the distance x and the angle θ, which are random values (Equ. (75)).
where, β is a value greater than one, d is the distance between the colonial country to its colonizing country, γ is a positive and arbitrary variable and is usually considered as π/4. After moving to the toward the colonizing country, if the power of colonial country becomes more than the colonizing country, they are changed. The total cost of the n th empire is calculated as follows:
Where, ζ is a positive value smaller than one. This algorithm is such that at each step the weakest colonial country of the weakest colonizing empire is separated from it and joins to one of the rest of the empires regarding a probability proportional to that empire power. In order to start the competition between empires, at first the probability of each of them to accept a new colony is calculated. The normalized value of the total cost of the nth empire (N.T.C._{n}) is calculated as relation (79). As a result, the probability of ownership of any empire is calculated as relation (78):
where, is the probability of ownership of the nth empire. To select the host empire, the vector P is built from the ownership probability of the empires as relation (79):
Then another vector R, the length of which is the same as vector P, is built. The components of this vector are random values between zero and one, with a uniform probability distribution (relation (80)).
Then, the vector D is obtained by subtracting vector R from vector P, as relation (81):
The component number of vector D that has the maximum value is the number of host empire.
In the end, all of the empires, except the strongest one, fail and all the countries are dominated by the most powerful empire in one place with equal power. In principle, all converge to a point which is considered to be the end of the optimization process.
5 SIMULATION AND RESULTS
In this section, simulation and comparison of the generated optimal paths and the controllers applied on the UUV are investigated. All simulations, including dynamical model of the UUV, applying different controllers and generating optimized paths, are done in MATLAB/Simulink r2013b. Generating the optimal path is performed for three different environments and then the UUV is guided on these paths. In order to optimize the controllers and simulate the UUV on the generated optimal and smooth paths, the hydrodynamic coefficients are determined using a simplified shape of "mUUV-WJ-1" in CFD-COMSOL 5.1 (Figure 12), which are presented in Table 2 . The physical parameters of this UUV are given in Table 3 (^{Zakeri and Farahat, 2015}).
Added mass coefficient | Damping and friction coefficients | ||
---|---|---|---|
m_{aFX} | 1.49 kg | [D_{FX|u|} D_{FX}] | [5.67 0.0117] |
m_{aFY} | 1.23 kg | [D_{FY|u|} D_{FY}] | [3.62 0.0102] |
m_{aFZ} | 1.98 kg | [D_{FZ|u|} D_{MZ}] | [7.35 0.00981] |
m_{aMX} | 0.011 kg.m^{2} | [D_{MX|u|} D_{MX}] | [0.0155 0.000191] |
m_{aMY} | 0.0165 kg.m^{2} | D_{MY|u|} D_{MY}] | [0.0183 0.000211] |
m_{aMZ} | 0.0086 kg.m^{2} | [D_{MZ|u|} D_{MZ}] | [0.0126 0.000311] |
In order to better compare the results obtained in the simulations, the optimal parallel PID controller (^{Moezi et al., 2016}), as well as T1FLC and IT2FLC, is also designed, optimized and applied to the UUV. The optimization results of PID, T1FLC and IT2FLC controllers by using Nelder Mead algorithm for 1000 function evaluations are shown in Figure 13 . According to this figure, the initial values of the cost of T1FLC and IT2FLC controllers are less than PID optimum value. The reason is the flexibility and more control parameters of these controllers. For the same reason, the optimum cost value of IT2FLC is less than T1FLC. The exact value of cost function before and after optimization is presented in Table 4 . According to this table, the J values for T1FLC and IT2FLC controllers are improved 68% and 70%, respectively, compared to PID controller.
Controller Type | PID | T1FLC | IT2FLC | |||
---|---|---|---|---|---|---|
Condition | Initial | Optimal | Initial | Optimal | Initial | Optimal |
Value of J index | 6.1819 | 2.7495 | 0.9712 | 0.8685 | 0.9694 | 0.8208 |
In this section, in order to simulate and optimize the 3D short path without collisions with obstacles and also to control the UUV on the path, three environments with various complex conditions and different obstacles are intended. The first environment is the one without any obstacle. In the second environment, between the starting point and the target point there are two cylinders in the normal direction that have radii equivalent to 1.5m. The location of the center of these cylindrical obstacles are located in (3, 1, z) and (7.5, -0.5, z). The third environment has a cylindrical wall with the center located at (3, -8.5, 4.75), a thickness of 1.5 mm, a radius of 10 meters and is located along the direction of y-axis in between the starting and target points. In the center of this cylinder there is a hole with a radius of 0.5m. Therefore, the function of collision with obstacle in these environments is defined as equation (82) to (84) (in calculating the size of the obstacles, the vehicle's radius is considered):
The starting and target points in each three paths are defined as relation (85):
For all paths, it is assumed that the vehicle moves with a constant speed of V_{d} = 0.2 m/s on the path. Also, n , which is the number of control points in the spline, is considered 5 for all three environments. Therefore, the optimization problem includes 15 optimization variables.
In addition to the optimization by using ICA, the other two methods (i.e., the Particle Swarm Optimization (PSO) (^{Kennedy and Eberhart, 1995}) and Artificial Bee Colony (ABC) (^{Guo, 2011}) that are other widely used algorithms in numerical optimization) are also used to compare the results with each other. The maximum optimization number of iterations for all three algorithms is considered 1000 and each method is executed 25 times for each environment. The values of the optimization algorithms parameters are described in Table 5 . These values are determined by optimizing various environments using several sets of different parameters of optimization methods (^{Moezi et al., 2015}; ^{Moezi et al., 2015}).
Figure 14 shows the graphs of convergence to the optimal solution for the best state of each algorithm during optimization cycles for each three environments. According to these graphs, it is clear that the rate of convergence to the optimal solution by ICA, is more than the other two methods. All the three methods converge to the solution at most in approximately 100, 150 and 250 iterations for the first, second and third environments, respectively.
In order to numerically compare the optimization methods applied to generate the optimal path for each three environments, the best and mean answer and also Practical Reliability Factor (PR) (^{Moezi et al., 2016}) of each of these algorithms are obtained and listed in Table 6 .PR indicates the probability value dof achieving the practical optimum answer. In this study, the practical optimal solution is defined as 10% of the global optimum solution. Furthermore, the global optimum answer is the best answer ever achieved by each of the three algorithms for each problem.
Env. No. | PSO | ABC | ICA | |||||||
---|---|---|---|---|---|---|---|---|---|---|
min | mean | PR | min | mean | PR | min | mean | PR | ||
1 | 9.501 | 9.603 | 0.92 | 9.503 | 9.523 | 0.96 | 9.500 | 9.502 | 1 | |
Cost (m) | 2 | 10.147 | 11.331 | 0.8 | 10.191 | 11.342 | 0.88 | 10.078 | 11.101 | 0.96 |
3 | 11.243 | 13.211 | 0.84 | 11.265 | 13.321 | 0.84 | 11.239 | 11.924 | 0.96 |
According to Table 6, it is clear that the minimum value of the cost for ICA for each three environments is less than PSO and ABC. This is also true for the average cost; such that the average cost of ICA is less than PSO and ABC by 1% and 0.2%, respectively, in the first environment, 2.1% and 2.9%, respectively, in the second environment and 10.8% and 11.6%, respectively, in the third environment. This comparison shows that in more complex environments ICA has a better optimization value than the other two algorithms. Furthermore, the larger size of PR for ICA, comparing with PSO and ABC in all three environments, shows that ICA achieves the optimum solution with more reliability. Since the cost of ICA in all three environments is less than PSO and ABC, the values obtained by this algorithm are used for generating optimal paths in all three environments.
It is obvious that the shortest path in the obstacle-free environment is the line connecting the starting point to the target point. As shown in Figure 15-a, the path generated for the first environment is almost the line connecting the starting point to the target point and this confirms the efficiency of the proposed method in generating the optimal path. Besides the optimal path generated in all three environments, the path that the vehicle traveled by each of the three controllers to reach the target point is shown in Figure 15 . In addition, the desired signals generated from the optimal path and tracked by vehicle by applying the controllers are shown in Figure 16 to Figure 20 . The forces exerted to the UUV by each of the three controllers in all the three environments are depicted in Figure 17 to Figure 21, respectively. In order to compare and evaluate the quantitative results obtained in all three environments, the value of quadratic index (J-index) for error and force applied to the system are presented in Table 7 .
Controller Type | PID | FLC | IT2-FLC | ||||||
---|---|---|---|---|---|---|---|---|---|
Env. No. | 1 | 2 | 3 | 1 | 2 | 3 | 1 | 2 | 3 |
J-index | 0.14 | 0.514 | 0.612 | 0.009 | 0.331 | 0.452 | 0.007 | 0.205 | 0.331 |
The generated paths in Figure 15 show the efficiency of the method of generating the optimal spline path by using the proposed computational algorithms. Furthermore, tracking the desired signals produced from the optimal path, which finally leads to the path tracking, approves the correctness of the method proposed for generating desired signals from the optimal path, as well as the appropriate performance of the three controllers and the method applied, especially for the IT2FLC. Regarding Figure 16, Figure 18 and Figure 20, on average, control signals are settled almost after 2s. According to Figure 17, Figure 19 and Figure 21, which show the forces applied in all environments by all three controllers, the forces applied by IT2FLC are less than T1FLC and the ones applied by T1FLC are less than PID, which causes a decrease of energy consumption. The lower values of applied forces also indicate the capability of the control methods to be applied on the real systems of underwater robots. Numerical results of quadratic objective function (J) for all three controllers in all three environments, which are presented in Table 7, confirm the superiority of the IT2FLC; such that in all the environments it has less value than the T1FLC and PID. Comparing to PID controller, the J-Index decreases for T1FLC and IT2FLC about 35% and 50%, respectively, for the first environment, 35.5% and 60%, respectively, for the second environment, and 26% and 46%, respectively, for the third environment.
6 CONCLUSION
In this research, the generation of a short and smooth path in three-dimensional space with obstacles without collision of the UUV with them by using Spline and ICA was introduced. In order to guide the UUV through the generated optimal path, the optimal IT2FLC was designed by Nelder-Mead and was applied on the UUV. An UUV, entitled "mUUV-WJ-1" , was modeled to be used in the simulations.
The results of the simulations for environments with presumed obstacles showed that in generating the optimal path, ICA has a better performance than PSO and ABC; such that its least reduction in the cost was 0.2% in the first environment compared to ABC and most reduction in the cost was 11.6% in the second environment compared to the ABC. Moreover, the high value ofPR in all three environments ensure that ICA achieves the optimal solution. The results of the control part of UUV showed that the optimal IT2FLC had a good performance; such that its J-index was improved even up to 60% compared to the PID in the second environment. Furthermore, applying small forces of the propulsions, as well as the high accuracy of control, shows that the selected objective function (i.e., the integral of sum of weighted square of input and output error of the UUV system) for optimization was a right choice. Applying small forces by the controller reduces the energy consumption and consequently reduces the cost as well. It is worth noting that finding a short path for guiding the UUV from the starting point to the end saves energy and time and consequently reduces the cost as well. Finally, it is concluded that the method proposed in this study is an efficient and convenient one in order to guide the UUV without collision in environments with different and complicated obstacles.