Path Tracking of an Underwater Snake Robot and Locomotion Efficiency Optimization Based on Improved Pigeon-Inspired Algorithm

Abstract

This paper considers the tracking control of curved paths for an underwater snake robot, and investigates the methods used to improve energy efficiency. Combined with the path-planning method based on PCSI (parametric cubic-spline interpolation), an improved LOS (light of sight) method is proposed to design the controller and guide the robot to move along the desired path. The evaluation of the energy efficiency of robot locomotion is discussed. In particular, a pigeon-inspired optimization algorithm improved by quantum rules (QPIO) is proposed for dynamically selecting the gait parameters that maximize energy efficiency. Simulation results show that the proposed controller enables the robot to accurately follow the curved path and that the QPIO algorithm is effective in improving robot energy efficiency.

1. Introduction

Underwater robots receive more attention with the increasing demand for ocean exploration. Underwater snake robots, a type of bionic robot, can perform efficiently in restricted locations due to their physical advantages, and academic research on them has steadily increased over the last decade. Snake robots above ground have been able to conduct complex maneuvers such as climbing, and their theoretical approaches and prototype investigations are relatively advanced [1,2]. Although most ground robots use guide wheels to achieve three-dimensional motion [3,4,5], underwater robots are still trapped in planar motion research. There is some research that applies the control mechanisms used in ground robots to underwater snake robots with some success [6,7]. As the technology continues to evolve, underwater snake robots will be applied to more work scenarios. In addition, underwater snake robots need more robust control methods and efficiency optimization methods to deal with the unknown environment.

The snake robot’s mechanical structure is more complex, with a certain number of connecting links. Due to the coupling relationship, its dynamics model is complex and has characteristics such as highly nonlinear and underdriven features. There are many methods to control the robot’s movement, including: (1) deriving control laws from differential equations of kinematics and dynamics [8]; (2) using central pattern generators (CPG) to directly generate control signals as control outputs [9,10]; and (3) conforming the robot pose to a Serpenoid curve to mimic snake motion. Among them, the Serpenoid curve-based approach is widely adopted, because it is simple to implement and tune the parameters. Many studies have designed controllers based on this strategy [11,12]. Recently, [6] proposed a simplified model that converts the angle between links into a relative pitch. The authors explored the robot characteristics by reducing the system complexity without affecting the system motion characteristics. In [13], a path-tracking control method with adaptive parameters is theoretically designed based on this model, and a stability proof is given.

In the research of path-tracking control, LOS (light of sight) methods have been used for robot heading controls [14,15], and there are articles that mention the use of integral LOS to calculate the reference heading angle [16]. However, most of these methods focus only on the control effect on a straight path. Path planning is the preorder task of path tracking, which provides the steering strategy of the robot through the waypoints, and offline path-planning methods can provide path functions and their parameters used in path-tracking heading control. A Dubins path method was proposed for planning a straight route between two waypoints, but the robot chooses a circular route to turn near the waypoint, rather than directly passing it [17]. A Bezier curve capable of generating smooth curves is given, but this method is too computationally intensive for robotic systems [18]. The theory of CHI (cubic Hermite interpolation) and PCSI (parametric cubic-spline interpolation) has been studied deeply in path planning [19,20], and the smooth path properties are well suited for use in snake robot path planning.

The snake robot moves forward by the swing of the links, and the choice of gait parameters is directly related to forward speed and locomotion efficiency. Several studies have also noticed the effect of friction in the environment on robot locomotion. In [21], the balance between forward speed and energy efficiency was considered in three different motion gaits based on the adoptive dynamics mode. In [22], the effect of the environmental friction coefficient being on the robot locomotion speed was analyzed, and an adaptive parameter adjustment method was designed, but the energy consumption was not considered. In [23], an energy-based control method was proposed to achieve the meandering motion of the robot. This article calculates and analyzes the value of the torque output that should be used in the energy balance state from the model equation, but only for a specific robot. In [24], a cuckoo search algorithm is used for the optimal selection of robot parameters under variable environmental conditions based on the CPG model. It evaluates energy efficiency using the displacement–dissipation ratio, but it is only suitable for dynamic parameter adjustment under defined environmental parameters. Ref. [25] used a particle swarm algorithm to analyze the correspondence between forward speed and the average power for different parameter combinations, but these analyses were offline and only suitable for a straight path. Inspired by their research, some new bionic algorithms are well suited to be used for parameter optimization and the fast iteration speed is suitable for real-time computation.

Pigeon-inspired optimization (PIO) is a novel swarm intelligence algorithm proposed by Duan [26], which has the advantages of simple update rules, strong adaptability, and no special requirements for optimization problems compared with optimization algorithms such as particle swarm. However, there are also some problems of slow convergence, and easy to fall into local optimum at the same time. Many researchers have further proposed more optimization algorithms based on the principle of PIO, such as PIO with predation escape rule [27] and improved PIO based on Gaussian strategy [28], which are applied to real-time optimization of robot system parameters.

