Attitude Calculation Based on the Fusion of Complementary Filter and Particle Filter for Spherical Robot
YU Chenyu1, ZHANG Zheng1,2,3, HUANG Weihua2,3, LI Yufeng1, GUO Qingrui1
1. Institute of Robotics and Intelligent Systems, Wuhan University of Science and Technology, Wuhan 430081, China; 2. Engineering Research Center for Metallurgical Automation and Measurement Technology of Ministry of Education, Wuhan University of Science and Technology, Wuhan 430081, China; 3. School of Information Science and Engineering, Wuhan University of Science and Technology, Wuhan 430081, China
Abstract：Aiming at the difficulty in obtaining the accuracy of position and attitude estimation of spherical robot in real time and accurately in motion control, an attitude calculation algorithm based on the fusion of complementary filter and particle filter is presented. In order to avoid the problem of the large error in pitch and roll angles caused by magnetometer data participating in the quaternion calculation, the yaw angle of the spherical robot is calculated by the complementary filtering of gyroscope and magnetometer independently. And the weight of the magnetometer in the complementary filtering is adjusted dynamically by evaluating the noise of the magnetometer using information entropy. Experimental results show that, the error of the yaw angle is less than 3° in the external magnetic field interference experiment. And it is about 0.3° in the dynamic and static experiments of the spherical robot, and the errors of pitch and roll angles can be controlled within 0.1°. Therefore, the algorithm can guarantee the real-time performance of the attitude calculation of the spherical robot, enhance the accuracy and the anti-interference ability, and effectively improve the motion control precision of the spherical robot.
[1] 战强，李伟. 球形移动机器人的研究进展与发展趋势[J]. 机械工程学报， 2019， 55(9)： 1-17. Zhan Q, Li W. Research progress and development trend of spherical mobile robots[J]. Journal of Mechanical Engineering, 2019, 55(9): 1-17. [2] 肖瑞. 两栖球形机器人场景识别及运动控制研究[D].北京：北京理工大学， 2016. Xiao R. Research on scene recognition and motion control of amphibious spherical robot[D]. Beijing: Beijing University of Technology, 2016. [3] 刘欢. 管道检测球形机器人的运动控制研究[D].昆明：昆明理工大学， 2018. Liu H. Research on motion control of spherical robot for pipeline inspection[D]. Kunming: Kunming University of Science and Technology, 2018. [4] Unsal D, Demirbas K. Estimation of deterministic and stochastic IMU error parameters[C]//IEEE/ION Position Location and Navigation Symposium. Piscataway, USA: IEEE, 2012. DOI: 10.1109/PLANS.2012.6236828. [5] Kim Y M, Ahn S S, Lee Y J. KisBot: New spherical robot with arms[C]//WSEAS International Conference on Robotics, Control and Manufacturing Technology. New York, USA: ACM, 2010: 63-67. [6] 郑一力，孙汉旭. 带高速旋转飞轮的球形机器人结构设计与运动稳定性分析[J].机械工程学报， 2013， 49(3)： 36- 41. Zheng Y L, Sun H X. Mechanical design and motion stability analysis of a spherical robot equipped with high-rate flywheel[J]. Journal of Mechanical Engineering, 2013, 49(3): 36- 41. [7] Chen W H, Chen C P, Tsai J S, et al. Design and implementation of a ball-driven omnidirectional spherical robot[J]. Mechanism and Machine Theory, 2013, 68: 35-48. [8] 曾鹏，张启伦，张超，等. 球形机器人动力学建模与运动控制设计[J].自动化与仪表， 2019(10)： 42-46.Zeng P, Zhang Q L, Zhang C, et al. Dynamical modelling and motion control design of spherical robot[J]. Automation and Instrumentation, 2019(10): 42-46. [9] Karavaev Y L, Ki-lin A A. The dynamics and control of a spherical robot with an internal omniwheel platform[J]. Regular and Chaotic Dynamics, 2015, 20(2): 134-152. [10] Liu D L, Sun H X. Nonlinear sliding-mode control for motion of a spherical robot[C]//29th Chinese Control Conference. Piscataway, USA: IEEE, 2010: 3244-3249. [11] 孙汉旭，刘大亮，贾庆轩，等. 基于容错策略的球形机器人控制系统[J].机械工程学报， 2010， 46(11)： 93-99. Sun H X, Liu D L, Jia Q X, et al. Spherical robot control system based on fault-tolerant strategy[J]. Journal of Mechanical Engineering, 2010, 46(11): 93-99. [12] 岑汝平，谌海云. 粒子滤波在微型飞行器姿态融合中的应用[J].自动化技术与应用， 2017， 36(11)： 5-8,13. Cen R P, Chen H Y. Application of particle filter in attitude fusion of MAV[J]. Techniques of Automation and Applications, 2017, 36(11): 5-8,13. [13] 田黎明，张冬梅. 一种基于信息熵的传感器数据异常检测方法[J].软件， 2018， 39(9)： 77-81. Tian L M, Zhang D M. An anomaly detection method of sensor data based on information entropy[J]. Software, 2018, 39(9): 77-81. [14] 余义，章政. 单球驱动自平衡机器人的互补滤波姿态解算[J].计算机工程与设计， 2019， 40(4)： 969-975. Yu Y, Zhang Z. Attitude algorithm based on complementary filter for self-balancing robot driven by single ball[J]. Computer Engineering and Design, 2019, 40(4): 969-975. [15] 李瑞涵，王耀南，谭建豪. Nesterov加速梯度无人机姿态融合算法[J].机器人， 2018， 40(6)： 852-859. Li R H, Wang Y N, Tan J H. Attitude fusion algorithm of UAV based on Nesterov accelerated gradient[J]. Robot, 2018, 40(6): 852-859. [16] 黄小平，王岩，缪鹏程. 粒子滤波原理及应用[M].北京： 电子工业出版社， 2017： 2-3. Huang X P, Wang Y, Miao P C. Principle and application of particle filter[M]. Beijing: Electronic Industry Press, 2017: 2-3. [17] Rigatos G, Busawon K, Pomares J, et al. Nonlinear optimal control for a spherical rolling robot[J]. International Journal of Intelligent Robotics and Applications, 2019, 3: 221-237. [18] 代波，何玉庆，谷丰，等. 基于加速度反馈增强的旋翼无人机抗风扰控制[J].机器人， 2020， 42(1)： 79-88. Dai B, He Y Q, Gu F, et al. Acceleration feedback enhanced controller for wind disturbance rejection of rotor unmanned aerial vehicle[J]. Robot, 2020, 42(1): 79-88. [19] Chen W H, Chen C P, Yu W S, et al. Design and implementation of an omnidirectional spherical robot Omnicron[C]//IEEE/ASME International Conference on Advanced Intelligent Mechatronics. Piscataway, USA: IEEE, 2012. DOI: 10. 1109/AIM.2012.6266036. [20] 周璠，郑伟，汪增福. 基于多异类传感器信息融合的微型多旋翼无人机实时运动估计[J].机器人， 2015， 37(1)： 94-101. Zhou F, Zheng W, Wang Z F. Real time motion estimation of micro multi rotor UAV based on multi heterogeneous sensor information fusion[J]. Robot, 2015, 37(1): 94-101. [21] 夏景平，胡辉，欧敏辉，等. 基于粒子滤波的INS/磁力计融合定位算法[J].全球定位系统， 2017， 42(3)： 1-6. Xia J P, Hu H, Ou M H, et al. INS/magnetometer fusion positioning algorithm based on particle filter[J]. Global Positioning System, 2017, 42(3): 1-6.
