一种基于红外地平仪和陀螺仪融合的大失准角对准方法

本发明涉及飞行器导航，具体涉及一种基于红外地平仪和陀螺仪融合的大失准角对准方法。

背景技术：

1、飞行器导航系统工作前通常需要确定机体坐标系相对于导航坐标系的初始姿态。对于存在大失准角及机体运动的应用场景，姿态传播和测量过程具有明显非线性，采用基于小失准角假设的线性误差模型容易产生较大的建模误差。

2、现有gnss辅助对准方法通过融合卫星定位信息和惯性测量单元输出进行姿态估计，但其依赖外部卫星信号。在建筑遮挡、森林、强干扰及其他卫星信号受限环境中，辅助信息的连续性和可靠性容易下降。同时，gnss主要提供位置及速度信息，不能直接提供高精度姿态角，在大失准角条件下通常需要较长时间完成姿态误差收敛。

3、现有视觉辅助对准方法通过提取和匹配图像中的角点、边缘及纹理特征确定飞行器姿态，但在低光照、强光、单色背景及高速运动条件下，容易出现图像模糊、特征不足和匹配失效。图像特征提取、匹配及姿态解算还需要较大的计算资源，对飞行器嵌入式处理平台提出较高要求。

4、红外地平仪能够根据天空与地面红外辐射差异输出滚转角和俯仰角信息，但单独使用红外地平仪难以利用陀螺仪的高频角速度信息连续传播姿态，也难以同时估计陀螺仪零偏。现有容积卡尔曼滤波大失准角对准方法主要采用gps、加速度计、dvl及主惯导信息作为辅助观测，尚需建立适配红外地平仪测量特点的状态初始化、协方差初始化和姿态观测模型。

技术实现思路

1、本发明旨在解决上述技术问题而提供一种基于红外地平仪和陀螺仪融合的大失准角对准方法，实现在不依赖gnss信号和环境视觉特征的条件下，利用红外地平仪提供的姿态角测量信息与陀螺仪提供的角速度信息，建立适用于大失准角的非线性状态传播模型和观测模型，并通过容积卡尔曼滤波完成飞行器姿态及陀螺仪零偏的联合估计。

2、本发明的目的可以通过以下技术方案来实现：

3、一种基于红外地平仪和陀螺仪融合的大失准角对准方法，包括：

4、s1、建立系统状态模型，系统状态向量包括机体姿态对应的广义罗德里格斯参数和陀螺仪零偏；根据所述广义罗德里格斯参数的更新关系和所述陀螺仪零偏的变化关系建立状态传播模型；根据红外地平仪的初始测量值和陀螺仪零偏初始值建立系统初始状态向量，并根据所述红外地平仪测得的欧拉角对应的欧拉角协方差矩阵确定系统初始协方差矩阵；

5、s2、建立系统观测模型，将所述红外地平仪测得的欧拉角转换至机体坐标系，以所述机体坐标系下的滚转角和俯仰角作为观测量，建立观测模型；

6、s3、采用容积卡尔曼滤波器融合陀螺仪和所述红外地平仪的姿态信息，依次进行系统初始化、时间更新和测量更新，得到更新后的广义罗德里格斯参数和陀螺仪零偏，并根据所述更新后的广义罗德里格斯参数确定飞行器姿态对准结果。

7、进一步的，所述系统状态向量为：

8、

9、其中，为时刻k的系统姿态的grp，为时刻k的陀螺仪零偏；

10、所述广义罗德里格斯参数为：

11、

12、其中，表示姿态四元数的向量部分，表示所述姿态四元数的标量部分，a表示广义罗德里格斯参数的形式参数。

13、进一步的，所述状态传播模型中的广义罗德里格斯参数按照下式更新：

14、

15、其中，δt表示系统离散采样时间，表示时刻k的陀螺仪测量角速度，表示陀螺仪过程噪声；

16、所述陀螺仪零偏按照随机游走过程更新：

17、

18、其中，表示陀螺仪零偏过程噪声。

19、进一步的，根据所述红外地平仪的初始测量值建立所述系统初始状态向量，包括：

20、获取所述红外地平仪的三轴输出电压，根据所述三轴输出电压确定红外地平仪坐标系下的滚转角和俯仰角；

21、根据所述滚转角和所述俯仰角确定所述红外地平仪坐标系下的第一方向余弦矩阵；