This paper is organized as follows. In the second part, a simplified model of the underwater snake robot is presented, and a combined PCSI and LOS method is proposed to design the path-tracking controller. In the third part, the evaluation equations for the robot locomotion efficiency are presented. The PIO algorithm is optimized by quantum rules and the advantages of the new algorithm, in terms of the speed and accuracy of the search, are verified, which are adopted to dynamically adjust the robot gait parameters. In the fourth part, simulations of two paths are performed and several parameter conditions are set to compare the optimization effects. The fifth part is the conclusion.

2. Path Tracking Based on ILOS

2.1. Motion Control Method of Snake Robot

2.1.1. Dynamic Model

A control-oriented simplified model of the underwater snake-like robot is as follows [7,29,30]:

$$\{\begin{array}{l}\dot{\varphi }=v_{\varphi }\\ \dot{\theta }=v_{\theta }\\ {\dot{p}}_x=v_t\cos \theta -v_n\sin \theta \\ {\dot{p}}_y=v_t\sin \theta +v_n\cos \theta \\ {\dot{v}}_{\mathsf{\varphi }}=-\frac{c_n}{m}{v}_{\mathsf{\varphi }}+\frac{c_p}{m}{v}_{t}A{D}^T\varphi +\frac{1}{m}D{D}^{T}u\\ {\dot{v}}_{\theta }=-{\lambda }_1{v}_{\theta }+\frac{{\lambda }_2}{N-1}{v}_t{e}^T\varphi \\ {\dot{v}}_t=-\frac{c_t}{m}{v}_t+\frac{2c_p}{Nm}{v}_n{e}^T\varphi -\frac{c_p}{Nm}{\varphi }^{T}A\overline{D}\dot{\varphi }\\ {\dot{v}}_n=-\frac{c_n}{m}{v}_n+\frac{2c_p}{Nm}{v}_t{e}^T\varphi \end{array}$$

(1)

where n is the number of snake-like robot connecting links, and each connecting link is numbered with i = 1, 2 … n; m is the mass of each connecting link; ${\varphi }_i$ is the angle between adjacent connecting links with $\varphi =\left[{\varphi }_1,\dots ,{\varphi }_{N-1}\right]$; θ is the heading angle of the robot as a whole; (p_x, p_y) is the robot centroid coordinates, and (v_t, v_n) is the forward velocity and normal velocity of robot centroid. More details could be found in [30]. The geometric relationship between the links is shown in Figure 1. c_n c_t c_p represent the normal friction coefficient, tangential friction coefficient and propulsion coefficient, respectively. λ₁, λ₂ are positive scalar constants. These parameters can affect the rotational motion characteristics of the serpentine robot [31]. At the same time, we set the calculation matrix $\overline{D}=D^T{\left(D{D}^T\right)}^{-1} ϵ R^{N\times N-1}$, $e^T=\left[1,1,\dots ,1\right] ϵ R^{N-1}$, $A,D ϵ R^{N\times N-1}$ to calculate the addition or subtraction between adjacent elements in a vector, with:

$$A=\left[\begin{array}{ccccc}1& 1& \cdots & 0& 0\\ 0& 1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \cdots & 1& 1\end{array}\right],D=\left[\begin{array}{ccccc}1& -1& \cdots & 0& 0\\ 0& 1& \cdots & 0& 0\\ ⋮& ⋮& \ddots & ⋮& ⋮\\ 0& 0& \cdots & 1& -1\end{array}\right].$$

Compared with the complex model composed of general differential equations, the simplified model converts the rotational motion between the links into relative translational motion. As shown in Figure 1, this strategy provides a convenient way to design and verify control methods. Simple and complex models have consistent states of motion under certain constraints. In addition, the stability of the entire system has also been proven in [29]. Compared with the land-based model, the underwater snake robot model additionally considers the effect of additional mass forces, which makes the state solution more complicated [30]. In the subsequent simulations in this paper, the required robot speed is low enough to allow the neglection of this additional mass force. In this way, the underwater model becomes similar to the land-based model, and Ref. [9] verifies experimentally that this strategy is feasible. Therefore, this paper selects this simplified model for the design and verification of the proposed optimization method.

2.1.2. Locomotion Pattern

Snake-like robots generally take sinuous motion to move forward. The entire body is shaped like a sinusoidal curve that fluctuates on a regular basis, and the links swing with the reaction force of friction, forcing the robot to move forward. Hirose in Japan first proposed the Serpenoid curve to describe the sinuous motion of a snake [32], and after continuous improvement by many scholars, one of the more popular curve forms is:

$${\varphi }_{\mathrm{ref},i}=\alpha \sin ({\omega }_{}t+(i-1)\beta )+{\varphi }_0$$

(2)

where α is the maximum angle of movement between the connecting links, ω is the frequency of the serpentine robot doing sinusoidal periodic motion, β is the phase difference between adjacent connecting links, and ϕ₀ is the offset of the linkage relative to the forward center axis that determines the overall steering tendency of the robot, as determined by the directional angle tracked by the system. Compared with the complex model, the relative displacement between the connecting links in the simple model has a certain corresponding relationship with the true included angle value. Under the same kind of correspondence, the locomotion posture and system characteristics of the snake-like robot are highly similar [29].

When each link of the snake-like robot can track the reference position given by (2), the whole system can realize the meandering locomotion in the sinusoidal mode. In practice, the PD controller is usually selected to calculate the control input of the robot to the reference signal:

$$\overline{u}=k_P({\varphi }_{\mathrm{ref}}-\varphi )+k_D({\dot{\varphi }}_{\mathrm{ref}}-\dot{\varphi })+{\ddot{\varphi }}_{\mathrm{ref}}$$

(3)

where $k_P$ and $k_D$ are the proportional coefficient and the differential coefficient, respectively. In order to reduce the complexity of the driving force under the simple model, perform local feedback linearization, and set the required control amount in (1) as:

$$u=m{(D{D}^T)}^{-1}(\overline{u}+\frac{c_n}{m}\dot{\varphi }-\frac{c_p}{m}{v}_{t}A{D}^T\varphi ).$$

(4)

Path tracking for the snake robot is now transformed into a computational problem for ϕ₀.

2.2. Path Planning and ILOS-Based Controller

2.2.1. Path Planning Based on PCSI

Path planning can be divided into real-time path selection and offline route planning, which is the precursor task to achieve path tracking. Most research on underwater robots has used waypoints to guide the robot forward and offline, to complete mission route planning. Unlike gliding AUV, the locomotion of the snake robot is continuous and more sensitive to sharp turns in its path, so it is necessary to plan a smooth path through waypoints. In this paper, the PCSI method is adopted to complete the path planning. Compared with the third Hermite interpolation and Dubins path methods, the path generated by PCSI is smoother at the waypoint and has continuous first-order derivatives. Moreover, the parametric interpolation method can generate closed curves that are not monotonically increasing along the coordinate axes.

First, n waypoints are set to $p_{way}\left(i\right)=\left(x_{way}\left(i\right),y_{way}\left(i\right)\right)\in R^2$, $i=1,\dots n,$ and the task of path planning is to generate continuous curves that pass sequentially through waypoint $p_{way}\left(i\right)$. In order to generate arbitrary curves, the parameter s is introduced to calculate a functional expression for the path, instead of setting y as a function about x in the general parametric interpolation. In this way, the coordinates of any point on the path are calculated by means of the path variable s, while the variable at the waypoint is set to $s_i, i=1,\dots n$. Incidentally the value of $s_i$ varies with the cumulative distance travelled. Then, the path function is calculated with the path points as endpoints in segments, and the path between $\left(x_{way}\left(i\right),y_{way}\left(i\right)\right)$ and $\left(x_{way}\left(i+1\right),y_{way}\left(i+1\right)\right)$ is $f_i\left(s\right)=\left(x\left(s\right),y\left(s\right)\right)$, with:

$$\begin{array}{l}x(s)=a_3{\left(s-s_i\right)}^3+a_2{\left(s-s_i\right)}^2+a_1\left(s-s_i\right)+a_0\\ y(s)=b_3{\left(s-s_i\right)}^3+b_2{\left(s-s_i\right)}^2+b_1\left(s-s_i\right)+b_0\end{array}.$$

(5)

For the calculation of the parameters in the above equation, please refer to the theory of interpolation spline curves [20]. The coordinates $\left(x\left(s\right),y\left(s\right)\right)$ of the PCSI-generated path, and the curve function, will be used for the calculation of the variables related to path tracing below.

2.2.2. LOS Guidance Law

The snake robot model given by Equation (1) takes the center-of-mass coordinates $\left(p_x,p_y\right)$ with heading $\theta$ as the external motion state quantity, which reduces the algorithmic difficulty of heading control. LOS law is a control strategy that points the reference heading to a virtual target point on the path, so that the control object can keep approaching the desire path while moving towards the virtual point. This is more commonly used in ship-heading control [33]. The reference heading calculated by the LOS guidance is:

$${\theta }_{\mathrm{ref}}=-\arctan (\frac{{\overline{p}}_y}{\mathsf{\Delta }})$$

(6)

where ${\overline{p}}_y$ denotes the y-direction distance from the robot center of mass to the reference path and $\mathsf{\Delta }$ is the forward distance. The schematic diagram of linear path tracking is shown in Figure 2a. For simple curve tracking, the general proportional control is adopted to calculate the curve offset angle presented in Equation (2):

$${\varphi }_0=k_{\mathrm{th}}({\theta }_{\mathrm{ref}}-\overline{\theta })$$

(7)

where $k_{\mathrm{th}}$ is the scaling factor, and $\overline{\theta }$ is the current robot heading angle. The steering control of the robot can be achieved by bringing (5) into (2).

2.2.3. Improved LOS Method

The LOS rules introduced in Section 2.2.2 are only suitable for the path tracking of simple curves such as straight lines. To achieve the tracking of the complex paths calculated in Section 2.2.1, the first step is to determine the track error relative to the coordinates $\left(x\left(s\right),y\left(s\right)\right)$:

$$e(t)=-\left(p_x(t)-x(s^{*})\right)\sin (\gamma )+\left(p_y(t)-y(s^{*})\right)\cos (\gamma )$$

(8)

where $\left(p_x\left(t\right),p_y\left(t\right)\right)$ is the position of the robot center of mass at the current moment, $x\left(s^{*}\right), y\left(s^{*}\right)$ are the path coordinates of the virtual point being tracked, and $\gamma$ is the angle between the tangent line and the coordinate axis at the current position of the virtual point, with:

$$\gamma =\mathrm{atan}2\left(y^{\prime }\left(s^{*}\right),x^{\prime }\left(s^{*}\right)\right).$$

(9)

In the general path-tracking problem, the motion of the virtual point is often associated with the robot motion speed, and the variation law of the parameter s satisfies:

$$\dot{s}=\frac{\sqrt{v_x{(t)}^2+v_y{(t)}^2}}{\sqrt{x^{\prime }{(s)}^2+y^{\prime }{(s)}^2}}$$

(10)

where $v_x\left(t\right)$ and $v_y\left(t\right)$ are the velocity components of the robot along the coordinate axes. When the robot only tracks a certain section of the path, or starts moving from a position far away from the path, the matching between the robot and the virtual point is poor, and the calculated trajectory error is not accurate enough. The equation of the normal at any path point is:

$$y_n-y(s)=-\frac{1}{y^{\prime }(s)/x^{\prime }(s)}\left(x_n-x(s)\right).$$

(11)

Solve for the nearest virtual point path parameters to the robot by requiring $\left(x_n,y_n\right)=\left(p_x\left(t\right),p_y\left(t\right)\right)$:

$$y^{\prime }\left(s^{*}\right)\left(p_y(t)-y\left(s^{*}\right)\right)+x^{\prime }\left(s^{*}\right)\left(p_x(t)-x\left(s^{*}\right)\right)=0.$$

(12)

For a cubic equation like (12), the Newton–Raphson method can be used to quickly obtain the solution for the path parameter:

$$s_{i+1}^{*}=s_i^{*}-\frac{f(s_i^{*})}{f^{\prime }(s_i^{*})}$$

(13)

With:

$$\begin{array}{l}f\left(s_i^{*}\right)=y^{\prime }\left(s_i^{*}\right)\left(p_y(t)-y\left(s_i^{*}\right)\right)+x^{\prime }\left(s_i^{*}\right)\left(p_x(t)-x\left(s_i^{*}\right)\right)\\ f^{\prime }\left(s_i^{*}\right)=y^{″}\left(s_i^{*}\right)\left(p_y(t)-y\left(s_i^{*}\right)\right)+x^{″}\left(s_i^{*}\right)\left(p_x(t)-x\left(s_i^{*}\right)\right)-x^{\prime }{\left(s_i^{*}\right)}^2-y^{\prime }{\left(s_i^{*}\right)}^2\end{array}$$

(14)

Using the values given in (10) as the initial iterative solution of (13) can speed up the solution of the equation. Figure 2b gives the relative position relationship and the angle labeling. By calculating the angle relationship, (7) changes to:

$${\varphi }_0=k_{\mathrm{th}}(\overline{\theta }-\gamma +\arctan (\frac{-e(t)}{\mathsf{\Delta }}))$$

(15)

Among (15), $\mathsf{\Delta }$ is able to change the reference heading value, so it is set as an adaptive parameter:

$$\mathsf{\Delta }=\left({\mathsf{\Delta }}_{\max }-{\mathsf{\Delta }}_{\min }\right)e^{-k_{\mathsf{\Delta }}{e}^2(t)}+{\mathsf{\Delta }}_{\min }.$$

(16)

When the robot is far from the path, $e^{-k_{\mathsf{\Delta }}{e}^2\left(t\right)}$ is close to 0 and $\mathsf{\Delta }\approx {\mathsf{\Delta }}_{\min }$, then ${\theta }_{\mathrm{ref}}$ is larger and the robot can approach the planned route faster. When the robot is close to the path, $e^{-k_{\mathsf{\Delta }}{e}^2\left(t\right)}$ is close to 1 and $\mathsf{\Delta }\approx {\mathsf{\Delta }}_{\max }$, which can effectively reduce the error overshoot. Typically, ${\mathsf{\Delta }}_{\max }=1.6L$, ${\mathsf{\Delta }}_{\min }=0.4L$, and $L$ is the length of the snake robot.

In summary, the iterative steps of path tracking are as follows:

Step 1:

Completing the path planning and calculating the parameters by means of Equation (5);

Step 2:

Matching of virtual points is completed by (10)–(14), and the heading error in (8) is calculated;

Step 3:

The angular bias in (2) is calculated by (9), (15), (16), and then the current iteration is completed according to the dynamics model in (1).

3. Locomotion Efficiency Optimization Based on QPIO

3.1. Optimization Problem Description

The propulsive force of the snake robot is generated by the interaction with the water as the chain link oscillates. During the robot motion, most of the energy is used to overcome the resistance of the water, which is consumed by the servo motors. The mechanical friction and physical energy loss are not considered to the control efficiency, so the energy consumed by the motor to rotate the connecting rod is considered as the energy required for robot motion. In the robot model (1), the torque of the motor is equivalent to the variable $u_i$, and the angular variable generated by the motor rotation is equivalent to ${\varphi }_i$. Therefore, in the ideal case, the energy consumed by the robot over time is defined as:

$$\mathrm{Energy}={\displaystyle {\int }_0^T{\displaystyle \sum _{i=1}^{N-1}\left(u_i\cdot {\dot{\varphi }}_i\right)}}dt.$$

(17)

In the time interval T, the path length of the snake robot movement is:

$$\mathrm{Distance}={\displaystyle \sum _{t=0}^T\sqrt{{(p_x(t+1)-p_x(t))}^2+{(p_y(t+1)-p_y(t))}^2}}.$$

(18)

In this paper, the ratio of (17) and (18) is used to evaluate the motion efficiency:

$$\mathrm{Efficiency}=\frac{\mathrm{Distance}}{\mathrm{Energy}}=\frac{{\displaystyle \sum _{t=0}^T\sqrt{{(p_x(t+1)-p_x(t))}^2+{(p_y(t+1)-p_y(t))}^2}}}{{\displaystyle {\int }_0^T{\displaystyle \sum _{i=1}^{N-1}(u_i\cdot {\dot{\varphi }}_i)}}dt}.$$

(19)

The robot should consume as little energy as possible over the same distance, and the locomotion efficiency will be better.

From the description of the motion pattern in Section 2.1.2, it is known that the gait of the snake robot is related to the parameters $\alpha$, $\omega$, and $\beta$. Obviously, the larger amplitude and frequency the links swing, the faster the robot moves forward. Ref. [25] tested, in detail, the correspondence between $\alpha$, $\omega$, and $\beta$ with respect to velocity under different combinations. The calculation of Equations (2) and (3) are affected when the gait parameters are different which, in turn, changes the value of (17). Moreover, the change in speed caused by gait also affects the value of the distance in (18). Thus, it can be determined that there is such a gait that maximizes the robot motion efficiency (19). In this way, the problem of motion efficiency is transformed into a problem of parameter selection.

Even in linear motion, it is impractical to rely on an exhaustive list to determine the combination of three parameters that will result in the highest efficiency. In this paper, in order to accomplish path tracking and efficiency optimization under arbitrary curves, it is necessary to find the appropriate combination of parameters dynamically and quickly. To this end, the following pigeon-inspired algorithm improved by quantum rules is proposed to solve the parameter selection problem.

3.2. Parameter Optimization with QPIO

3.2.1. Principle of the Pigeon-Inspired Algorithm

The pigeon-inspired algorithm is a bionic optimization algorithm that finds the combination of parameters at the extremes of the function, according to the set evaluation rules [26]. All parameters are considered as individual “pigeons” that change their positions with certain update rules, and the most suitable combination of values are determined after a certain number of iterations.

During the pre-migration period, pigeons rely on pointing information such as the geomagnetic field and the sun to correct the general direction of their flight. After reaching the vicinity of their destination, they switch to landmark information for navigation. In the whole flock of pigeons, individuals unfamiliar with the map also approach individuals familiar with the landmarks during the movement. Before the start of the optimization search, the velocity information $V_i$ and the position information $X_i$ of $N$ particles are initialized. Then, two stages of iteration are completed as follows.

A pre-optimal search is navigated by compass operators, and the velocity update of particles is influenced by both the compass information and the optimal individual within the population:

$$V_i^{Nc}=V_i^{Nc-1}\cdot e^{-R\times Nc}+\mathrm{rand}\left(X_{\mathrm{best}}-X_i^{Nc-1}\right)$$

(20)

$$X_i^{Nc}=X_i^{Nc-1}+V_i^{Nc}$$

(21)

where N_C is the current iteration number, X_best is the optimal particle position information in the current iteration, and R is the compass operator, which can adjust the influence weight of the guide information.

The second stage navigates through the landmark operator. In each iteration, each particle generates an adaptation value based on the set evaluation method, generates the corresponding sequence, and discards the individuals with poor evaluation values by halving the total number of particles to speed up the merit search. The remaining individuals converge to the optimal particle position:

$$X_{\mathrm{center}}^{Nc-1}=\frac{{\displaystyle \sum _{i=1}^N{X}_i^{Nc-1}\cdot f(X_i^{Nc-1})}}{N^{Nc-1}\cdot {\displaystyle \sum _{i=1}^{N}f(X_i^{Nc-1})}}$$

(22)

$$N^{Nc}=N^{Nc-1}/2$$

(23)

$$X_i^{Nc}=X_i^{Nc}+\mathrm{rand}(X_{\mathrm{center}}^{Nc-1}-X_i^{Nc-1})$$

(24)

where X_center is the current particle center position and f(X) is the fitness value of the particle. We used (19) as an evaluation function to calculate the fitness value of the group of particles. A strategy was adopted to complete the calculation, which was simulating the motion after a period of time with the current parameters and robot state quantities, and returning the calculated value of the efficiency during this period. This process is integrated into Figure 3.

3.2.2. Improve PIO by Quantum Rules

Quantum behavior rules play a good role in solving the problems of particle swarm algorithms which are prone to fall into optimality and slow in finding the optimal speed [34]. In this paper, quantum rules are used in the particle position updating process for dynamically decreasing shrinkage. Dilation coefficients A are introduced as compass operators to improve the convergence of the optimal search:

$$A=\frac{\left(T-Nc\right)}{2T}+0.5$$

(25)

$$P=\frac{\left(\mathrm{rand} 1\cdot X_{{\mathrm{pbest}}_i}^{Nc-1}+\mathrm{rand} 2\cdot X_{\mathrm{gbest}}^{Nc-1}\right)}{\left(\mathrm{rand} 1+\mathrm{rand} 2\right)}$$

(26)

$$X_i^{Nc}=P\pm A\cdot \left|X_{\mathrm{mbest}}^{Nc-1}-X_i^{Nc-1}\right|\cdot \ln \left(1/\mathrm{rand}\right)$$

(27)

where T is the number of iterations of the compass operator, X_pbest is the individual historical optimal, X_gbest is the global historical optimal and X_mbest is the individual historical average optimal.

In the landmark operator stage, the landmark operator is changed to a population optimal operator and a learning factor Β is introduced to update the particle information.

$$B=round(1+\mathrm{rand})$$

(28)

$$X_{\mathrm{new}}{}_{,i}=X_i^{Nc-1}+\mathrm{rand}\cdot \left(X_{\mathrm{gbest}}^{Nc-1}-B\cdot X_{\mathrm{mbest}}^{Nc-1}\right)$$

(29)

$$X_i^{Nc}=\{\begin{array}{c}{X}_{\mathrm{new},i}\begin{array}{cc}& f\left(X_{\mathrm{new},i}\right)<f\left(X_i^{Nc-1}\right)\end{array}\\ X_i^{Nc-1}\begin{array}{cc}& f\left(X_{\mathrm{new},i}\right)>f\left(X_i^{Nc-1}\right)\end{array}\end{array}$$

(30)

The corresponding schematic diagram of the algorithm is shown in Figure 4. Under this rule, the particles that are far from the optimal solution first converge to the same position at a rate proportional to the difference between the global historical optimal solution and the individual historical average optimal solution, while expanding the global search degree. Then, the difference between the global historical optimal solution and the individual historical average optimal solution is reduced with the increase of iterations, until the search for the optimal solution is completed and all particles converge to the vicinity of the optimal solution.

3.2.3. Algorithm Performance Test

To verify the effectiveness of the improved PIO algorithm in this paper, several common test functions were selected for comparing the optimization results of different methods. The relevant contents of the function expressions are shown in

Table 1. Test function and condition settings for optimization algorithm.

Number	Test Function	Expressions	Definition Domain	Optimal Solution
F1	Sphere	$f\left(X\right)={\displaystyle \sum _{i=1}^n{x}_i{}^2}$	[−100, 100]	(0, 0)
F2	Schwefel	$f\left(X\right)=418.9829\ast n+{\displaystyle \sum _{i=1}^n\left[-x_i\sin \left(\sqrt{\left|x_i\right|}\right)\right]}$	[−500, 500]	(420.9687, 0)
F3	Rastrigin	$f\left(X\right)=10\ast n+{\displaystyle \sum _{i=1}^n\left[x_i{}^2-10\cos \left(2\pi x_i\right)\right]}$	[−10, 10]	(0, 0)
F4	Griewangk	$f\left(X\right)=\frac{1}{4000}{\displaystyle \sum _{i=1}^n{x}_i{}^2}+{\displaystyle \prod _{i=1}^n\cos \left(\frac{x_i}{\sqrt{i}}\right)}+1$	[−600, 600]	(multi-extreme-points)

.

Four PIO related algorithms were used to compare convergence speed and convergence accuracy, including two other optimization methods:

(1)

Basic pigeon-inspired algorithm (PIO) [26];

(2)

Improved pigeon-inspired algorithm with inverse and Gaussian factor (IPIO) [35];

(3)

Adaptive pigeon-inspired algorithm with constriction factor (CFPIO) [36];

(4)

Pigeon-inspired algorithm based on quantum behavior rules (QPIO).

The main variables and coefficients in the algorithm were set as: functional dimensions D = 10; number of particle N_P = 5 ∗ D; contraction-expansion coefficient A = 1 − 0.5 (degression); compass operator R = 0.5; iteration times of compass T₁ = 800; iteration times of landmark T₂ = 200 and number of optimization Times = 30.

The optimal solution (Min), the most inferior solution (Max), the average solution (Mean) and the standard deviation (STD) of the results of 30 simulation operations were taken for comparison, and the statistical results are shown in

Table 2. Result statistics of different algorithms.

Function		PIO	IPIO	CFPIO	QPIO
F1	Mean	0.5012	0.1147	0.0392	0
	STD	0.7583	0.4522	0.1489	0
	Min	0	0	0	0
	Max	3.1724	2.2073	0.5681	0
Time(s)	Tmean	1.0635	0.9297	0.9964	1.7328
F2	Mean	2.2368	0.6955	1.1002	0.0785
	STD	4.8849	1.6066	2.7023	0.2001
	Min	1.26 × 10−5	1.26 × 10−5	1.26 × 10−5	1.26 × 10−5
	Max	21.7294	7.5386	14.8372	0.6527
Time(s)	Tmean	0.9651	1.0318	1.0229	2.0073
F3	Mean	0.2019	0.0156	0.0041	0
	STD	0.3766	0.0599	0.0144	0
	Min	0	0	0	0
	Max	1.2109	0.2866	0.0832	0
Time(s)	Tmean	0.9380	1.0161	1.0818	1.8141
F4	Mean	0.0786	0.0441	0.0048	0.0042
	STD	0.1375	0.1114	0.0062	0.0041
	Min	0.0025	0.0025	0.0025	0.0025
	Max	0.5703	0.5482	0.0353	0.0239
Time(s)	Tmean	0.9948	1.0005	1.0354	1.9682

, and the optimal value is bold. The data of a random optimization process were taken out to plot the convergence process curve, as shown in Figure 5.

From the various statistical results, it can be seen that the proposed algorithm performs better in terms of convergence results and has a greater improvement in the optimization search effect. Besides, under certain test functions, the proposed algorithm can achieve the same optimization results in fewer iterations as other methods with high iterations. For the high real-time parameter requirements of the snake robot, this significantly reduces the computational power requirements of the robot system and provides the possibility of dynamic parameter adjustment in practical applications.

4. Simulation

4.1. Parameters of Robot and Opitimizaion Algorithm

Currently, experiments related to underwater snake robots usually verify the control method through indoor pools, where the robot completes the set motion in still water or in constant irrotational currents. Moreover, physical prototypes are designed so that the robot is subjected to a buoyancy force equal to its own gravity, allowing the robot to move on a horizontal plane to avoid other disturbances. Therefore, the simulation environment set in this paper conforms to the above settings and refers to the friction coefficients $c_t$ and $c_n$ given by [30], which are often obtained experimentally.

We considered an underwater snake robot with n = 10 links, 2l = 0.14 m length of each link and mass m = 1 kg. The hydrodynamic-related parameters were set as $c_t=0.5$, $c_n=3$, ${\lambda }_1=0.5$, ${\lambda }_2=20$, and thus $c_p=9$. The control parameters were fixed with $k_P=20$, $k_D=5,$ $k_{\mathsf{\Delta }}=100$, and $k_{\mathrm{th}}=0.2$. The initial values of the states of the robot were set to 0 or referred to the special instructions.

Some QPIO parameters were adjusted, which was different from the previous section, to $D=3$, $N_P=5\times D=15$, and the number of iterations was reduced to $T_1=180$, $T_2=20$. The model proposed in [9] ignores the effect of additional mass forces, which requires that the robot should not be at high speed and that the simplified model is relatively accurate when the rotation of the linkage is below 20 degrees. Therefore, in the later simulations, the gait parameters should be taken in a range of values during the optimization process, which are $\alpha \in \left[0.02, 0.06\right]$, $\omega \in \left[1, 2\right]$ and $\beta \in \left[0.3,0.7\right]$.

4.2. Path Tracking and Efficiency Optimization

4.2.1. Closed-Loop Curve Path

To highlight the advantages of generating curve functions with the parametric method, a closed-loop curve path was generated using the PCSI method and the waypoints were set at (0, 0), (2, 0), (4.2, 0.5), (3.5, 2.3), (1.6, 1.8) and (0.5, −1), as shown in Figure 6a with the blue circle.

First, three constant gait parameter conditions were set for comparison: (1) Condition 1 for $\alpha =0.04\mathrm{m}$, $\omega =1.75$ and $\beta =0.52$; (2) Condition 2 for $\alpha =0.03\mathrm{m}$, $\omega =2$ and $\beta =0.43$; (3) Condition 3 for $\alpha =0.06\mathrm{m}$, $\omega =1.57$ and $\beta =0.7$. These reflect the robot locomotion under different amplitudes and frequencies.

Condition 1 was adopted to verify the tracking effect of the proposed method on the curved path, as shown in Figure 6a,b. It can be seen that the real path and the planned path almost overlapped, so the proposed strategy achieved this task well. It can be seen that the heading angle in Figure 6b has some fluctuations. In fact, the curve bends more when passing the heading point; the robot corrected this after offsetting the planned path slightly.

Then, three optimization simulations with the same conditions were completed to show the efficiency optimization results, avoiding the influence of the algorithm randomness. Figure 6c shows that the three optimizations had the same change trend, and that energy efficiency was significantly improved at the position with larger curvature. The efficiency accumulation was better than the results under Condition 1 and Condition 2, even if the final result was slightly different due to the randomness of the algorithm. This shows that the proposed optimization strategy can reduce the required energy within the same distance. This result may not have been obvious enough and will be further verified in the next simulation.

It seems that there should be a gait pattern that allows the robot to maintain high efficiency, similar to the results under Condition 3. In fact, the robot moves faster with greater amplitude and frequency in this gait pattern, and Figure 6d shows that in this condition, the robot takes only half the time to complete the entire motion. In curved path tracking, there is no need for the robot to move at such a speed, which also increases the tracking error. That is, this increase in efficiency comes at the cost of increased tracking error, and is more pronounced in more complex paths. In the process of optimizing the parameters with QPIO, some unreasonable parameter combinations were removed, for example, the robot can still maintain a high frequency when swinging in the large amplitude. It was too demanding for the robot mechanical system, and a small gait with the speed of only 0.02 m/s was efficient. The reasonable speed of the robot should be around 0.1 m/s–0.2 m/s, according to some experiments. Incidentally, the fluctuation of the tracking error was due to the asymmetric attitude of the robot, where the lateral components of the drag force could not cancel each other out.

4.2.2. Large Curvature Curve Path

To illustrate some inferences in the previous subsection, a path with a greater curvature resembling a sine curve was generated, and the waypoint was set at (0, 0), (1, 0.1), (3, 1), (5, −0.6), (7, 1) and (9, 0), as shown in Figure 7a with the blue circle. Meanwhile, the starting position of robot was set to $(p_x\left(0\right),p_y\left(0\right))=$ (0.5, 0.2) to demonstrate the validity of (16). Other conditions were the same as in the previous simulation.

Figure 7a shows that the robot tracked the planned path well under the LOS strategy with the gait of Condition 1, and that the method of (16) can guide the robot to approach the target route faster. The effect of this strategy was more evident in the small window where the start point was set at (0.5, 0.5). In the path of continuous turns, the robot shifted its course more, and this strategy was effective in reducing the tracking error. Figure 7b shows that the robot’s heading angle shifts a little at the turn, but has little effect on the robot in this gait at a slow speed of about 0.09 m/s.

Similarly, three optimization simulations with the same parameters were performed to contrast with the simulation of fixed parameters, as shown in Figure 7c. The efficiency accumulation of the three simulations had a very similar trend in the first period, with the final value varying as the number of optimizations increased, which was caused by the randomness of the algorithm’s choice of parameters. Nevertheless, the results of the optimization strategy were much better than the constant conditions, and the robot efficiency increased significantly for all the large angle turns. The gait of Condition 3 corresponded to a somewhat higher efficiency, but Figure 7d shows that the robot will have a larger tracking error at turns. The speed had some effect on the tracking error, and their error curves were not of the same length; the error curves for all gaits are not recorded in Figure 7d, which would make the results appear confusing.

When the robot deviated from the intended route, a larger linkage offset was calculated by (15), which indirectly led to a change in the control volume in (3), and the efficiency evaluation method (19) proposed in this paper was affected. The energy efficiency was improved by dynamically selecting the gait parameters, while the tracking error did not become large. The dynamic selection of parameters to improve the energy efficiency proposed in this paper is effective while maintaining the path-tracking accuracy.

5. Conclusions

In this paper, curved path-tracking control of an underwater snake robot was investigated, and a commonly used LOS law was used to design the controller. A PCSI method was adopted for path planning, which made the generated path more compatible with the locomotion characteristics of the snake robot. An algorithm improving the PIO with quantum rules was proposed to dynamically select the gait parameters to enhance the energy efficiency of the robot. Simulation results showed that the snake robot can achieve tracking of the planned path with low error, and the intelligent algorithm helps the robot to improve the energy efficiency, especially in the case of large angle turns.

It was found in the simulation that the speed of the robot had a significant impact on the tracking accuracy. In the future, adaptive path tracking controllers will be designed to cope with more complex routes, and to enhance the performance of intelligent algorithms to achieve more stable parameter optimizations. Experiments will be performed to show the validity of the proposed method, along with the development of the physical robot.