# Path-Integral-Based Reinforcement Learning Algorithm for Goal-Directed Locomotion of Snake-Shaped Robot

Authors: Qi Yongqiang; Yang Hailan; Rong Dan; Ke Yi; Lu Dongchen; Li chunyang; Liu Xiaoting
Journal: Discrete Dynamics in Nature and Society
 (2021)

---

## Abstract

This paper proposes a goal-directed locomotion method for a snake-shaped robot in 3D complex environment based on path-integral reinforcement learning. This method uses a model-free online Q-learning algorithm to evaluate action strategies and optimize decision-making through repeated “exploration-learning-utilization” processes to complete snake-shaped robot goal-directed locomotion in 3D complex environment. The proper locomotion control parameters such as joint angles and screw-drive velocities can be learned by path-integral reinforcement learning, and the learned parameters were successfully transferred to the snake-shaped robot. Simulation results show that the planned path can avoid all obstacles and reach the destination smoothly and swiftly.


---

## Body

## 1. Introduction

Research in the field of snake-shaped robots has been carried out for several decades [1–3]. This is because snake-shaped robots can not only be used as experimental platforms [4, 5] but also be employed for searching and rescuing operations, such as mine disaster rescue, search, and detection of people buried after the earthquake [6]. The snake-shaped robots are multimodule serially connected robots with no fixed base by imitating the snake’s body structure and movement mode.Generally speaking, the snake-shaped robot consists of several segments connected in a serial manner as shown in Figure1, and the main form of movement is undulation movements, which imitate real snake movements [7, 8]. In this paper, the snake-shaped robot is composed of screw-drive units with passive wheels, and the screw-drive units are serially connected through active joints. With this structure, the snake-shaped robot can move in any direction by appropriately adjusting the angular velocity of screw-drive units.Figure 1
Structural diagram of the snake-shaped robot.The problem of robot path planning in an unknown environment has attracted more and more researchers’ attention. The main research methods include the genetic algorithm, the gradient algorithm, the nonlinear mapping algorithm, the linear mapping algorithm, and so on [9, 10]. These traditional approaches are improved to solve the path planning problem. Liljebäck obtains the deviation of the centroid of the snake-shaped robot from the expected path through real-time monitoring. This deviation is used as a joint control parameter to adjust the head movement direction so that the snake-shaped robot follows the desired straight path [11–16]. Rezapour uses the desired angle of the head joint to adjust the swing of the head link, so that the centroid of the snake-shaped robot follows the desired straight path [17]. Hasanabadi uses sensors to measure the distance between the robot and the desired path to calculate the desired angle, and the angle is used as the joint control parameter to realize that the robot follows a straight path and a circle without side slip path [18]. Matsuno introduces a functional model of the desired path to establish the controller, so that the robot head follows the desired circular path [19]. Mohammadi controls the velocity components in all directions to make the snake robot’s center of mass follow a circular path, but it requires that the path function is known [20]. Ariizumi established a cost function based on the expected path and head trajectory and used the cost function as the control parameter to realize the snake-shaped robot following a sinusoidal path [21].In order to make the snake-shaped robot reach the destination safely and smoothly, the first task is to find proper control parameters for generating goal-directed locomotion. This is a complex and challenging task, and we apply a special type of reinforcement learning, called path-integral reinforcement learning combining the time series analysis method for the task. Path-integral reinforcement learning is selected because this method can stably solve the “dimensional disaster” problem [22] and has proved successful for different robot learning tasks [23].This paper proposes a collision-free goal-directed locomotion method based on path-integral reinforcement learning in a three-dimensional environment. The novelty of this paper and the main contributions of this work are as follows: a path-integral reinforcement learning model is established, which describes in detail how the robot can accumulate experience from historical data to optimize decision-making. TheQ table is updated based on the prior information and provided to the robot as historical experience, which greatly improves the learning efficiency of the robot. To describe the influence of collision avoidance, the perturbation matrix is introduced and the planned path can avoid obstacle safely and reach the destination eventually. The simulation results show that the planned paths can avoid all obstacles smoothly and swiftly and reach the destination eventually.The rest of the paper is organized as follows: Section2 describes the kinematic model of the snake-shaped robot. Section 3 focuses on the path-integral-based reinforcement learning algorithm. Section 4 explains goal-directed locomotion of the snake-shaped robot. The simulation results are given in Section 5. Section 6 concludes the paper.


## 2. The Description of Kinematic Model of Snake-Shaped Robot

