用于低精度捷联惯导系统的动基座初始对准方法及装置

本发明属于捷联惯导系统领域，涉及一种用于低精度捷联惯导系统的动基座初始对准方法及装置。

背景技术：

1、捷联惯导系统具有优越的自主性，在军事和民用领域有着广泛的应用。低精度捷联惯导系统，凭借其体积小和低成本的优点，成为了机器人和无人机系统上不可或缺的一部分。

2、基于航位推算原理，惯性导航坐标系统在正常运行前必须要进行初始对准。低精度捷联惯导系统的初始对准，通常采用卫星导航系统辅助，并且在动基座上进行。通过采集传感器输出的信息，构造对准矢量，可以将初始对准转化成基于矢量观测器的姿态估计问题，通过最优化对准方法完成姿态的确定。但是，最优化的对准方法未考虑低精度捷联惯导系统的陀螺仪零偏的累积误差，同时卫星导航系统接收机易受外部环境的影响如障碍物和多径效应等，输出有时会出现野值。这些因素导致对准矢量性能下降，影响初始对准的收敛速度和精度。

技术实现思路

1、本发明的目的在于克服上述现有技术中，用于低精度捷联惯导系统的动基座初始对准时，初始对准的收敛速度和精度较低的缺点，提供一种用于低精度捷联惯导系统的动基座初始对准方法及装置，能够抑制陀螺零偏累积误差，同时也能对卫星导航系统接收机输出中的野值进行检测和剔除，提高了用于低精度捷联惯导系统的动基座初始对准的精度和收敛速度。

2、为达到上述目的，本发明采用以下技术方案予以实现：

3、本发明第一方面，提供一种用于低精度捷联惯导系统的动基座初始对准方法，包括：

4、s1：根据捷联惯导系统和卫星导航系统接收机的输出信息求解导航坐标系和载体坐标系的变换矩阵微分方程，得到导航坐标系和载体坐标系的变换矩阵；

5、s2：根据捷联惯导系统和卫星导航系统接收机的输出信息、载体坐标系和导航坐标系的变换矩阵，构建动基座的参考矢量和观测矢量；

6、s3：基于动基座的参考矢量和观测矢量，通过最优化对准方法求解对准起始时刻的常值载体姿态矩阵；

7、s4：定义动基座的参考矢量与观测矢量的模值平方残差，将模值平方残差标准化得到标准化模值平方残差，并通过huber鲁棒滤波理论构造关于标准化模值平方残差的权值函数；

8、s5：以误差广义罗德里格参数和陀螺仪零偏作为状态向量，结合权值函数和预设的无迹四元数姿态估计器，构建鲁棒无迹四元数姿态估计器，并根据预设的预测模型和预设的量测模型，通过鲁棒无迹四元数姿态估计器估计载体坐标系的变换四元数，得到最优的载体坐标系的变换四元数；

9、s6：将最优的载体坐标系的变换四元数转化为最优的载体坐标系的变换矩阵，结合导航坐标系的变换矩阵和最优的载体坐标系的变换矩阵，以及对准起始时刻的常值载体姿态矩阵，得到实时的载体姿态矩阵。

10、可选的，所述根据捷联惯导系统和卫星导航系统接收机的输出信息求解导航坐标系和载体坐标系的变换矩阵微分方程，得到导航坐标系和载体坐标系的变换矩阵包括：

11、根据卫星导航系统接收机的输出信息，求解导航坐标系的变换矩阵微分方程：

12、

13、其中，n表示导航坐标系，i表示惯性坐标系，n(0)表示初始对准开始时刻的导航坐标系，相对于惯性坐标系保持静止，n(t)表示t时刻的导航坐标系，表示从n(0)系转到n(t)系的坐标系变换矩阵，表示的微分，表示导航坐标系相对于惯性坐标系的旋转角速度在导航坐标系下的投影坐标，表示的反对称矩阵；

14、得到导航坐标系的变换矩阵：

15、

16、其中，tk表示从初始对准开始的第k个卫星导航系统数据的采样时刻，tk-1表示从初始对准开始的第k-1个卫星导航系统数据的采样时刻，δtg为两个采样时刻的差值即卫星导航系统数据采样间隔，tk-tk-1＝δtg；

17、

18、其中，i3表示3×3的单位矩阵，φk表示导航坐标系在时间(tk-1,tk)旋转变化的等效旋转矢量，为在时间(tk-1,tk)的积分，通过下式计算得到：

19、

20、其中，在时间(tk-1,tk)内为常量；

21、根据捷联惯导系统的输出信息，求解载体坐标系的变换矩阵微分方程：

