3.1. Path planning based on DQN

It is well known that RL can be used for agents to maximize their cumulative rewards in unknown environments by exploring and exploiting collected experiences. In RL, exploration is contradictory to utilization. In the exploration, an agent experimenting with novel strategies may improve returns after long exploring. In the utilization, an agent focuses on maximizing rewards through actions. It is difficult to balance them, especially when an environment contains a huge state space or sparse rewards. An excessively large exploration rate may lead to endless exploration activities in some unexplored areas without any rewards. It thus sacrifices too much short-term benefits. An extremely large utilization rate may converge to suboptimal behaviors while ignoring action spaces with huge benefits.

Exploitation and utilization can be balanced explicitly by utilizing an $\varepsilon$ -greedy policy with a random selected exploration rate $\varepsilon \in (0,1)$ . The snake robot chooses an action corresponding to the maximum $Q$ value. For actions with same $Q$ values, the robot randomly selects an action. With a probability of $1-\varepsilon$ , the robot randomly selects an action in the action set.

The final strategy and convergence rate of the agent are determined by a reward function. In this work, the reward function is defined as:

(11) \begin{equation} r= \left \{ \begin{array}{l@{\quad}r} r_{a}, \quad collision\\[4pt] r_{b}, \quad collisionless\\[4pt] r_{c}, \quad others\\[4pt] \end{array}\!\!\!\!, \right. \end{equation}

where $r_{a}$ is a negative constant, while $r_{b}$ and $r_{c}$ are positive constants.

The pseudocode of the proposed DQN algorithm is shown in Algorithm 1. The algorithm has two key modules. The first one is a biology-inspired experience replay mechanism used to randomize the data. Effected by the mechanism, correlations in the observation sequence are removed and changes in the data distribution are smoothed. Updates of rewards and status obtained from each interaction are saved to update $Q_{\theta }$ .

Algorithm 1:

The DQN algorithm

With an empirical playback mechanism, the error between $Q_{\theta }$ and $\tilde{Q}$ is used to update parameters $\boldsymbol{\omega }$ of the neural network by using a gradient back-propagation method. An approximate $Q$ value and a specific strategy can be obtained when $w$ converges.

For the second module, an iterative strategy is used to update action values toward target values periodically. Correlations between them are deduced by this strategy. A snake robot selects and executes action $a_{t}$ from the action set based on history experiences of a restore buffer and a specific strategy in the current state $s_{t}$ . After action $a_{t}$ , the robot switches to a new state $s_{t+1}$ and obtains a reward $r_{t+1}$ .

The snake robot repeats a process of exploration-learning-decision-utilization and gains experiences by interacting with environments. The experience replay technology is used to update its knowledge, that is, as historical experience to select the next action. The flow diagram of the algorithm is shown in Fig. 2. Its parameters are explained in Table II.

3.2. Path smoothing

Due to the simplification of action space, there are many redundant inflection points in the generated paths. The planned path points are not smooth enough for snake robots. To solve this problem, we adopt a Floyd-MA algorithm to smooth the planned paths.

Table II.

Parameters of the DQN-based path planning algorithm.

Figure 2.

The DQN-based path planning algorithm of snake robots.

The Floyd algorithm is used to select proper path points and MA used to smooth and interpolate the path between two neighbor path points. Points on the quadratic curve are selected as filtered results. New values of center points are computed by averaging their neighbor points, that is,

(12) \begin{equation} y_{s}(i)=\frac{1}{2n_{p}+1}\left(y\left(i+n_{p}\right)+y\left(i+n_{p}-1\right)+\ldots +y\left(i-n_{p}\right)\right), \end{equation}

where $n_{p}$ is the number of selected path points. Pseudocode of the Floyd method is shown in Algorithm 2. Its parameters are listed in Table III.

4.1. Stochastic optimal control

Stochastic optimal control aims at maximized goal performance. To evaluate performances of the trajectory $\tau _{i}$ between $t_{i}$ and $t_{N})$ , a reward function is defined as:

(13) \begin{equation} \begin{split} J(\tau _{i})=\phi [\boldsymbol{x}(t_{N}),t_{N}]+\int ^{t_{N}}_{t_{i}}L[\boldsymbol{x}(t),\boldsymbol{u}(t),t]dt, \end{split} \end{equation}

where $\boldsymbol{x}(t)$ and $\boldsymbol{u}(t)$ are the state and control vector at time $t$ , respectively. Parameters $\phi [\cdot ]$ and $L[\cdot ]$ are a terminal reward and an immediate reward at time $t$ , respectively. The minimum cost function in stochastic optimal control is as follows:

(14) \begin{equation} \begin{split} V_{t_{i}}=\min \limits _{u_{t_{i}}:t_{N}}E_{\tau _{i}}[J(\tau _{i})], \end{split} \end{equation}

where $E_{\tau _{i}}[\cdot ]$ is expectations of all trajectories starting at state $\boldsymbol{x}_{t_{i}}$ . The stochastic dynamic system is considered as:

(15) \begin{equation} \begin{split} \dot{\boldsymbol{x}_{t}}=\boldsymbol{f}(\boldsymbol{x}_{t},t)+\boldsymbol{G}(\boldsymbol{x}_{t})(\boldsymbol{u}_{t}+\boldsymbol{\varepsilon }_{t})=\boldsymbol{f}_{t}+\boldsymbol{G}_{t}(\boldsymbol{u}_{t}+\boldsymbol{\varepsilon }_{t}), \end{split} \end{equation}

where $\boldsymbol{x}_{t}\in R^{n\times 1}$ is a state vector of the system, $\boldsymbol{G}_{t}=\boldsymbol{G}(\boldsymbol{x}_{t})\in R^{n\times p}$ is a control matrix, $\boldsymbol{f}_{\boldsymbol{t}}=\boldsymbol{f}(x_{\boldsymbol{t}},t)\in R^{n\times 1}$ is a passive dynamics, $\boldsymbol{u}_{t}\in R^{p\times 1}$ is a control vector, and $\boldsymbol{\varepsilon }_{t}\in R^{p\times 1}$ is a Gaussian noise with a zero mean and a variance $\Sigma _{\varepsilon }$ . The immediate reward is defined as:

(16) \begin{equation} \begin{split} L_{t}=L[\boldsymbol{x}(t),\boldsymbol{u}(t),t]=q_{t}+\frac{1}{2}\boldsymbol{u}_{t}^{T}\boldsymbol{R}\boldsymbol{u}_{t}, \end{split} \end{equation}

where $q_{t}=q(\boldsymbol{x}_{t},t)$ is an arbitrary state-dependent cost function, and $\boldsymbol{R}$ is a positive semi-definite weight matrix of the quadratic control cost. The HJB equation of this stochastic optimal control problem is defined as:

(17) \begin{equation} \begin{split} -\partial _{t}V_{t}=\min (L_{t}+(\boldsymbol{\nabla }_{x}V_{t})^{T}\boldsymbol{F}_{t})+\frac{1}{2}trace\left(\left(\boldsymbol{\nabla }_{xx}V_{t}\right)\boldsymbol{G}_{t}\boldsymbol{\Sigma _{\varepsilon }}\boldsymbol{G}_{t}^{T}\right), \end{split} \end{equation}

where $\boldsymbol{\nabla }_{x}$ and $\boldsymbol{\nabla }_{xx}$ are the Jacobian matrix and Hessian matrix, respectively. Substituting the optimal control into (17), we can get a second-order nonlinear partial differential equation (PDE):

(18) \begin{align} -\partial _{t}V_{t} & = q_{t}+(\boldsymbol{\nabla }_{x}V_{t})^{T}f_{t}-\frac{1}{2}(\boldsymbol{\nabla }_{x}V_{t})^{T}\boldsymbol{G}_{t}^{T}\boldsymbol{R}^{-1}\boldsymbol{G}_{t}^{T}(\boldsymbol{\nabla }_{x}V_{t})\nonumber\\[4pt]& \quad +\frac{1}{2} trace\left(\left(\boldsymbol{\nabla }_{xx}V_{t}\right)\boldsymbol{G}_{t}\boldsymbol{\Sigma _{\varepsilon }}\boldsymbol{G}_{t}^{T}\right). \end{align}

4.2. Transformation of HJB into a linear PDE

Due to the complexity of HJB equation, an exponential transformation of the value function is proposed as $V_{t}=-\lambda \lg \Psi _{t}$ . A second-order linear PDE can be obtained as:

(19) \begin{equation} -\partial _{t}\Psi _{t}=-\frac{1}{\lambda }q_{t}\Psi _{t}+\boldsymbol{f}_{t}^{T}(\boldsymbol{\nabla }_{x}\Psi _{t})+\frac{1}{2}trace\left(\left(\boldsymbol{\nabla }_{xx}\Psi _{t}\right)\boldsymbol{G}_{t}\Sigma _{\varepsilon }\boldsymbol{G}_{t}^{T}\right),\\[4pt] \end{equation}