The kinematic model of the snake-shaped robot [7] can be described as(1)Aω˙=Bu,where ω=x,y,ψ,ϕ1,ϕ2,ϕ3,…,ϕn−1T is the state vector and u=θ.1,θ.2,θ.3,…,θ.n,ϕ˙1,ϕ˙2,ϕ˙3,…,ϕ˙n−1T is the control input vector. x,y is the position of the head of the snake-shaped robot. ψ is the orientation of the first screw-drive unit with respect to x,y and is automatically adjusted with the snake-shaped robot kinematics. ϕ1,ϕ2,ϕ3,…,ϕn−1 are the yaw joint angles which are relative orientations of one screw unit to the previous one. θ.1,θ.2,θ.3,…,θ.n are the angular velocities in corresponding screw-drive units from the head, respectively. ϕ˙1,ϕ˙2,ϕ˙3,…,ϕ˙n−1 are the angular velocities of the joints starting from ϕ˙1 which is the angular velocity of the first joint from the head. A and B are the system matrices and depend on system configurations such as screw-drive unit radius, inclination of blade, and length of one screw-drive unit [7].The learning of locomotion control parameters such as screw velocities and joint angles can be carried out as follows. Suppose that the initial position of the head of the snake-shaped robot is atx0,y0 and the target position of the snake-shaped robot is xg,yg. The two parameter vectors used to represent the control policy can be described as follows:(2)U=θ.1,θ.2,θ.3,…,θ.n,ϕ1,ϕ2,ϕ3,…,ϕn−1.The snake-shaped robot’s locomotion control policy following the kinematic model in equation (1) can be represented by U of equation (2). After learning final values of U, the snake-shaped robot will start from the initial position x0,y0 and then move toward the desired goal xg,yg.We know that the cost function or the objective function is the key component that affects the convergence of learning. In this article, we use Euclidean distance as a cost function as follows:(3)Cx,y=x−xg2+y−yg2.The cost function provides the distance between the reached head positionx,y and the target position xg,yg of the snake-shaped robot. It can be seen that when the cost function is almost zero, the learned parameter vector will reach the final value. That is to say, the snake-shaped robot reaches the goal and completes the task. Therefore, with the final parameter vector, the robot will move toward the given target point.In addition, the influence of external noise on path-integral reinforcement learning needs to be considered. As an open parameter in the learning process, external noise needs to be designed in a task-based manner. For path-integral reinforcement learning in this paper, a random valueε is selected by us from the normal distribution N0,1, and ε can be dynamically adjusted according to the noise-free cost Ct−1 obtained at the end of the previous update. Then, the external noise interference can be expressed as follows:(4)em,k=ε9e−1/Ct−1,Ct−1>5,0.05ε,1≤Ct−1≤5,0.025ε,0<Ct−1<1,where m is the number of update, k is the number of noisy rollout, and em,k is the external noise interference during k-th noisy trajectory or rollout of the m-th update.Suppose that the noise applied to screw velocities iseθ.1m,k,eθ.2m,k,…,eθ.nm,k, respectively. Similarly, assume that the noise applied to joint angles is eϕ1m,k,eϕ2m,k,…,eϕn−1m,k, respectively. All these noise distributions follow above description of the external noise interference in every noisy trajectory τm,k, but their patterns are different in a trajectory as ε is a random value.The main steps for path-integral reinforcement learning applied to goal-directed locomotion of the snake-shaped robot are as follows: firstly, the parameter vectorU can be selected according to the learning task. Secondly, the number of rollouts K per update needs to be fixed and K=45 in this paper. According to the parameter vector U to be learned, in every rollout k, the trajectory with noisy parameters is simulated starting from robot start position x0,y0:(5)τm,kθ.1+eθ.1m,k,θ.2+eθ.2m,k,…,θ.n+eθ.nm,k,ϕ1+eϕ1m,k,ϕ2+eϕ2m,k,…,ϕn−1+eϕn−1m,k,where θ.i+eθ.im,k,i=1,2,…,n represent the noisy of screw velocity parameters and ϕj+eϕjm,k,j=1,2,…,n−1 denote the noisy of joint angles. Trajectory of the snake-shaped robot can be generated by using the robot kinematic model described as equation (1) and the parameters of locomotion control such as screw velocities and joint angles. It can be seen that both the noisy screw velocities and the noisy joint angles as required for learning U are given in equation (5). According to the reached position xm,k,ym,k of the snake-shaped robot’s head at the end of this rollout, then the final cost for this rollout can be calculated by evaluating the cost function as follows: Im,k=Cxm,k,ym,k. With this method, all K noisy rollouts from start position of the snake-shaped robot within one update process m can be completed and corresponding Im,k can be stored. Then, the exponential value can be calculated for each rollout as follows:(6)Sτm,k=exp−45Im,k−minkIm,k/maxkIm,k−minkIm,k.Then, the probability weightingPmk for each rollout can be calculated as follows:(7)Pmk=Sτm,k∑i=1KSτm,i.Then, corrections for the parameter vector can be calculated based onPmk:(8)Δθ.i=∑k=1KPmkeθ.im,k,Δϕ˙i=∑k=1KPmkeϕ˙im,k.The complete update vector can be constructed according to equation (8):(9)ΔU=Δθ.1,Δθ.2,Δθ.3,…,Δθ.n,Δϕ1,Δϕ2,Δϕ3,…,Δϕn−1.Then, the control vectorUm at the end of an update m can be calculated as Um=U+ΔU.


## 3. Path-Integral-Based Reinforcement Learning Algorithm

The path-integration algorithm is used to solve the stochastic optimal control problem, and its purpose is to minimize the performance index of the stochastic dynamic system. The analysis and derivation of stochastic optimal control is based on the optimal Bellman principle and the Hamilton–Jacobi–Bellman (HJB) equation. In this paper, we first analyze the stochastic optimal control of the mobile robot and then use the generalized path-integral control method to obtain the numerical solution of the stochastic dynamic system.


### 3.1. Stochastic Dynamical System

In general, the stochastic dynamic system can be expressed as follows [10]:(10)X˙t=fXt,t+GXtut+εt=ft+Gtut+εt,where Xt∈ℜn×1 denotes the system state, Gt∈ℜn×p is the control matrix, ft∈ℜn×1 represents the passive dynamics, ut∈ℜp×1 stands for the control vector, and εt∈ℜp×1 is the Gaussian noise with variance Σε∈ℜp×p and zero expectation. For mobile robots, the general stochastic dynamic system can be expressed by the following equation:(11)x˙t=vcosφt,y˙t=vsinφt,φ.t=ωt+εt,where v and ωt are the linear and angular velocities, xt,yt denotes the position of the robot on the 2-D plane, and φt represents its orientation, and Xt=xt,yt,φtT, ft=vcosφt,vsinφt,0T, Gt=0,0,1T, ut=ωt, and εt=εt. This paper studies the path planning of the snake robot based on the path-integration RL algorithm. Therefore, the linear velocity v remains constant, and only the angular velocity ωt is used as a control to reduce the parameters in the RL, thereby accelerating the training convergence process.


### 3.2. Stochastic Optimal Control