22、

23、其中，b表示载体坐标系，b(0)表示初始对准开始时刻的载体坐标系，相对于惯性坐标系保持静止，b(t)表示t时刻的载体坐标系，表示从b(0)系转到b(t)系的坐标系变换矩阵，表示的微分，表示陀螺仪的输出，表示的反对称矩阵；

24、得到载体坐标系的变换矩阵：

25、

26、其中，通过下式得到：

27、

28、其中，θk表示载体坐标系在时间(tk-1,tk)转动的等效旋转矢量，为时间(tk-1,tk)的陀螺仪输出角度增量：

29、

30、其中，δts表示捷联惯导系统的采样间隔，d表示预设的正整数，表示在时间(tk-1,tk)内陀螺仪的第l次采样角速度。

31、可选的，所述根据捷联惯导系统和卫星导航系统接收机的输出信息、载体坐标系和导航坐标系的变换矩阵，构建动基座的参考矢量和观测矢量包括：

32、由姿态矩阵的链式法则和导航坐标系下的比力方程，得到导航坐标系下载体速度的微分方程：

33、

34、其中，vn表示导航坐标系下载体的速度矢量，表示vn的微分，表示对准起始时刻的常值载体姿态矩阵，fb表示加速度计的输出，e表示地球坐标系，表示地球自转角速度在导航坐标系下的投影，表示导航坐标系相对于地球系的角速度在导航坐标系下的投影，gn表示重力加速度在导航坐标系下的投影；

35、在导航坐标系下载体速度的速度微分方程两边对时间(0,t)积分，再把从积分中提取出来，整理并离散化对准矢量可得：

36、

37、

38、其中，t＝tk，ti+1-ti＝δtg，δtg是卫星导航系统接收机采样间隔时间，αok是tk时刻动基座的参考矢量，βok是tk时刻动基座的观测矢量，

39、可选的，所述基于动基座的参考矢量和观测矢量，通过最优化对准方法求解对准起始时刻的常值载体姿态矩阵包括：

40、将tk时刻动基座的参考矢量αok和观测矢量βok构建如下矩阵：

41、

42、其中，yk表示tk时刻的最优化对准姿态确定矩阵，yk-1表示tk-1时刻的最优化对准姿态确定矩阵，与[αok⊙]矩阵的表达式如下所示：

43、

44、

45、求解yk的最小特征值对应的特征向量，得到最优的对应的四元数，然后通过最优的对应的四元数和姿态矩阵的转化公式，求解对准起始时刻的常值载体姿态矩阵

46、可选的，所述定义动基座的参考矢量与观测矢量的模值平方残差，将模值平方残差标准化得到标准化模值平方残差，并通过huber鲁棒滤波理论构造关于标准化模值平方残差的权值函数包括：

47、建立卫星导航系统接收机输出模型：

48、

49、其中，表示tk时刻卫星导航系统接收机的原始速度输出信息，vn(tk)表示tk时刻卫星导航系统接收机输出的速度真值，δvkn表示tk时刻的高斯白噪声，卫星导航系统接收机的导航坐标系速度输出的噪声服从高斯分布，δvkn～n(0 σ2i3)，σ2表示噪声各通道的方差，代入βok的表达式可得：

50、

51、其中，表示tk时刻的带有噪声的观测矢量，βok表示tk时刻的观测矢量的真实值，表示tk时刻的观测矢量中的高斯白噪声；

52、定义和αok的模值平方的残差如下：

53、

54、其中，表示定义的tk时刻的残差，均值和方差为：

55、

56、

57、其中，σ表示接收机速度输出中高斯白噪声的标准差；

58、通过下式将进行标准化：

59、

60、其中，λk表示tk时刻标准化的残差；

61、根据huber鲁棒滤波理论，构造关于λk的权值函数：

62、

63、其中，|λk|是λk的绝对值，γ是高斯阈值。

64、可选的，所述以误差广义罗德里格参数和陀螺仪零偏作为状态向量，结合权值函数和预设的无迹四元数姿态估计器，构建鲁棒无迹四元数姿态估计器，并根据预设的预测模型和预设的量测模型，通过鲁棒无迹四元数姿态估计器估计载体坐标系的变换四元数，得到最优的载体坐标系的变换四元数包括：

65、所述预设的预测模型为：

66、

67、

68、其中，表示从b(0)系到b(t)系的坐标系变换矩阵，表示的微分，εb(t)表示εb(t)的微分，ηgv表示均值为0的高斯白噪声；

69、求解预设的预测模型，以载体坐标系变换四元数形式表示如下：

70、

