Adaptive climbing motion of snake robot based on joint decoupling and extended Kalman filter

This paper proposes a decoupled gait control function to solve the adaptive transition problem of the snake robot climbing the rod with variable diameter. Firstly, the Sigmoid function is combined with the general gait control function to achieve the decoupling control of the robot joints. Then an adaptive adjustment algorithm of key control parameters is designed based on the extended Kalman filter (EKF) to realize the decoupling gait changes continuously and compliantly as the environment changes. Finally, the physical meanings of the key control parameters are reiterated by simulation, and their influence on the decoupling of climbing gait is analyzed. The smooth transition climbing experiments of the snake robot on rods with different diameters verify the effectiveness of the decoupling and adaptive algorithm.


Introduction
In recent years, snake robots have been widely used in terrain exploration, bridge detection, etc. [1]. The distinctive mechanical structure and motion of the snake robot enable it to climb steadily and quickly to a rod with fixed diameter [2]. However, due to the limitations of the traditional gait control function, the joints of the robot are coupled with each other, resulting in the problem that it cannot adapt to the change of rod diameter quickly and compliantly in the actual climbing process. Therefore, it is very challenging and practical to improve the control function of climbing gait to solve the joint coupling problem and design an adaptive climbing motion controller to achieve a smooth transitional climbing.
Since Professor Hirose first made a prototype of the snake-like robot in 1972 [3], some researchers began to study the various motion gaits of snake robots [4]. After experiments and comparisons, most scholars choose to use a sine function to control the snake robot with an orthogonal structure [5]. At the same time, with the improvement of mechanical structure and control algorithm, more and more scholars have begun to study the adaptive ability of snake-like robots in the process of three-dimensional climbing movement. Among them, the more prominent one is that Carnegie Mellon University proposes to use the extended Kalman filter (EKF) to realize the robot's detection of the thickness of the rod and adaptive climbing [6]. However, due to the limitations of gait functions, the snake-like robot cannot be decoupled and maintain a constant shape during the climbing process, which is likely to lead to slippage caused by insufficient climbing force. Therefore, in this paper, the improved climbing gait control function based on Sigmoid function is proposed, which achieves the decoupling of robot joints, and makes each part of the robot adjust parameters according to the rod diameter, thus completing a more efficient adaptive climbing motion. The rest of this paper is organized as follows. Section 2 presents the design of snake robot and general gait control function. The decoupling gait control function and EKF-based parameter design are proposed in section 3. The simulation results in section 4 analyse the meaning of the gait parameters and verify the adaptive gait algorithm. Section 5 concludes the work.

Mechanical design
At present, there are four main joint connection methods for snake robots, which are parallel connection [7], P-R connection, orthogonal connection and universal connection [8]. Among them, orthogonal connection is highly praised by most scholars due to its simple structure, easy control and threedimensional motion ability [9,10]. In order to achieve complex three-dimensional climbing, the snake robot designed in this paper also uses orthogonal connection, which means that the rotation axes of two adjacent joints are perpendicular to each other, as shown in Figure 1. Due to mechanical limitations and motion constraints of physical machine, the range of rotation angles of each joint is set to [-80°, +80°]. In this paper, two adjacent joints are called a module, and the overall prototype of the snake robot consists of eight modules, all of which have exactly equivalent mechanical structure.

Gait control function
The mathematical expression of the general gait control function of the snake robot is as follows: (1) Among them, denotes the rotation angle of the joint, which satisfies ∈ 80°, 80° in this paper; denotes the frequency of joint rotation which determines the motion velocity of the snake robot; and are the key parameters to control the robot's movement, and their different pairs correspond to different gait patterns; is the offset angle, which can compensate the motion direction, that is generally ignored in theoretical analysis.
Generally, as long as /2, then the snake robot can climb with spiral gait, and the corresponding general climbing posture is shown in Fig 2.

Decoupling gait control function
Due to the continuous differentiability and activation characteristics of the Sigmoid function, this paper proposes a gait control method based on the combination of Sigmoid function and sine function to achieve the decoupling control of the snake robot joints. The specific mathematical model is In the above formula, and respectively denote the control parameters before and after the changes in rod diameter; denotes the joint index value that is expected to be decoupled; denotes the rate of changes in coil radius while climbing, which corresponds to the speed of changes in rod diameter; And the other parameters have the same meaning as formula (1). Section 4 will further illustrate the influence of these parameters on the change of gait function through simulation.