For a trajectoryτi with a start time of ti and an end time of tf, its performance function is defined as follows:(12)Jτi=ϕtf+∫titfLXt,ut,tdt,where ϕtf=ϕXtf,tf is a terminal value and ∫titfLXt,ut,tdt is a process value. The immediate cost can be defined as(13)Lt=LXt,ut,t=qt+12utTRut,where qt=qXt,t is the cost function related to the state and R is the positive semidefinite weight matrix of the quadratic control cost. The goal of stochastic optimal control is to find the control ut to minimize the following performance criterion function:(14)Vtt=minuti:tfEτiJτi,where Eτi⋅ is the expectation of all trajectories starting from Xti. In order to solve the noiseless optimal control problem, there are two main methods: one is to use the Ponteria Gold Minimal Principle (PMP), which requires solving a pair of ordinary differential equations, and the other is to use partial differential equations (PDEs) of the HJB equation. For the stochastic dynamic system with noise shown in equation (10), the extension of the HJB method for stochastic optimal control is expressed as(15)−∂tVt=minuti:tfqt+12utTRut+∇xVtTFt+12trace∇xxVtGtΣεGtT,where ∇xVt and ∇xxVt are the corresponding Jacobian and Hessian matrices and Ft is defined as Ft=fXt,t+GXtut. The corresponding optimal control can be calculated as ut=−R−1GtT∇xtVt [10]. Then, a nonlinear second-order PDE can be obtained as follows:(16)−∂tVt=qt+∇xVtTft−12∇xVtTGtR−1GtT∇xVt+12trace∇xxVtGtΣεGtTGtT.Vt can be transformed into exponential form Vt=−λlogΨt, where λ is a positive constant, and then use λR−1=Σε [24, 25]; then, PDE (16) can be simplified to the following form with the boundary condition which is Ψtf=e−1/λϕtf:(17)−∂tΨt=−1λqtΨt+ftT∇xVt+12trace∇xxΨtGtΣεTGtT.


### 3.3. Simplification of Path-Integral-Based RL

It is very difficult to obtain the analytical solution of PDE (17), so we replace it with commonly used numerical solutions, and one of which uses the Feynman–Kac theorem [26] as follows:(18)Ψti=EτiΨtfe−∫titf1/λqtdt=Eτie−1/λΨtf−1/λ∫titfqtdt.After discretization, the stochastic optimal control problem can be approximately transformed into the following path-integration problem:(19)Ψti=limdt⟶0∫pτi|xiexp−1λϕtf+∑j=iK−1qtjdtdτi,where τi=xti,…,xtf is a sample trajectory with its probability as pτi|xi. After some analysis [27], the following numerical solutions of optimal control can be obtained:(20)uti=∫pτi|xiuτidτi,where uτi=uti,…,utf is used to generate the trajectory τi and the probability is defined as(21)pτi|xi=e−1/λSτi∫e−1/λSτidτi,where Sτi is the general version of the path cost defined in [25]. However, the analytical form of calculating the path cost is very time-consuming because it involves many matrix inversions. Considering this fact, in order to simplify the calculation and speed up the training, we choose the linear path cost Sτi function:(22)Sτi=Jτi=tf−ti.It can be clearly seen that the generalized path-integral control method using the numerical solution of the stochastic dynamic system can avoid the calculation of gradient and kinematic inverse, ensuring fast and reliable convergence.


## 3.1. Stochastic Dynamical System

In general, the stochastic dynamic system can be expressed as follows [10]:(10)X˙t=fXt,t+GXtut+εt=ft+Gtut+εt,where Xt∈ℜn×1 denotes the system state, Gt∈ℜn×p is the control matrix, ft∈ℜn×1 represents the passive dynamics, ut∈ℜp×1 stands for the control vector, and εt∈ℜp×1 is the Gaussian noise with variance Σε∈ℜp×p and zero expectation. For mobile robots, the general stochastic dynamic system can be expressed by the following equation:(11)x˙t=vcosφt,y˙t=vsinφt,φ.t=ωt+εt,where v and ωt are the linear and angular velocities, xt,yt denotes the position of the robot on the 2-D plane, and φt represents its orientation, and Xt=xt,yt,φtT, ft=vcosφt,vsinφt,0T, Gt=0,0,1T, ut=ωt, and εt=εt. This paper studies the path planning of the snake robot based on the path-integration RL algorithm. Therefore, the linear velocity v remains constant, and only the angular velocity ωt is used as a control to reduce the parameters in the RL, thereby accelerating the training convergence process.


## 3.2. Stochastic Optimal Control

For a trajectoryτi with a start time of ti and an end time of tf, its performance function is defined as follows:(12)Jτi=ϕtf+∫titfLXt,ut,tdt,where ϕtf=ϕXtf,tf is a terminal value and ∫titfLXt,ut,tdt is a process value. The immediate cost can be defined as(13)Lt=LXt,ut,t=qt+12utTRut,where qt=qXt,t is the cost function related to the state and R is the positive semidefinite weight matrix of the quadratic control cost. The goal of stochastic optimal control is to find the control ut to minimize the following performance criterion function:(14)Vtt=minuti:tfEτiJτi,where Eτi⋅ is the expectation of all trajectories starting from Xti. In order to solve the noiseless optimal control problem, there are two main methods: one is to use the Ponteria Gold Minimal Principle (PMP), which requires solving a pair of ordinary differential equations, and the other is to use partial differential equations (PDEs) of the HJB equation. For the stochastic dynamic system with noise shown in equation (10), the extension of the HJB method for stochastic optimal control is expressed as(15)−∂tVt=minuti:tfqt+12utTRut+∇xVtTFt+12trace∇xxVtGtΣεGtT,where ∇xVt and ∇xxVt are the corresponding Jacobian and Hessian matrices and Ft is defined as Ft=fXt,t+GXtut. The corresponding optimal control can be calculated as ut=−R−1GtT∇xtVt [10]. Then, a nonlinear second-order PDE can be obtained as follows:(16)−∂tVt=qt+∇xVtTft−12∇xVtTGtR−1GtT∇xVt+12trace∇xxVtGtΣεGtTGtT.Vt can be transformed into exponential form Vt=−λlogΨt, where λ is a positive constant, and then use λR−1=Σε [24, 25]; then, PDE (16) can be simplified to the following form with the boundary condition which is Ψtf=e−1/λϕtf:(17)−∂tΨt=−1λqtΨt+ftT∇xVt+12trace∇xxΨtGtΣεTGtT.


