Pose Estimation For Snake-like Robot Based-on Extended Kalman Filter

Snake-like robot is a new hotspot in robot research both in our country and abroad.Its modular,structured and highly redundant design makes the snake-like robot perform a good stability and robustness in motion.Most of the machine snakes have two-dimensional athletic ability,can do serpenoid curve,rolling,sidewinding and other gait.In this paper,the snake-like robot prototype is composed of 16 modules,and the modules are connected by cross-orthogonal connecting rod.In addition to the two-dimensional plane's movement ability,The machine also have three-dimensional movement ability such as obstacle crossing,climbing stairs and climbing trees.In this paper,the kinematic model of the snake-like robot is improved,and the accuracy of the system kinematics model is improved by decomposing the acceleration and angular velocity.On this basis,the extended Kalman filter algorithm is used to integrate the data of 16 joint sensor data,To achieve the snake-like robot when the shape and direction of the accurate estimates.In this paper,the model is applied to the scene where the robot is set to climb the gait,and the inclination of the slope is deduced by the result of the pose estimation.A new control method of the control equation is proposed by adjust gait parameter according to the slope angle value.The main contents of the article include:(1)design snake-like robot control system and communication systemThe snake robot uses STM32 as the micro controller,and uses the CAN bus as control bus.We designed the distributed system structure according to master-slave relationship.To ensure the communication and control tasks real-time feature and reliability,Microprocessor embedded real-time operating system RT-Thread which ismaintained by china open source community.(2)Snake-like robot modeling and ‘virtual chassis' introductionFirst,The control function of the orthogonal serpentine robot is introduced,and the orthogonal joint snake robot model is established by D-H analysis method.The singular value of the inertial element of the serpentine robot is decomposed,and a new spatial coordinate system based on the main inertia axis of the robot is used to estimate the virtual coordinate system.(3)A system prediction and observation model based on kinematics equations is proposedIn this paper,the kinematics modeling method of the new snake robot is proposed to improve the accuracy of the kinematic model of the serpentine robot.The predictive equation and the observation equation of the extended Kalman filter are established according to the kinematic model.(4)Attitude estimation of snake-like robot under climbing sceneIn the process of robot climbing,the new kinematic model is used to estimate the attitude and the slope angle is estimated according to the result of attitude estimation.Finally,the correctness of the pose estimation model and the slope angle estimation model is verified by a large number of experimental data.