Research of Pose Control Algorithm of Coal Mine Rescue Snake Robot

Aiming at how to achieve optimal control of joint pitch angles in the process of the robot surmounting obstacle, taking the developed coal mine rescue snake robot as an experimental platform, a pose control algorithm based on particle swarm optimization weight coefficient of extreme learning machine (PSOELM) is proposed. In order to obtain the optimized hidden layer matrix of the extreme learning machine (ELM), particle swarm optimization (PSO) is applied to optimize the weight coefficient of hidden layer matrix.The simulation and experiment results prove that, compared with the ELM algorithm, the smaller mean square error (MSE) between the joint pitch angles of robot and the expected values is acquired by the PSOELM, which overcomes the shortcoming that traditional extreme learning machine cannot reach the best performance because of the random selection of the parameters of the hidden layer nodes. PSOELM is superior to ELM algorithm in control accuracy, fast searching for the optimal and stability. Optimal control of robot’s joint pitch angles is achieved.The algorithm is applied to the surmounting obstacle control of the developed snake robot, and it lays the foundation for further implement of the coal mine rescue.


Introduction
The snake robot has the characteristics of good stability, multiple degrees of freedom, multimovement gaits, small cross-section, and so forth.The snake robot can walk on the rugged grounds, can move through caves, and has strong obstacle surmounting abilities.Therefore, compared with the traditional mobile robot driven by wheels or caterpillar tracks, faced with the complex environment after a coal mine disaster, the snake robot can quickly and reliably respond to rescue work.The robot can replace the rescue personnel to enter the site as a first responder.The site information can be collected, and scientific basis for a rapid rescue is provided.Hence developing a coal mine rescue snake robot has a very important practical significance.At the same time, multiple degrees of freedom and flexibility in the movement of the snake robot has brought opportunities and challenges for the research of its pose control.To allow the snake robot to move as flexibly as biological snakes, there are a variety of methods for pose control such as the discrete curve method, dynamics and kinematics model generation method, neural network model generation method (e.g., CPG: central pattern generator) [1], and so on.
Discrete Curve Method.This method refers to that a curve which is defined according to biological snake gait is fitted by the body of snake robot; thus each joint rotating angle of the robot is obtained, also known as the "inverse kinematics method."According to the serpenoid curve of serpentine locomotion proposed by Hirose [2], Wang et al. [3] used the specific parameters including the wave propagation rate and the number to disperse the serpenoid curve, and the governing equations of serpentine locomotion were obtained.Ye et al. [4] proposed a simple snake curve for rotational and lateral motion and thus made the motion control equation of snake robot simpler.Hatton and Choset [5] used a back curve (backbone curve) to extract the waveform of the snake robot and to fit the waveform by using the simulated annealing algorithm; hence multiple gaits for the snake robot were acquired.Xie et al. [6] developed a prototypical underwater snake robot.The snake's body is composed of 16 light waterproof small servomotors, and the serpentine locomotion control of robot was realized by using the snake curve.The discrete curve method has the advantages of simple implementation and easy control.The disadvantage is that the jumping of joint angular velocity and torque can lead to the servomotors locking phenomenon and the damage of servomotors.
Dynamic and Kinematic Model Generation Method.Aiming at the snake robot with linkage type equipped with three driven wheels, Ostrowski and Burdick [7] established a dynamic model of serpentine locomotion by using the Lagrange method and analyzed the controllability of the snake robot.Liljeback et al. [8] established a dynamic model of the snake robot by making use of the relationship between contact force and relative angle, thereby achieving control of obstacle-aided snake robot locomotion.Chen et al. [9] established a space kinematic model (spatial linkage model), and two kinds of three-dimensional motion of snake robot, side winding and lateral rolling, were implemented.Wei and Sun [10] established a kinematic model of snake robot based on orthogonal joints, achieving control of the bridge cable climbing gait.Guo et al. [11] put forward a velocity tracking control algorithm avoiding the singular posture based on the dynamic and control unified model.The algorithm was applied to motion control of a snake robot with passive wheels.Cheng et al. [12] presented the method of surmounting obstacle for snake robot with 16 P-R-T unit modules based on the kinematic model.The advantage of the dynamic and the kinematic model generation method is that complex and accurate gait control can be realized.The disadvantage is that the modeling process is difficult, and the controller must perform a large amount of calculation, and the adaptability to unknown environments is poor.
Neural Network Model (CPG) Generation Method.The CPG method has been widely used in the control of snake robots.Crespi and Ijspeert [13] made use of the CPG model and accomplished optimal control of the swimming and crawling of a snake robot.Lu et al. [14] proposed a cyclic inhibitory CPG controller and serpentine locomotion of a snake robot was successfully implemented.Aiming to resolve the problem of low efficiency and instability of parameter tuning in the cyclic inhibitory CPG control model, Lian et al. [15] presented a parameter optimization method of CPG model based on genetic algorithms, and the method was effectively applied to the gait control of a snake robot.Gao et al. [16] established a CPG motion control network by using Hopf oscillators and applied the network to the serpentine locomotion of a snake robot, which improved the environmental adaptability of the robot.CPG control has the advantage of easy integration of environmental information, so that the snake robot has the ability to adapt to the environment.The disadvantage is that the control parameters in CPG model need to be further optimized.
Based on the present methods for pose control of the snake robot, combined with the advantages of particle swarm optimization (PSO) algorithm and extreme learning machine (ELM), a pose control algorithm based on particle swarm optimization weight coefficient of extreme learning machine (PSOELM) is put forward.Taking the developed coal mine rescue snake robot as an experimental platform, and, in order to achieve optimal control of robot's joint pitch angles, the PSOELM algorithm is applied to the robot's surmounting obstacle behavior.