## 3.3. Simplification of Path-Integral-Based RL

It is very difficult to obtain the analytical solution of PDE (17), so we replace it with commonly used numerical solutions, and one of which uses the Feynman–Kac theorem [26] as follows:(18)Ψti=EτiΨtfe−∫titf1/λqtdt=Eτie−1/λΨtf−1/λ∫titfqtdt.After discretization, the stochastic optimal control problem can be approximately transformed into the following path-integration problem:(19)Ψti=limdt⟶0∫pτi|xiexp−1λϕtf+∑j=iK−1qtjdtdτi,where τi=xti,…,xtf is a sample trajectory with its probability as pτi|xi. After some analysis [27], the following numerical solutions of optimal control can be obtained:(20)uti=∫pτi|xiuτidτi,where uτi=uti,…,utf is used to generate the trajectory τi and the probability is defined as(21)pτi|xi=e−1/λSτi∫e−1/λSτidτi,where Sτi is the general version of the path cost defined in [25]. However, the analytical form of calculating the path cost is very time-consuming because it involves many matrix inversions. Considering this fact, in order to simplify the calculation and speed up the training, we choose the linear path cost Sτi function:(22)Sτi=Jτi=tf−ti.It can be clearly seen that the generalized path-integral control method using the numerical solution of the stochastic dynamic system can avoid the calculation of gradient and kinematic inverse, ensuring fast and reliable convergence.


## 4. Goal-Directed Locomotion Algorithm

In the process of snake-shaped robot path-integral reinforcement learning, the reward that the snake-shaped robot gets depends on its own actions. Therefore, this paper extends the Markov decision process (MDP) model of path-integral reinforcement learning to multiple Markov decision processes (MDPs). Suppose that the snake-shaped robot can choosem actions (ai,i=1,2,…,m), and the number of states of the snake-shaped robot is k (sj,j=1,2,…,m). Based on the basic model of path-integral reinforcement learning, combined with task goal, this paper defines a snake-shaped robot goal-directed locomotion learning framework as shown in Figure 2.Figure 2
The snake-shaped robot path planning learning framework.It can be seen from Figure2 that in order to improve the learning speed of the robot, first of all, environmental factors are preprocessed to eliminate some irrelevant environmental factors, and then a three-dimensional environmental model is established. Then, the prior information is updated to the knowledge base, which improves the learning efficiency. In this model, based on the current state St, the snake-shaped robot takes an action At in the action set according to a certain policy rule based on the historical experience of the knowledge base. Next, when the action is executed, the environment will be transformed into a new state St+1, and a new reward value Rt+1 will be obtained. Finally, the historical experience is updated through continuous learning and further the knowledge base is improved. The snake-shaped robot periodically interacts with the environment, repeating the process of “exploration-learning-decision-utilization,” learning from historical actions, and updating their knowledge base as historical experience to guide the next action selection.


### 4.1. Guidelines for Setting the Reward Function

The reward function defines the snake-shaped robot’s learning goal and determines its perception state based on the environment; that is, it defines the value of the action. Since the robot tries to maximize the reward, the reward function is essentially used to guide the robot to achieve its goal. Therefore, the setting of the reward function will determine the convergence speed and degree of the reinforcement learning algorithm. In this article, the reward function is defined in the form of sparse reward, as shown in the following:(23)R=R1,when the robot collides with an obstacle,R2,when there is a collision between robots,R3,when the robot reaches the target point,0,other situations,where R1,R2,R3 are constants and R1<0,R2<0,R3>0.


### 4.2. Guidelines for Setting the Update Value Function

The goal-directed locomotion of the snake-shaped robot uses the Q-learning algorithm; that is to say, after determining all the environment statesS and actions A, a matrix Q will be generated, and the element QS,A in the matrix Q represents the value of robot’s choice of action At under the environment state St.The process of updating the value function is as follows: in the environment stateSt, when the snake-shaped robot selects an action At according to the action selection strategy, the snake-shaped robot reaches a new environment state St+1 after executing the action. At this time, we update the value of QS,A in the matrix Q. The update formula of QS,A is as follows:(24)QSt,At←QSt,At+αR+γmaxAt+1QSt+1,At+1−QSt,At,where γ is the attenuation coefficient and α is the learning efficiency.


### 4.3. Guidelines for Setting the Action Selection

In the path-integral reinforcement learning problem, exploration and utilization are a pair of contradictions: exploration means that the robot must try different behaviors and then collect more information, but utilization is that the robot makes the best decision under the current information. Exploration may sacrifice some short-term benefits and obtain more long-term and accurate benefit estimates by collecting more information; utilization focuses on maximizing short-term benefits based on the information already available. The exploration cannot be carried out endlessly, otherwise, too much short-term benefits will be sacrificed, and the overall benefits will be damaged; at the same time, short-term benefits should not be valued too much while ignoring some unexplored behaviors that may bring huge benefits. In this paper,ε-greedy exploration is used, where ε is the probability of the snake-shaped robot randomly selected (0<ε<1). In the case of probability 1−ε, the snake-shaped robot selects an action corresponding to the maximum Q value. If there is multiple Q with the same value, an action is randomly selected. When the probability is ε, the snake-shaped robot randomly selects an action from the action set.


### 4.4. Guidelines for Setting the Collision Avoidance

