Autonomous navigation and steering control based on wireless non-wheeled snake robot

Abstract

This paper mainly studies an autonomous path-planning and real-time path-tracking optimization method for snake robot. Snake robots can perform search and rescue, exploration, and other tasks in a variety of complex environments. Robots with visual sensors such as LiDAR can avoid obstacles in the environment through autonomous navigation to reach the target point. However, in an unstructured environment, the navigation of snake robot is easily affected by the external environment, causing the robot to deviate from the planned path. In order to solve the problem that snake robots are easily affected by environmental factors in unstructured environments, resulting in poor path-following ability, this paper uses the Los algorithm combined with steering control to plan the robot in real time and control the robot’s steering parameters in real time, ensuring that the robot can stably follow the planned path.

1. Introduction

Snake robot has super redundant degrees of freedom, strong flexibility, and can adapt to various complex environments [1]. At present, snake robot is mainly used in disaster area search and rescue [2], Aerospace [3], firefighting [4], pipeline exploration [5, 6], dangerous area investigation [7], battlefield blasting, medical treatment [8], and other aspects. Due to its various advantages, snake robot has high research value.

Since snake robot typically needs to complete tasks in unstructured environments, external sensors are needed to construct a global map, plan paths, and autonomously avoid obstacles based on the map, track paths based on planned path, and complete tasks at the target point. The path planning of snake robot is usually carried out using fast extended random Tree (RRT) algorithm or Artificial Potential Field method (APF). RRT algorithms are often suitable for finding optimal path in complex maps. The APF algorithm is often used for autonomous obstacle avoidance in environments with many obstacles, by which planned path is relatively smooth. The path tracking of snake robot is usually carried out using the line of sight (LOS) algorithm, which has characteristics of high computational efficiency and small error and can efficiently control snake robot to follow the planned path.

Kenglung Hsu uses bidirectional RRT algorithm for path planning, introducing virtual forces to optimize the path, so that snake robot can reach the target point along a smooth path [9]; Zhen Wang established a kinematic model of snake robot arm and obstacles based on an improved RRT algorithm, conducted collision detection, and generated an effective collision-free path [10]. Xu Chen uses the APF algorithm in conjunction with ant colony algorithm to plan the path of snake robot [11]. Weixin Yang uses the LOS algorithm and cubic spline interpolation path-planning method to plan the path of an under-actuated snake robot [12]. Yang Xiu uses an adaptive path-tracking controller and combines the anti-sideslip LOS algorithm for path tracking [13].

When snake robot adopts a peristaltic gait for navigation, to ensure that the robot can stably follow the path, it is necessary to carry out real-time steering control of the robot to reduce the error between the robot and the path during the navigation process. To solve this problem, this paper improves the prototype of the non-wheeled snake robot [14] on the basis of the RRT algorithm and the LOS algorithm and uses the improved steering control strategy to control the robot in real time to ensure that snake robot can stably follow the path to the target point.

2. Hardware improvement

Facing the autonomous navigation task of snake robot, this paper improves and optimizes the hardware equipment of the snake robot prototype. Based on the original 16-joint snake robot [15], a head module and battery module were added.

In the past, we have developed a non-wheeled snake robot with 16 DOF (degrees of freedom). The robot consists of 16 modular joints. The robot is shown in Fig. 1(a). Snake robot communicates and supplies power to the host computer and power supply through wired means. Snake robot can complete tasks such as peristaltic gait, rolling gait, spiral rolling gait, and head control based on Bezier curve [16], and relevant main work is described in ref. [15]. Snake robot communicates and supplies power to the host computer and power supply through wired means.

Figure 1. Snake robot prototype improvements.

In this paper, wired communication and power supply are improved to wireless means. Snake robot consists of 16 modular joints, a head module, and a tail battery module. The robot is shown in Fig. 1(b). The joint modules of snake robot are orthogonally connected and can perform three-dimensional motion in space. The tail battery module is responsible for supplying power to snake robot. The head module includes devices such as a head controller, inertial sensors, and laser rangefinder. The head controller of snake robot is a computing rod responsible for processing sensor information and overall motion planning of snake robot.

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 [17], performs PD control [18] 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. Autonomous navigation and steer control

Autonomous navigation of snake robot is carried out by synchronous positioning and map construction through hector slam [19], path planning is carried out by RRT, and path tracking of snake robot is carried out by LOS with steering control [20]. Basic process is shown in Fig. 4.

Figure 4. Autonomous navigation flowchart of snake robot.

Figure 5. Robot positioning and map construction.

3.1. Hector slam algorithm

Based on the IMU posture feedback method to stabilize head [21], 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 [22], 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 [23].

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 [24] is used to plan the robot’s autonomous navigation path. After the planned path is obtained, the LOS algorithm [25, 26] 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 [27]. 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 [28] 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.

4. Simulation and experiment

This article uses the GAZEBO simulation environment to conduct autonomous navigation simulation verification of snake robot.

Fig. 8 shows the map and planned path constructed by LiDAR under RVIZ, and it can be seen that snake robot can stably follow the planned path and reach the target point.

Figure 8. Snake robot’s autonomous navigation simulation.

In this paper, the wireless snake robot is used to carry out autonomous navigation and path-following experiments on snake robot. Snake robot and experimental scene are shown in Fig. 9. The ground in the scene is made of rubber to increase the friction parameters between the robot and the ground and improve the robot’s navigation efficiency. Based on theory in section 3, experiments on autonomous navigation and path following were conducted on snake robot prototype.

Figure 9. Snake robot’s autonomous navigation experiment.

Trajectory tracking error under simulation and experiment is shown in Fig. 10. By comparing the planned path in the experiment with the actual tracking path of the robot, it can be seen that snake robot’s trajectory tracking error is between 0-6 cm, the average tracking error in experiment is 2.54 cm, which is 3.66 cm in simulation. Snake robot can stably follow the planned path, avoid obstacles autonomously, and finally reach the target point to complete its autonomous navigation.

Figure 10. Trajectory tracking error under simulation and experiment.

During the simulation and experimental verification process, it can be found that snake robot completed autonomous navigation in simulation and experiment for about 30 minutes. The average trajectory error in the simulation is 3.66 cm, while the average trajectory error in the experiment is 2.54 cm. There are two reasons for the trajectory error, one is caused by the error of the map data itself during the robot movement and the other is caused by the communication interval between the robot path planning and the robot sensor data.

5. Discussion

From the error data of simulation and experiment, it can be seen that with the help of RRT algorithm for path planning and LOS algorithm for path tracking, the trajectory error of snake robot is small in the process of real-time steering, and the robot can stably follow the established path to the target point, with high efficiency. The creeping gait movement of the snake robot itself may introduce fluctuations in the LiDAR scanning data, leading to errors in the mapped data and affecting trajectory accuracy. Additionally, there is a time interval between the communication of snake robot and the host computer, resulting in a delay in updating the robot’s pose on the host computer. These factors collectively contribute to trajectory errors, which are targeted for improvement in future developments.

6. Conclusion

By improving the prototype of snake robot, the robot can use its own equipment to carry out synchronous positioning and map construction in an unknown environment, and with the help of Wi-Fi, it can communicate with the host computer in real time to complete the real-time control the robot by the host computer, and the battery module ensures the power supply problem under the stable operation the robot. Based on the improved snake robot, with the help of RRT algorithm and LOS algorithm for path planning and tracking, this paper adopts a real-time steering control strategy based on the peristaltic gait of snake robot to control the robot, to ensure that snake robot can stably follow the path to the target point during autonomous navigation.