2.1. Head module

Snake robot head controller consists of external sensors and internal controllers. External sensors include LiDAR and inertial sensors (IMU).

IMU: The IMU in the head module of snake robot measures the head posture and stably controls the head through the posture compensation control strategy to ensure stable LiDAR mapping.

LiDAR: Snake robot adopts 2D planar LiDAR, which has the ability of laser ranging sampling of 16,000 times per second, which can realize 360° all-round ranging scanning within a radius of 25 m and generate spatial plane point cloud data. The measurement blind area is 0.2 m, the angular resolution is 0.225°, and the scanning pitch angle range is ± 1.5°.

Head controller: The control system of snake robot adopts a hierarchical control method, which are human-computer interaction layer, head controller layer, and joint layer. The head controller integrates a computing stick, camera, photosensitive element, LED light, and PCB board. Among them, the computing stick is the main controller, which can complete the collection, processing, and sending of visual information; the gait planning of snake robot is based on the gait equation calculates the joint angle of each joint, and transmits it to the FPGA via USB. After receiving the instruction, the FPGA performs trajectory planning, reads the actual angle of each joint through the PPSECO bus [Reference He, Jin, Yang, Wei, Liu, Cai, Liu, Seitz, Butterfass and Hirzinger17], performs PD control [Reference Tomei18] or speed damping control, and converts the control parameters into PWM values and sends them to each joint controller, thereby achieving control of each joint motor.

The main controller in the head controller is a computing stick, which is produced by Intel. Its model is STK1AW32SCL. The processor is Atom X5-Z8000. It has 2 g, 32G memory, and 2 USB sockets (USB2.0 and USB3.0), its built-in program can be developed using C++ on VS2017, using Windows system, and has high computing processing capabilities. The computing stick is equipped with a wireless network card, which can receive instructions sent by the host computer through Wi-Fi communication; send instructions to the FPGA; receive data from the FPGA; and then forward the data to the host computer through Wi-Fi. The wireless communication capabilities of snake robot were verified through wireless communication test experiments. The snake robot control system is shown in Fig. 2.

Figure 2. Snake robot control system block diagram.

Figure 3. Snake robot battery module driving test.

Head controller solves the problem of remote communication between snake robot, ensuring real-time communication and control real time between host computer and robot in the process of autonomous navigation.

2.2. Battery module

To meet autonomous obstacle avoidance task of snake robot, robot needs to be wirelessly powered. Tail battery is added to robot body to power the robot. Robot uses lithium batteries for power supply.

After installing the battery module, the power supply test was carried out, and the results proved that battery module could provide stable power supply to the robot, which could ensure the stable operation of snake robot and solve the problem of wireless power supply of snake robot. The battery drive test is shown in Fig. 3.

After adding head module and battery module, robot can perform tasks such as stabilizing head and real-time mapping through laser radar, IMU, and other equipment. It can realize real-time motion control of robot through head controller and battery module. Autonomous power supply for robot.

3.1. Hector slam algorithm

Based on the IMU posture feedback method to stabilize head [Reference Bao, Sun, Wang and Xie21], the head LiDAR is used to map the environment when snake robot is moving. In Fig. 5, the schematic diagram of robot positioning and mapping is depicted.

For Hector slam [Reference Nagla22], this project uses planar LiDAR, so planar map construction is required. First, a raster map is constructed based on the center of the LiDAR as the origin, and the scale coefficient of the raster map and the actual world coordinates is set to correspond to the scale of the raster map and the real environment. For raster maps, each raster has an occupied probability value M(Si), ranging from [0,1]. 0 means completely free and not occupied. Finger 1 is fully occupied. Unknown areas are assigned a value of −1.

(1) \begin{equation} {\unicode[Arial]{x03C7}}^{\ast} =\underset{\boldsymbol{\chi }}{\it argmin}\sum _{i=1}^{n}\left[1-M\left(S_{i}\left({\unicode[Arial]{x03C7}} \right)\right)\right]^{2} \end{equation}

where n is the number of points acquired in one LiDAR scan. χ = [P _x , P _y , φ], contains the displacement (P _x , P _y ) and plane rotation angle φ when scan matching occurs. S _i (χ) is the position of the scanning point in the world coordinate system.

