Simultaneous Localization and Mapping Methods for Snake-like Robots Based on Gait Adjustment

Abstract

Snake robots require autonomous localization and mapping capabilities for field applications. However, the characteristics of their motion, such as large turning angles and fast rotation speeds, can lead to issues like drift or even failure in positioning and map building. In response to this situation, this paper starts from the gait motion characteristics of the snake robot itself, proposing an improved gait motion method and a tightly coupled method based on IMU and visual information to solve the problem of poor algorithm convergence caused by head-shaking in snake robot SLAM. Firstly, the adaptability of several typical gaits of the snake robot to SLAM methods was evaluated. Secondly, the serpentine gait was selected as the object of gait improvement, and a head stability control method for the snake robot was proposed, thereby reducing the interference of the snake robot’s motion on the sensors. Thirdly, a visual–inertial tightly coupled SLAM method for the snake robot’s serpentine gait and Arc-Rolling gait was proposed, and the method was verified to enhance the robustness of the visual SLAM algorithm and improve the positioning and mapping accuracy of the snake robot. Finally, experiments proved that the methods proposed in this paper can effectively improve the accuracy of positioning and map building for snake robots.

1. Introduction

Currently, robots have a wide range of applications in many fields, such as security, search and rescue, and transportation. Operating in narrow and confined spaces is one of the practical requirements for robots in these fields, requiring specialized capabilities such as compact size and strong environmental adaptability [1]. Among all types of robots, serpentine robots have a thin cross-section and an elongated flexible body, which helps the body to move forward in very narrow spaces with minimal interference to the surrounding environment, making it highly advantageous in the detection of complex and narrow environments, such as post-disaster rescue, underground exploration, and so on [2]. However, in GPS denial environments, snake robots are unable to obtain their position and environmental information, making it difficult for them to realize autonomous navigation and thus difficult to discover the purpose of performing tasks autonomously [3]. SLAM technology addresses the problem of recovering the relative spatial relationship between the observer itself and its surroundings, giving a description of the surrounding environment. With this foundation, the snake robots can carry out path planning, detect obstacles encountered promptly, and avoid obstacles encountered in time [4,5].

SLAM is an important technique to estimate the state of the target and its surroundings [6,7,8]. Currently, snake robots face many challenges in SLAM applications, such as large hardware size, high energy consumption, algorithmic complexity, and high computational effort [9]. Although LiDAR and ultrasonic sensors can provide environmental depth information to help navigation and localization, they usually have insufficient depth resolution and lack color information, which limits their application in target detection and recognition [10]. In contrast, vision cameras can capture rich scene details and have the advantages of small size, low energy consumption, and cost-effectiveness, making them ideal for SLAM solutions for snake robots [11].

However, the snake robot itself has a variety of motion forms, such as serpentine, rolling, side-shifting, side-rolling, lifting up, overcoming obstacles, etc. During the rapid movement and rotation of the snake robot, there are motion characteristics such as violent shaking, large corners, and fast rotation speed that occur as a result of feature-point tracking failure, leading to poor localization and mapping accuracy, poor robustness, and even failure [12,13]. In a previous work, Gong. C et al. [14] used a modal decomposition method to keep the head of a snake robot in the same direction during rolling gait. This method ensures that the head camera of the snake robot works properly during rolling gait. H. Yamada et al. [15] stabilized the head of the snake robot by controlling the movement of its neck. However, when the method is converted from a continuous model to a discrete model, it causes the head position to move back and forth, which affects the control of the robot. Z. Bing [16] compensated for the head offset angle of the snake robot by calculating the derivative of the snake curve concerning time. Wu et al. [17] introduced virtual joints as the desired directions and analyzed the virtual joints in the HNC (Head Navigational Control) about the first two head joint angle relationships. However, this lacks validation of the head navigation effect, and it is difficult to adjust the parameters using a cyclic inhibition model. G. Qiao et al. [18] implemented partial control by proposing to divide the snake robot into different parts such as the head, neck, and torso. The torso is used to propel the snake robot forward, and the neck and head are used to control the direction of the snake robot. However, this method requires multiple modules to be treated as necks, thus increasing the number of joints and making it difficult to apply to curvature control-based CPG models. Wu et al. [19] used virtual joints based on a CPG network to achieve head stabilization. Although this scheme can effectively ensure the head stabilization of the snake robot, it lacks the feedback of head position information and is not efficient.

The low viewing angle of snake robots makes them susceptible to obstacles, which leads to visual loss and trajectory offset. Kazunori Ohno et al. [20] implemented snake robot trajectory estimation and SLAM reconstruction using an SR-2 RGBD camera and a gyroscope, which has a large yaw angle error, large size, and high power consumption. Liu et al. [21] proposed a bi-directional long short-term memory (BiLSTM) neural network for a snake robot trajectory prediction method and visual localization system, which solves the problem of the visual tracking loss of the snake robot. Kim et al. [22] proposed an error-based adaptive robust RBF-NN-BSC for effective image data reading of the snake robot head system while the snake robot is traveling. Li et al. [23] proposed a system based on the ORB-SLAM3 for stabilized RGB-D SLAM to improve localization accuracy in dynamic environments. Wang et al. [24] develop Dym-SLAM for reconstructing 4D dynamic scenes using rigid moving objects. Despite their contributions, existing methods often face challenges related to generalization, scene awareness, or accurate segmentation.