Design of the Coal Mine
Rescue Snake Robot 2.1.Design of Mechanical Structure.The mechanical body of the coal mine rescue snake robot adopts orthogonal joint connection, which has four orthogonal joints, as is shown in Figure 1.The total length of the snake robot is 1 m, and it is composed of the head, the body, and the tail.Considering the rugged tunnel environment after the coal mine disaster, the robot is driven by the self-made blades wheels.Compared with snake robot driven by wheels or caterpillar tracks, this robot has better obstacle surmounting capabilities.The mechanical structure of the robot is shown in Figure 2. The robot's characteristics are as follows.
(1) Unit module idea is adopted; the head of snake robot (6) is composed of a night vision device and sensor group, installed in the front end of the unit module (1).
(3) The five unit modules are, respectively, installed on the five connecting plates and are connected successively through the four orthogonal joints to form the body of the snake robot.In which, the unit modules (2, 3, and 4) have the same structure, each having two direction servomotors, two direct current speed reduction motors, and two blades wheels; the unit modules (1,5) are identical in structure, each having one direction servomotors, two direct current speed reduction motors, and two blades wheels.
(6) The speed servomotor ( 28) is used to control the rotation of the blades wheels (17, 19, 21, 23, and 25) on the left side of the snake robot.The speed servomotor ( 27) is used to control the rotation of the blades wheels (18, 20, 22, 24, and 26) on the right side of the snake robot.
The snake robot enters the simulated coal mine tunnel, which is semioval tunnel, as shown in Figure 3.

Design of the Control
System.The control system, as shown in Figure 4, is made up of the power source, environmental detection system, pose control system, master controller, mobile mechanism, obstacle avoidance module, communication module, and host computer system.Among them, the power source includes E1 and E2, of which E1 is a 12.6v Li-PO rechargeable battery with charging current of 1300 mAh.E1 supplies power to servomotors and direct current speed reduction motors.E2 is 4.5v mercuryfree alkaline battery and supplies power to the obstacle avoidance module.
The environmental detection system consists of a night vision device and sensor group.The night vision device can be set to night mode and general mode.The sensor group is composed of two ultrasonic sensors and an infrared sensor.Taking into account the special environment of coal mine tunnel, the night vision device is installed on the top of head, and the overall environment information of simulated coal mine tunnel can be obtained by the night vision device.Two ultrasonic sensors are installed on the left and right sides of the head, and the infrared sensor is installed in the middle.The distance information between the snake robot and obstacles can be obtained by using ultrasonic sensors and infrared sensor, and the infrared sensor can effectively compensate for the fade zone existing in ultrasonic sensor ranging.
The pose control system is mainly composed of four horizontal and four vertical direction servomotors.The horizontal direction servomotors control serpentine and straight posture of the snake robot.The vertical direction servomotors control concertina and head rising posture.The M-24 digital servomotor is selected.When used as the joint motor, it can rotate 0∼300 degrees and provide up to 16 kg⋅cm of torque, which is 2 times that of the general digital servomotor.Each servomotor has an individual ID.Each servomotor can be independently controlled, and thus the control of snake robot's gait and pose is acquired.
The master controller uses the Vensmile W10 Mini PC, which has the Atom Z3735F Bay Trail processor, 2 GB memory, and 64 GB storage space.
The mobile mechanism is made up of two speed servomotors, ten direct current speed reduction motors, and ten blades wheels.By controlling the speed servomotors, the direct current speed reduction motors are driven to motivate the blades wheels to rotate, and then the purpose of controlling the speed of the snake robot is realized.
The obstacle avoidance module uses Arduino Uno R3 control board, and its main control chip is ATMEGA328P-PU of Atmel Company.
The communication system uses a self-organized LAN (local area network) to complete data and video image transmission function.
The host computer system includes a monitoring screen of the snake robot's locomotion and the real-time display screen of the environment of the simulated coal mine tunnel.And intelligent identification and location of cracks on the tunnel could also be achieved.