Collision avoidance must be considered in the process of the goal-directed locomotion, to simplify the calculation process, the ideal velocity for robot is assumed to bevT, and then the actual velocity vd can be calculated as follows:(25)vd=Pu,vvT,where Pu,v is the improved perturbation matrix [28], and the actual velocity vd is calculated by modifying the target velocity vT. Then, the planned path can be obtained by the recursive integration of vd. To describe the influence of collision avoidance, the perturbation matrix Pu,v is defined as follows:(26)Pu,v=I−nqnqTD∗u,vρu,vnq2+λu,vfunqTD∗u,vσu,vfunq,where I is a 2×2 identity matrix, nq is a column vector, fu is a tangential vector (the partial derivative of fu,v on u) at the point Qu,v. λu,v is a saturation function defining the orientation of tangential velocity; ⋅ is 2-norm of a vector or a matrix. Besides, ρu,v and σu,v are defined as the weight of nq and tq, respectively: (27)ρu,v=ρ0exp1D0−D∗u,v,σu,v=σ0exp1D0−D∗u,v,λu,v=1,vTTfunqTvT>λ0,vTTfunqTvTλ0,−λ0≤vTTfunqTvT≤λ0,−1,vTTfunqTvT<−λ0,where ρ0 is the repulsive parameter, σ0 is the tangential parameter, D0 is the minimum permitted distance between the boundary of the robot, and λ0 is a small positive threshold of the saturation function λu,v. Then, the actual velocity vd can be expressed as(28)vd=Pu,vvT=vT−nqTvTD∗u,vρu,vnq2nq+λu,vnqTvTD∗u,vσu,vfunqfu.It can be seen from equation (28) that vd consists of three parts: vT can be called the target velocity; nqTvT/D∗u,vρu,vnq2nq is taken as the repulsive velocity; λu,vnqTvT/D∗u,vσu,vfunqfu can be called the tangential velocity. Similarly, the perturbation matrix Pu,v can be divided into three parts: attractive matrix I, repulsive matrix nqnqT/D∗u,vρu,vnq2, and tangential matrix λu,vfunqT/D∗u,vσu,vfunq. Therefore, we can readjust the shape of the path by changing parameters ρu,v and σu,v. However, the perturbation matrix in this method can fully consider the shape of the obstacle and the position of the goal-directed locomotion and can more objectively describe the influence of the obstacle on the planned path.Theorem 1.
The planned path can avoid obstacle safely.Proof.
Vectorsnq and fu are perpendicular exactly, i.e., nqTfu=0. Suppose that the distance between the boundary of the robot is close to the minimum permitted distance D0; that is, suppose that D∗u,v=D0+δu,v and δu,v is a monotonically decreasing function:(29)D∗u,vρu,v=D0+δu,vρ0e−1/δu,v=D0ρ0e−1/δu,v1+δu,vD0D0/δu,vδu,v/D0ρ0e−1/δu,v.
It can be inferred thatδu,v⟶0+, then D0ρ0e−1/δu,v⟶1, 1+δu,v/D0D0/δu,v⟶e, and δu,v/D0ρ0e−1/δu,v⟶0; therefore, D∗u,vρu,v⟶1. Pu,v can be simplified as(30)Pu,v=I−nqnqTnq2+λu,vfunqTfunq.
Therefore, we can infer(31)nqTvd=nqTPu,vvT=nqTI−nqnqTnq2+λu,vfunqTfunqvT=nqT−nqT+λu,vnqTfunqTfunqvT.
Because the equationnqTfu=0 means that nqTvd=0, so the path is outside of the minimum permitted distance and there is no collision.Theorem 2.
The planned path can reach the destination eventually.Proof.
Because the goal of the path planning is to make the robot reach the destination safely, therefore, the velocity of the robot should have a component in the direction of the target velocity; that is, velocityvT and vd should satisfy vTTvd≥0, and the planned path will converge to the target point. Besides, vd≈vT should be satisfied if the robot near to the destination ξT=xT,yT,zT:(32)vTTvd=vTTPu,vvT=vT21−cos2vT,nqD∗u,vρu,v+λu,vvTTfunqTvTD∗u,vσu,vfunq,where ⋅ is the inner product of vectors, where vT,nq denotes the angle between vT and nq. It is obvious that when the robot approaches to the destination, then D∗u,v⟶0+:(33)limD∗u,v⟶0+D∗u,vρu,v=limD∗u,v⟶0+D∗u,vρ0exp1/D0−D∗u,v=+∞.
AsD∗u,vρu,v⟶+∞ and cos2vT,nq≤1 hold, it can be inferred that 1−cos2vT,nq/D∗u,vρu,v≥0. From equation (30), it can be inferred that λu,vvTTfunqTvT≥0. Therefore, vTTvd≥0 holds. When the robot approaches to the destination, that is to say, D∗u,vρu,v⟶+∞ and D∗u,vσu,v⟶+∞, then according to equation (29), it can be inferred that Pu,v⟶I. Therefore vd≈vT holds.


### 4.5. The Algorithm Steps of Goal-Directed Locomotion

In the process of goal-directed locomotion, the snake-shaped robot continuously interacts with the environment according to the current environment state and updates and modifies the learning results to guide the robot’s action selection. Finally, find a set of action sequences that can maximize the reward, and complete the goal-directed locomotion. The pseudocode of this method is shown in Algorithm1.Algorithm 1: The algorithm steps of goal-directed locomotion.
Initialize:St,Qs,arepeat(for each episode) Initialize:SWhileSt is not STIf (Probability less thanε)chooseAt=maxQStElseRandom chooseAtEndIf (There is no collision)Take actionAt, returnRandSUpdateQs,arepeatSt←STuntilSt=STuntil The task of goal-directed locomotion is finishedEnd


## 4.1. Guidelines for Setting the Reward Function

The reward function defines the snake-shaped robot’s learning goal and determines its perception state based on the environment; that is, it defines the value of the action. Since the robot tries to maximize the reward, the reward function is essentially used to guide the robot to achieve its goal. Therefore, the setting of the reward function will determine the convergence speed and degree of the reinforcement learning algorithm. In this article, the reward function is defined in the form of sparse reward, as shown in the following:(23)R=R1,when the robot collides with an obstacle,R2,when there is a collision between robots,R3,when the robot reaches the target point,0,other situations,where R1,R2,R3 are constants and R1<0,R2<0,R3>0.


## 4.2. Guidelines for Setting the Update Value Function

