[发明专利]一种两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法

权利要求书

1.一种两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法，其特征在于，实现步骤如下：

步骤(1)建立两套旋转惯导系统各自的内杆臂、系统间外杆臂误差与两套旋转惯导系统间速度之差的量测模型；

步骤(2)基于以上步骤(1)的模型分析实现两套旋转惯导系统各自的内杆臂分离的原理与途径，设计两套旋转惯导的旋转策略；

步骤(3)利用以上步骤(2)的旋转策略分别控制1号旋转惯导系统RINS1与2号旋转惯导系统RINS2的框架旋转，并进行导航解算，实时输出两套旋转惯导的速度信息；

步骤(4)计算以上步骤(3)中得到的两套旋转惯导的速度之差，并利用以上步骤(1)的量测模型，构建卡尔曼滤波器，实现对两套旋转惯导系统内杆臂与外杆臂误差的在线动态标定；

其中，所述的步骤(1)中，两套旋转惯导系统各自的内杆臂、系统间外杆臂误差与两套旋转惯导系统间速度之差的量测模型的建立过程为：

对于1号旋转惯导系统RINS1的x加速度计的杆臂误差模型，1号旋转惯导系统RINS1与载体质心的空间距离构成一组外杆臂，同时，x加速度计与IMU中心构成一组内杆臂，当存在载体角运动和1号旋转惯导系统RINS1绕框架的旋转运动时，x加速度计敏感的加速度等于牵连加速度、相对加速度与科氏加速度的矢量和；

牵连加速度为：

式(1)中，矢量进一步表示为：

相对加速度为：

科氏加速度为：

因此，在内杆臂、外杆臂的作用下，x加速度计输出的加速度信息中包含的加速度误差为：

将上式展开为：

式(1)—(5)中，分别为载体角速度和1号旋转惯导系统RINS1的框架旋转角速度，分别为载体角速度微分和框架旋转角速度微分，为1号旋转惯导系统RINS1的x加速度计与IMU中心之间的内杆臂，为x加速度计与载体质心之间的杆臂，为1号旋转惯导系统RINS1的IMU中心与载体质心之间的外杆臂，根据矢量合成原理，有同时，杆臂矢量在空间三维坐标系中表示为沿三个坐标轴方向的分量，即R_x1的下角标中x表示在x方向的投影，1表示1号旋转惯导系统RINS1，r_xx1的下角标中第一个x表示在x方向的投影，第二个x表示1号惯导系统的x加速度计，1表示1号旋转惯导系统RINS1；

同理，2号旋转惯导系统RINS2的x加速度计的杆臂误差为：

式(6)中，分别为载体角速度和2号旋转惯导系统RINS2的框架旋转角速度，分别为载体角速度微分和框架旋转角速度微分，为2号旋转惯导系统RINS2的x加速度计与IMU中心之间的内杆臂，为2号旋转惯导系统RINS2的IMU中心与载体质心之间的外杆臂；同理，R_x2的下角标中x表示在x方向的投影，2表示2号旋转惯导系统(RINS2)，r_xx2的下角标中第一个x表示在x方向的投影，第二个x表示2号惯导系统的x加速度计，2表示2号旋转惯导系统RINS2；

由于两套旋转惯导系统同时与载体固连，因此1号旋转惯导系统RINS1、2号旋转惯导系统RINS2敏感的相同，将式(5)与式(6)作差，得到两套旋转惯导的x加速度计在各自内杆臂、系统间外杆臂共同作用下的加速度误差为：

式(7)中，为1号旋转惯导系统RINS1与2号旋转惯导系统RINS2之间的外杆臂矢量；

进一步的，得到两套旋转惯导的x加速度计的内杆臂、系统间外杆臂误差与两套旋转惯导系统间速度之差的量测模型为：

式(8)中，表示1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的敏感轴坐标系至导航坐标系的旋转变换矩阵，表示1号旋转惯导系统RINS1内杆臂引起的加速度误差，表示2号旋转惯导系统RINS2内杆臂引起的加速度误差，表示1号旋转惯导系统RINS1与2号旋转惯导系统RINS2之间的外杆臂引起的加速度误差；

所述的步骤(2)中，实现两套旋转惯导系统各自的内杆臂分离的分析过程及旋转策略的设计过程如下：

1)分析实现两套旋转惯导系统各自的内杆臂分离的原理与途径：由式(7)得到，1号旋转惯导系统RINS1与2号旋转惯导系统RINS2之间的外杆臂仅由载体角运动激励，1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的内杆臂同时由载体角运动和框架的旋转运动激励，因此，当载体存在角运动，即同时，RINS1、RINS2存在框架的旋转运动，并满足时，实现外杆臂内杆臂的分离与估计；

