Attitude Algorithm of Optimal Complementary Filter for Omnidirectional AGV
-
摘要: 针对单一的MEMS陀螺仪无法解决本身的易发散和磁力计易受磁场干扰导致引入新的噪声，而带来的姿态估计不精确的问题，提出了一种基于全向AGV的优化互补滤波的姿态解算方法。偏航角不参与四元数解算，在水平姿态角四元数解算的基础上，利用共轭梯度法减小陀螺仪的漂移误差。再引进经过椭球修正后的磁力计数据作为观测量，与偏航角进行一阶互补滤波融合, 并确定加权因子。搭建了基于STM32和MEMS传感器为核心的全向AGV实验平台，实验结果表明，该方法能有效抑制陀螺仪易发散和磁力计易受干扰的问题，提高姿态解算的精确性，使姿态解算具有良好的动态和静态性能，同时保证了系统的稳定性。Abstract: As a single MEMS gyroscope can not solve the problems that it is easy to diverge and the magnetometer is easy to be interfered by the magnetic field, which lead to the introduction of new noise and the inaccurate attitude estimation, a new attitude estimation method based on omnidirectional AGV is proposed in this paper. According to this method, the yaw angle does not participate in the quaternion solution; on the basis of the quaternion algorithm of the horizontal attitude angle, the conjugate gradient method is used to reduce the drift error of the gyroscope. Then, the data of magnetometer modified by ellipsoid is introduced as the observation data, which is fused with yaw angle by first-order complementary filtering, and the weighting factor is determined. An omnidirectional AGV experiment platform based on STM32 and MEMS sensor is built up. The experimental results show that this method can effectively suppress the divergence of gyroscope and the interference of magnetometer, improve the accuracy of attitude algorithm, make the attitude algorithm have good dynamic and static performances, and ensure the stability of the system.
-
表 1 MEMS传感器输出数据
m/kg 0.9 lx/(kg·m2) 17.326×10-3 l/m 0.45 ly/(kg·m2) 17.383×10-3 Ki/(N·s2) 4.15×10-5 lz/(kg·m2) 5.628×10-2 Kd/(N·m·s2) 6.5×10-7 J/(kg·m2) 5.5×10-5 
下载: 导出CSV
表 2 四元数姿态解算数据
标准值/(°) 测量值/(°) 线性误差/% Yaw Roll Pitch Yaw Roll Pitch Yaw Roll Pitch 90.0 90.0 90.0 103.0 82.0 106.0 14.5 8.9 17.8 90.0 -90.0 90.0 96.0 -99.0 86.0 6.7 10.0 4.5 -90.0 90.0 -90.0 -98.0 98.0 -101.0 8.9 6.7 12.3 180.0 -90.0 90.0 175.0 -82.0 96.0 2.8 8.9 6.7 180.0 -90.0 -90.0 195.0 -87.0 -104.0 8.3 3.4 15.6 -90.0 90.0 90.0 -96.0 102.0 81.0 6.7 13.4 10.0 -90.0 -90.0 -90.0 -102.0 -94.0 -100.0 13.3 4.5 11.1 180.0 90.0 90.0 163.0 97.0 95.0 9.4 7.8 5.6 90.0 90.0 -90.0 95.0 103.0 -82.0 6.5 14.4 8.9 -90.0 -90.0 90.0 -98.0 -85.0 94.0 8.9 5.6 4.5
下载: 导出CSV
表 3 优化互补滤波解算数据
标准值/(°) 测量值/(°) 线性误差/% Yaw Roll Pitch Yaw Roll Pitch Yaw Roll Pitch 90.0 90.0 90.0 92.0 86.0 93.0 2.2 4.4 3.3 90.0 -90.0 90.0 89.0 -88.0 86.0 1.1 2.2 4.4 -90.0 90.0 -90.0 -88.0 88.0 -91.0 2.2 2.2 1.1 180.0 -90.0 90.0 185.0 -91.0 93.0 2.7 1.1 3.3 180.0 -90.0 -90.0 183.0 -93.0 -88.0 1.7 3.3 2.2 -90.0 90.0 90.0 -88.0 93.0 88.0 2.2 3.3 2.2 -90.0 -90.0 -90.0 -89.0 -88.0 -89.0 1.1 2.2 1.1 180.0 90.0 90.0 177.0 94.0 92.0 1.7 4.4 2.2 90.0 90.0 -90.0 92.0 88.0 -87.0 2.2 2.2 3.3 -90.0 -90.0 90.0 -88.0 -86.0 92.0 2.2 4.4 2.2
下载: 导出CSV
-
[1] WU X, LOU P H, YU J, et al. Intersection recognition and guide-path selection for a vision-based AGV in a bidirectional flow network[J]. International Journal of Advanced Robotic Systems, 2014, 11(1): 39 [2] WU X, YU J, LOU P H, et al. Agent-oriented embedded control system design and development of a vision-based automated guided vehicle[J]. International Journal of Advanced Robotic Systems, 2012, 9(2): 37 doi: 10.5772/46127 [3] 郑新芳. 视觉惯导融合实时6DOF机器人定位方法研究[D]. 北京: 北京交通大学, 2017ZHENG X F. Research on real time 6DOF robot localization based on visual and inertial fusion[D]. Beijing: Beijing Jiaotong University, 2017 (in Chinese) [4] 吴涛, 白茹, 朱礼尧, 等. 基于卡尔曼滤波的航姿参考系统设计[J]. 传感技术学报, 2016, 29(4): 531-535 doi: 10.3969/j.issn.1004-1699.2016.04.012WU T, BAI R, ZHU L Y, et al. Design of AHRS based on Kalman filter[J]. Journal of Transduction Technology, 2016, 29(4): 531-535 (in Chinese) doi: 10.3969/j.issn.1004-1699.2016.04.012 [5] 周卫东, 乔相伟, 吉宇人, 等. 基于四元数平方根UKF算法的SINS/GPS紧组合导航系统研究[J]. 系统工程与电子技术, 2010, 32(12): 2643-2647 doi: 10.3969/j.issn.1001-506X.2010.12.31ZHOU W D, QIAO X W, JI Y R, et al. SINS/GPS tightly integrated navigation system based on quaternion square root unscented Kalman filter[J]. Systems Engineering And Electronics, 2010, 32(12): 2643-2647 (in Chinese) doi: 10.3969/j.issn.1001-506X.2010.12.31 [6] MENEGAZ H M T, SHIHARA J Y. Unscented and square-root unscented Kalman filters for quaternionic systems[J]. International Journal of Robust and Nonlinear Control, 2018, 28(15): 4500-4527 [7] 陈国成, 侯宗琰. 一种基于模糊控制的自适应互补滤波算法[J]. 自动化与仪表, 2018, 33(8): 20-25 https://www.cnki.com.cn/Article/CJFDTOTAL-ZDHY201808005.htmCHEN G C, HOU Z Y. An adaptive complementary filtering algorithm based on fuzzy control[J]. Automation & Instrumentation, 2018, 33(8): 20-25 (in Chinese) https://www.cnki.com.cn/Article/CJFDTOTAL-ZDHY201808005.htm [8] 赵晗, 汪立新, 赵曦晶, 等. 基于K-矩阵的捷联惯导系统初始四元数提取[J]. 电光与控制, 2015, 22(6): 35-38 https://www.cnki.com.cn/Article/CJFDTOTAL-DGKQ201506008.htmZHAO H, WANG L X, ZHAO X J, et al. K-matrix based initial quaternion extraction of strapdown inertial navigation system[J]. Electronics Optics & Control, 2015, 22(6): 35-38 (in Chinese) https://www.cnki.com.cn/Article/CJFDTOTAL-DGKQ201506008.htm [9] 许万, 罗西, 王琪, 等. 单舵轮AGV路径跟踪控制方法的研究[J]. 机械科学与技术, 2019, 38(10): 1526-1534 doi: 10.13433/j.cnki.1003-8728.20190021XU W, LUO X, WANG Q, et al. Exploring single steering wheel AGV path tracking controller[J]. Mechanical Science and Technology for Aerospace Engineering, 2019, 38(10): 1526-1534 (in Chinese) doi: 10.13433/j.cnki.1003-8728.20190021 [10] 钱晓明, 朱立群, 楼佩煌, 等. 基于位姿状态的全向运行型AGV路径跟踪优化控制方法[J]. 农业机械学报, 2018, 49(4): 20-26 https://www.cnki.com.cn/Article/CJFDTOTAL-NYJX201804002.htmQIAN X M, ZHU L Q, LOU P H, et al. Optimal path tracking control method of Omni-directional mobile AGV based on pose state[J]. Transactions of the Chinese Society for Agricultural Machinery, 2018, 49(4): 20-26 (in Chinese) https://www.cnki.com.cn/Article/CJFDTOTAL-NYJX201804002.htm [11] 钱晓明, 吴斌, 武星, 等. 潜入牵引式自动导引车运动特性分析[J]. 农业机械学报, 2015, 46(2): 294-300 https://www.cnki.com.cn/Article/CJFDTOTAL-NYJX201502043.htmQIAN X M, WU B, WU X, et al. Trajectory properties of automated guided vehicle with sneak traction[J]. Transactions of the Chinese Society for Agricultural Machinery, 2015, 46(2): 294-300 (in Chinese) https://www.cnki.com.cn/Article/CJFDTOTAL-NYJX201502043.htm [12] 朱本辉. 基于ANSYS的户外AGV车辆结构设计及疲劳寿命分析[D]. 西安: 陕西科技大学, 2015ZHU B H. A study of fatigue life analysis and vehicle structural design of the outdoor AGV based on ANSYS[D]. Xi′an: Shaanxi University of Science & Technology, 2015 (in Chinese) [13] 付雷, 章政, 余义. 基于改进型显性互补滤波的MENS姿态解算[J]. 自动化与仪表, 2018, 33(11): 7-12 https://www.cnki.com.cn/Article/CJFDTOTAL-ZDHY201811002.htmFU L, ZHANG Z, YU Y. Attitude estimation of MEMS based on improved explicit complementary filter[J]. Automation & Instrumentation, 2018, 33(11): 7-12 (in Chinese) https://www.cnki.com.cn/Article/CJFDTOTAL-ZDHY201811002.htm [14] 王录, 刘明雍, 王梦凡, 件等. 基于地磁场自适应修正的航姿系统姿态解算研究[J]. 西北工业大学学报, 2019, 37(2): 225-231 doi: 10.3969/j.issn.1000-2758.2019.02.003WANG L, LIU M Y, WANG M F, et al. Attitude calculation of AHRS based on geomagnetic field adaptive correction[J]. Journal of Northwestern Polytechnical University, 2019, 37(2): 225-231 (in Chinese) doi: 10.3969/j.issn.1000-2758.2019.02.003 [15] 徐晓苏, 周峰, 张涛, 等. 基于四元数自适应卡尔曼滤波的快速对准算法[J]. 中国惯性技术学报, 2016, 24(4): 454-459 https://www.cnki.com.cn/Article/CJFDTOTAL-ZGXJ201604007.htmXU X S, ZHOU F, ZHANG T, et al. Initial alignment algorithm for SINS based on quaternion adaptive Kalman filter[J]. Journal of Chinese Inertial Technology, 2016, 24(4): 454-459 (in Chinese) https://www.cnki.com.cn/Article/CJFDTOTAL-ZGXJ201604007.htm [16] 孙斌, 常晓明, 段晋军. 基于四元数的机械臂平滑姿态规划与仿真[J]. 机械科学与技术, 2015, 34(1): 56-59 doi: 10.13433/j.cnki.1003-8728.2015.0112SUN B, CHANG X M, DUAN J J. Smooth orientation planning and simulation of manipulator based on quaternion[J]. Mechanical Science and Technology for Aerospace Engineering, 2015, 34(1): 56-59 (in Chinese) doi: 10.13433/j.cnki.1003-8728.2015.0112 [17] 成津赛, 张秋菊. 机器人姿态插补的四元数直接逆解方法[J]. 机械科学与技术, 2016, 35(9): 1354-1358 doi: 10.13433/j.cnki.1003-8728.2016.0908CHENG J S, ZHANG Q J. Orientations interpolation algorithm for robot with quaternion-based direct inverse kinematics[J]. Mechanical Science and Technology for Aerospace Engineering, 2016, 35(9): 1354-1358 (in Chinese) doi: 10.13433/j.cnki.1003-8728.2016.0908 [18] 麻文刚, 王小鹏, 张永芳, 等. 基于修正粒子滤波的MEMS传感器飞行器姿态解算(英文)[J]. 测试科学与仪器, 2018, 9(2): 180-187 doi: 10.3969/j.issn.1674-8042.2018.02.013MA W G, WANG X P, ZHANG Y F, et al. Aircraft attitude estimation of MEMS sensor based on modified particle filter[J]. Journal of Measurement Science and Instrumentation, 2018, 9(2): 180-187 (in English) doi: 10.3969/j.issn.1674-8042.2018.02.013 -
点击查看大图
图(8) / 表(3)
计量
- 文章访问数: 275
- HTML全文浏览量: 60
- PDF下载量: 19
- 被引次数: 0