EKF-based parameter adaptive
According to the physical meaning of the parameters in the gait control function, the values of the parameters , , and determine the corresponding relationship between the coil radius of the snake robot and the changes in rod diameter while climbing. Therefore, the selected control parameter pair, , with ≜ , , , and ≜ , , , , is the state variable that needs to be adjusted adaptively in this paper, and the joint angle value, , is selected as the observation. Then the following state-measurement system model can be established: State equation: Observation equation: Using Taylor series to linearize and discretize the above system, we can get: ≜ ⋮ where and is the sample time step, 1, and ≜ sin • • . After approximately linearizing the model, the extended Kalman filter method is used to adaptively adjust the control parameters, and then the satisfactory values of the adaptive climbing gait are obtained. The corresponding algorithm flowchart is shown in Figure 3. In addition, the filtered control parameters are further improved as follows: where ⋅ is the symbolic function and , ≜ , , , The physical meaning of formula (8) is to add a constant to the posterior estimated value of at each moment, so as to reduce the coil radius of the climbing motion, and make sure that sufficient friction force can be generated between the snake robot and the rod; should have a trend of convergence to 0, so that the robot can gradually adapt to the rod with fixed diameter; and ∈ 0, , ∈ 0.1,10 correspond to actual physical constraints.

Influence of parameter changes on gait function
The influence of the parameters in the general gait function on the motions of the snake robot has been described in detail in the literature [5]. The values of and are directly proportional to the screw radius of the snake robot while climbing. Therefore, this section mainly discusses the effect of the parameters and in formula (2) on decoupled gait. The Robotics Toolbox is used in the MATLAB platform to build a simple snake robot with 30 joints ( 30). The effects of parameters in decoupling gait on the whole posture of the robot are analysed as follows.
The effect of parameter on decoupling gait is shown in Figure 4. The corresponding control parameter values are set to 1.0, 0.5, 2, 1.6, and 10,15,20. It can be seen from the figure that the control parameter determines the position where the snake robot's posture changes. The larger the value, the closer the coil radius changes to the snake tail; the smaller the value, the closer the coil radius changes to the snake head. a) Top view. b) Front view. Figure 4. The effect of control parameter on gait decoupling. The effect of parameter on decoupled gait is discussed below. Fix 1.0, 0.5, 15, 1.6, take 1,3,5 respectively, then the simulation results of the overall curves of the control function at the initial moment (t=0) are shown in Figure 5. It can be seen from the figure that the control parameter determines the rapidity of the change in the coil radius of different parts of the snake robot. The larger the value, the slower the change of the coil radius, the more suitable for the situation where the rod diameter changes slowly; the smaller the value, the sharper the change of the coil radius, the more suitable for the rod diameter changes quickly. b) Front view. Figure 5. The effect of control parameter on gait decoupling.

Adaptive climbing experiment on rod with variable diameter
Taking ROS as the simulation platform, the snake robot model with 16 joints and the environmental scenes are built in Gazebo [11]. According to the decoupling adaptive climbing motion algorithm proposed above, the initial values of the control parameters are set to 1.4, 0, 0, 2, 0.5, /2 8, and 2, then the result is shown in Figure 6. Experiments show that the snake robot designed in this paper can smoothly transit from a 10cm rod to 14cm rod by climbing. The change curves of the corresponding control parameters and in this process are shown in Figure 7. It can be seen that in the transitional climbing process, the control parameters are continuously adjusted with the change of rod diameter, and finally can adapt to the new climbing environment. These simulation results and analysis show that joint decoupling and parameter adaptive are very effective to improve the climbing efficiency of the snake robot.

Conclusion
Based on the idea of joint decoupling, this paper solves the adaptive transition problem of a snake robot when climbing rods of different diameters. In order to achieve the complex three-dimensional climbing motion, a snake robot model connected by orthogonal joints is first constructed. Secondly, the decoupled climbing gait function is designed based on Sigmoid function, and the physical significance of the parameters in the new function for gait decoupling is analysed. Then based on EKF, an adaptive control algorithm is designed to achieve the snake robot's self-adaptive climbing in the environment of variable rod diameter.