改进型互补滤波在三维姿态估计中的应用
|
摘要
在四轴飞行器的姿态解算过程中,一般是通过互补滤波算法融合多传感器数据解算姿态角的,但将互补滤波算法应用到三维姿态解算中遇到了偏航角的变化引起俯仰角和横滚角的变化这一问题。针对这个问题,详细分析了四轴飞行器三维姿态角的解算过程,并对三维姿态解算算法进行改进,修改了互补滤波器中的误差补偿成分,去掉X、Y轴的磁力计误差补偿、添加了Z轴磁力计误差补偿。实验平台包括STM32控制器、MPU6050和AK8975姿态传感器,在该实验平台下验证改进的三维姿态解算算法的有效性。通过实验验证,改进后的姿态解算算法能实时准确的解算出四轴飞行器的三维姿态角,解决了姿态角互相干扰的问题。
In the process of attitude calculation of the four axis aircraft, the attitude angle is solved by fusing the multi sensor data. However, there is a problem that the yaw angle of quad-rotor aircraft will change the pitch angle and roll angle in the application of the complementary filtering algorithm to 3D attitude calculation.Aiming at the problem, The solution process of 3D attitude angle of four axis aircraft is analyzed in detail, the improvement of 3D attitude calculation algorithm is also presented, the error compensation component of the complementary filter is modified, the error compensation of the Y axis and the X axis is removed, and the error compensation of the Z axis magnetometer is added. The experimental platform includes STM32 controller,MPU6050 and AK8975 attitude sensor, which the validity of the improved algorithm is verified on. The experimental results show that the improved attitude algorithm can solve the 3D attitude angle of the four axis vehicle in real time and solve the problem of the mutual interference between the attitude angles.
In the process of attitude calculation of the four axis aircraft, the attitude angle is solved by fusing the multi sensor data. However, there is a problem that the yaw angle of quad-rotor aircraft will change the pitch angle and roll angle in the application of the complementary filtering algorithm to 3D attitude calculation.Aiming at the problem, The solution process of 3D attitude angle of four axis aircraft is analyzed in detail, the improvement of 3D attitude calculation algorithm is also presented, the error compensation component of the complementary filter is modified, the error compensation of the Y axis and the X axis is removed, and the error compensation of the Z axis magnetometer is added. The experimental platform includes STM32 controller,MPU6050 and AK8975 attitude sensor, which the validity of the improved algorithm is verified on. The experimental results show that the improved attitude algorithm can solve the 3D attitude angle of the four axis vehicle in real time and solve the problem of the mutual interference between the attitude angles.
引文
[1]汪芳,朱少华,雷宏杰.基于卡尔曼滤波器的数字式捷联航姿系统算法设计[J].中国惯性技术学报,2008,16(02):208-211.Wang F,Zhu S H,Lei H J.Algorithm Design of Digital Strapdown Attitude System Based on Calman Filter[J].Chinese Journal of inertial technology,2008,16(02):208-211
[2]盛汉霖,张天宏,刘冬冬.基于扩展卡尔曼滤波器的低成本航姿系统设计[J].系统工程与电子技术,2013,35(10):2 158-2 164.Sheng H L,Zhang T H,Liu D D.Design of Low Cost Attitude and Heading System Based on Extended Kalman Filter[J].Systems Engineering and Electronic Technology,2013,35(10):2 158-2 164.
[3]乔相伟,周卫东,吉宇人.基于四元数粒子滤波的飞行器姿态估计算法研究[J].兵工学报,2012,33(09):1 070-1 075.Qiao X W,Zhou W D,Ren J.Spacecraft Attitude Estimation Four Element Particle Filtering Algorithm Based on[J].Acta ARMAMENTARII,2012,33(09):1 070-1 075.
[4]肖宇.基于互补滤波算法的四轴飞行器姿态和高度解算[J].工业控制计算机,2016,29(10):94-96.Xiao Y.Calculation of Attitude and Height of Four Axis Aircraft Based on Complementary Filtering Algorithm[J].industrial control computer,2016,29(10):94-96.
[5]张承岫,李铁鹰,王耀力.基于MPU6050和互补滤波的四轴飞控系统设计[J].传感技术学报,2016,29(07):1 011-1 015.Zhang C X,Li T Y,Wang Y L.Design of Four Axis Flight Control System Based on MPU6050 and Complementary Filtering[J].Journal of sensing technology,2016,29(07):1 011-1 015.
[6]李文鹏,唐海洋.基于STM32的四轴飞行器姿态解算的研究[J].单片机与嵌入式系统应用,2016,16(06):13-16.Li W P,Tang H.Research on Attitude Calculation of Four Axis Aerocraft Based on[J].STM32 single chip computer and embedded system application,2016,16(06):13-16.
[7]戴青燃,李航宇.四轴飞行器的姿态解算及控制[J].科技资讯,2015,13(35):33-34.DAI Q R,Li H Y.Four Axis Aircraft Attitude Calculation and Control of[J].science and technology information,2015,13(35):33-34.
[8]蒋钰,谌海云,岑汝平.基于四元数的四旋翼飞行器姿态解算算法[J].制造业自动化,2015,37(23):77-80.Jiang Y,Chen H Y,Cen R P.Four Rotor Aircraft Attitude Four Element Solution Algorithm Based on[J].manufacturing automation,2015,37(23):77-80.
[9]陈孟元,谢义建,陈跃东.基于四元数改进型互补滤波的MEMS姿态解算[J].电子测量与仪器学报,2015,29(09):1 391-1 397.Chen M Y,Xie Y J,Chen Y D.MEMS Attitude Calculation Based on Improved Complementary Filtering of Four Variables.[J].Journal of electronic measurement and instrument,2015,29(09):1 391-1 397.
[10]黄志伟,徐苏楠,韦一,等.STM32的多传感器融合姿态检测[J].华侨大学学报(自然科学版),2015,36(04):422-426.Huang Z W,Xu S N,Wei Y,et al.STM32 Multi-Sensor Fusion Attitude Detection[J].Journal of Huaqiao University(NATURAL SCIENCEEDITION),2015,36(04):422-426.
[11]梁延德,程敏,何福本,等.基于互补滤波器的四轴飞行器姿态解算[J].传感器与微系统,2011,30(11):56-58+61.Liang Y D,Cheng M,He F B,et al.What Forbay,Aircraft Attitude Complementary Filter Solution[J].sensor and micro system,based on the 2011,30(11):56-58+61.
[12]王立,章政,孙平.一种自适应互补滤波姿态估计算法[J].控制工程,2015,22(05):881-886.Wang L,Zhang Z,Sun P.An Adaptive Complementary Filtering Attitude Estimation Algorithm[J].,control engineering,2015,22(05):881-886.
[13]孙金秋,游有鹏,傅忠云.基于自适应显式互补滤波的姿态解算方法[J].测控技术,2015,34(04):24-27+31.Sun J Q,You Y P,Fu Z Y.Attitude Resolution Method Based on Adaptive Explicit Complementary Filtering[J].measurement and control technology,2015,34(04):24-27+31.
[14]杨涛远,张凤登.基于互补融合算法的四轴飞行器软件设计[J].软件导刊,2017,16(03):86-88.Yang T Y,Zhang F D.The Complementary Fusion Algorithm of Four Axis Aircraft Design Software[J].software based on the herald,2017,16(03):86-88.
[15]何川,李智,王勇军.基于STM32的四轴飞行器的姿态最优估计研究[J].电子技术应用,2015,41(12):61-64.He C,Li Z,Wang Y J.Research on Attitude Optimal Estimation of Four Axis Aircraft Based on[J].Application of STM32 electronic technology,2015,41(12):61-64.
[16]姜强,曾勇,刘强,等.四轴飞行器姿态航向参考系统设计与实现[J].控制工程,2013,20(S1):167-169+172.Jiang Q,Zeng Y,Liu Q,et al.Design and Implementation of Attitude and Heading Reference System for Four Axis Aircraft.[J].control engineering,2013,20(S1):167-169+172
[2]盛汉霖,张天宏,刘冬冬.基于扩展卡尔曼滤波器的低成本航姿系统设计[J].系统工程与电子技术,2013,35(10):2 158-2 164.Sheng H L,Zhang T H,Liu D D.Design of Low Cost Attitude and Heading System Based on Extended Kalman Filter[J].Systems Engineering and Electronic Technology,2013,35(10):2 158-2 164.
[3]乔相伟,周卫东,吉宇人.基于四元数粒子滤波的飞行器姿态估计算法研究[J].兵工学报,2012,33(09):1 070-1 075.Qiao X W,Zhou W D,Ren J.Spacecraft Attitude Estimation Four Element Particle Filtering Algorithm Based on[J].Acta ARMAMENTARII,2012,33(09):1 070-1 075.
[4]肖宇.基于互补滤波算法的四轴飞行器姿态和高度解算[J].工业控制计算机,2016,29(10):94-96.Xiao Y.Calculation of Attitude and Height of Four Axis Aircraft Based on Complementary Filtering Algorithm[J].industrial control computer,2016,29(10):94-96.
[5]张承岫,李铁鹰,王耀力.基于MPU6050和互补滤波的四轴飞控系统设计[J].传感技术学报,2016,29(07):1 011-1 015.Zhang C X,Li T Y,Wang Y L.Design of Four Axis Flight Control System Based on MPU6050 and Complementary Filtering[J].Journal of sensing technology,2016,29(07):1 011-1 015.
[6]李文鹏,唐海洋.基于STM32的四轴飞行器姿态解算的研究[J].单片机与嵌入式系统应用,2016,16(06):13-16.Li W P,Tang H.Research on Attitude Calculation of Four Axis Aerocraft Based on[J].STM32 single chip computer and embedded system application,2016,16(06):13-16.
[7]戴青燃,李航宇.四轴飞行器的姿态解算及控制[J].科技资讯,2015,13(35):33-34.DAI Q R,Li H Y.Four Axis Aircraft Attitude Calculation and Control of[J].science and technology information,2015,13(35):33-34.
[8]蒋钰,谌海云,岑汝平.基于四元数的四旋翼飞行器姿态解算算法[J].制造业自动化,2015,37(23):77-80.Jiang Y,Chen H Y,Cen R P.Four Rotor Aircraft Attitude Four Element Solution Algorithm Based on[J].manufacturing automation,2015,37(23):77-80.
[9]陈孟元,谢义建,陈跃东.基于四元数改进型互补滤波的MEMS姿态解算[J].电子测量与仪器学报,2015,29(09):1 391-1 397.Chen M Y,Xie Y J,Chen Y D.MEMS Attitude Calculation Based on Improved Complementary Filtering of Four Variables.[J].Journal of electronic measurement and instrument,2015,29(09):1 391-1 397.
[10]黄志伟,徐苏楠,韦一,等.STM32的多传感器融合姿态检测[J].华侨大学学报(自然科学版),2015,36(04):422-426.Huang Z W,Xu S N,Wei Y,et al.STM32 Multi-Sensor Fusion Attitude Detection[J].Journal of Huaqiao University(NATURAL SCIENCEEDITION),2015,36(04):422-426.
[11]梁延德,程敏,何福本,等.基于互补滤波器的四轴飞行器姿态解算[J].传感器与微系统,2011,30(11):56-58+61.Liang Y D,Cheng M,He F B,et al.What Forbay,Aircraft Attitude Complementary Filter Solution[J].sensor and micro system,based on the 2011,30(11):56-58+61.
[12]王立,章政,孙平.一种自适应互补滤波姿态估计算法[J].控制工程,2015,22(05):881-886.Wang L,Zhang Z,Sun P.An Adaptive Complementary Filtering Attitude Estimation Algorithm[J].,control engineering,2015,22(05):881-886.
[13]孙金秋,游有鹏,傅忠云.基于自适应显式互补滤波的姿态解算方法[J].测控技术,2015,34(04):24-27+31.Sun J Q,You Y P,Fu Z Y.Attitude Resolution Method Based on Adaptive Explicit Complementary Filtering[J].measurement and control technology,2015,34(04):24-27+31.
[14]杨涛远,张凤登.基于互补融合算法的四轴飞行器软件设计[J].软件导刊,2017,16(03):86-88.Yang T Y,Zhang F D.The Complementary Fusion Algorithm of Four Axis Aircraft Design Software[J].software based on the herald,2017,16(03):86-88.
[15]何川,李智,王勇军.基于STM32的四轴飞行器的姿态最优估计研究[J].电子技术应用,2015,41(12):61-64.He C,Li Z,Wang Y J.Research on Attitude Optimal Estimation of Four Axis Aircraft Based on[J].Application of STM32 electronic technology,2015,41(12):61-64.
[16]姜强,曾勇,刘强,等.四轴飞行器姿态航向参考系统设计与实现[J].控制工程,2013,20(S1):167-169+172.Jiang Q,Zeng Y,Liu Q,et al.Design and Implementation of Attitude and Heading Reference System for Four Axis Aircraft.[J].control engineering,2013,20(S1):167-169+172