The goal-directed locomotion of the snake-shaped robot uses the Q-learning algorithm; that is to say, after determining all the environment statesS and actions A, a matrix Q will be generated, and the element QS,A in the matrix Q represents the value of robot’s choice of action At under the environment state St.The process of updating the value function is as follows: in the environment stateSt, when the snake-shaped robot selects an action At according to the action selection strategy, the snake-shaped robot reaches a new environment state St+1 after executing the action. At this time, we update the value of QS,A in the matrix Q. The update formula of QS,A is as follows:(24)QSt,At←QSt,At+αR+γmaxAt+1QSt+1,At+1−QSt,At,where γ is the attenuation coefficient and α is the learning efficiency.


## 4.3. Guidelines for Setting the Action Selection

In the path-integral reinforcement learning problem, exploration and utilization are a pair of contradictions: exploration means that the robot must try different behaviors and then collect more information, but utilization is that the robot makes the best decision under the current information. Exploration may sacrifice some short-term benefits and obtain more long-term and accurate benefit estimates by collecting more information; utilization focuses on maximizing short-term benefits based on the information already available. The exploration cannot be carried out endlessly, otherwise, too much short-term benefits will be sacrificed, and the overall benefits will be damaged; at the same time, short-term benefits should not be valued too much while ignoring some unexplored behaviors that may bring huge benefits. In this paper,ε-greedy exploration is used, where ε is the probability of the snake-shaped robot randomly selected (0<ε<1). In the case of probability 1−ε, the snake-shaped robot selects an action corresponding to the maximum Q value. If there is multiple Q with the same value, an action is randomly selected. When the probability is ε, the snake-shaped robot randomly selects an action from the action set.


## 4.4. Guidelines for Setting the Collision Avoidance

Collision avoidance must be considered in the process of the goal-directed locomotion, to simplify the calculation process, the ideal velocity for robot is assumed to bevT, and then the actual velocity vd can be calculated as follows:(25)vd=Pu,vvT,where Pu,v is the improved perturbation matrix [28], and the actual velocity vd is calculated by modifying the target velocity vT. Then, the planned path can be obtained by the recursive integration of vd. To describe the influence of collision avoidance, the perturbation matrix Pu,v is defined as follows:(26)Pu,v=I−nqnqTD∗u,vρu,vnq2+λu,vfunqTD∗u,vσu,vfunq,where I is a 2×2 identity matrix, nq is a column vector, fu is a tangential vector (the partial derivative of fu,v on u) at the point Qu,v. λu,v is a saturation function defining the orientation of tangential velocity; ⋅ is 2-norm of a vector or a matrix. Besides, ρu,v and σu,v are defined as the weight of nq and tq, respectively: (27)ρu,v=ρ0exp1D0−D∗u,v,σu,v=σ0exp1D0−D∗u,v,λu,v=1,vTTfunqTvT>λ0,vTTfunqTvTλ0,−λ0≤vTTfunqTvT≤λ0,−1,vTTfunqTvT<−λ0,where ρ0 is the repulsive parameter, σ0 is the tangential parameter, D0 is the minimum permitted distance between the boundary of the robot, and λ0 is a small positive threshold of the saturation function λu,v. Then, the actual velocity vd can be expressed as(28)vd=Pu,vvT=vT−nqTvTD∗u,vρu,vnq2nq+λu,vnqTvTD∗u,vσu,vfunqfu.It can be seen from equation (28) that vd consists of three parts: vT can be called the target velocity; nqTvT/D∗u,vρu,vnq2nq is taken as the repulsive velocity; λu,vnqTvT/D∗u,vσu,vfunqfu can be called the tangential velocity. Similarly, the perturbation matrix Pu,v can be divided into three parts: attractive matrix I, repulsive matrix nqnqT/D∗u,vρu,vnq2, and tangential matrix λu,vfunqT/D∗u,vσu,vfunq. Therefore, we can readjust the shape of the path by changing parameters ρu,v and σu,v. However, the perturbation matrix in this method can fully consider the shape of the obstacle and the position of the goal-directed locomotion and can more objectively describe the influence of the obstacle on the planned path.Theorem 1.
The planned path can avoid obstacle safely.Proof.
Vectorsnq and fu are perpendicular exactly, i.e., nqTfu=0. Suppose that the distance between the boundary of the robot is close to the minimum permitted distance D0; that is, suppose that D∗u,v=D0+δu,v and δu,v is a monotonically decreasing function:(29)D∗u,vρu,v=D0+δu,vρ0e−1/δu,v=D0ρ0e−1/δu,v1+δu,vD0D0/δu,vδu,v/D0ρ0e−1/δu,v.
It can be inferred thatδu,v⟶0+, then D0ρ0e−1/δu,v⟶1, 1+δu,v/D0D0/δu,v⟶e, and δu,v/D0ρ0e−1/δu,v⟶0; therefore, D∗u,vρu,v⟶1. Pu,v can be simplified as(30)Pu,v=I−nqnqTnq2+λu,vfunqTfunq.
Therefore, we can infer(31)nqTvd=nqTPu,vvT=nqTI−nqnqTnq2+λu,vfunqTfunqvT=nqT−nqT+λu,vnqTfunqTfunqvT.
Because the equationnqTfu=0 means that nqTvd=0, so the path is outside of the minimum permitted distance and there is no collision.Theorem 2.
The planned path can reach the destination eventually.Proof.
Because the goal of the path planning is to make the robot reach the destination safely, therefore, the velocity of the robot should have a component in the direction of the target velocity; that is, velocityvT and vd should satisfy vTTvd≥0, and the planned path will converge to the target point. Besides, vd≈vT should be satisfied if the robot near to the destination ξT=xT,yT,zT:(32)vTTvd=vTTPu,vvT=vT21−cos2vT,nqD∗u,vρu,v+λu,vvTTfunqTvTD∗u,vσu,vfunq,where ⋅ is the inner product of vectors, where vT,nq denotes the angle between vT and nq. It is obvious that when the robot approaches to the destination, then D∗u,v⟶0+:(33)limD∗u,v⟶0+D∗u,vρu,v=limD∗u,v⟶0+D∗u,vρ0exp1/D0−D∗u,v=+∞.
AsD∗u,vρu,v⟶+∞ and cos2vT,nq≤1 hold, it can be inferred that 1−cos2vT,nq/D∗u,vρu,v≥0. From equation (30), it can be inferred that λu,vvTTfunqTvT≥0. Therefore, vTTvd≥0 holds. When the robot approaches to the destination, that is to say, D∗u,vρu,v⟶+∞ and D∗u,vσu,v⟶+∞, then according to equation (29), it can be inferred that Pu,v⟶I. Therefore vd≈vT holds.