Pose Control Algorithm Based on the PSOELM
Pose control algorithm based on particle swarm optimization weight coefficient of extreme learning machine (PSOELM) is proposed, and the algorithm is applied to the robot's surmounting obstacle behavior.The obstacles are divided into cuboids, cubes, or cylinders.The flowchart of the pose control algorithm is shown in Figure 5, in which R-1, R-2, and R-3 refer to the first three rules in the expert rules of robot's pose control.The algorithm is composed of three steps: firstly, the expert rules of robot's pose control are obtained; secondly, the control model based on extreme learning machine (ELM) is established; thirdly, the particle swarm optimization algorithm is used to optimize the hidden layer matrix weight coefficient of extreme learning machine, so as to obtain the optimized hidden layer matrix, and the optimal control of joint pitch angles of the snake robot is achieved.

The First
Step: Expert Rules of Pose Control.(1) When the sensors detect the height of obstacle is low (ℎ < 5 cm), the first joint is raised to 45 degrees and the second joint is raised to 15 degrees by UK1 and UK3, and then UK5 and UK7 cause the third and fourth joints to synchronously move ahead.Here, UK1, UK3, UK5, and UK7, respectively, refer to the output variables of master controller controlling four vertical direction servomotors.
(2) When the sensors detect the height of obstacle is higher (5 cm < ℎ < 8 cm), the first joint is raised to 45 degrees and the second joint is raised to 35 degrees by UK1 and UK3, and then UK5 and UK7 cause the third and fourth joints to synchronously move ahead.
(3) When the sensors detect the height of obstacle is higher (8 cm < ℎ < 12 cm), the first joint is raised to 45 degrees and the second joint is raised to 45 degrees and the third joint is raised to 25 degrees by UK1, UK3, and UK5, and then UK7 lets the fourth joint move ahead.
(4) When the sensors detect the height of obstacle is highest (ℎ > 12 cm), the robot chooses the obstacle avoidance action to bypass the obstacle.
(5) While surmounting the obstacle, the robot must bend at the joint (waist).When the head joint of the snake is past the apex of the obstacle, it must tilt downwards such that the joint contacts the ground and the angle of the joint in the vertical direction is 0 degrees.Such as the first joint inclines downward to −45 degrees and, at the same time, the joint is associated with the first joint rises.When the first joint is on the ground and the joint angle changes to 0 degrees, the actions of surmounting the obstacle are completed.In summary, the whole process of surmounting an obstacle includes the six related actions, which are raising the head, moving forward, bowing head, bowing the body, raising the tail, and making straightening out.