To solve the above problems, this paper starts from the typical gait motion characteristics of a snake robot, analyzes the gait characteristics of a snake robot and the adaptability with SLAM, and based on this, puts forward a scheme to improve the visual SLAM localization accuracy of the snake robot and the effect of constructing a map: by controlling the joint angle of the motion of the snake robot to improve the form of the snake robot’s movement and enhance the stability of the snake robot in the process of movement and thus improve the SLAM effect. On the other hand, from the point of view of the accuracy and robustness of the SLAM algorithm itself, the multi-sensor fusion method is used to enhance the robustness of the visual SLAM algorithm.

The overall structure of this paper is as follows: Section 2 analyzes the adaptability of each gait of the snake robot to the SLAM method. Section 3 proposes the head stability algorithm of the snake robot to reduce the amplitude of sensor shaking based on serpentine gait. Section 4 details a vision-inertial SLAM approach for snake robot motion swaying. Finally, Section 5 presents the snake robot platform for the related experiments and validates the algorithms proposed.

2. Adaptability Study of SLAM for Typical Gaits of Snake-like Robots

Gait generation methods are used to determine the form of motion of the snake robot by controlling the change in joint angles, while the control function method is currently the main method for controlling the gait generation of snake robots, which is characterized by model independence and flexible adjustability. The parameterized gaits are all based on sinusoidal waves in two mutually perpendicular planes: a horizontal plane parallel to the ground and a vertical plane perpendicular to the ground and parallel to the snake’s longitudinal axis. When the wave perpendicular to the ground is sent, only the even-numbered joints are involved in the motion and the odd-numbered joints remain stationary; similarly, only the odd-numbered modules are involved in the motion of the transverse wave sent through the robot parallel to the ground [25]. Each joint angle is modeled as a sinusoidal function with a phase shift by describing the gait through joint angles. Then, the angle of the nth joint at the moment $t$ can be expressed as follows:

$$\alpha (n,t)=\left\{\begin{array}{l}{\beta }_{odd}+A_{odd}\sin ({\theta }_{odd}),n=odd\\ {\beta }_{even}+A_{even}\sin ({\theta }_{even}+\delta ),n=even\end{array}\right.$$

(1)

$$\theta ={\displaystyle \frac{d\theta }{dn}}n+{\displaystyle \frac{d\theta }{dt}}t$$

(2)

where $\beta$ is the offset, $A$ is the amplitude, $\theta$ is the frequency, $\delta$ is the phase shift, $A_{even}$ and $A_{odd}$ is the amplitude term, $d\theta /dn$ determines the shape of the serpentine robot, and $d\theta /dt$ is the magnitude of the frequency.

Based on the above model, the motion adaptations of four typical gaits, namely, serpentine gait, rolling gait, pedal gait, and sidewinding gait, are analyzed sequentially for ORB-SLAM.

The meandering gait, as the most common two-dimensional locomotion for snake-like robots, is suitable for movement on flat surfaces, propagating body waves from the head to the tail, which is the most widespread mode of movement exhibited by biological snakes. The rolling gait is unique to snake-like robots, including two-dimensional arc rolling within a plane and three-dimensional helix rolling in space, which is not a natural movement for biological snakes. In the pedal gait, the robot makes contact with the ground at only a few points to generate thrust while lifting areas with high curvature to reduce friction. The sidewinding gait combines transverse and longitudinal waves, allowing the robot to both flip and side-shift perpendicular to the body direction.

The form of snake robot motion under serpentine gait is sinusoidal, so the size of the sinusoidal wave amplitude will be an important factor affecting the adaptability of this gait with ORB-SLAM, while the increase in speed will make the snake robot’s left and right swing frequency speed up. Therefore, the idea of studying the adaptability of testing the serpentine gait with ORB-SLAM is to change the wave amplitude of the snake robot’s motion and the speed of the snake robot’s motion under each wave amplitude. Based on the analysis of the error between the estimated value and the true value of the motion, the effect of the size of the wave amplitude and the speed of the motion on the SLAM adaptability of the snake robot is judged.

The snake robot model for the adaptive analysis in this section consists of 35 joints with a length of 89.95 mm, and the robot model’s joints are connected orthogonally to each other. Since amplitude A is related to $\alpha$, the change in parameter A is replaced by changing $\alpha$. When the parameters of the fitting curve are five joint lengths and $\alpha$ is 0.1π, the wave amplitude is small during the motion of the snake robot, and the maximum pendulum amplitude is 0.173 m (5.50%). The head stability of the snake robot in this state is better, the feature extraction is continuous, and there is no tracking loss. When $\alpha$ = 0.3π, the swing amplitude of the snake robot increases significantly, with a maximum swing amplitude of 0.416 m (13.2%) and the SLAM method estimates the trajectory to be cluttered during the simulation, then the SLAM method is assumed to be invalid. When $\alpha$ = 0.4π, 0.5π, even if the fitting curve parameter is set to a joint length, it is still impossible to build a map, and the maximum swing is 0.56 m (17.8%) when $\alpha$ = 0.4π, and 0.62 m (19.7%) when $\alpha$ = 0.5π. As the parameter increases, the waveforms of the motion become bigger and bigger, which leads to less or even no feature extraction, and the visual front-end cannot obtain data correlation through feature matching, which leads to the failure of camera position solving.

Under the arc-rolling gait, the snake robot rotates around its axis. Therefore, the idea of testing the localization effect of the snake robot under this gait is to change the size of the fitting curve parameters while keeping other parameters unchanged. Through the simulation analysis, the arc-rolling gait movement speed is lower, the image captured by the camera is clearer, at this time the feature extraction is more stable, and there is no loss of keyframes; pure rotary motion lacks translation-related data excitation, the motion matrix degrees of freedom remain unchanged, while the motion itself decreases in the degrees of freedom, which makes the extra degrees of freedom susceptible to the influence of noise; with the increase in speed, the image captured by the camera appears to be dragged, which is a good idea. With the increase in speed, the image captured by the camera suffers from the phenomenon of dragging, leading to a reduction in the number of features extracted, and the system suffers from the problem of lost trajectory tracking due to insufficient features, which in turn leads to the failure of the ORB-SLAM scheme.

Due to the characteristics of the pedal gait, the snake robot sacrifices stability characteristics while pursuing high motion efficiency. In the vision SLAM method with ORB features as the front-end, the pedal gait causes the camera attached to the head of the snake robot to shake up and down as the snake robot jerks up and down drastically during the motion process, which leads to the failure of the extraction of the ORB feature points and the matching of the failure of the feature. The ORB-SLAM scheme fails in this gait.

In comparison with the arc-rolling gait, the rotation of the sidewinding gait is a rotation around the support point with the surface at different moments rather than a rotation around its axis. The head joints of the snake robot cannot operate smoothly in this gait because the snake robot undergoes a large displacement at the same time as the rotation. The camera shoots the ground for a long time in this gait, and the ORB feature extraction fails due to the lack of texture information and the high speed. So the ORB-SLAM scheme fails in this gait.

From the above analysis, it can be concluded that the pedal gait and the sidewinding gait are poorly adapted to SLAM due to high-frequency jerks and are not suitable for performing the tasks of localization and map building. Therefore, the sidewinding gait and the arc-rolling gait are selected as SLAM research objects. To improve the SLAM adaptability of the snake robot gait, several methods are proposed to address the problems of the large swings of the head joints and the too fast pure rotational speed in the movement process.

3. Research on Visual SLAM Method Based on Head Stability Control of Snake-like Robots

For the snake robot’s motion gait on the horizontal plane, which refers to the robot’s movement within the two-dimensional space defined by the ground surface, the serpentine gait makes head stability control a possibility because of the smooth gait motion characteristics. Therefore, the serpentine gait is selected as the research object to improve head stability during its motion, which in turn improves the accuracy and stability of the visual SLAM during the serpentine gait.

3.1. Modeling of Head Stability Control Based on Link Angle Correction

The snake robot consists of three parts: the “body”, “neck”, and “head”. It is assumed that when the snake robot carries out the serpentine movement, the “body” carries out the undulating movement. During the process, the “head” and “neck” parts are influenced by the fluctuation and thus perform the same undulating movement as the “body”. Therefore, the key to the control principle is to find a way to control the “neck” to eliminate the effect of the “body” fluctuation and stabilize the “head”. Figure 1 shows the model of the snake robot, where the X-axis represents its direction of movement and the Y-axis represents the left side of the movement.

Where $P_M$ is the point of demarcation between the “cervical region” and the “neck”, and $P_N$ is the boundary point between the “head” and the “neck”.

To realize the stability control of the motion direction and position of the snake robot that is equivalent to satisfy $\stackrel{•}{{\phi }_N}\left(t\right)=0$ and $\stackrel{•}{y_N}\left(t\right)=0$. At this moment, the head of the snake robot is kept in a neutral position, located on the x-axis, namely, the forward direction of the snake robot.

3.2. Head Direction Stability Control

The head direction stabilization control method ensures that the head of the snake robot points in one direction as much as possible, rather than constantly and drastically changing direction as the snake swings. Assuming that the motion of point $P_M$ of the serpentine robot is as shown in Figure 2. $P_M$ is moving around a curve of radius of $1/k(s,t)$, and we can obtain the following:

$$\stackrel{•}{{\phi }_M}(t)=k(s_M,t)\cdot v$$

(3)

where $\stackrel{•}{{\phi }_M}(t)$ is the angle between the X-axis and the tangent line at $P_M$, $s_M$ is the arc length at $P_M$, $k(s,t)$ is the curvature at $s=s,t=t$, and $v$ is the velocity at moment $t$.

It is assumed during the motion of the snake robot that the snake robot will not slide on the y-axis during its motion, which means that there will be no rotational motion due to sliding. Since the parameters on the right side of the equation are controlled by the motion of the snake robot, we obtain the following:

$${\phi }_N(t)-{\phi }_M(t)=\phi (s_N,t)-{\phi }_M(s_M,t)={\displaystyle {\int }_{s_M}^{s_N}k(s,t)ds}$$

(4)

Differentiating Equation (4) and shifting the terms yields Equation (5).

$$\stackrel{•}{{\phi }_N}(t)=\stackrel{•}{{\phi }_M}(t)+{\displaystyle {\int }_{s_M}^{s_N}k(s,t)ds}$$

(5)

To satisfy head direction stability, namely, make $\stackrel{•}{{\phi }_N}\left(t\right)=0$, $\stackrel{•}{{\phi }_M}(t)$ can be obtained as follows:

$$\stackrel{•}{{\phi }_M}(t)=-{\displaystyle {\int }_{s_M}^{s_N}k(s,t)ds}$$

(6)

Using Equations (3) and (6), then Equation (7) can be expressed as follows:

$${\displaystyle {\int }_{s_M}^{s_N}k(s,t)ds}=-k(s_M,t)\cdot v$$

(7)

Equation (7) is the control theory for the head stability of the snake robot in the serpentine gait described in this chapter. From Equation (4), when fitting the head direction of the snake robot to the motion direction at the beginning of the motion, $k(s,t)$ should also satisfy the equation as follows:

$${\displaystyle {\int }_{s_M}^{s_N}k(s,t)ds}=-{\phi }_M(t)$$

(8)

Therefore, for simplicity of calculation, it is assumed that when $s_M<s<s_N$, the curvature is a constant, namely $k(s,t)=c_M(t)$. The “neck” is a circular arc in this case. At any moment, the arc of the circle and the body part of the sine wave articulation can point the direction of the head of the snake robot to the direction of its movement. According to the curvature of the arc, to solve the formula, the ratio of the angle of the center of the circle to the length of the arc and combined with Equation (7) can be seen when Equation (9) is valid and is then a constant. The $\stackrel{•}{c_M}(t)$ is expressed as follows:

$$\stackrel{•}{c_M}(t)=-{\displaystyle \frac{k(s_M,t)}{l_M}}t$$

(9)

Thus, the curvature of the snake curve in serpentine gait is obtained as a segmented function:

$$k(s,t)=\left\{\begin{array}{l}-\alpha b\sin (bs+\sigma (t)),s<s_M\\ -{\displaystyle \frac{{\phi }_M(t)}{l_N}},s_M<s<s_N\end{array}\right.$$

(10)

where $b=2\pi /L$. Finally, the mathematical formulation for controlling the head stability of the snake robot in the serpentine gait is obtained by integration as follows:

$$\phi (s,t)=\left\{\begin{array}{l}\alpha \cos (bs+\sigma (t)),s<s_M\\ (1-{\displaystyle \frac{s-s_M}{l_M}})\alpha \cos (bs+\sigma (t)),s_M<s<s_N\end{array}\right.$$

(11)

where $\sigma (t)$ is the function of time.

From Equation (9), the joint position of the ith joint can be expressed as follows:

$$Angle=\phi ((2i)\cdot l_0)-\phi ((2i-2)\cdot L_0)$$

(12)

3.3. Head Position Stability Control

The head position stabilization control method can build on the algorithm in the previous section to minimize the sensor wobble caused by the snake robot’s head swinging from side to side during its serpentine motion. As shown in Figure 2, point $P_M$ of the “neck” moves with speed $v$ in the direction of the tangent point $M$. The case of satisfying the assumptions made in the previous section, $\stackrel{•}{y_M}(t)$ can be expressed as follows:

$$\stackrel{•}{y_M}(t)=-\sin {\phi }_M(t)\cdot v$$

(13)

Substitute Equation (6) into Equation (13).

$$y_N(t)=y_M(t)+{\displaystyle {\int }_{s_M}^{s_N}\sin (k(s,t)ds+{\phi }_M(t))ds}$$

(14)

Derivate Equation (14) and substitute Equation (6) into Equation (13).

$${\displaystyle {\int }_{s_M}^{s_N}\sin ({\displaystyle {\int }_s^{s_H}k(s,t)ds)ds=-\sin ({\displaystyle {\int }_{s_M}^{s_N}k(s,t)ds})\cdot v}}$$

(15)

From the above derivation, it can be seen that any curvature satisfying Equation (15) can stabilize the position and direction of the head of the snake robot, but the complexity of Equation (15) leads to the problem of complicated and difficult curvature calculation. In the previous subsection, to simplify the calculation, it is assumed that the “neck” of the snake robot is a circular arc. So it is assumed that the snake robot consists of two circular arcs with openings at both ends in the opposite direction, and Equation (15) is linearized. Thus, when $s>s_M$, the curvature expression is divided into two parts:

$$k(s,t)=\left\{\begin{array}{l}{c}_M(t)+f_M(t),s_M<s<s_M+l_N/2\\ c_M(t)-f_M(t),s>s_M+l_N/2\end{array}\right.$$

(16)

Substitute Equation (15) into Equation (16). The differential expression for $f(t)$ can be expressed as follows:

$$\stackrel{•}{f_M}(t)={\displaystyle \frac{4c_M(t)}{l_N}}-2{\displaystyle \frac{k(s_M,t)}{l_N}}$$

(17)

Integrating Equation (17) yields an expression for the curvature. To facilitate the computation of the curvature, in this paper we use the discretization method to solve $f(t)$.

$$f_M(t_{k+1})=f_M(t_k)+\stackrel{•}{f_M}(t_k)\cdot \delta t$$

(18)

Finally, the curvature of the head direction and position stability of the snake robot in the serpentine gait is obtained into three segments as in Equation (19).

$$k(s,t)=\left\{\begin{array}{l}-\alpha b\sin (bs+\sigma (t)),s<s_M\\ c_M(t)+f_M(t),s_M<s<s_M+l_N/2\\ c_M(t)-f_M(t),s>s_M+l_N/2\end{array}\right.$$

(19)

Integrate over the curvature. $\phi (s,t)$ can be expressed as follows:

$$\phi (s,t)=\left\{\begin{array}{l}\alpha \cos (bs+\sigma (t)),s<s_M\\ (1-{\displaystyle \frac{s-s_M}{l_N}})\alpha \cos (bs+\sigma (t))+f_M(t)(s-s_M),s_M<s<s_M+l_N/2\\ (1-{\displaystyle \frac{s-s_M}{l_N}})\alpha \cos (bs+\sigma (t))+f_M(t)(s-s_M-l_N/2),s>s_M+l_N/2\end{array}\right.$$

(20)

Similarly, the joint position of the ith joint can be obtained as follows:

$$Angle=\phi ((2i)\cdot l_0)-\phi ((2i-2)\cdot L_0)$$

(21)

4. Research on Visual–Inertial Tightly Coupled SLAM Method for Snake-like Robots

From the analysis in Section 2, it can be seen that the snake robot’s serpentine gait and arc-rolling gait suffer from poor head stability due to large head joint swing and large SLAM drift due to too fast rotation speed, respectively. In this chapter, we will improve the adaptability of the snake robot’s typical gait with SLAM from the perspective of improving the robustness of the algorithm itself. Inertial Measurement Units (IMU) complement the vision sensors well, so this chapter will incorporate new sensor-IMUs within the vision SLAM framework to act as a constraint on the vision estimation [26,27]. Firstly, the vision-inertial SLAM is used as an observation; then, the kinematic models of the snake robot’s serpentine gait and arc-rolling gait are utilized as the motion models, and finally, the IESKF algorithm is utilized for estimating the final results, thus improving the robustness of the algorithm.

4.1. Visual–Inertial-Based Observation Models

When the camera is only the sensor of the SLAM system, the surrounding waypoints will only constrain the camera, and at this time, it is only necessary to construct the reprojection error as a least-squares problem to find the optimal solution. However, a trajectory will also be generated based on the IMU data as well as the equations of motion as the carrier is in motion when an IMU is added. Since both the camera and the IMU are solidly attached to a carrier, there is a connection between the trajectory information estimated by each of the two sensors, and this connection can be determined by the external reference between the camera and the IMU.

The sampling frequency of the IMU is higher than that of the camera, and the bit position estimated by the camera is usually the bit position between the keyframes, so the IMU observations between two frames need to be accumulated. When constructing the optimization problem of the vision-inertial system, the visual information, along with the IMU zero-bias, velocity, and gravity direction, need to be used as the optimization vectors, and while iteratively updating these vectors, the information of the key frames’ position, velocity, and so on, will continuously be performed as integration operations, which wastes the computational resources, and therefore, the pre-integration of the IMU is needed, i.e., the terms that are invariant at each iteration of the optimization are extracted, and the PVQ is obtained for each cycle. Position, velocity, and attitude increments are measured against other estimates of PVQ increments obtained by non-IMU means, and then the residuals of the PVQ increments are obtained. This residual is then used to construct a cost function to iteratively update and optimize the PVQ for each node.

The measured values of the gyroscope and accelerometer are first constructed as follows:

$$\begin{array}{l}{\omega }_M(t)={\omega }_b(t)+b_g(t)+n_g\\ a_M(t)=a_b(t)+b_a(t)+R_w^{b_t}{g}^w+n_a\end{array}$$

(22)

where ${\omega }_m\left(t\right)$ is the angular velocity measurement of the gyroscope at moment $t$, ${\omega }_b\left(t\right)$ is the true value of the gyroscope angular velocity at moment $t$, $b_g(t)$ is the random zero bias of the gyroscope at moment $t$, $n_g$ is the gyroscopic white noise, $a_m\left(t\right)$ is the measured acceleration of the gyroscope at moment $t$, $a_b(t)$ is the true value of the acceleration of the gyroscope at moment $t$, $b_a\left(t\right)$ is the random zero bias of the accelerometer at moment $t$, $R_w^{b_t}$ is the rotation matrix for conversion from the world coordinate system to the IMU coordinate system, $g^w$ is the acceleration in the world coordinate system, and $n_a$ is the accelerometer white noise.

Integrate the PVQ of frame k, and the PVQ of frame (k + 1) can be expressed as follows:

$$\begin{array}{l}{p}_{b_{k+1}}^w=p_{b_k}^w+v_{b_k}^w\Delta t_k+{\displaystyle {\iint }_{t\in [t_k,t_{k+1}]}\{R_{b_t}^w[a_m(t)-b_a(t)-n_a]-g^w\}d{t}^2}\\ v_{b_{k+1}}^w=v_{b_k}^w+{\displaystyle {\int }_{t_k}^{t_{k+1}}\{R_{b_t}^w[a_m(t)-b_a(t)-n_a]-g^w\}dt}\\ q_{b_{k+1}}^w=q_{b_k}^w\otimes {\displaystyle {\int }_{t_k}^{t_{k+1}}\frac{1}{2}\Omega ({\omega }_m(t)-b_g(t)-n_g)q_t^{b_k}dt}\end{array}$$

(23)

where $w$ is the world coordinate system, $b_k$ and $b_{k+1}$ are the different image sampling moments, $\otimes$ denotes the operations between rotating quaternions, and $q_t^{b_k}$ denotes the transformation matrix of the position from moment $t$ to moment $b_k$.

$$\Omega (\omega )=\left[\begin{array}{cc}-{[\omega ]}_{\times }& \omega \\ -{\omega }^T& 0\end{array}\right],{[\omega ]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$

(24)

Perform the left-multiplication of Equation (24).

$$\begin{array}{l}{R}_w^{b_k}{p}_{b_{k+1}}^w=R_w^{b_k}(p_{b_k}^w+v_{b_k}^w\Delta t_k-{\displaystyle \frac{1}{2}}{g}^w\Delta t_{t_k}^2)+{\alpha }_{b_{k+1}}^{b_t}\\ R_w^{b_k}{v}_{b_{k+1}}^w=R_w^{b_k}(v_{b_k}^w-g^w\Delta t_k)+{\beta }_{b_{k+1}}^{b_k}\\ q_w^{b_k}\otimes q_{b_{k+1}}^w={\gamma }_{b_{k+1}}^{b_k}\end{array}$$

(25)

where ${\alpha }_{b_{k+1}}^{b_k}$, ${\beta }_{b_{k+1}}^{b_k}$, and ${\gamma }_{b_{k+1}}^{b_k}$ are pre-integral, and the expression can be expressed as follows:

$$\begin{array}{l}{\alpha }_{b_{k+1}}^{b_k}={\displaystyle {\iint }_{t\in [t_k,t_{k+1}]}\{R_{b_l}^{b_k}[a_m(t)-b_a\left(t\right)-n_a]\}d{t}^2}\\ {\beta }_{b_{k+1}}^{b_k}={\displaystyle {\int }_{t_k}^{t_{k+1}}\{R_{b_l}^{b_k}[a_m(t)-b_a(t)-n_a]\}}dt\\ {\gamma }_{b_{k+1}}^{b_k}={\displaystyle {\int }_{t_k}^{t_{k+1}}\frac{1}{2}\Omega (\omega (t)-b_g(t)-n_g){\gamma }_t^{b_k}dt}\end{array}$$

(26)

The IMU pre-integration error term between image sampling from moment $b_k$ to moment $b_{k+1}$ is $r_B({\hat{z}}_{b_{k+1}}^{b_k},\chi )$

$$r_B(\stackrel{\wedge }{z_{b_{k+1}}^{b_k}},\chi )=\left[\begin{array}{l}\delta {\alpha }_{b_{k+1}}^{b_k}\\ \delta {\beta }_{b_{k+1}}^{b_k}\\ \delta {\gamma }_{b_{k+1}}^{b_k}\\ \delta b_a\\ \delta b_w\end{array}\right]=\left[\begin{array}{c}{R}_w^{b_k}(p_{{\delta }_k+1}^w-p_{{\delta }_k}^w-v_{{\delta }_k}^w\Delta t_k+{\displaystyle \frac{1}{2}}{g}^w\Delta t_k^2)-{\alpha }_{b_{k+1}}^{b_k}\\ R_w^{b_k}(v_{b_{k+1}}^w-v_{b_k}^w-g^w\Delta t_k)-{\beta }_{b_{k+1}}^{b_k}\\ 2[q_w^{b_k}\otimes q_{b_{k+1}}^w{({\gamma }_{b_{k+1}}^{b_k})}^{-1}]xyz\\ b_{a_{k+1}}-b_{a_k}\\ b_{g_{k+1}}-b_{g_k}\end{array}\right]$$

(27)

where $\stackrel{\wedge }{z_{b_{k+1}}^{b_k}}=\left({\alpha }_{b_{k+1}}^{b_k},{\beta }_{b_{k+1}}^{b_k},{\gamma }_{b_{k+1}}^{b_k}\right)$ is the IMU integral term, and $\chi$ is the state variables to be optimized.

4.2. Snake Robot Motion Estimation

Suppose for the serpentine gait, the snake robot is equivalently composed of N connecting rods with length of $2l$ and uniform mass distribution m and connected at the head and tail. The center of mass inertia coordinates of the whole snake robot are expressed as follows:

$$P=\left[\begin{array}{l}{p}_x\\ p_y\end{array}\right]=\left[\begin{array}{l}{\displaystyle \frac{1}{Nm}}{\displaystyle \sum _{i=1}^{n}m{x}_i}\\ {\displaystyle \frac{1}{Nm}}{\displaystyle \sum _{i=1}^{n}m{y}_i}\end{array}\right]={\displaystyle \frac{1}{N}}\left[\begin{array}{l}{e}^{T}X\\ e^{T}Y\end{array}\right]$$

(28)

The serpentine robot linkage joint constraints in matrix form can be expressed as follows:

$$\begin{array}{l}DX+lA\cos \theta =0\\ DY+lA\sin \theta =0\end{array}$$

(29)

where $D$ is subtraction of matrices, $X={\left[x_1,\dots ,x_n\right]}^T$, $Y={\left[y_1,\dots ,y_n\right]}^T$, and $\theta ={\left[{\theta }_1,\dots ,{\theta }_n\right]}^T$. Combining Equations (28) and (29) yields Equation (30)

$$\begin{array}{c}TX=\left[\begin{array}{c}-lA\cos \theta \\ p_x\end{array}\right]\\ TY=\left[\begin{array}{c}-lA\sin \theta \\ p_y\end{array}\right]\end{array}$$

(30)

where $T=\left[\begin{array}{l}D\\ {\displaystyle \frac{1}{N}}{e}^T\end{array}\right]\in R^{N\ast N}$, then $T^{-1}={\left[\begin{array}{c}{D}^T{(D{D}^T)}^{-1}\\ e\end{array}\right]}^T$.

The linear velocity of the connecting rod can be expressed as follows:

$$\begin{array}{l}\stackrel{•}{X}=l{K}^T{S}_{\theta }\stackrel{•}{\theta }+e\stackrel{•}{p_x}\\ \stackrel{•}{Y}=-l{K}^T{C}_{\theta }\stackrel{•}{\theta }+e\stackrel{•}{p_y}\end{array}$$

(31)

where $S_{\theta }=diag(\sin \theta )\in R^{N\ast N}$, $C_{\theta }=diag(\cos \theta )\in R^{N\ast N}$.

According to the motion characteristics of the arc-rolling gait, the motion of the snake robot along the forward direction is divided into two parts; one is the rolling of the joints in contact with the ground, and its velocity can be expressed as follows:

$$v_{wheel}=\omega \cdot r$$

(32)

where $r$ is the radius of the robot module. The other is the forward swing of the elevated joint, the motion of which can be expressed in polar coordinates with the value of:

$$\left\{\begin{array}{l}{r}_l(t)=l\cdot \sin ({\displaystyle \frac{1}{2}}\alpha (t))=l\cdot \sin ({\displaystyle \frac{A}{2}}\cos (\omega t))\\ \theta (t)=\omega t\end{array}\right.$$

(33)

where $l$ is the length of the robot joint. Then, the oscillating velocity of the joint in the x-axis and z-axis can be expressed as follows:

$$\left\{\begin{array}{l}{v}_{swing}^x={\displaystyle \frac{dx}{dt}}={\displaystyle \frac{d}{dt}}(r_l(t)\cdot \cos \theta (t))\\ v_{swing}^z={\displaystyle \frac{dz}{dt}}={\displaystyle \frac{d}{dt}}(r_l(t)\cdot \sin \theta (t))\end{array}\right.$$

(34)

The velocity of motion of the joint along the target direction is summarized as follows:

$$v(t)=\left\{\begin{array}{l}{v}_{wheel}+v_{swing}^x,t\in (0,T/4]\\ v_{wheel},t\in (T/4,T/2]\end{array}\right.$$

(35)

Then, the explicit expression for the motion velocity of the robot’s joint points can be obtained as follows:

$$\begin{array}{c}v(t)=\omega \cdot r+\omega \cdot l\cdot [{\displaystyle \frac{A}{2}}\cdot \cos ({\displaystyle \frac{A}{2}}\cos (\omega t))\cdot (-\sin (\omega t))\cdot \cos (\omega t)\\ +\sin ({\displaystyle \frac{A}{2}}\cos (\omega t))\cdot (-\sin (\omega t))]\end{array}$$

(36)

4.3. State Fusion Estimation for Snake Robots Based on IESKF

First, we define the state vector of the snake robot, which can be expressed as follows:

$$x={\left[p_x,p_y,\theta ,v,\omega \right]}^T$$

(37)

where $p_x$ and $p_y$ is the coordinates of the robot’s position in a two-dimensional plane, $\theta$ is the angle of the robot’s forward direction for the global coordinate system, $v$ is the linear velocity, and $\omega$ is the angular velocity.

Considering the motion characteristics of the snake robot, the state transfer model can be expressed as:

$$x_{k+1}=f(x_k,u_k,\Delta t)+w_k$$

(38)

where $f$ is the state transition function, $u_k$ is the control input, $\Delta t$ is the time interval, and $w_k$ is the process noise.

The observation model combines data from vision and IMU and can be expressed as follows:

$$z_k=h(x_k)+v_k$$

(39)

where $z_k$ is the observation vector, $h$ is the observation function, and $v_k$ is the observational noise.

The IESKF algorithm consists of two main steps: prediction and calibration. In the prediction step, the state at the next moment is predicted based on the current state estimation and control inputs:

$$\begin{array}{l}{\stackrel{\wedge }{x}}_{k\left|k-1\right.}=f({\stackrel{\wedge }{x}}_{k-1\left|k-1\right.},u_{k-1},\Delta t)\\ P_{k\left|k-1\right.}=F{P}_{k-1\left|k-1\right.}{F}^T+Q\end{array}$$

(40)

In the calibration step, the state estimates are updated using the observed data:

$$\begin{array}{l}{y}_k=z_k-h({\stackrel{\wedge }{x}}_{k\left|k-1\right.})\\ S_k=H_k{P}_{k\left|k-1\right.}{H}_k^T+R\\ K_k=P_{k\left|k-1\right.}{H}_k^T{S}_k^{-1}\\ {\stackrel{\wedge }{x}}_{k\left|k\right.}={\stackrel{\wedge }{x}}_{k\left|k-1\right.}+K_k{y}_k\\ P_{k\left|k\right.}=(I-K_k{H}_k)P_{k\left|k-1\right.}\end{array}$$

(41)

where $F$ is the state transfer Jacobi matrix, $H_k$ is the observation of the Jacobi matrix, $Q$ is the process noise covariance matrix, $R$ is the observation noise covariance matrix, $K_k$ is the Kalman gain, $y_k$ is the residual vector, and $I$ is the identity matrix.

5. Simultaneous and Experimental Research on SLAM for Snake-like Robots

5.1. Simulation Verification

In this paper, we use CoppeliaSim software (V4.0.0) as the robot simulation platform and directly use CoppeliaSim to build the snake robot model. The snake robot model built in this paper consists of 35 joints with a length of 89.95 mm, and the robot model’s joints are connected orthogonally to each other.

The serpentine motion simulation experiments were conducted by taking $\alpha$ as 0.3π, 0.5π, and 0.7π, respectively, and the comparison results of the no algorithm, stability algorithm, and fusion algorithm serpentine motion SLAM simulations are shown in Figure 3:

The arc-rolling motion simulation experiment is divided into two groups; in the case of keeping other parameters unchanged, the first group is set to five snake robot joint lengths, and the second group is set to seven joint lengths. The simulation comparison results are shown in Figure 4.

5.2. Experiment Verification

5.2.1. Experimental Platform Introduction

The snake robot control system is co-built with an orthogonal snake robot as the ontological structure, an ROS open source robot system as the overall architecture, and two-level distribution of the upper and lower computers. The whole system consists of an orthogonal snake robot body, a perception module, and other hardware components. The orthogonal snake robot is alternately composed of orthogonal joints and pitch joints, with a total of 24 joints, a joint length of 96 mm, and a diameter of 64 mm. Each joint is covered with rubber to increase friction, and the prototype of the snake robot is shown in Figure 5. The sensing module mainly integrates the depth camera, core module, and battery pack. The high-resolution depth camera is used to collect image information and provide raw data for SLAM; the core module serves as the core of the perception module and provides arithmetic support for the realization of the SLAM scheme; the battery pack supplies power to the head module. Among them, the Intel RealSense l515 camera is selected as the vision sensor in this paper, which has a very small size and weight and can capture high-quality color images. The NVIDIA Jetson Xavier NX core board is used as the embedded computing platform, and the Y-C7 is used as the industrial-grade carrier board that is used in conjunction with the NVIDIA Jetson Xavier NX core module, which can capture high-quality color images. The Y-C7 is an industrial-grade carrier board for use with the NVIDIA Jetson Xavier NX core module, featuring low cost and small size.

5.2.2. Experiments on Simultaneous Localization and Mapping for Snake-like Robots in the Serpentine Gait

The experiments on simultaneous localization and mapping of the snake robot under the serpentine gait are divided into three groups of cases, namely, the condition without head stability control, the condition with head stability control added, and the vision-inertia tight coupling method. The amplitude parameter $\alpha =0.5\pi$ is set for all three groups, and the simulation analysis shows that the vision algorithm fails under this amplitude parameter. The first group of experiments can be compared with the second and third groups of experiments: the first and the second groups control the effect of the head stability control method on the improvement of the SLAM accuracy of the snake robot; the first and the third groups control the effect of the tightly coupled vision-inertia SLAM method on the improvement of the SLAM accuracy of the snake robot and the robustness of the algorithm itself. The results of the experimental comparison are shown in Figure 6 and Figure 7:

5.2.3. Experiments on Simultaneous Localization and Mapping for Snake-like Robots in the Arc-Rolling Gait

The analysis indicates that when the fitting curve parameter is 7 joint lengths, the estimated trajectory of the snake robot under the arc-rolling gait has a large drift, so this set of experiments set the fitting curve parameter of the snake robot to 7 joint lengths. The experiment is divided into two groups; in the case of keeping other parameters unchanged, the first group is set to without algorithm, and the second group is set to fusion algorithm. The experimental results are shown in Figure 8 and Figure 9:

The trajectory values show that in the arc-rolling gait, the snake robot moved 1.85 m in the x-axis direction and rotated around the axis in the y-axis direction. The mean root mean square (RMS) of the mean difference in this set of experiments is 0.04, which indicates that the snake robot has high accuracy in localization and map building under this method.

6. Conclusions

In this paper, we propose a SLAM method based on the head stabilization control method and the fusion of kinematic estimation of the snake robot to address the problem of the easy failure of localization and mapping caused by the large head joint angle and fast rotation speed during the motion of the snake robot, which can effectively improve the accuracy of the localization and mapping of the snake robot. Through simulation and physical experimentation, the head stability control method, under the undulating gait and with a waveform amplitude of 0.7π, achieved an average root mean square (RMS) error of 0.058 in localization and map building, significantly outperforming the SLAM scheme that fails under an amplitude of 0.3π without head stability control. The visual–inertial tightly coupled SLAM method demonstrated average root mean square (RMS) errors of 0.1 for the undulating gait and 0.08 for the arc-rolling gait, both below 0.1, indicating that this approach effectively enhances the robustness of the algorithm. The SLAM algorithm based on the head stability method can reduce the root-mean-square (RMS) of the error from 0.3 to 0.05 when the parameter $\alpha =0.5\pi$. The simulation and experimental validation show that the proposed method can effectively improve the accuracy of the localization and mapping method of the snake robot and broaden the scope of the localization and mapping method under the serpentine gait. However, this paper only investigates the SLAM adaptability of the snake robot’s motion gait in a two-dimensional plane and the application scenarios are relatively simple. The validation of the snake robot’s three-dimensional gait and more complex scenarios will be carried out at a later stage.