where $\Psi _{tN}=\exp ({-}\frac{1}{\lambda }\phi _{tN})$ is the boundary condition, and the analytic solution can be obtained according to the Feyman–Kac theorem:

(20) \begin{equation} \Psi _{t_{i}}=E\left(\Psi _{t_{i}}\exp \left({-}\int _{t_{i}}^{t_{N}}\frac{1}{\lambda }q_{t}dt\right)\right)=E\left(\Psi _{t_{i}}\left(\exp \left({-}\frac{1}{\lambda }\phi _{tN}-\frac{1}{\lambda }\int _{t_{i}}^{t_{N}}q_{t}dt\right)\right)\right).\\[4pt] \end{equation}

In this way, the stochastic optimal control problem is transformed into an approximate PI problem:

(21) \begin{equation} \Psi _{t_{i}}=\lim \limits _{dt\rightarrow 0}\int p(\boldsymbol{\tau}_{\boldsymbol{i}}|\boldsymbol{x}_{i})\exp \left[-\frac{1}{\lambda }\left(\phi _{t_{N}}+\sum \limits _{j=i}^{N-1}q_{t_{j}}dt\right)\right]d\boldsymbol{\tau}_{\boldsymbol{i}}, \end{equation}

where $\boldsymbol{\tau}_{i}=[\boldsymbol{x}_{t_{i}},\boldsymbol{x}_{t_{i+1}},\ldots,\boldsymbol{x}_{t_{N}}]^{T}$ is a sample path which starts from $\boldsymbol{x}_{t_{i}}$ . The probability of $\boldsymbol{\tau}_{\boldsymbol{i}}$ is as follows:

(22) \begin{equation} p(\boldsymbol{\tau}_{\boldsymbol{i}}|\boldsymbol{x}_{i})=\frac{\exp \left({-}\frac{1}{\lambda }\right)}{\int \exp \left({-}\frac{1}{\lambda }\right)d\boldsymbol{\tau}_{\boldsymbol{i}}}. \end{equation}

The optimal controls can be simplified as:

(23) \begin{equation} \begin{split} \boldsymbol{u}_{t_{i}}=\int p(\boldsymbol{\tau}_{i}|\boldsymbol{x}_{i})\boldsymbol{u}_{L}(\boldsymbol{\tau }_{i})d\boldsymbol{\tau }_{i}. \end{split} \end{equation}

However, the above analytic solutions depend on the system model $\boldsymbol{f}_{t}$ . Using the PI algorithm, the probability of trajectory $\tau _{i,k}$ , that is, the $k$ -th path generated randomly in the start state $\boldsymbol{x}_{t_{i}}$ , is updated as:

(24) \begin{equation} P(\tau _{i,k})=\frac{\exp \left({-}\frac{1}{\lambda }S(\tau _{i,k})\right)}{\sum \limits _{j=1}^{K}\exp \left({-}\frac{1}{\lambda }S\left(\tau _{i,j}\right)\right)}. \end{equation}

The control vector can be iterated as:

(25) \begin{equation} \begin{split} \boldsymbol{u}=\boldsymbol{u}+\sum \limits _{k=1}^{K}P(\tau _{i,k})\frac{\boldsymbol{R}^{-1}\boldsymbol{G}_{t_{i},k}\boldsymbol{G}_{t_{i},k}^{T}}{\boldsymbol{G}_{t_{i},k}^{T}\boldsymbol{R}^{-1}\boldsymbol{G}_{t_{i},k}}. \end{split} \end{equation}

The objective is to find $\boldsymbol{u}_{i}$ that minimizes the cost function corresponding to the trajectory $\tau _{i}$ generated by the control variable:

(26) \begin{equation} \begin{split} V=\min \limits _{u}E_{\tau _{i}}[R(\tau _{i})]=\min \limits _{u}\int R(\tau _{i})P(\tau _{i})d\tau _{i} \end{split}. \end{equation}

The probability of trajectory $\tau _{i}$ is computed as:

(27) \begin{equation} \begin{split} P(\tau _{i})=\frac{S(\tau _{i})}{\sum \limits _{j=1}^{K}\tau _{i}}, \end{split} \end{equation}

where $K$ is the number of generated trajectories and $S(\tau _{i})$ is computed as:

(28) \begin{equation} S(\tau _{i})=\exp \left({-}\frac{R(\tau _{i})-\min (\boldsymbol{R})}{\max (\boldsymbol{R})-\min (\boldsymbol{R})}\right), \end{equation}