The Second Step: The Control Model Based on Extreme
Learning Machine (ELM).Snake robot's locomotion of surmounting obstacle refer to that, in the orthogonal joint, the angle of joint in the horizontal direction (namely left and right) keeps 0 degrees, while the angle of joint in the vertical direction (namely up and down) changes according to the cosine curve.Here, according to the equation of serpenoid curve curvature of Hirose,   is defined as the angle between the th joint and the horizontal direction (namely the pitch angle) and expressed as follows: where  0 is the initial angle of serpenoid curve. is the scale factor of robot's pitch angle. is the total length of snake robot. is the number of unit modules. − 1 is the number of joints. represents th joint. = 1, 2, . . .,  − 1.   is defined as the relative rotation angle between the th joint and the ( − 1)th joint.The relative rotation angle function of the th joint in dynamic condition can be obtained as follows: where  0 is the initial angle of serpenoid curve. 1 is the scale factor of joint's relative rotation angle. is the total length of serpenoid curve.2 is the length of unit module of snake robot. represents th joint. = 1, 2, . . .,  − 1.Here,  1 = 2/( + ),  is the adjustment parameter.
A model of extreme learning machine (ELM) algorithm is a feed-forward neural network with single hidden layer.The basic idea is that all the hidden node parameters (including the weight between the hidden layer and the input layer and the bias of the hidden layer nodes) are randomly generated and are independent of the objective functions and the training sample and do not need iterative adjustment.Only the number of hidden layer nodes needs to be set, and then the weight between the hidden layer and the output layer is analyzed and calculated.The optimal weight coefficient is obtained by using the particle swarm optimization (PSO) algorithm and the final network output acquired is the optimal solution.If the pitch angle is optimized, the algorithm is used to optimize formula (1).If the relative rotation angle is optimized, the algorithm is used to optimize formula (2).The following is mainly aimed at the optimization of pitch angle in the second step.
The system model is the 3-6-1 type network.The network input is three kinds of random permitted solution   = [ 1 ,  2 ,  3 ]  , ( = 1, 2, 3), which indicates the first three joint angles of snake robot when the obstacle is three different heights.The hidden layer nodes are 6.The output node of network is 1, which corresponds to the actual angles of the first three joints of the snake robot when surmounting the obstacle.The angles specified by each expert rule in the first step are used as the expected angles.The error between the actual angles and the expected angles of the joints is minimized by the extreme learning algorithm; that is, the error between the network output (actual angles) and the expected output (expected angles) is minimized and Δ  → 0 is accomplished.
The extreme learning algorithm is equivalent to solving the objective function : where ) is the output of the th hidden node through the activation function.  is the weight between the th hidden layer node and the network input.  is the bias of the th hidden layer node.Here, the hyperbolic function is chosen as the activation function, the input of the th hidden node of the network is gained, as the following formula: So, the output vector of the th hidden node about   is as follows: where  is the optimal weight coefficient.Calculate the partial derivative for each variable of the objective function : where  is a 3 × 6-dimensional hidden layer output matrix: because From formula (3) we obtain /(  , (Δ  ),   ) > 0. So, according to the principle of least squares, the following formula can be derived: Then, the weight  can be deduced as shown in the following formula: Therefore, the output formula ( 11) of the extreme learning machine is obtained: . (11)

The Third
Step: Particle Swarm Optimization Weight Coefficient .In formula (11), in order to solve the optimal weight coefficient  of hidden layer matrix, the particle swarm optimization algorithm is used.In the process of solving, the particle is updated by tracking the two optimal values.The first one is the optimal solution which is searched by the particle itself, and the other is the optimal solution searched by the whole swarm so far.The formula used is as follows: where  represents a particle.  indicates the position of the particle at  moment (corresponding to the optimal weight coefficient  of output matrix of the hidden layer at  moment).V  indicates the velocity of particles at  moment. best  represents the best position of the particle so far. best  represents the best position searched by the whole swarm so far.0 <  < 1 is inertial coefficient. 1 and  2 are learning factors. 1 and  2 are the random numbers uniformly distributed on interval (0, 1).After calculating V  ( + 1), the position   ( + 1) of the particle  at next moment can be calculated.That is, position   is changed by modifying speed V  to make Δ  → 0, achieving optimal weight coefficient .
In this system, the weight coefficient of hidden layer matrix is optimized by the particle swarm optimization algorithm.It makes that the error between the actual output of extreme learning machine () and the expected output  is minimized, that is, Δ  → 0. The optimal pitch angle of each joint is obtained.