71、其中，qk表示载体坐标系的变换矩阵对应的四元数，h(.)表示四元数更新函数；

72、所述预设的量测模型为：

73、

74、其中，表示通过最优化对准方法求解出的对准起始时刻的常值载体姿态矩阵，qk表示载体坐标系的变换矩阵对应的四元数，c(qk)表示将qk转化为载体坐标系的变换矩阵，ηr表示均值为0的高斯白噪声，βk的表达式与βok相同，

75、

76、结合权值函数，以改进的tk时刻最优化对准姿态确定矩阵gk来求解姿态矩阵

77、

78、通过权值函数和预设的无迹四元数姿态估计器构造tk时刻的改进的观测矢量

79、

80、其中，是量测预测值；

81、将gk、和无迹四元数姿态估计器相结合，得到鲁棒无迹四元数姿态估计器，并通过鲁棒无迹四元数姿态估计器估计载体坐标系的变换四元数，得到tk时刻最优的载体坐标系的变换四元数

82、可选的，所述将最优的载体坐标系的变换四元数转化为最优的载体坐标系的变换矩阵，结合导航坐标系的变换矩阵和最优的载体坐标系的变换矩阵，以及对准起始时刻的常值载体姿态矩阵，得到实时的载体姿态矩阵包括：

83、根据tk时刻导航坐标系的变换矩阵和tk时刻最优的载体坐标系的变换矩阵以及tk时刻的对准起始时刻的常值载体姿态矩阵，通过下式得到tk时刻的载体姿态矩阵：

84、

85、其中，表示tk时刻的载体姿态矩阵，表示最优的载体坐标系的变换矩阵。

86、本发明第二方面，提供一种用于低精度捷联惯导系统的动基座初始对准系统，包括：

87、变换矩阵求解模块，用于根据捷联惯导系统和卫星导航系统接收机的输出信息求解导航坐标系和载体坐标系的变换矩阵微分方程，得到导航坐标系和载体坐标系的变换矩阵；

88、矢量构建模块，用于根据捷联惯导系统和卫星导航系统接收机的输出信息、载体坐标系和导航坐标系的变换矩阵，构建动基座的参考矢量和观测矢量；

89、常值求解模块，用于基于动基座的参考矢量和观测矢量，通过最优化对准方法求解对准起始时刻的常值载体姿态矩阵；

90、权值函数构造模块，用于定义动基座的参考矢量与观测矢量的模值平方残差，将模值平方残差标准化得到标准化模值平方残差，并通过huber鲁棒滤波理论构造关于标准化模值平方残差的权值函数；

91、优化模块，用于以误差广义罗德里格参数和陀螺仪零偏作为状态向量，结合权值函数和预设的无迹四元数姿态估计器，构建鲁棒无迹四元数姿态估计器，并根据预设的预测模型和预设的量测模型，通过鲁棒无迹四元数姿态估计器估计载体坐标系的变换四元数，得到最优的载体坐标系的变换四元数；

92、载体姿态求解模块，用于将最优的载体坐标系的变换四元数转化为最优的载体坐标系的变换矩阵，结合导航坐标系的变换矩阵和最优的载体坐标系的变换矩阵，以及对准起始时刻的常值载体姿态矩阵，得到实时的载体姿态矩阵。

93、本发明第三方面，提供一种计算机设备，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述用于低精度捷联惯导系统的动基座初始对准方法的步骤。

94、本发明第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现上述用于低精度捷联惯导系统的动基座初始对准方法的步骤。

95、与现有技术相比，本发明具有以下有益效果：

96、本发明用于低精度捷联惯导系统的动基座初始对准方法，通过huber鲁棒滤波理论构造关于标准化模值平方残差的权值函数，能够检验观测矢量中卫星导航接收机输出的野值。并且，通过预设的预测模型与量测模型，使用无迹四元数估计器估计陀螺仪零偏和最优的载体坐标系的变换矩阵，继而将权值函数与无迹四元数估计器结合，构建鲁棒无迹四元数姿态估计器，并通过鲁棒无迹四元数姿态估计器估计载体坐标系的变换四元数，有效剔除卫星导航接收机输出的野值，提高了对准过程的稳定性，并且能够估计陀螺仪零偏并去除，最终得到最优的载体坐标系的变换四元数并转换为最优的载体坐标系的变换矩阵，继而根据最优的载体坐标系的变换矩阵，结合导航坐标系的变换矩阵和最优的载体坐标系的变换矩阵，以及对准起始时刻的常值载体姿态矩阵，得到实时的载体姿态矩阵，完成低精度捷联惯导系统动基座的初始对准，有效提高了对准的精度和收敛速度。