where $\boldsymbol{R}=[R(\tau _{1}), R(\tau _{2}), \cdots, R(\tau _{K})]$ . The control vector $\boldsymbol{u}^{\ast }$ of the next iteration is as follows:

(29) \begin{equation} \boldsymbol{u}^{\ast }=\sum \limits _{i=1}^{K}P(\tau _{i})\boldsymbol{u}_{i}. \end{equation}

4.3. Policy improvements with PI

A compound serpenoid curve templet is proposed based on research of biological snakes in ref. [Reference Dai, Travers, Dear, Gong and Choset33]. For the lateral undulation gait, snake robots move forward by swinging their yaw joints. Joint angles are defined as:

(30) \begin{equation} \phi _{i,ref} = \alpha _{h}\sin (\omega _{h} t+(i-1)\beta _{h})+\gamma _{h}, \end{equation}

where $\phi _{i,ref}$ is the reference yaw angle of joint $i$ at time $t$ , and $\alpha _{h}$ , $\omega _{h}$ , $\beta _{h}$ , and $\gamma _{h}$ are amplitude, angular frequency, phase difference, and phase offset, respectively. Gait parameters of the robot can be expressed as a vector $\boldsymbol{U}=[\alpha _{h},\omega _{h},\beta _{h},\gamma _{h}]^{T}$ . Values of initial and the $\xi$ -th iteration of gait parameters are denoted as $\boldsymbol{U}_{\textbf{0}}=[\alpha _{0}, \omega _{0}, \beta _{0}, \gamma _{0}]^{T}$ and $\boldsymbol{U}_{\boldsymbol{\xi}}=[\alpha _{\xi },\omega _{\xi },\beta _{\xi },\gamma _{\xi }]^{T}$ , respectively. Each iteration of training generates $K$ paths randomly. Path $\tau _{i}$ is determined by $\boldsymbol{u}_{\boldsymbol{i}}=[\varepsilon _{\alpha _{h_{i}}},\varepsilon _{\omega _{h_{i}}},\varepsilon _{\beta _{h_{i}}},\varepsilon _{\gamma _{h_{i}}}]^{T}$ , where $\varepsilon$ obeys normal distribution with a zero mean. The corresponding loss function $R(\tau _{i})$ is expressed as:

(31) \begin{equation} R(\tau _{1})=\sqrt{(x_{0}-x_{g})^{2}+(y_{0}-y_{g})^{2}}. \end{equation}

During the goal-oriented movement of multiple path points, the snake robot may fail to track unsmooth paths. In order to make the robot complete the goal-oriented motion, we design a new loss function that combines the deviation of trajectory and moving difficulty:

(32) \begin{equation} R(\tau _{1})=\sqrt{(x_{0}-x_{g})^{2}+(y_{0}-y_{g})^{2}}+\left|\arctan \overline{\theta }-\frac{y_{g+1}-y_{g}}{x_{g+1}-x_{g}}\right|, \end{equation}

where $\bar{\theta }=\frac{1}{N}\sum ^{N}_{i=1}\theta _{i}$ is a robot’s forward direction. Parameters of path $i$ in the $(\xi +1)$ - $th$ training are as follows:

(33) \begin{equation} {\boldsymbol{U}_{\boldsymbol{\xi} +\textbf{1}}^{\boldsymbol{i}}}=\boldsymbol{U}_{\boldsymbol\xi }+ \boldsymbol{u}_{\boldsymbol{i}}=\left[\alpha _{\xi }+\varepsilon _{\alpha _{i}},\omega _{\xi }+\varepsilon _{\omega _{i}},\beta _{\xi }+\varepsilon _{\beta _{i}},\gamma _{\xi }+\varepsilon _{\gamma _{i}}\right]^{T}.\\[4pt] \end{equation}

Gait parameters after $\xi +1$ iterations are updated as:

(34) \begin{equation} \begin{split} \boldsymbol{U}_{\boldsymbol\xi +\textbf{1}}= \boldsymbol{U}_{\boldsymbol\xi}+ \boldsymbol{u}^{\boldsymbol\ast}. \end{split} \end{equation}

The variance of $\varepsilon$ is related to the loss function value produced by $\boldsymbol{U}_{\boldsymbol\xi}$ after $\xi$ iterations, and the standard deviation in the $(\xi +1)$ - $th$ training is

(35) \begin{equation} \boldsymbol{\sigma }=\left[\sigma _{\alpha },\sigma _{\omega },\sigma _{\beta },\sigma _{\gamma }\right]^{T}=\left[f_{\alpha }(r), f_{\omega }(r), f_{\beta }(r), f_{\gamma }(r)\right]^{T}. \end{equation}

