基于多源IMU和粒子滤波优化的姿态融合算法.pdf
《基于多源IMU和粒子滤波优化的姿态融合算法.pdf》由会员分享,可在线阅读,更多相关《基于多源IMU和粒子滤波优化的姿态融合算法.pdf(8页珍藏版)》请在咨信网上搜索。
1、 仪 表 技 术 与 传 感 器Instrument Technique and Sensor2023 年第 8 期基金项目:国家自然科学基金项目(51875534,62075199)收稿日期:2023-04-24基于多源 IMU 和粒子滤波优化的姿态融合算法宗意凯,苏淑靖,高瑜宏(中北大学,省部共建动态测试技术国家重点实验室,山西太原 030051)摘要:针对利用惯性传感器解算姿态易受单种六轴传感器噪声特性和漂移特性影响的问题,提出了一种基于多源 IMU 组合与粒子滤波优化的互补滤波姿态融合算法。该算法利用多源数据与粒子滤波方法,实现了对姿态解算精度的提升。基于 BMI088、ICM2068
2、9 两种六轴传感器与 TMS320F28379S微处理器构建数据采集平台,利用高精度三轴转台驱动数据采集平台进行双轴摇摆实验。基于摇摆实验所得陀螺仪数据和加速度计数据进行改进型互补滤波融合以计算摇摆实验中的姿态角。基于均方误差(MSE)理论利用转台记录得到的三轴实际姿态角曲线对解算所得姿态角曲线做迟滞计算和均方误差计算。结果表明:在设计的改进型姿态解算算法下,横滚角均方误差为 0.159 6()2,俯仰角均方误差为 0.149 8()2,有效提高了互补滤波的解算精度。关键词:MEMS;多源 IMU;姿态解算;互补滤波;粒子滤波中图分类号:TP391 文献标识码:A 文章编号:1002-1841
3、(2023)08-0088-08Attitude Fusion Algorithm Based on Multi-source IMUand Particle Filter OptimizationZONG Yikai,SU Shujing,GAO Yuhong(North University of China,State Key Laboratory of Dynamic Measurement Technology,Taiyuan 030051,China)Abstract:A complementary filtering attitude fusion algorithm based
4、 on multi-source IMU combination and particle filter opti-mization was proposed to address the issue of using inertial sensors to calculate attitude,which was easily affected by the noise and drift characteristics of a single six axis sensor.This algorithm utilized multi-source data and particle fil
5、tering methods to im-prove the accuracy of attitude calculation.A data acquisition platform was constructed based on two types of six axis sensors,BMI088 and ICM20689,and a TMS320F28379S microprocessor.A high-precision three-axis turntable was used to drive the data acquisition platform for biaxial
6、swing experiments.Based on the gyroscope data and accelerometer data obtained from the swing experiment,an improved complementary filtering fusion was performed to calculate the attitude angle in the swing experiment.Based on the mean square error(MSE)theory,hysteresis calculation and mean square er
7、ror calculation were performed on the calculated attitude angle curve using the three-axis actual attitude angle curve recorded by the turntable.The results show that un-der the designed improved attitude solving algorithm,the mean square error of roll angle is 0.159 6()2,and the mean square error o
8、f pitch angle is 0.149 8()2,effectively improving the solving accuracy of complementary filtering.Keywords:MEMS;multi-ssource IMU;attitude solution;complementary filtering;particle filter0 引言姿态解算系统1-2广泛应用于无人机、自主机器人3、航空航天等强烈需求自身姿态信息的自动化系统中。随着近年来无人机4-6行业的发展,对微型航姿参考系统(AHRS)的精度要求进一步提高。受限于微机电系统(MEMS)的成本和
9、尺寸限制,低成本惯性测量单元(IMU)方案存在较大的噪声误差和漂移误差,使得仅用陀螺仪解算得到姿态角具有明显的解算漂移7-8。针对低成本 IMU 传感器的误差问题,文献9基于自回归滑动平均系统设计了一套零滞后补偿的测量方法并实现了一种自适应滤波方法以降低噪声,但由于自协方差的求解不能保证无偏,因而对自协方差的反复迭代存在增大自协方差误差风险。文献10以样本长度、模型参数、降低模型阶数等方面为基础设计了一种随机误差补偿方法以优化 Kalman滤波器11-12输出,但所建立的模型在预测误差较大时去噪效果不明显。目前主要使用互补滤波法、梯度下降法、卡尔曼滤波法实现姿态解算,其中互补滤波因其简化的计算
10、量而被广泛用于嵌入式设备中。88 第 8 期宗意凯等:基于多源 IMU 和粒子滤波优化的姿态融合算法 互补滤波基于陀螺仪积分得到的角度,利用加速度计绝对静态下重力指向的唯一性对陀螺仪解算得到的姿态角进行收敛补偿。传统互补滤波模型利用单种六轴惯性传感器或九轴惯性传感器进行姿态解算,受所选惯性传感器噪声特性、漂移特性的影响。例如,BMI088 六轴惯性传感器具有良好的零漂特性与温漂特性,但传感器数据存在较高噪声;ICM20689 六轴惯性传感器具有较低的噪声特性,但其却存在较严重的零漂与温漂问题。此外,受限于 MEMS 传感器工艺限制,低成本 IMU 传感器的数据输出频率通常不高于 1 000 H
11、z,使得陀螺仪的积分误差较大。基于以上两点,本文设计的 IMU 载板由 BMI088 与 ICM20689组成,基于 TMS320F28379S 微处理器实现数据的采集与传输。以互补滤波算法为基础,发挥互补滤波传感器噪声频段互补的优势的同时结合粒子滤波处理非线性问题的优点,优化多源 IMU 数据融合补偿过程,从而更快、更准获取测量系统的姿态信息。1 数据预处理因数据采集平台与转台之间存在安装误差,需要对数据采集平台获取的六轴数据进行校准以消除坐标偏移误差对姿态解算的影响。因数据采集系统与转台系统分属 2 个独立系统,两者采集所得数据存在时间轴不对齐问题,需对两者时间数据进行对齐校正并裁剪多余数
12、据以供结果分析。1.1 传感器参数校准由于数据采集平台坐标系与转台坐标系存在一定的安装误差,在解算姿态前需要计算校准参数以修正漂移误差和旋转误差。建立采集平台加速度真实值与测量值关系表达式:Ature=AxAyAz=RScalex000Scaley000ScalezAmx-offsetxAmy-offsetyAmz-offsetz(1)式中:Ature为采集平台加速度计真实值;R 为表征测量值向真实值旋转的 33 矩阵;Scalex、Scaley、Scalez为表征真实加速度向量与测量加速度向量之间的缩放尺度;Amx、Amy、Amz为加速度计测量值;offsetx、offsety、offset
13、z为加速度计测量值的零漂。对式(1)进行简化得到:AxAyAz=C1C2C3C4C5C6C7C8C9AmxAmyAmz+CoffxCoffyCoffz(2)式中:Coffx、Coffy、Coffz均为加速度计偏置。将式(2)进一步转换为齐次标准型后转置带入六面参数得:Amx_upAmy_upAmz_up1Amx_downAmy_downAmz_down1Amx_frontAmy_frontAmz_front1Amx_backAmy_backAmz_back1Amx_leftAmy_leftAmz_left1Amx_rightAmy_rightAmz_right1C1C4C7C2C5C8C3C6
14、C9CoffxCoffyCoffz=00g00-g0g00-g0g00-g00(3)利用最小二乘法求解式(3)方程:C=(ATmAm)-1ATmG(4)式中:C 为式(3)中的 43 待定校准系数矩阵;Am为式(3)中的 64 加速度计六面参数矩阵;G 为式(3)中的 63 加速度计六面真值参数矩阵。对静置 50 s 的陀螺仪数据求平均得到陀螺仪零漂量以修正陀螺仪零漂偏置。1.2 时间轴对齐转台记录得到的角速度数据与陀螺仪采集得到的角速度数据存在一致性,但由于惯性传感器数据采集与转台数据采集由 2 个独立系统分别实现,两者之间存在采样时间点不重合的问题。惯性传感器数据采集的时间由 28379S
15、 定时器记录,精确到 1 s,转台数据采集时间间隔为 1 ms。因此首先使用分段三次样条插值方法对惯性传感器数据进行 1 ms 等间隔时间插值,生成获取同为 1 ms 时间间隔的惯性传感器数据序列。基于 MSE 理论通过对 2 条曲线进行均方误差计算以获取两者重合度,损失函数为Loss(f1,f2)=1b-abaf1(x)-f2(x)2dx(5)离散计算公式为Loss(f1,f2)=1Tnk=1f1(x)-f2(x)2t(6)对滞后曲线逐次时移并计算均方误差,均方误差最小时的时移系数即数据滞后时间。即计算式(7)最小值:Loss(f1,f2)=1Tnk=1f1(x)-f2(x+)2t(7)98
16、 仪 表 技 术 与 传 感 器第 8 期假设 f2(x)滞后于 f1(x),当式(7)中均方误差最小时,即滞后时间,计算得到滞后时间后对曲线进行时间轴裁剪以完成时间轴对齐。2 改进型互补滤波算法姿态解算即横滚角、俯仰角、偏航角 的求解,基于陀螺仪积分获取得到的姿态角信息具有较好的高频特性,但由于陀螺仪的数据采样及解算过程为离散过程,在积分的过程中不能避免积分误差。因而纯陀螺仪解算得到的姿态角信息将会随着时间推移产生较大的漂移。利用加速度计解算姿态角即利用重力加速度在运载体坐标系下的矢量来求解运载体的姿态信息,加速度计具有较好的低频特性,且不随时间产生漂移,但当运载体进行大幅度运动时,加速度计
17、不能表征运载体的剧烈运动姿态。基于陀螺仪较好的高频特性与加速度计较好的低频特性,进行互补融合可以获取兼顾高频特性与低频特性的姿态解算算法。受低成本 MEMS 惯性传感器限制,参与解算的传感器数据频率基本不高于 1 000 Hz,且易受单种六轴传感器设计本身的噪声特性影响。为此,本文基于多源惯性传感器优化姿态解算算法,提高单位时间样本量的同时,削弱单种传感器芯片噪声特性对姿态解算的影响。尽管有大量研究表明基于线性高斯模型搭建的Kalman 滤波器具有最小方差与最小误差的最优性,但在运载体的实际运动过程中,传感器受外部环境影响,引入的噪声信息并不完全符合线性高斯模型,因此本文依据粒子滤波理论13-
18、14对互补滤波进行优化处理,期望获得相比传统互补滤波更优的姿态解算算法。算法结构如图 1 所示。图 1 姿态解算算法结构图2.1 同步传感器互补滤波姿态四元数解算定义四元数:q=q0q1q2q3T=cos2rxsin2rysin2rzsin2T(8)四元数 q 描述坐标系以rxryrzT为旋转轴,旋转 的过程。基于上一次解算获取得到的姿态四元数估计重力方向向量:axayaz=1-2(q22+q23)2(q1q2-q0q3)2(q1q3+q0q2)2(q1q2+q0q3)1-2(q21+q23)2(q2q3-q0q1)2(q1q3-q0q2)2(q2q3+q0q1)1-2(q21+q22)T00
19、g=g2(q1q3-q0q2)2(q2q3+q0q1)1-2(q21+q22)(9)对加速度计测得的重力向量与姿态四元数估计所得的重力向量做叉乘得到误差角的近似:ama=amasin(10)式中:am为加速度计测得的重力向量;a 为姿态四元数估计所得的重力向量。由于 am与 a 均为单位向量,因此当运载体小角度运动时误差近似为e=ama=sin(11)误差角基于 PI 模型对陀螺仪测量所得角速度进行补偿得到修正后的角速度:=gyro+Kpe+Kink=1et(12)得到补偿后的三轴角速度后由四元数微分方程迭代更新四元数:qt=qt-1+12qt-1qt=qt-1+12t0-x-y-zx0z-y
20、y-z0 xzy-x0q0q1q2q3(13)计算所得的四元数利用四元数定义可解算出三轴姿态角:=tan-1(2q2q3-2q0q12q20+2q23-1)=-sin-1(2q1q3+2q0q2)=tan-1(2q1q2-2q0q32q20+2q21-1)(14)2.2 异步传感器角速度补偿优化理想情况下,加速度计数据与陀螺仪数据的采样频率一致且采样同步,例如 ICM20689 六轴传感器在1 000 Hz 采样率下加速度计与陀螺仪数据为同步采样。但 BMI088 六轴传感器加速度计最大采样频率为09 第 8 期宗意凯等:基于多源 IMU 和粒子滤波优化的姿态融合算法 800 Hz,陀螺仪最大
21、采样频率为 1 000 Hz,加速度计数据与陀螺仪数据间存在较严重的异步现象,且两者采样时间间隔并不固定。为削弱 BMI088 加速度与陀螺仪异步采样数据对解算精度的影响,需对式(12)的角速度补偿公式进行优化。利用 DSP C2000 的高精度捕获单元(eCAP)捕获BMI088 加速度计与陀螺仪的数据就绪信号,计算得到陀螺仪数据与加速度数据之间的采样间隔时间。利用采样间隔时间 动态修正式(12)角速度补偿公式中的比例项,优化后的角速度补偿公式为=gyro+Tgyro-Tgyro(1-a)+Kpe+Kink=1et(15)式中:Tgyro为陀螺仪的采样周期;为陀螺仪数据与加速度数据之间的采样
22、间隔时间;为比例项下限系数。当加速度数据采样离陀螺仪数据时间较远,则比例系数被相应减小,削弱加速度计数据对角度的修正。2.3 多源姿态四元数融合为融合多源传感器计算所得的四元数,建立多源姿态四元数融合公式:qfusion=KAqA+KBqB(16)式中:qfusion为融合所得四元数;qA为基于 ICM20689加速度计数据与陀螺仪数据进行同步传感器互补滤波姿态解算得到姿态四元数;qB为基于 BMI088 加速度计数据与陀螺仪数据进行异步传感器互补滤波姿态解算得到姿态四元数。为削弱噪声特性对四元数解算的影响,利用信息熵对 2 种传感器的噪声进行评估,并基于评估调整 2组四元数的融合权值 KA、
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- 基于 IMU 粒子 滤波 优化 姿态 融合 算法
1、咨信平台为文档C2C交易模式,即用户上传的文档直接被用户下载,收益归上传人(含作者)所有;本站仅是提供信息存储空间和展示预览,仅对用户上传内容的表现方式做保护处理,对上载内容不做任何修改或编辑。所展示的作品文档包括内容和图片全部来源于网络用户和作者上传投稿,我们不确定上传用户享有完全著作权,根据《信息网络传播权保护条例》,如果侵犯了您的版权、权益或隐私,请联系我们,核实后会尽快下架及时删除,并可随时和客服了解处理情况,尊重保护知识产权我们共同努力。
2、文档的总页数、文档格式和文档大小以系统显示为准(内容中显示的页数不一定正确),网站客服只以系统显示的页数、文件格式、文档大小作为仲裁依据,平台无法对文档的真实性、完整性、权威性、准确性、专业性及其观点立场做任何保证或承诺,下载前须认真查看,确认无误后再购买,务必慎重购买;若有违法违纪将进行移交司法处理,若涉侵权平台将进行基本处罚并下架。
3、本站所有内容均由用户上传,付费前请自行鉴别,如您付费,意味着您已接受本站规则且自行承担风险,本站不进行额外附加服务,虚拟产品一经售出概不退款(未进行购买下载可退充值款),文档一经付费(服务费)、不意味着购买了该文档的版权,仅供个人/单位学习、研究之用,不得用于商业用途,未经授权,严禁复制、发行、汇编、翻译或者网络传播等,侵权必究。
4、如你看到网页展示的文档有www.zixin.com.cn水印,是因预览和防盗链等技术需要对页面进行转换压缩成图而已,我们并不对上传的文档进行任何编辑或修改,文档下载后都不会有水印标识(原文档上传前个别存留的除外),下载后原文更清晰;试题试卷类文档,如果标题没有明确说明有答案则都视为没有答案,请知晓;PPT和DOC文档可被视为“模板”,允许上传人保留章节、目录结构的情况下删减部份的内容;PDF文档不管是原文档转换或图片扫描而得,本站不作要求视为允许,下载前自行私信或留言给上传者【自信****多点】。
5、本文档所展示的图片、画像、字体、音乐的版权可能需版权方额外授权,请谨慎使用;网站提供的党政主题相关内容(国旗、国徽、党徽--等)目的在于配合国家政策宣传,仅限个人学习分享使用,禁止用于任何广告和商用目的。
6、文档遇到问题,请及时私信或留言给本站上传会员【自信****多点】,需本站解决可联系【 微信客服】、【 QQ客服】,若有其他问题请点击或扫码反馈【 服务填表】;文档侵犯商业秘密、侵犯著作权、侵犯人身权等,请点击“【 版权申诉】”(推荐),意见反馈和侵权处理邮箱:1219186828@qq.com;也可以拔打客服电话:4008-655-100;投诉/维权电话:4009-655-100。