2)旋转策略的设计原则与具体实现：1号旋转惯导系统RINS1与2号旋转惯导系统RINS2旋转策略的设计需满足的原则，由于角速度是既有大小又有方向的矢量，因此提供不同的角速度有三种实现途径：其一，1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转轴方向相同，但旋转角速度大小不同；其二，1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转角速度大小相同，但旋转轴方向不同；其三，1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转轴方向与旋转角速度大小均不相同，以下分别从这三方面进行分析：

(a)1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转轴方向相同，但旋转角速度大小不同；

假设1号旋转惯导系统RINS1、2号旋转惯导系统RINS2均绕内框轴旋转，但旋转角速度大小不同，即满足时，1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的内杆臂造成的加速度误差分别为：

式(9)中，r_xx1,r_xy1,r_xz1、r_yx1,r_yy1,r_yz1、r_zx1,r_zy1,r_zz1为1号旋转惯导系统RINS1的9个内杆臂分量，

此时，1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转矩阵分别为：

将式(9)与(10)代入式(7)并展开，得到1号旋转惯导系统RINS1与2号旋转惯导系统RINS2的内杆臂共同作用下的加速度误差为：

式(9)至式(11)中，分别为1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的内框旋转角度，且旋转角度与旋转角速度之间存在积分关系，如下：

由式(11)和式(12)得到，当大小不同时，旋转角度及旋转矩阵的变化周期不同，根据周期的差异，实现对1号旋转惯导系统RINS1、2号旋转惯导系统RINS2内杆臂的分离；

(b)1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转角速度大小相同，但旋转轴方向不同；

假设1号旋转惯导系统RINS1绕内框轴旋转，2号旋转惯导系统RINS2绕中框轴旋转，即满足且时，1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的内杆臂造成的加速度误差分别为：

此时，分别表示为：

其中，表示1号旋转惯导系统RINS1的内框旋转角度、表示2号旋转惯导系统RINS2的中框旋转角度；

同理，将式(13)与(14)代入式(7)并展开，得到1号旋转惯导系统RINS1与2号旋转惯导系统RINS2的内杆臂共同作用下的加速度误差为：

由式(15)得到，当1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转轴方向不同时，1号旋转惯导系统RINS1的内杆臂分量与2号旋转惯导系统RINS2的内杆臂分量在的体现上有正弦和余弦之分，根据正弦和余弦的差异，实现对1号旋转惯导系统RINS1、2号旋转惯导系统RINS2内杆臂的分离；

(c)1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转轴方向与旋转角速度大小均不相同；

当1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的旋转轴方向与旋转角速度大小均不相同，即同时满足前两个实现途径时，由前述分析得到，1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的内杆臂分量在加速度误差的体现上既有周期上的差异，又有正余弦之分，根据这两方面的差异，实现对1号旋转惯导系统RINS1、2号旋转惯导系统RINS2内杆臂的分离。

2.根据权利要求1所述的两套旋转惯导系统内杆臂与外杆臂误差在线动态标定方法，其特征在于：所述的步骤(4)中，卡尔曼滤波器的构建过程如下：

1)卡尔曼滤波器的状态模型为：

式(16)中，表示1号旋转惯导系统RINS1内杆臂引起的加速度误差，表示2号旋转惯导系统RINS2内杆臂引起的加速度误差，表示1号旋转惯导系统RINS1与2号旋转惯导系统RINS2之间的外杆臂引起的加速度误差；

根据状态模型，选取状态变量为：

其中，δφ_E,δφ_N,δφ_U分别为1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的东向平台偏角、北向平台偏角、天向平台偏角之差；δV_E,δV_N分别为1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的东向速度、北向速度之差；ε_x1,ε_y1,ε_z1、ε_x2,ε_y2,ε_z2分别为1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的陀螺漂移；分别为1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的加速度计零偏；r_xx1,r_xy1,r_xz1、r_yx1,r_yy1,r_yz1、r_zx1,r_zy1,r_zz1为1号旋转惯导系统RINS1的9个内杆臂分量，r_xx2,r_xy2,r_xz2、r_yx2,r_yy2,r_yz2、r_zx2,r_zy2,r_zz2为2号旋转惯导系统RINS2的9个内杆臂分量，R_x,R_y,R_z为1号旋转惯导系统RINS1与2号旋转惯导系统RINS2之间的3个外杆臂分量；

2)卡尔曼滤波器的量测模型为：

式(17)中，δV_E,δV_N分别为1号旋转惯导系统RINS1、2号旋转惯导系统RINS2的东向速度、北向速度之差；下角标中INS1、INS2分别表示1号旋转惯导系统RINS1、2号旋转惯导系统RINS2输出的速度信息；

由此，以两套旋转惯导系统的速度之差作为量测量，以两套旋转惯导的平台偏角、速度误差、两套旋转惯导系统各自的内杆臂、系统间外杆臂作为状态量，构建卡尔曼滤波器，实现对1号旋转惯导系统RINS1的9个内杆臂分量、2号旋转惯导系统RINS2的9个内杆臂分量以及1号旋转惯导系统RINS1与2号旋转惯导系统RINS2之间的3个外杆臂分量的估计。