4.4. Closed-loop control framework

To realize the autonomous movement of snake robots in complex environments, this work designs a closed-loop control framework. As shown in Fig. 3, it is composed of path planning, path smoothing, and motion planning modules.

In path planning, actions are selected by a neural network. The robot gains experiences by continuous interacting with environments. Weights of the network are updated by a back-propagation algorithm based on an experience replay technology. In path smoothing, path points generated by the trained strategy are filtered and adjusted by the proposed Floyd-MA path smoothing algorithm.

In motion planning, obtained reliable path points are used to obtain gait parameters trained by PI algorithm in parallel to achieve goal-oriented motion. Its pseudocode is shown in Algorithm 3.

Algorithm 3

The Goal-oriented Motion Planning Algorithm Based on PI Method

Figure 3.

Global framework for motion planning of snake robots.

5.1. Path planning of snake robots

A virtual snake robot prototype is built in CoppeliaSim. Same with our previous work [Reference Cao, Zhang and Zhou20], a robot prototype is constructed. To generate anisotropic friction between a robot and the ground, a pair of passive wheels are equipped at each link. Communication and control of snake robots in CoppeliaSim are implemented through a remote API.

Obstacles are represented by rectangles with arbitrary poses. They are dilated on each side. The dilated distance is the maximum gait amplitude set in the optimization problem. Parameters of the neural network and a path smoothing algorithm are set as Table I and Table II, respectively. Simulation results are exhibited to show the process of motion planning.

As shown in Fig. 4(a), obstacles (red and yellow) are dilated by safety margins (green). The start position of a snake robot is the lower right corner. The target position is the upper left corner. As shown in Fig. 4(b), a passable path in a complex environment is generated by using DQN, but there exist collinear path points and excessive curvature changes between neighboring points. As shown in Fig. 4(c), redundant collinear path points and points with excessive curvature changes are deleted by the Floyd algorithm. As shown in Fig. 4(d), MA improves the path smoothness and reduces the difficulty of path following.

Table IV.

Parameters of the PI motion planning algorithm.

Figure 4.

Path planning of a snake robot. (a) Simulation environment; (b) path planning; (c) path point filtering; and (d) path smoothing.

Figure 5.

Performances of PI in goal-oriented motion of the snake robot. (a) The first stage. (b) The second stage. (c) The third stage.

Figure 6.

Values of gait control parameters. (a) The first stage. (b) The second stage. (c) The third stage.

5.2. Motion planning of snake robots

In order to verify the feasibility of the proposed PI-based motion planning algorithm, we conduct extensive simulation in CoppeliaSim. The PI method is used to train three path segments in parallel as prior knowledge, and then the spaces of gait parameters on the global path are explored serially to realize end-to-end mapping between the planned path points and gait parameters. Parameters used in motion planning are listed in Table IV. The standard deviation is computed as:

(36) \begin{equation} \sigma =f(r)=\left \{ \begin{array}{l@{\quad}r} \sigma _{0}, \quad \quad \,\,\,\, r\geq 0.5 \\[4pt] r\sigma _{0}/2, \quad 0.2\leq r\lt 0.5\\[4pt] r\sigma _{0}/4, \quad 0.05\leq r\lt 0.2\\[4pt] r\sigma _{0}/8, \quad 0.2\leq r\lt 0.05 \end{array}. \right. \end{equation}

As shown in Fig. 5(a)-(c), loss functions of the first stage to the third stage converge to stable states. As shown in Fig. 6(a)-(c), corresponding to the change curve of the parameters of the gait equation as the iteration count increases. These curves converge to stable states from initial states quickly. After training, gait parameters in the first stage converge to $\boldsymbol{U}^{(1)}=[0.6129, 7.2016, 0.7238, 0.0367]^{T}$ as shown in Fig. 6(a). Gait parameters in the second stage converge to $\boldsymbol{U}^{(2)}=[0.5869, 6.5121,$ $0.7915, 0.0227]^{T}$ as shown in Fig. 6(b). Gait parameters in the third stage converge to $\boldsymbol{U}^{(3)}=[0.5914, 6.5964, 0.8163, 0.0381]^{T}$ as shown in Fig. 6(c).

Trajectory of the robot’s CM is shown in Fig. 7. It can be got that the snake robot can move toward to a target smoothly in a multi-obstacle environment controlled by using the proposed path planning and motion planning methods.

Figure 7.

A snake robot moves in a multi-obstacle environment controlled by the PI-based motion planning algorithm.