22、根据所述红外地平仪相对于机体的安装角确定安装角方向余弦矩阵，并根据所述安装角方向余弦矩阵和所述第一方向余弦矩阵确定机体坐标系下的第二方向余弦矩阵；

23、将所述第二方向余弦矩阵转换为初始姿态四元数，并将所述初始姿态四元数转换为初始广义罗德里格斯参数；

24、将所述陀螺仪零偏初始值设置为零，并根据所述初始广义罗德里格斯参数和所述陀螺仪零偏初始值组成所述系统初始状态向量。

25、进一步的，根据所述欧拉角协方差矩阵确定所述系统初始协方差矩阵，包括：

26、确定所述广义罗德里格斯参数对欧拉角的雅可比矩阵，并按照下式确定广义罗德里格斯参数初始协方差矩阵：

27、

28、其中，表示所述广义罗德里格斯参数初始协方差矩阵，表示所述广义罗德里格斯参数对欧拉角的雅可比矩阵，表示所述欧拉角协方差矩阵；

29、将所述广义罗德里格斯参数初始协方差矩阵作为所述系统初始协方差矩阵中的姿态协方差子矩阵。

30、进一步的，所述步骤s2包括：

31、获取所述红外地平仪的当前三轴输出电压，根据所述当前三轴输出电压确定所述红外地平仪坐标系下的当前滚转角和当前俯仰角；

32、根据所述安装角方向余弦矩阵，将所述当前滚转角和所述当前俯仰角转换为所述机体坐标系下的机体滚转角和机体俯仰角；

33、以所述机体滚转角和所述机体俯仰角建立所述观测模型：

34、

35、其中，表示时刻k的观测向量，表示时刻k的所述机体滚转角，表示时刻k的所述机体俯仰角，表示观测噪声。

36、进一步的，所述系统初始化包括：

37、将所述系统初始状态向量设置为所述容积卡尔曼滤波器的初始状态估计值；

38、将所述系统初始协方差矩阵设置为所述容积卡尔曼滤波器的初始状态协方差矩阵；

39、根据所述初始状态估计值和所述初始状态协方差矩阵生成初始容积点。

40、进一步的，所述时间更新包括：

41、对前一时刻的状态协方差矩阵进行cholesky分解，根据分解结果和前一时刻的状态估计值生成多个状态容积点；

42、将各所述状态容积点输入所述状态传播模型，得到多个状态预测容积点；

43、根据多个所述状态预测容积点确定状态预测均值；

44、根据多个所述状态预测容积点、所述状态预测均值和过程噪声协方差矩阵确定状态预测协方差矩阵。

45、进一步的，所述测量更新包括：

46、根据所述状态预测均值和所述状态预测协方差矩阵生成多个测量更新容积点；

47、将各所述测量更新容积点输入所述观测模型，得到多个测量预测容积点；

48、根据多个所述测量预测容积点确定测量预测均值、测量误差协方差矩阵以及状态与测量的互协方差矩阵；

49、根据所述测量误差协方差矩阵和所述状态与测量的互协方差矩阵确定卡尔曼增益；

50、根据所述卡尔曼增益、所述观测向量和所述测量预测均值更新所述系统状态向量，并更新对应的状态协方差矩阵。

51、进一步的，根据所述更新后的广义罗德里格斯参数确定所述飞行器姿态对准结果，包括：

52、将所述更新后的广义罗德里格斯参数转换为更新后的姿态四元数；

53、根据所述更新后的姿态四元数确定所述机体坐标系相对于导航坐标系的方向余弦矩阵；

54、从所述方向余弦矩阵中提取滚转角、俯仰角和偏航角，并将所述滚转角、所述俯仰角和所述偏航角确定为所述飞行器姿态对准结果。

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

56、1. 本发明通过红外地平仪的三轴输出电压确定滚转角和俯仰角，并经安装角转换、方向余弦矩阵、姿态四元数及广义罗德里格斯参数的逐级转换确定系统初始状态向量，减少对gnss信号及环境视觉特征的依赖。

57、2. 本发明通过广义罗德里格斯参数和陀螺仪零偏构建系统状态向量，并根据陀螺仪测量的角速度建立状态传播模型，使所述状态传播模型适用于飞行器大失准角对应的非线性姿态变化。

58、3. 本发明通过欧拉角协方差矩阵和雅可比矩阵确定系统初始协方差矩阵，并利用容积卡尔曼滤波器对状态传播模型和观测模型进行时间更新及测量更新，联合确定广义罗德里格斯参数和陀螺仪零偏。