## 4.5. The Algorithm Steps of Goal-Directed Locomotion

In the process of goal-directed locomotion, the snake-shaped robot continuously interacts with the environment according to the current environment state and updates and modifies the learning results to guide the robot’s action selection. Finally, find a set of action sequences that can maximize the reward, and complete the goal-directed locomotion. The pseudocode of this method is shown in Algorithm1.Algorithm 1: The algorithm steps of goal-directed locomotion.
Initialize:St,Qs,arepeat(for each episode) Initialize:SWhileSt is not STIf (Probability less thanε)chooseAt=maxQStElseRandom chooseAtEndIf (There is no collision)Take actionAt, returnRandSUpdateQs,arepeatSt←STuntilSt=STuntil The task of goal-directed locomotion is finishedEnd


## 5. Simulation Examples

### 5.1. Build the 3D Environment Model

To guarantee real-time performance, parameters are first learned offline according to different initial states and then correspondingly adjusted online based on the robot’s states. To verify the effectiveness of the method proposed in this paper, a40×40 virtual environment is set up, which contains several obstacles of different shapes. Suppose that ξ=x,y,z is robot position and obstacle center is ξ0=x0,y0,z0, with axis lengths a, b, c, and index parameters d, e, f:(34)Fξ=x−x0a2d+y−y0b2e+z−z0c2f.Then, the parameters of the path-integral-based reinforcement learning algorithm are given in Table1.Table 1
The parameters of the path-integral-based reinforcement learning algorithm.
AttenuationLearning efficiencyε-Greedy exploration probabilityRewardRewardRewardParametersγ=0.85α=0.15ε=0.05R1=−0.5R2=−0.5R3=1Suppose that there are eight obstacles in environment as shown in Table2, and ρ0ξ=1 and σ0ξ=1.Table 2
The shape parameters of each obstacle.
ObstacleShapeabcdefCenter of the obstacle1Sphere555111(20, 20, 0)2Sphere666111(30,−2,0)3Sphere555111(15, 30, 0)4Cylinder55151110(22, 7, 0)5Cylinder44201110(35, 15, 0)6Cone669110.3(3, 27, 0)7Cone5519110.3(6, 3, 0)8Cone6621110.3(30, 36, 0)


### 5.2. Collision-Free Goal-Directed Locomotion that the Robot Does Not Need to Climb Obstacles

The method proposed in this paper adds prior information and has historical experience as decision support, so collisions are avoided during path planning. After 660 rounds of training, robots can successfully reach the target point. When there is no trap area, the robot does not need to climb obstacles and can avoid all obstacles smoothly and swiftly and reach the destination eventually. Suppose that the starting position of the robots is5,−3,0 and the target position is 39,39,0. The planned path is shown in Figure 3.Figure 3
Goal-directed locomotion that the robot does not need to climb obstacles from different starting positions.It can be shown from Figure3 that when the snake-shaped robot starts from different starting points, the algorithm proposed in this paper can avoid all obstacles smoothly and swiftly and reach the destination eventually.


### 5.3. Compared with Ant Colony Algorithm

By using Taguchi’s experimental design method and other intelligent optimization methods [29, 30], the ant colony algorithm is compared to the algorithms proposed in this paper. Suppose that the starting position of the on-water automatic rescue intelligent robot is 0,20,0, and the target position is 19,2,0. In the ant colony algorithm of this paper, the number of ants is 100. After each iteration, the pheromone attenuation ratio is 0.5, and each time the ant passes a path, the pheromone increase ratio is 2. It can be seen from Figure 4 that when the 3D ant colony algorithm is used for fast path planning, with different iteration times, the number of ants, etc., the planned path is also different. Moreover, 3D ant colony algorithm generally only seeks the shortest path, so the planned path is not very smooth as shown in Figure 5.Figure 4
The change of best individual fitness.
(a)(b)Figure 5
Planned path based on the ant colony algorithm.It can be seen from Figures4 and 5 that when the number of iterations is the same, the best individual fitness curve varies with the number of ants. In other words, ant colony algorithm relies on prior knowledge to a large extent, so it has its shortcomings in fast path planning. The method proposed in this paper considers prior information, uses historical experience as decision support, and introduces a climbing coefficient, so that obstacles can be successfully overcome. After more than 900 rounds of training, the two robots reached the target point safely without collision. In the process of goal-directed locomotion, the saturation λ represents the climbing ability of the robots. Furthermore, different λ will lead to different planned paths and will cause different climbing difficulties and different climbing heights. Therefore, we need to consider how to choose an appropriate coefficient λ to plan the most effective path. Suppose that the starting position of the robot is −10,−10,0, and the target position is 39,20,0. The planned path is shown in Figure 6.Figure 6
The planned path of the snake-shaped robot with climbing ability.It can be seen from Figure6 that the planned path according to different saturation λ and the saturation λ of the robot is 1/6. Both paths can bypass obstacles, escape the trap area, and reach the destination.


### 5.4. The Situation that the Robot Encounters the Trap Area

During the process of goal-directed locomotion, the robot will encounter the trap area. It is necessary to consider the distribution of obstacles and the climbing ability of the robot; otherwise, the robot will not be carried out timely and effectively. Suppose that there are six obstacles in environment as shown in Table3, and ρ0ξ=1 and σ0ξ=1. Suppose that the starting position is −10,−10,0, and the target position is 39,20,0.Table 3
The shape parameters of each obstacle.
ObstacleShapeabcdefCenter of the obstacle1Sphere555111(20, 20, 0)2Sphere777111(30,−2,0)3Cone669110.3(33, 27, 0)4Cone121219110.3(3, 27, 0)5Cone121221110.3(6, 3, 0)6Cone6621110.3(30, 36, 0)It can be seen from Figure7 that the trap area is formed by two different obstacles, and the snake-shaped robot will fall into the trap area. Therefore, we need to consider the climbing ability of the robot during path planning in the 3D environment.Figure 7
The robot falls into the trap area.