(2) \begin{equation} S_{i}\left(\chi \right)=\left(\begin{array}{c@{\quad}c} \cos \left(\psi \right) & -\sin \left(\psi \right)\\ \sin \left(\psi \right) & \cos \left(\psi \right) \end{array}\right)\left(\begin{array}{l} s_{i,x}\\ s_{i,y} \end{array}\right)+\left(\begin{array}{l} p_{x}\\ p_{y} \end{array}\right) \end{equation}

where S_i = [S _i,x , S _i,y ] is the coordinate of the LiDAR scanning point. Given the initial value χ ₀ = [P _x0 ,P _y0 ,φ], based on the steepest descent method (1), the scan matching pose, χ* can be solved, and then, the plane pose of snake robot can be calculated. Once the pose of the robot head is determined, the grid map is updated through the Bresenham’s straight-line algorithm [Reference Kaleem, Verma and Idrisi23].

3.2. LOS algorithm tracking path

Based on IMU posture feedback method to stabilize the head, the head LiDAR is used to map the environment when snake robot is moving. LOS algorithm is shown in Fig. 6.

Figure 6. LOS algorithm diagram.

After obtaining the raster map, the RRT algorithm [Reference Ullah and Mahmood24] is used to plan the robot’s autonomous navigation path. After the planned path is obtained, the LOS algorithm [Reference Singh, Anshul and Choset25, Reference Li, Zhang, Li, Law, Xiang, Xu, Zhu and Wu26] is used for path tracking.

P_s represents the current position of the robot head, P_k , P_{k + 1} , P_{k + 2} represent the trajectory points of the planned trajectory. Select the radius R_LOS to represent the point that intersects with P_k -P_{k + 1} . Select the point P_Los close to P_{k + 1} as the LOS point, the direction the robot to P_LOS is the LOS angle, and the LOS angle can be calculated by the following equation:

(3) \begin{equation} \psi _{LOS}=\arctan \!\left(\frac{y_{k+1}-y_{s}}{x_{k+1}-x_{s}}\right) \end{equation}

ψ_LOS in formula is the angle of LOS angle. By calculating the angle between the head direction and the LOS angle, the robot steering is controlled.

(4) \begin{equation} u=\psi _{los}-\psi _{head} \end{equation}

The steering angle of the robot is indirectly controlled by controlling u.

When the robot reaches the vicinity of P_{k + 1} and the radius is within the R_AC range, the current path point is updated as the next path point.

3.3. Robot steering control

Based on the steering control quantity u, snake robot is steered and controlled [Reference Cao, Zhang and Zhou27]. The block diagram of robot’s autonomous navigation control system is shown in Fig. 7.

Figure 7. Block diagram of robot’s autonomous navigation control system.

For the creeping gait [Reference Shugen Ma, Li and Inoue28] of snake robot, the gait equation is as follows:

(5) \begin{equation} \theta =\left\{\begin{array}{l@{\quad}l} A\sin \left(\omega _{t}t_{k}+\omega _{n}n\right) & n=odd\\ u_{k} & n=even \end{array}\right. \end{equation}

where θ is the n-th joint angle at the k-th moment. ω _t controls the frequency of creeping gait changing with time, and ω _n is the spatial frequency of creeping gait. u _k can control the steering of snake robot. Based on the LiDAR local coordinate system, we have

(6) \begin{equation} \varphi =\arctan 2\left(\det \left(M\right),v_{h}\cdot v_{t}\right) \end{equation}

where M=[v _h ;v _t ], v _h =[-1,0] are the unit direction vectors of the head module axis in the LiDAR local coordinate system. v _t is the unit direction vector from the origin of the radar local coordinate system to the center of the fitting circle. φ is the deflection angle of the dynamic target relative to the robot head. Arctan2 is the arctangent function of two parameters. For the steering control amount $\delta u$ , PD control is used as:

(7) \begin{equation} \delta u=-k_{p}\varphi -k_{d}\dot{\varphi } \end{equation}

(8) \begin{equation} u_{k+1}=u_{k}+\delta \mathrm{u}-u_{lim}\leq u_{k+1}\leq u_{lim} \end{equation}

where k _p and k _d are pd control parameters, respectively. u _lim is the upper limit of the turning control variable u _{k + 1} .

Real-time steering control of the robot is performed through steering control parameters, and the robot’s path tracking is completed in conjunction with the creeping gait.

The steering joints of snake robot are controlled by the steering parameters of robot.