Simulation Experiment and Result Analysis
Combined with the snake robot's behavior of surmounting obstacle which are shown in Figures 6(a (1) MSE (Mean Square Error) and Determinant Coefficient  2 .Here, In formula (13),  represents the number of samples.(  ) is the output of the optimal control model, and   is the expected output.The smaller the MSE, the better the performance of the model.The value of  2 is in the interval of [0, 1].The closer to 1, the better the performance of the model.
PSOELM and ELM are used, respectively, to control the joint pitch angles of snake robot, and the simulation results are shown in Figure 7.The -axis represents the first three joints of snake robot, and the -axis indicates the joint pitch angle.During surmounting the obstacle, when the obstacle height ℎ < 5 cm, pitch angles of the first three joints are, respectively, controlled by the PSOELM and ELM, and the results are, respectively, given as shown in Figures 7(a  (3) The Stability of the Model.The effect of the number of hidden layer nodes on MRE is analyzed to evaluate the stability of the model.The relationship curves between the hidden layer node  and MRE are shown in Figure 9.As obtained from Figure 9, the ELM model shows great volatility with the change of , which is caused by random selection of input weight and hidden layer node bias.In the POSELM model, the fluctuation of MRE is smaller with the change of , and the smaller hidden layer nodes can ensure that MRE is the smallest.When  is 6, MRE is the smallest, which is 1.3673 × 10 −16 .The POSELM model has the characteristic of fast searching for the optimal and stability, and it has significant advantages in the control of the joint pitch angles of the snake robot.

Conclusion
A coal mine rescue snake robot is developed, and, taking the robot as an experimental platform, aiming at the robot's obstacle surmounting behavior, a pose control algorithm based on particle swarm optimization weight coefficient of extreme learning machine (PSOELM) is studied in this paper.
The following conclusions are drawn.
(1) The mechanical structure and control system of the coal mine rescue snake robot are designed and made, respectively.The mechanical part is consisted of five unit modules, five connecting plates, four orthogonal joints, and two speed servomotors.The robot has better capability to surmount the obstacle because of using the self-made blades wheels.The control system is made up of the power source, environmental detection system, pose control system, master controller, mobile mechanism, obstacle avoidance module, communication module, and host computer system.The snake robot can walk on the rugged grounds.By controlling the direction servomotors, serpentine, straight, concertina, and head rising posture of the snake robot can be acquired, and the speed of the snake robot can be changed by controlling the speed servomotors.
(2) The pose control algorithm based on PSOELM is proposed and discussed; compared with simulation results of the algorithm based on ELM, the PSOELM algorithm has the following advantages: the smaller mean square error and better control accuracy and generalization ability are acquired; the characteristics of fast learning of ELM are continued, having the characteristic of fast searching for the optimal and better stability; so optimal control of robot's joint pitch angles is achieved by the PSOELM algorithm.
(3) The PSOELM algorithm is applied to the surmounting obstacle control of the developed snake robot, and it lays the foundation for further implement of the coal mine rescue.

Figure 2 :Figure 3 :
Figure 2: Mechanical structure of the blades wheels snake robot.

Figure 4 :
Figure 4: Control system of the snake robot.

Figure 5 :
Figure 5: Flowchart of the pose control algorithm.
) and 6(b), the 15 training samples and 5 test samples were randomly selected.Input variables are pitch angles of the first three joints when the obstacle is three different heights, that is,   = [ 1 ,  2 ,  3 ]  ,  = 1, 2, 3, and input layer nodes are 3. Output variables are the actual pitch angles of the first three joints, that is, () = [( 1 ) ( 2 ) ( 3 )]  , and the output node is 1.The computer simulation is carried out in MATLAB.The effects of the ELM and POSELM control models are evaluated by comparing the following 4 indicators.The performance and generalization ability of the model are evaluated by calculating MSE (mean square error) and determinant coefficient  2 .The computation speed is evaluated by comparing the runtime of models.The stability of the model is evaluated by analyzing the effect of the number of hidden layer nodes on MRE (mean relative error).

Table 1 :
MSE comparison between PSOELM and ELM. 10 -33 and 1.3132 × 10 −32 ; when the obstacle height 8 cm < ℎ < 12 cm, pitch angles of the first three joints are, respectively, controlled by the PSOELM and ELM, and the results are, respectively, given as shown in Figures7(e) and 7(f).Determinant coefficient  2 is 1, and mean square error of pitch angles, respectively, reaches 1.0272 × 10 -33 and 1.2326 × 10 −32 .MSE based on PSOELM and ELM is shown in

Table 1 .
It can be seen from Figure7and Table1, compared with ELM, the smaller mean square error and better control accuracy and generalization ability are acquired by PSOELM.Optimal control of joint pitch angles can be realized.As shown in Figure8, the average runtime of the 10 ELM models and the 10 POSELM models is, respectively, 0.0426 S and 0.0535 S.So the runtime of the ELM and POSELM models is roughly the same, and it is all around 50 ms.It can be seen that the POSELM model continues the characteristics of the fast learning of the ELM model;