## 5.1. Build the 3D Environment Model

To guarantee real-time performance, parameters are first learned offline according to different initial states and then correspondingly adjusted online based on the robot’s states. To verify the effectiveness of the method proposed in this paper, a40×40 virtual environment is set up, which contains several obstacles of different shapes. Suppose that ξ=x,y,z is robot position and obstacle center is ξ0=x0,y0,z0, with axis lengths a, b, c, and index parameters d, e, f:(34)Fξ=x−x0a2d+y−y0b2e+z−z0c2f.Then, the parameters of the path-integral-based reinforcement learning algorithm are given in Table1.Table 1
The parameters of the path-integral-based reinforcement learning algorithm.
AttenuationLearning efficiencyε-Greedy exploration probabilityRewardRewardRewardParametersγ=0.85α=0.15ε=0.05R1=−0.5R2=−0.5R3=1Suppose that there are eight obstacles in environment as shown in Table2, and ρ0ξ=1 and σ0ξ=1.Table 2
The shape parameters of each obstacle.
ObstacleShapeabcdefCenter of the obstacle1Sphere555111(20, 20, 0)2Sphere666111(30,−2,0)3Sphere555111(15, 30, 0)4Cylinder55151110(22, 7, 0)5Cylinder44201110(35, 15, 0)6Cone669110.3(3, 27, 0)7Cone5519110.3(6, 3, 0)8Cone6621110.3(30, 36, 0)


## 5.2. Collision-Free Goal-Directed Locomotion that the Robot Does Not Need to Climb Obstacles

The method proposed in this paper adds prior information and has historical experience as decision support, so collisions are avoided during path planning. After 660 rounds of training, robots can successfully reach the target point. When there is no trap area, the robot does not need to climb obstacles and can avoid all obstacles smoothly and swiftly and reach the destination eventually. Suppose that the starting position of the robots is5,−3,0 and the target position is 39,39,0. The planned path is shown in Figure 3.Figure 3
Goal-directed locomotion that the robot does not need to climb obstacles from different starting positions.It can be shown from Figure3 that when the snake-shaped robot starts from different starting points, the algorithm proposed in this paper can avoid all obstacles smoothly and swiftly and reach the destination eventually.


## 5.3. Compared with Ant Colony Algorithm

By using Taguchi’s experimental design method and other intelligent optimization methods [29, 30], the ant colony algorithm is compared to the algorithms proposed in this paper. Suppose that the starting position of the on-water automatic rescue intelligent robot is 0,20,0, and the target position is 19,2,0. In the ant colony algorithm of this paper, the number of ants is 100. After each iteration, the pheromone attenuation ratio is 0.5, and each time the ant passes a path, the pheromone increase ratio is 2. It can be seen from Figure 4 that when the 3D ant colony algorithm is used for fast path planning, with different iteration times, the number of ants, etc., the planned path is also different. Moreover, 3D ant colony algorithm generally only seeks the shortest path, so the planned path is not very smooth as shown in Figure 5.Figure 4
The change of best individual fitness.
(a)(b)Figure 5
Planned path based on the ant colony algorithm.It can be seen from Figures4 and 5 that when the number of iterations is the same, the best individual fitness curve varies with the number of ants. In other words, ant colony algorithm relies on prior knowledge to a large extent, so it has its shortcomings in fast path planning. The method proposed in this paper considers prior information, uses historical experience as decision support, and introduces a climbing coefficient, so that obstacles can be successfully overcome. After more than 900 rounds of training, the two robots reached the target point safely without collision. In the process of goal-directed locomotion, the saturation λ represents the climbing ability of the robots. Furthermore, different λ will lead to different planned paths and will cause different climbing difficulties and different climbing heights. Therefore, we need to consider how to choose an appropriate coefficient λ to plan the most effective path. Suppose that the starting position of the robot is −10,−10,0, and the target position is 39,20,0. The planned path is shown in Figure 6.Figure 6
The planned path of the snake-shaped robot with climbing ability.It can be seen from Figure6 that the planned path according to different saturation λ and the saturation λ of the robot is 1/6. Both paths can bypass obstacles, escape the trap area, and reach the destination.


## 5.4. The Situation that the Robot Encounters the Trap Area

During the process of goal-directed locomotion, the robot will encounter the trap area. It is necessary to consider the distribution of obstacles and the climbing ability of the robot; otherwise, the robot will not be carried out timely and effectively. Suppose that there are six obstacles in environment as shown in Table3, and ρ0ξ=1 and σ0ξ=1. Suppose that the starting position is −10,−10,0, and the target position is 39,20,0.Table 3
The shape parameters of each obstacle.
ObstacleShapeabcdefCenter of the obstacle1Sphere555111(20, 20, 0)2Sphere777111(30,−2,0)3Cone669110.3(33, 27, 0)4Cone121219110.3(3, 27, 0)5Cone121221110.3(6, 3, 0)6Cone6621110.3(30, 36, 0)It can be seen from Figure7 that the trap area is formed by two different obstacles, and the snake-shaped robot will fall into the trap area. Therefore, we need to consider the climbing ability of the robot during path planning in the 3D environment.Figure 7
The robot falls into the trap area.


## 6. Conclusions

This paper proposes a collision-free goal-directed locomotion method based on path-integral reinforcement learning in a three-dimensional environment. First, a path-integral reinforcement learning model is established, which describes in detail how the robot can accumulate experience from historical data to optimize decision-making. In order to improve the learning efficiency of this method, some irrelevant environmental conditions are removed. TheQ table is updated based on the prior information and provided to the robot as historical experience, which greatly improves the learning efficiency of the robot. The simulation results show that the planned paths can avoid all obstacles smoothly and swiftly and reach the destination eventually.



---

*Source: 8824377-2021-02-08.xml*