本发明属于惯导系统误差参数标定技术领域,具体涉及一种两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法,可同时实现两套旋转惯导系统各自内杆臂、系统间外杆臂的在线动态标定。
背景技术
惯性导航系统的惯性测量单元(imu)由三个陀螺仪和三个加速度计组成,三个陀螺仪和三个加速度计可分别测量载体三个方向的角速度分量和线加速度分量,通过角速度与加速度的积分可实时获得载体的姿态、速度、位置信息。然而,在实际的惯导系统中,加速度计是具有一定大小和体积的实体元件,因此三个加速度计的安装位置不可能与imu的中心位置完全重合,且由于不可避免的安装误差,三个加速度敏感轴方向的延长线也无法相交于一点,由此造成每个加速度计的敏感测量点互不相同。由imu的中心至各加速度计测量点的连线矢量构成一组内杆臂,当存在角速度激励时,会产生“杆臂效应”,引起加速度误差,积分解算后会分别造成速度误差、位置误差。因此,内杆臂参数的标定和补偿是提高单套惯导系统导航精度的关键技术之一。
对于大型运载体而言,为确保安全性和可靠性,运载体一般会装配两套及以上的惯导系统,构成冗余配置结构,实现故障情况下的应急处置。由于安装位置的不同,两套惯导系统之间存在外杆臂,当载体存在角运动时,两套惯导系统输出的速度信息在杆臂效应的作用下有所差异,因此,为综合利用两套系统的导航信息,需进一步对两套惯导系统之间的杆臂参数进行精确标定并补偿。
针对上述问题,本发明提出了一种两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法,以两套旋转惯导的速度之差作为观测量,构建卡尔曼滤波器,实现对两套旋转惯导各自内杆臂、系统间外杆臂参数的在线动态标定。
技术实现要素:
本发明提出了一种两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法,以两套旋转惯导系统的速度之差作为量测量,构建卡尔曼滤波器,实现对两套旋转惯导系统各自的内杆臂、系统间外杆臂参数的在线动态标定。
本发明的技术方案为:一种两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法,具体步骤如下:
步骤(1)建立两套旋转惯导系统各自的内杆臂、系统间外杆臂误差与两套旋转惯导系统间速度之差的量测模型;
步骤(2)基于模型分析实现两套旋转惯导系统各自的内杆臂分离的原理与途径,设计两套旋转惯导的旋转策略;
步骤(3)利用以上步骤(2)的旋转策略控制rins1与rins2的框架旋转,并进行导航解算,实时输出两套旋转惯导的速度信息;
步骤(4)计算以上步骤(3)中得到的两套旋转惯导的速度之差,并利用以上步骤(1)的量测模型,构建卡尔曼滤波器,实现对两套旋转惯导系统内杆臂与外杆臂误差的在线动态标定。
本发明与现有技术相比的优点在于:
(1)本发明提出的两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法,充分利用两套旋转惯导的速度之差,通过构建卡尔曼滤波器,可同时实现对两套旋转惯导系统6个内杆臂与系统间外杆臂参数的最优估计和动态标定。
(2)本发明提出的两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法所需标定时间短,标定过程简单,且内杆臂与外杆臂的标定精度高,优于2mm。
(3)本发明提出的两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法,仅此两套旋转惯导的速度之差作为观测量,无需依赖其他外界参考信息,因此自主性强、易于实现。
附图说明
图1为本发明两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法的流程图;
图2为本发明具体实施例的两套旋转惯导系统各自内杆臂、系统间外杆臂的示意图;
图3为本发明具体实施例中载体角运动的姿态/航向角;
图4为本发明具体实施例中rins1、rins2的旋转策略;其中,图4(a)为rins1的旋转策略,图4(b)为rins2的旋转策略;
图5为本发明具体实施例中两套旋转惯导系统内杆臂与外杆臂误差参数的收敛曲线;其中,图5(a)为rins1的9个内杆臂分量的收敛曲线,图5(b)为rins2的9个内杆臂分量的收敛曲线,图5(c)为rins1与rins2之间的3个外杆臂分量的收敛曲线。
具体实施方式
下面结合附图和具体实施例对本发明作进一步介绍。
如图1所示,本发明一种两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法的具体实施方法如下:
1、建立两套旋转惯导系统各自的内杆臂、系统间外杆臂误差与两套旋转惯导系统间速度之差的量测模型;
如图2所示,为本发明的两套旋转惯导系统各自内杆臂、系统间外杆臂的示意图。其中,ob1-xb1yb1zb1、ob2-xb2yb2zb2分别为rins1、rins2的机体坐标系,原点ob1、ob2分别与rins1、rins2的imu中心重合,on-xnynzn为导航坐标系,其原点on与载体质心重合,ob1至ob2的矢量为rins1与rins2之间的外杆臂。ox1,oy1,oz1分别代表rins1的x,y,z加速度计的安装位置,为rins1的三个内杆臂;ox2,oy2,oz2分别代表rins2的x,y,z加速度计的安装位置,为rins2的三个内杆臂。
以rins1的x加速度计为例推导杆臂误差模型。如图2所示,rins1与载体质心的空间距离构成一组外杆臂同时,x加速度计与imu中心构成一组内杆臂当存在载体角运动和rins1绕框架的旋转运动时,x加速度计敏感的加速度等于牵连加速度、相对加速度与科氏加速度的矢量和。
牵连加速度为:
式(1)中,矢量可进一步表示为:
相对加速度为:
科氏加速度为:
因此,在内杆臂、外杆臂的作用下,x加速度计输出的加速度信息中包含的加速度误差为:
将上式展开为:
式(1)—(5)中,分别为载体角速度和rins1的框架旋转角速度,分别为载体角速度微分和框架旋转角速度微分,为rins1的x加速度计与imu中心之间的内杆臂,为x加速度计与载体质心之间的杆臂,为rins1的imu中心与载体质心之间的外杆臂,根据矢量合成原理,有
式(5)中,杆臂矢量在空间三维坐标系中可表示为沿三个坐标轴方向的分量,即rx1的下角标中x表示在x方向的投影,1表示1号旋转惯导系统(即rins1),rxx1的下角标中第一个x表示在x方向的投影,第二个x表示1号惯导系统的x加速度计,1表示1号旋转惯导系统(即rins1)。
同理,rins2的x加速度计的外杆臂与内杆臂造成的加速度误差为:
式(6)中,分别为载体角速度和rins2的框架旋转角速度,分别为载体角速度微分和框架旋转角速度微分,为rins2的x加速度计与imu中心之间的内杆臂,为rins2的imu中心与载体质心之间的外杆臂。
由于两套旋转惯导系统同时与载体固连,因此rins1、rins2敏感的相同,将式(5)与式(6)作差,可得两套旋转惯导各自内杆臂、系统间外杆臂共同作用下的加速度误差为:
式(7)中,为rins1与rins2之间的外杆臂矢量。
进一步的,可得两套旋转惯导系统各自的内杆臂、系统间外杆臂误差与两套旋转惯导系统间速度之差的量测模型为:
式(8)中,表示rins1、rins2的敏感轴坐标系至导航坐标系的旋转变换矩阵,表示rins1内杆臂引起的加速度误差,表示rins2内杆臂引起的加速度误差,表示rins1与rins2之间的外杆臂引起的加速度误差。
2、分析实现两套旋转惯导系统各自的内杆臂分离的原理与途径,设计两套旋转惯导的旋转策略;
由式(7)可知,rins1与rins2之间的外杆臂仅由载体角运动激励,rins1、rins2的内杆臂同时由载体角运动和框架的旋转运动激励,因此,当载体存在角运动,即同时,rins1、rins2存在框架的旋转运动,并满足时,可实现外杆臂内杆臂的分离与估计。
因此,rins1与rins2旋转策略的设计需满足的原则。由于角速度是既有大小又有方向的矢量,因此提供不同的角速度有三种实现途径:其一,rins1、rins2的旋转轴方向相同,但旋转角速度大小不同;其二,rins1、rins2的旋转角速度大小相同,但旋转轴方向不同;其三,rins1、rins2的旋转轴方向与旋转角速度大小均不相同。以下分别从这三方面进行分析:
(a)rins1、rins2的旋转轴方向相同,但旋转角速度大小不同;
假设rins1、rins2均绕内框轴旋转,但旋转角速度大小不同,即满足时,rins1、rins2的内杆臂造成的加速度误差分别为:
此时,rins1、rins2的旋转矩阵分别为:
将式(9)与(10)代入式(7)并展开,可得rins1与rins2的内杆臂共同作用下的加速度误差为:
式(9)至式(11)中,分别为rins1、rins2的内框旋转角度,且旋转角度与旋转角速度之间存在积分关系,如下:
由式(11)和式(12)可知,当大小不同时,旋转角度及旋转矩阵的变化周期不同,根据周期的差异,可实现对rins1、rins2内杆臂的分离。
(b)rins1、rins2的旋转角速度大小相同,但旋转轴方向不同;
假设rins1绕内框轴旋转,rins2绕中框轴旋转,即满足且时,rins1、rins2的内杆臂造成的加速度误差分别为:
此时,可分别表示为:
其中,表示rins1的内框旋转角度、表示rins2的中框旋转角度。
同理,将式(13)与(14)代入式(7)并展开,可得rins1与rins2的内杆臂共同作用下的加速度误差为:
由式(15)可知,当rins1、rins2的旋转轴方向不同时,rins1的内杆臂分量与rins2的内杆臂分量在的体现上有正弦和余弦之分,根据正弦和余弦的差异,可实现对rins1、rins2内杆臂的分离。
(c)rins1、rins2的旋转轴方向与旋转角速度大小均不相同;
当rins1、rins2的旋转轴方向与旋转角速度大小均不相同,即同时满足前两个实现途径时,由前述分析可知,rins1、rins2的内杆臂分量在加速度误差的体现上既有周期上的差异,又有正余弦之分,根据这两方面的差异,可实现对rins1、rins2内杆臂的分离。
3、计算两套旋转惯导的速度之差,以此速度之差作为量测量,构建卡尔曼滤波器,实现对两套旋转惯导系统内杆臂与外杆臂误差的在线动态标定。
1)卡尔曼滤波器的状态模型为:
式(16)中,表示rins1内杆臂引起的加速度误差,表示rins2内杆臂引起的加速度误差,表示rins1与rins2之间的外杆臂引起的加速度误差。
根据状态模型,选取状态变量为:
其中,δφe,δφn,δφu分别为rins1、rins2的东向平台偏角、北向平台偏角、天向平台偏角之差;δve,δvn分别为rins1、rins2的东向速度、北向速度之差;εx1,εy1,εz1、εx2,εy2,εz2分别为rins1、rins2的陀螺漂移;分别为rins1、rins2的加速度计零偏;rxx1,rxy1,rxz1、ryx1,ryy1,ryz1、rzx1,rzy1,rzz1为rins1的9个内杆臂分量,rxx2,rxy2,rxz2、ryx2,ryy2,ryz2、rzx2,rzy2,rzz2为rins2的9个内杆臂分量,rx,ry,rz为rins1与rins2之间的3个外杆臂分量。
2)卡尔曼滤波器的量测模型为:
式(17)中,δve,δvn分别为rins1、rins2的东向速度、北向速度之差;下角标中ins1、ins2分别表示rins1、rins2输出的速度信息。
由此,以两套旋转惯导系统的速度之差作为量测量,以两套旋转惯导的平台偏角、速度误差、两套旋转惯导系统各自的内杆臂、系统间外杆臂作为状态量,构建卡尔曼滤波器,实现对rins1的9个内杆臂分量、rins2的9个内杆臂分量以及rins1与rins2之间的3个外杆臂分量的估计。
4、设计仿真实验,对本发明提出的两套旋转惯导内杆臂与外杆臂误差在线动态标定方法进行仿真验证。
仿真实例中,设置载体角运动分别符合如下规律:
载体俯仰角:
载体横滚角:
载体航向角:
其中,ai符合均值为4°,方差为2°的正态分布,tθi符合均值为5s,方差为3s的正态分布;bi符合均值为5°,方差为2°的正态分布,tγi符合均值为6s,方差为4s的正态分布;ci符合均值为3°,方差为2°的正态分布,tψi符合均值为8s,方差为3s的正态分布。符合区间[02π]的均匀分布,m=20。如图3所示,为载体的航向/姿态变化曲线。
图4为本发明仿真实例中rins1、rins2的旋转策略,其中,图4(a)为rins1的旋转策略、图4(b)为rins2的旋转策略。如图所示,rins1、rins2的框架旋转角度遵循余弦变化规律式中t为周期。rins1框架旋转角度的余弦周期为60s,框架旋转顺序为:内框-外框-中框;rins2框架旋转角度的余弦周期为48s,框架旋转顺序为:内框-外框-中框。
此外,仿真实例中分别加入陀螺漂移与加速度计零偏为:陀螺随机常值漂移为0.01°/h,随机游走为加计常值零偏为50μg,随机游走为
在上述仿真条件下,验证本发明提出的两套旋转惯导内杆臂与外杆臂误差在线动态标定方法,仿真结果如图5所示:其中,图5(a)为rins1的9个内杆臂分量的收敛曲线,图5(b)为rins2的9个内杆臂分量的收敛曲线,图5(c)为rins1与rins2之间的3个外杆臂分量的收敛曲线。从各收敛曲线可以看出,两套旋转惯导系统内杆臂与外杆臂各分量的收敛速度快,收敛过程平稳。为定量分析标定精度,将两套旋转惯导系统内杆臂与外杆臂在线动态标定进行了6次重复性实验,得到的估计结果的平均值及标准差如表1所示。由表1可知,本发明提出的两套旋转惯导系统内杆臂与外杆臂在线动态标定的标定精度优于1mm,证明了本发明的有效性。
表1两套旋转惯导系统内杆臂与外杆臂误差参数在线动态标定结果
总之,本发明可实现对两套旋转惯导系统内杆臂与外杆臂误差的在线动态标定,对提高两套旋转惯导系统导航信息交互与传递的精度具有重要意义。
本发明未详细公开的部分属于本领域的公知技术。
尽管上面对本发明说明性的具体实施方式进行了描述,以便于本技术领域的技术人员理解本发明,但应该清楚,本发明不限于具体实施方式的范围,对本技术领域的普通技术人员来讲,只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内,这些变化是显而易见的,一切利用本发明构思的发明创造均在保护之列。