基于MEMS振动陀螺的旋翼无人机组合导航算法设计.pdf

第4 5卷第4期压 电 与 声 光V o l.4 5N o.42 0 2 3年8月P I E Z O E L E C T R I C S&A C OU S TOO P T I C SA u g.2 0 2 3 收稿日期:2 0 2 3-0 2-1 5 作者简介:刘昕(1 9 7 9-),男,河北省石家庄市人,高级工程师,硕士,主要从事卫星通信、伺服控制和组合导航算法的研究。文章编号:1 0 0 4-2 4 7 4(2 0 2 3)0 4-0 5 1 7-0 5D O I:1 0.1 1 9 7 7/j.i s s n.1 0 0 4-2 4 7 4.2 0 2 3.0 4.0 0 7基于MEM S振动陀螺的旋翼无人机组合导航算法设计刘 昕1,秦 超1,2(1.中国电子科技集团公司 第五十四研究所,河北 石家庄0 5 0 0 8 1;2.哈尔滨工程大学 智能科学与工程学院,黑龙江 哈尔滨1 5 0 0 0 1)摘 要:旋翼无人机需要惯导提供准确的角速度和加速度测量信息,而由于无人机的振动会对陀螺输出噪声产生影响,进而影响无人机的飞行平稳性。因此,该文提出了一种面向旋翼无人机的组合导航算法,基于扩展卡尔曼滤波,结合全球导航卫星系统/惯性导航系统(G N S S/I N S)组合导航算法和无地磁传感器的航姿参考系统算法,在一定程度上解决了GN S S失效情况下的姿态稳定问题。针对旋翼无人机对导航系统输出信息平稳性的要求,采用自适应陀螺采样平滑方法,在少量增加数据处理复杂度的前提下,降低了航姿和角速度信息的噪声,在无人机应用中实现了良好的飞行轨迹重复性和控制平稳性。关键词:自适应平滑采样;扩展卡尔曼滤波;组合导航;旋翼无人机中图分类号:T N 8 0 4;V 2 4 9.3 2+8 文献标志码:A D e s i g no f I n t e g r a t e dN a v i g a t i o nA l g o r i t h mf o rR o t o rU A VB a s e do nMEM SV i b r a t i n gG y r oL I UX i n1,Q I NC h a o1,2(1.T h e5 4 t hI n s t i t u t eo fC h i n aE l e c t r o n i c sT e c h n o l o g yG r o u pC o r p o r a t i o n,S h i j i a z h u a n g0 5 0 0 8 1,C h i n a;2.C o l l e g eo f I n t e l l i g e n tS c i e n c ea n dE n g i n e e r i n g,H a r b i nE n g i n e e r i n gU n i v e r s i t y,H a r b i n1 5 0 0 0 1,C h i n a)A b s t r a c t:T h er o t o rUAV sr e q u i r e t h e i n e r t i a l n a v i g a t i o ns y s t e mt op r o v i d ea c c u r a t ea n g u l a rv e l o c i t ya n da c c e l-e r a t i o nm e a s u r e m e n t i n f o r m a t i o n,w h i l et h ev i b r a t i o no fUAV w i l la f f e c tt h eo u t p u tn o i s eo fg y r o s c o p e,w h i c hi nt u r na f f e c t s t h e f l i g h t s t a b i l i t yo fUAV.I n t h i sp a p e r,a n i n t e g r a t e dn a v i g a t i o na l g o r i t h mf o r r o t o rUAVi sp r o p o s e d.T h ea l g o r i t h mc o m b i n e s t h e i n t e g r a t e dn a v i g a t i o na l g o r i t h mo f t h eg l o b a l n a v i g a t i o ns a t e l l i t e s y s t e m/i n e r t i a l n a v i g a-t i o ns y s t e m(G N S S/I N S)a n dt h e a l g o r i t h mo f t h e a t t i t u d e r e f e r e n c e s y s t e mw i t h o u t g e o m a g n e t i c s e n s o r,w h i c ht oac e r t a i ne x t e n t s o l v e s t h ea t t i t u d es t a b i l i t yp r o b l e mi nt h ec a s eo fG N S Sf a i l u r e.I no r d e r t om e e t t h er e q u i r e m e n to ft h es t a b i l i t yo f t h eo u t p u t i n f o r m a t i o no f t h en a v i g a t i o ns y s t e m,t h ea d a p t i v eg y r o-s a m p l i n gs m o o t h i n gm e t h o d i sa-d o p t e dt or e d u c e t h en o i s e so f t h ea t t i t u d ea n da n g u l a rv e l o c i t y i n f o r m a t i o no nt h ep r e m i s eo f s l i g h t l y i n c r e a s i n gt h ec o m p l e x i t yo fd a t ap r o c e s s i n g,a n da c h i e v eg o o dr e p e a t a b i l i t yo f f l i g h t t r a j e c t o r ya n dc o n t r o l s t a b i l i t y i nt h eUAV.K e yw o r d s:a d a p t i v es m o o t h-s a m p l i n g;e x t e n d e dK a l m a nf i l t e r i n g;i n t e g r a t e dn a v i g a t i o n;r o t o rUAV 0 引言近年来国内四旋翼飞行器产业的发展迅猛,与传统的直升机和固定翼相比,旋翼无人机的易用性(运动相互解耦)、可靠性(机械磨损小)和维护性(结构简单、模块化)等刚性体验让人们的选择成为必然1。旋翼无人机实现精准姿态控制与位置控制的前提是导航模块提供稳定可靠的姿态、位置数据,目前多采取组合导航技术,将多种传感器对姿态、位置等数据进行信息融合2。文献3 介绍了过去1 0年中提出的解决方案。文献4-5 在旋翼无人机控制中应用了传统的导航算法。针对如何确保无人机控制平稳性的问题,本文使用微机电系统(MEM S)振动陀螺提出了一种基于扩展卡尔曼滤波(E K F)的导航算法,融合全球导航卫星系统(GN S S)观测和自身加速度计输出观测,克服GN S S数据失效时的姿态稳定问题。同时引入自适应平滑采样方法,降低了角速度输出噪声。1 系统模型组合导航算法基于扩展卡尔曼滤波模型,以全球导航卫星系统/惯性导航系统(GN S S/I N S)组合导航的框架为基础,结合了无地磁传感器的航姿参考系统算法6。算法以加速度计的输出值和GN S S测量的位置Pm、速度Vm为观测量,估计出四元数偏差和陀螺零偏b。组合导航系统框架如图1所示,图中m、am分别为三轴MEM S陀螺仪和加速度的输出,P、V和q分别为惯性解算得到的位置、速度和四元数,、和为组合导航系统输出的航向、俯仰和横滚。图1 组合导航系统框架1.1 惯性测量单元(I MU)误差模型I MU的误差包括系统误差和随机误差。系统误差可通过器件常规标定方法进行消除或降低,包括零偏、刻度系数误差及非正交性误差等。本文选用的高解耦微型陀螺如图2所示。微型陀螺敏感芯片采用多质量块解耦设计方案,驱动与检测音叉分离,通过挠性支撑结构连接,避免了各模态的机械耦合误差,达到解耦效果,隔离框的设计可有效地提升敏感芯片的环境适应性7。图2 高解耦微型陀螺总体结构示意图随机误差包括高斯白噪声及随机游走噪声,其中白噪声在概率上服从高斯分布,其功率谱密度为均匀分布。偏置随机游走被认为是离散化的维纳过程,由于I MU每次更新位置时都会叠加一个新的白噪声,故将其偏置噪声建模为随机游走噪声8。I MU误差模型为m=-w+bam=a-wa-Cbn0 0 gT(1)式中:m,am分别为陀螺仪和加速度计的输出;,a分别为陀螺仪敏感的真实角速度和加速度计敏感的真实比力;w,wa为传感器噪声矢量;g为重力加速度;Cbn是地理系(n系)至载体系(b系)的姿态旋转矩阵。1.2 航姿表示形式四元数可完美描述物体在空间的姿态,其没有奇异性,无人机的每种姿态都对应一个唯一的四元数。以四元数q=q0 q1 q2 q3T描述无人机的姿态,其与姿态旋转矩阵Cbn的转换关系为 Cbn=q20+q21-q22-q232q1q2+q0q3()2q1q3-q0q2()2q1q2-q0q3()q20-q21+q22-q232q2q3+q0q1()2q1q3+q0q2()2q2q3-q0q1()q20-q21-q22+q23(2)航向角、俯仰角、横滚角为=at a n2(Cbn(2,1),Cbn(2,2)as i n(Cbn(2,3)-at a n2(Cbn(1,3),Cbn(3,3)(3)2 基于E K F的组合导航算法设计2.1 状态方程以位置、速度、四元数和陀螺零偏为状态变量,建立状态方程x=f(x,w),且:x=PVqb =VCnb(am+wa)+(0 0 g)T12(m+w-b)wb(4)式中:b为陀螺零偏矢量;w为噪声矢量;为简化的四元数乘法矩阵,且:815压 电 与 声 光2 0 2 3年=-q1-q2-q3q0-q3q2q3q0-q1-q2q1q0(5)2.2 量测方程GN S S系统是辅助I N S进行姿态保持的良好互补系统,此处以其输出位置、速度为观测量建立输出方程1:y1=h1(x)=PV(6)由于无人机的运动形式有别于地面车辆,当GN S S系统失效时,无法引入车辆的运动学约束来抑制导航系统的误差9,因此,参照常规的航姿参考系统算法,即以陀螺仪和加速度计输出、航姿变化量等参数为判断条件,当判定无人机处于静态、准静态和低动态条件下,以加速度计输出为观测量建立输出方程2:y2=h2(x)=am xam yam zT(7)2.3 滤波算法依照E K F的工作流程,将连续时间模型进行离散化,得到:xk=xk-1+wk-1zk=H xk+vk(8)式中:wk为过程噪声矢量;vk为量测噪声。假定wk,vk均为白噪声,各自的协方差矩阵分别为Q、R。离散化计算采用一阶近似:I+F TG T(9)式中:T为滤波周期。F=fx=03 3I3 303 7 01 0 6FV q03 3Fq q/2-/203 7 G=fw=03 903 3Cnb/2 04 307 303 6I3 3(1 0)中间过程矩阵计算如下:FV q=FV q0FV q1FV q2FV q3-FV q3-FV q2FV q1FV q0FV q2-FV q3-FV q0FV q1(1 1)FV q0=2q0am x-q3am y+q2am z()(1 2)FV q1=2q1am x+q2am y+q3am z()(1 3)FV q2=2-q2am x+q1am y+q0am z()(1 4)FV q3=2-q3am x-q0am y+q1am z()(1 5)Fq q=0b x-m xb y-m yb z-m zx-b x0z-b zb y-m yy-b yb z-m z0 x-b xz-b zy-b yb x-m x0(1 6)量测矩阵H为H=I6 606 703 62gHq q03 3(1 7)Hq q=-q2q3-q0q1q1q0q3q2q0-q1-q2q3(1 8)以GN S S系统的位置、速度为观测量时,使用此矩阵的前6行;以航姿参考系统算法中的加速度计输出为观测量时,使用此矩阵的后3行。3 陀螺数据的自适应平滑采样旋翼无人机控制要求导航系统输出的姿态和角速度信息平稳,数据噪声会影响控制的稳定性。在尽量降低数据处理复杂度的前提下,将组合导航算法划分为机械编排、卡尔曼滤波预测过程和卡尔曼滤波更新过程3个主要耗时模块。分别在机械编排前、机械编排与卡尔曼滤波预测过程之间、卡尔曼滤波预测与更新之间进行采样,并依次将陀螺输出分别记为1、2和3,在下次机械编排前进行陀螺数据的自适应平滑,算法流程图如图3所示。图3 基于自适应平滑采样的滤波流程自适应平滑采样器基于S i g m o i d函数1 0,考虑无人机动态性和陀螺的噪声水平,依据无人机控制915 第4期刘 昕等:基于MEM S振动陀螺的旋翼无人机组合导航算法设计角速度输入c及其变化率c构造权重函数:S=11+e-c/c-0.3()(1 9)则下一周期机械编排时角速度输入可表示为m=S1+1-S()22+3()(2 0)4 实验结果与分析为了验证该算法的正确性,在某型搭载了S B GE l l i p s e-N型导航模块的旋翼无人机上进行了对比测试,该模块的姿态精度可达0.0 5,航向精度可达0.2。分别在导航计算机中使用E K F和本文方法进行了导航解算,并与安装在无人机上的法国S B G导航模块的输出进行对比,陀螺和姿态数据对比如图48和表1、2所示。结果表明,最大角速度误差和角误差标准差有明显提升。图4 S B G、E K F以及本文方法陀螺x轴输出对比图5 S B G、E K F以及本文方法陀螺y轴输出对比图6 S B G、E K F及本文方法陀螺z轴输出对比图7 S B G、E K F及本文方法横滚角对比图8 S B G、E K F及本文方法俯仰角对比表1 E K F与本文方法最大角速度误差对比输出最大角速度误差/()s-1E K F本文方法陀螺x3.51.9陀螺y4.22.1陀螺z7.94.5表2 E K F与本文方法角误差标准差对比输出角误差标准差/()E K F本文方法横滚角0.50.3俯仰角0.60.2 图9为在旋翼无人机平台上采用本文方法后的无人机飞行轨迹图。由图可看出轨迹重复性高,控制平稳。图9 旋翼无人机飞行轨迹5 结束语本文提出了一种面向旋翼无人机的组合导航算法。算法基于扩展卡尔曼滤波,结合了GN S S/I N S025压 电 与 声 光2 0 2 3年 组合导航算法和无地磁传感器的航姿参考系统算法,在一定程度上解决了GN S S失效情况下的姿态稳定问题。针对旋翼无人机对导航系统输出信息平稳性的要求,采用自适应陀螺采样平滑方法,在少量增加数据处理复杂度的前提下,降低了航姿和角速度信息的噪声,在无人机时实现了良好的飞行轨迹重复性和控制平稳性。参考文献:1 王勇军.融合多源信息的小型多旋翼无人机位姿估计方法研究 D.桂林:桂林电子科技大学,2 0 2 1.2 裴信彪.新型多旋翼无人机的低成本组合导航与稳定控制 D.长春:中国科学院大学(中国科学院长春光学精密机械与物理研究所),2 0 1 8.3 G YA G E N D AN,HA T I L I MAJV,R O TH H,e ta l.Ar e v i e wo fG N S S-i n d e p e n d e n tUA V n a v i g a t i o nt e c h n i q u e sJ.R o b o t i c s a n d A u t o n o m o u s S y s t e m s,2 0 2 2,1 5 2:1 0 4 0 6 9.4 WAN W,L I UY.D e s i g no fa t t i t u d ec o n t r o l s y s t e mf o rf o u r r o t o rUAVJ.I O PC o n f e r e n c eS e r i e s:M a t e r i a l sS c i e n c ea n dE n g i n e e r i n g,2 0 1 8,4 5 2(4):0 4 2 1 6 5.5 王宏凯,钟斌,赵屹男,等.基于G P S/B D S的四旋翼无人机导 航 研 究 与 实 现 J.舰 船 电 子 工 程,2 0 1 8,3 8(1 1):3 0-3 4.WANGH o n g k a i,Z HON GB i n,Z HAOY i n a n,e t a l.R e-s e a r c ha n d i m p l e m e n t a t i o no f n a v i g a t i o n f o r q u a d c o p t e ru n m a n n e da e r i a lv e h i c l eb a s e do nG P S/B D SJ.S h i pE l e c t r o n i ce n g i n e e r i n g,2 0 1 8,3 8(1 1):3 0-3 4.6 WANG Y u n l o n g,HU S S A I N A,S O L TAN I M.AMEM S-b a s e d a d a p t i v e AHR S f o r m a r i n e s a t e l l i t et r a c k i n ga n t e n n aJ.I F A C-P a p e r so nL i n e,2 0 1 5,4 8(1 6):1 2 1-1 2 6.7 林日乐,李文蕴,谢佳维,等.石英微机械陀螺敏感芯片的结构解耦特性研究 J.压电与声光,2 0 1 7,3 9(3):3 2 1-3 2 3.L I NR i l e,L IW e n y u n,X I EJ i a w e i,e ta l.S t u d yo nt h es t r u c t u r ed e c o u p l i n gc h a r a c t e r i s t i co f t h es e n s i t i v ec h i po f q u a r t z MEM S g y r o s c o p eJ.P i e z o e l e c t r i c s&A c o u s t o o p t i c s,2 0 1 7,3 9(3):3 2 1-3 2 3.8 吴涛,谢志军.一种改进的视觉/I MU导航系统初始化方法 J.压电与声光,2 0 2 1,4 3(6):8 6 3-8 6 8.WU T a o,X I E Z h i j u n.A n i m p r o v e d i n i t i a l i z a t i o nm e t h o do fv i s u a l/I MUn a v i g a t i o ns y s t e mi n i t i a l i z a t i o nJ.P i e z o e l e c t r i c s&A c o u s t o o p t i c s,2 0 2 1,4 3(6):8 6 3-8 6 8.9 姜博文,王可东.低成本车载组合导航在G P S失效时的算法设计J.传感技术学报,2 0 1 7,3 0(3):4 1 2-4 1 7.J I AN G B o w e n,WAN G K e d o n g.A l g o r i t h m d e s i g no fl o wc o s tv e h i c l ei n t e g r a t e dn a v i g a t i o ni nt h ee v e n to fG P Sf a i l u r eJ.J o u r n a lo fS e n s i n gT e c h n o l o g y,2 0 1 7,3 0(3):4 1 2-4 1 7.1 0马琳琳,李冀,肖岩.一种自适应平滑滤波算法J.电子世界,2 0 2 1,6 2 3(1 7):1 0 6-1 0 7.MAL i n l i n,L I J i,X I AOY a n.A na d a p t i v es m o o t hf i l t e-r i n ga l g o r i t h mJ.E l e c t r o n i cw o r l d,2 0 2 1,6 2 3(1 7):1 0 6-1 0 7.(上接第5 1 6页)5 L IX,L IY,HONGC.N o v e l a d a p t i v eMEM Sg y r o s c o p ef i l t e r i n g a l g o r i t h mC/V i e n n a,A u s t r i a:2 0 2 2E u r o p e a n C o n f e r e n c e o n C o mm u n i c a t i o n S y s t e m s(E C C S),2 0 2 2:8 3-8 6.6 X I NG H,Z HAO X,Z HOU B,e ta l.A n a l y s i s o fd y n a m i cn o r t hf i n d i n gb a s e do nm u l t i p l e l o w-p r e c i s i o nM I MU sC/H i r o s h i m a,J a p a n:2 0 2 0I E E EI n t e r n a-t i o n a lS y m p o s i u m o nI n e r t i a lS e n s o r sa n d S y s t e m s(I N E R T I A L),2 0 2 0:1-4.7 A R I F F I N N H,A R S A D N.MEM Sg y r oa n da c c e l e r-o m e t e ra s n o r t h-f i n d i n g s y s t e m f o r b u l k d i r e c t i o nm a r k i n gJ.I E E EA c c e s s,2 0 2 2:1 1 4 2 1 4-1 1 4 2 2 2.8 GU H,Z HAO B,Z HOU H,e ta l.MEM Sg y r o s c o p eb i a sd r i f ts e l f-c a l i b r a t i o n b a s e d o n n o i s e-s u p p r e s s e dm o d er e v e r s a lJ.M i c r o m a c h i n e s,2 0 1 9,1 0(1 2):8 2 3.9 P O R AN GT,MAN E E THAMD,AUN G M M.K a l m a na n db u t t e r w o r t hf i l t e r c o m p a r i s o nf o rG P Sa n dm a g n e-t o m e t e rs e n s o r sC/S u r a b a y a,I n d o n e s i a:2 0 2 2I E E E8 t h I n f o r m a t i o n T e c h n o l o g y I n t e r n a t i o n a l S e m i n a r(I T I S),2 0 2 2:3 2-3 6.1 0R O S UF.R e c u r s i v ee r r o rd i v i s i o nK a l m a nf i l t e r f o r a d-v a n c e dd r i v e ra s s i s t a n c e r a d a r s y s t e m sC/B u c h a r e s t,R o m a n i a:2 0 2 01 3 t hI n t e r n a t i o n a lC o n f e r e n c eo nC o mm u-n i c a t i o n s,2 0 2 0:3 1-3 5.1 1X U Y,L IQ,Z HAN GY,e t a l.H o n e y c o m b-l i k ed i s kr e-s o n a t o rg y r o s c o p eJ.I E E ES e n sJ,2 0 1 9:8 5-9 4.1 2X U Y,L IQ,WAN G P,e ta l.0.0 1 5 D e g r e e-p e r-h o u rh o n e y c o m bd i s kr e s o n a t o rg y r o s c o p eJ.I E E ES e n sJ,2 0 2 0,7 3 2 6-7 3 3 8.1 3CHE NL,M I AO T,L IQ,e ta l.At e m p e r a t u r ed r i f ts u p p r e s s i o nm e t h o do fm o d e-m a t c h e dMEM Sg y r o s c o p eb a s e do na c o m b i n a t i o no fm o d e r e v e r s a l a n dm u l t i p l er e g r e s s i o nJ.M i c r o m a c h i n e s,2 0 2 2,1 3(1 0):1 5 5 7.1 4M I AOT,L IQ,HU X,e ta l.V i r t u a lr o t a t i n g MEM Sg y r o c o m p a s s i n gw i t hh o n e y c o m bd i s kr e s o n a t o rg y r o-s c o p eJ.I E E EE l e c t r o nD e v i c eL e t t,2 0 2 2:1 3 3 1-1 3 3 4.125 第4期刘 昕等:基于MEM S振动陀螺的旋翼无人机组合导航算法设计