CN112284412B - 一种避免欧拉转换奇异导致精度下降的地面静态对准方法 - Google Patents

Abstract

本发明公开了一种避免欧拉转换奇异导致精度下降的地面静态对准方法，包括如下步骤：将惯组陀螺角速率和加表比力转换到准北东地坐标系；基于陀螺计算的角增量，利用四元数乘法修正准北东地坐标系；基于加表计算的加速度，利用最小二乘估计失准角；基于失准角和粗对准姿态角，利用四元数乘法计算精对准角度。本发明基于粗对准姿态角建立的准北东地坐标系进行精对准，并基于四元数乘法计算精对准四元数，生成高精度对准角度，避免了欧拉转换奇异导致精度下降的问题。该地面静态对准算法简单，易于工程应用。

Description

一种避免欧拉转换奇异导致精度下降的地面静态对准方法

技术领域

本发明涉及惯导初始对准技术领域，具体是一种避免欧拉转换奇异导致精度下降的地面静态对准方法，用于惯导地面对准。

背景技术

运载、导弹等发射前都需要进行初始对准，一般基于发射点经度、纬度和高度进行水平和方位对准。

现有地面对准一般直接基于本体建立导航坐标系，并在此坐标系基于陀螺和加表测得的角速率和比力进行精对准。基于陀螺计算的角增量常规修正方式如下：

计算角增量：

式中，

为本体系相对于地理系的角速度增量在本体系投影；Δθ_x为本体系相对于地理系角速度增量在本体x轴投影；Δθ_y为本体系相对于地理系角速度增量在本体y轴投影；Δθ_z为本体系相对于地理系角速度增量在本体z轴投影；

为陀螺测量的惯性角速度；

为地理系相对于惯性系的角速度；A_bn,k-1为k-1时刻地理系到本体系的转换矩阵；T为计算周期。

式中，Δθ为矢量[Δθ_x Δθ_y Δθ_z]的模长。

构造矩阵：

更新四元数及转换矩阵：

式中，q_bn,k-1为k-1时刻地理系到本体系的旋转四元数；q_bn,k为k时刻地理系到本体系的旋转四元数。

则k时刻地理系到本体系的转换矩阵为：

直接基于本体坐标系进行精对准，基于陀螺角速度计算的角增量与对准误差角并非一一对应，且为非线性关系，特别是粗对准坐标系与北东地坐标系之间存大角度转换时。

按3-2-1转序定义对准姿态时，当俯仰角在90°附近时，常规地面对准方法精度将大幅下降。

发明内容

本发明的技术解决问题是：针对现有技术中存在的上述不足，提供一种避免欧拉转换奇异导致精度下降的地面静态对准方法，该方法基于粗对准姿态角建立的准北东地坐标系进行精对准，并基于四元数乘法计算精对准四元数，生成高精度对准角度，避免了欧拉转换奇异导致精度下降的问题，该地面静态对准算法简单，易于工程应用。

本发明是通过以下技术方案实现的。

一种避免欧拉转换奇异导致精度下降的地面静态对准方法，包括如下步骤：

步骤1，根据惯组陀螺测量的角速度信息和加表测量的比力信息采用粗对准的方法得到粗对准姿态角，将惯组陀螺角速度和加表比力转换到准北东地坐标系；

步骤2，根据惯组陀螺计算角增量，利用四元数乘法修正准北东地坐标系；

步骤3，根据加表计算加速度，利用最小二乘估计失准角；

步骤4，根据步骤3得到的失准角和步骤1得到的粗对准姿态角，利用四元数乘法计算精对准四元数，再基于精对准四元数按3-2-1转序求精对准角度。

所述步骤1具体包括如下步骤：

步骤1.1，基于粗对准姿态角建立准北东地坐标系：

计算重力加速度g

式中，R_e为地球赤道半径；L为当地纬度；h为当地高度。

计算本体相对准北东地初始姿态角(3-2-1转序)

如果

则

γ＝0

式中，

为加表测量的比力；

为惯组陀螺测量的角速度；γ为横滚角、θ为俯仰角、ψ为偏航角。

否则即

时，

根据粗对准姿态角，建立准北东地坐标系(准NED系)，在准NED系建立n′导航坐标系。

计算3-2-1转序姿态四元数：

q_n′b＝q_bn′ ^*

式中，q_bn′为准北东地坐标系到本体系的转换四元数；q_n′b本体系到准北东地坐标系的转换四元数。

计算姿态转换阵：

A_n′b＝A_bn′ ^T

式中，A_bn′为准北东地坐标系到本体系的转换矩阵；A_n′b为本体系到准北东地坐标系的转换矩阵。

步骤1.2，将惯组陀螺角速度和加表比力转换到准北东地坐标系，方法为：

将惯组陀螺角速率和加表比力转换到准NED系：

f^n′＝A_n′b·f^b

ω^n′＝A_n′b·ω^b

式中，f^n′为准北东地系的比力；ω^n′为准北东地系的角速度。

预处理：

式中，

为第k时刻准北东地系比力的滤波值；

为第k-1时刻准北东地系比力的滤波值；

第k时刻准北东地系角速度的滤波值；

第k-1时刻准北东地系角速度的滤波值。

所述步骤2具体包括如下步骤：

步骤2.1，在准北东地坐标系基于陀螺计算角增量：

计算地球自转角速率在导航系分量

式中，ω_ei为地球自转角速度；

为地球自转角速率在北东地坐标系的分量；

为北东地坐标系的角速度。

计算角度增量：

q_n′n,0＝[1 0 0 0]^T

式中，q_n′n,0为初始时刻北东地坐标系到准北东地坐标系的旋转四元数；A_n′n,k-1为第k-1时刻北东地坐标系到准北东地坐标系的转换矩阵；T为计算周期。

步骤2.2，利用四元数乘法修正准北东地坐标系：

式中，dq_ω为北东地坐标系到准北东地坐标系的误差四元数；q_n′n,k-1为第k-1时刻北东地坐标系到准北东地坐标系的旋转四元数；q_n′n,k为第k时刻北东地坐标系到准北东地坐标系的旋转四元数。

所述步骤3具体包括如下步骤：

步骤3.1，在修正后的准北东地坐标系基于加表计算加速度：

由q_n′n,k＝[q₀ q₁ q₂ q₃]^T求A_n′n,k

式中，A_n′n,k为第k时刻北东地坐标系到准北东地坐标系的转换矩阵。

加速度计算：

式中，

为第k时刻北东地坐标系速度的变化率。

步骤3.2，利用最小二乘估计失准角：

计算加速度

式中，a_k为第k时刻北东地坐标系总的加速度。a_N为第k时刻北向加速度；a_E第k时刻东向加速度；a_D第k时刻地向加速度。

第一步初始化：

式中，(a_N)_k-1为第k-1时刻北向加速度；(a_E)_k-1为第k-1时刻东向加速度；(b_N)_k-1为第k-1时刻北向加速度滤波系数；(b_E)_k-1为第k-1时刻东向加速度滤波系数。

第二步开始迭代计算

计算失准角

所述步骤4具体包括如下步骤：

步骤4.1，基于失准角和粗对准姿态角，利用四元数乘法计算精对准四元数：

由

求得：

式中，dq_φ为准北东地坐标系到北东地坐标系的误差四元数；q_bn,k为第k时刻北东地坐标系到本体系转换四元数。

步骤4.2，基于精对准四元数，按3-2-1转序求精对准角度：

由q_bn,k求北东地坐标系到本体系转换矩阵A_bn；

再由A_bn按3-2-1转序求取三轴姿态角。

姿态按3-2-1转序表示的姿态转换矩阵如下：

将A_bn表示为矩阵形式：

姿态四元数按3-2-1转序求三轴姿态角，如果|a₁₃|≤0.99999，则：

sinθ＝-a₁₃，θ＝asin(-a₁₃)

否则，即|a₁₃|＞0.99999，则

γ＝0

θ＝asin(-a₁₃)

所述的地面静态对准方法基于粗对准建立的准北东地坐标系计算角增量和估计失准角，避免欧拉转换奇异导致精度下降：

对于3-2-1转序姿态运动学方程如下：

当俯仰对准角接近90°时，姿态运动学方程受除零影响精度下降。基于粗对准求得的粗初始对准角建立准北东地坐标系，相对准北东地坐标系的角增量和失准角都为小角度，不会受除零影响。

所述基于四元数乘法修正准北东地坐标系和计算精对准四元数，再基于精对准四元数，按3-2-1转序求精对准角度。

本发明提供的避免欧拉转换奇异导致精度下降的地面静态对准方法，与现有技术相比，具有以下优点和有益效果：

(1)本发明提供的避免欧拉转换奇异导致精度下降的地面静态对准方法，提供了一种避免欧拉转换奇异导致精度下降的地面静态对准方法，该方法基于粗对准姿态角建立的准北东地坐标系进行精对准，并基于四元数乘法计算精对准四元数，生成高精度对准角度，避免了欧拉转换奇异导致精度下降的问题。该地面静态对准算法简单，易于工程应用。

(2)基于粗对准姿态角建立的准北东地坐标系进行精对准，避免欧拉转换奇异导致精度下降；在准北东地坐标系基于陀螺计算的角增量与对准误差角之间基本为一一对应关系；

(3)基于四元数乘法计算精对准四元数，生成高精度对准角度，地面静态对准算法简单，易于工程应用。

(4)本发明公开了一种避免欧拉转换奇异导致精度下降的地面静态对准方法，基于粗对准姿态角建立的准北东地坐标系进行精对准，并基于四元数乘法计算精对准四元数，生成高精度对准角度，避免了欧拉转换奇异导致精度下降的问题。该地面静态对准算法简单，易于工程应用。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1是本发明北东地坐标系(NED)示意图；

图2是本发明地面静态对准计算过程。

具体实施方式

下面对本发明的实施例作详细说明：本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

如图1和图2所示，一种避免欧拉转换奇异导致精度下降的地面静态对准方法，包括如下步骤：

一种避免欧拉转换奇异导致精度下降的地面静态对准方法，包括如下步骤：

步骤1，将惯组陀螺角速度和加表比力转换到准北东地坐标系：

(1)基于粗对准姿态角建立准北东地坐标系；

(2)将惯组陀螺角速度和加表比力转换到准北东地坐标系；

步骤2，基于陀螺计算的角增量，利用四元数乘法修正准北东地坐标系：

(1)在准北东地坐标系基于陀螺计算角增量；

(2)利用四元数乘法修正准北东地坐标系；

步骤3，基于加表计算的加速度，利用最小二乘估计失准角：

(1)在修正后的准北东地坐标系基于加表计算加速度；

(2)利用最小二乘估计失准角；

步骤4，基于失准角和粗对准姿态角，利用四元数乘法计算精对准角度：

(1)基于失准角和粗对准姿态角，利用四元数乘法计算精对准四元数；

(2)基于精对准四元数，按3-2-1转序求精对准角度。

所述步骤1具体包括如下步骤：

步骤1.1，基于粗对准姿态角建立准北东地坐标系：

计算重力加速度

式中，R_e为地球赤道半径；L为当地纬度；h为当地高度。

计算本体相对北东地初始姿态角(3-2-1转序)

如果

γ＝0

式中，

为加表测量的比力；

为惯组陀螺测量的角速度；γ为横滚角、θ为俯仰角、ψ为偏航角。

否则

根据粗对准姿态角，建立准北东地坐标系(准NED系)，在准NED系建立n′导航坐标系。

计算3-2-1转序姿态四元数：

q_n′b＝q_bn′ ^*

式中，q_bn′为准北东地坐标系到本体系的转换四元数；q_n′b本体系到准北东地坐标系的转换四元数。

计算姿态转换阵：

A_n′b＝A_bn′ ^T

式中，A_bn′为准北东地坐标系到本体系的转换矩阵；A_n′b为本体系到准北东地坐标系的转换矩阵。

步骤1.2，将惯组陀螺角速率和加表比力转换到准北东地坐标系：

本体系比力和角速率转换到准NED系：

f^n′＝A_n′b·f^b

ω^n′＝A_n′b·ω^b

式中，f^n′为准北东地系的比力；ω^n′为准北东地系的角速度。

预处理：

式中，

为第k时刻准北东地系比力的滤波值；

为第k-1时刻准北东地系比力的滤波值；

第k时刻准北东地系角速度的滤波值；

第k-1时刻准北东地系角速度的滤波值。

所述步骤2具体包括如下步骤：

步骤2.1，在准北东地坐标系基于陀螺计算角增量：

计算地球自转角速率在导航系分量

式中，ω_ei为地球自转角速度；

为地球自转角速率在北东地坐标系的分量；

为北东地坐标系的角速度。

计算角度增量：

q_n′n,0＝[1 0 0 0]^T

式中，q_n′n,0为初始时刻北东地坐标系到准北东地坐标系的旋转四元数；A_n′n,k-1为第k-1时刻北东地坐标系到准北东地坐标系的转换矩阵；T为计算周期。

步骤2.2，利用四元数乘法修正准北东地坐标系：

式中，dq_ω为北东地坐标系到准北东地坐标系的误差四元数；q_n′n,k-1为第k-1时刻北东地坐标系到准北东地坐标系的旋转四元数；q_n′n,k为第k时刻北东地坐标系到准北东地坐标系的旋转四元数。

所述步骤3具体包括如下步骤：

步骤3.1，在修正后的准北东地坐标系基于加表计算加速度：

由q_n′n,k＝[q₀ q₁ q₂ q₃]^T求A_n′n,k

式中，A_n′n,k为第k时刻北东地坐标系到准北东地坐标系的转换矩阵。

步骤3.2，利用最小二乘估计失准角：

计算加速度

式中，a_k为第k时刻北东地坐标系总的加速度。a_N为第k时刻北向加速度；a_E第k时刻东向加速度；a_D第k时刻地向加速度。

第一步初始化：

式中，(a_N)_k-1为第k-1时刻北向加速度；(a_E)_k-1为第k-1时刻东向加速度；(b_N)_k-1为第k-1时刻北向加速度滤波系数；(b_E)_k-1为第k-1时刻东向加速度滤波系数。

第二步开始迭代计算

计算失准角

所述步骤4具体包括如下步骤：

步骤4.1，基于失准角和粗对准姿态角，利用四元数乘法计算精对准四元数：

由

求得：

式中，dq_φ为准北东地坐标系到北东地坐标系的误差四元数；q_bn,k为第k时刻北东地坐标系到本体系转换四元数。

步骤4.2，基于精对准四元数，按3-2-1转序求精对准角度：

由q_bn,k求北东地坐标系到本体系转换矩阵A_bn；

再由A_bn按3-2-1转序求取三轴姿态角。

姿态按3-2-1转序表示的姿态转换矩阵如下：

将A_bn表示为矩阵形式：

姿态四元数按3-2-1转序求三轴姿态角，如果|a₁₃|≤0.99999，则：

sinθ＝-a₁₃，θ＝asin(-a₁₃)

否则，即|a₁₃|＞0.99999，则

γ＝0

θ＝asin(-a₁₃)

具体为：

a.将惯组陀螺角速率和加表比力转换到准北东地坐标系

基于粗对准姿态角建立准北东地坐标系：

计算重力加速度g

式中，R_e为地球赤道半径；L为当地纬度；h为当地高度。

计算本体相对准北东地初始姿态角(3-2-1转序)

如果

则

γ＝0

式中，

为加表测量的比力；

为惯组陀螺测量的角速度；γ为横滚角、θ为俯仰角、ψ为偏航角。

否则即

时，

根据粗对准姿态角，建立准北东地坐标系(准NED系)，在准NED系建立n′导航坐标系。

计算3-2-1转序姿态四元数：

q_n′b＝q_bn′ ^*

式中，q_bn′为准北东地坐标系到本体系的转换四元数；q_n′b本体系到准北东地坐标系的转换四元数。

计算姿态转换阵：

A_n′b＝A_bn′ ^T

式中，A_bn′为准北东地坐标系到本体系的转换矩阵；A_n′b为本体系到准北东地坐标系的转换矩阵。

本体系比力和角速率转换到准NED系：

f^n′＝A_n′b·f^b

ω^n′＝A_n′b·ω^b

式中，f^n′为准北东地系的比力；ω^n′为准北东地系的角速度。

预处理：

式中，

为第k时刻准北东地系比力的滤波值；

为第k-1时刻准北东地系比力的滤波值；

第k时刻准北东地系角速度的滤波值；

第k-1时刻准北东地系角速度的滤波值。

b.基于陀螺计算的角增量，利用四元数乘法修正准北东地坐标系

在准北东地坐标系基于陀螺计算角增量：

计算地球自转角速率在导航系分量

式中，ω_ei为地球自转角速度；

为地球自转角速率在北东地坐标系的分量；

为北东地坐标系的角速度。

计算角度增量：

q_n′n,0＝[1 0 0 0]^T

式中，q_n′n,0为初始时刻北东地坐标系到准北东地坐标系的旋转四元数；A_n′n,k-1为第k-1时刻北东地坐标系到准北东地坐标系的转换矩阵；T为计算周期。

式中，dq_ω为北东地坐标系到准北东地坐标系的误差四元数；q_n′n,k-1为第k-1时刻北东地坐标系到准北东地坐标系的旋转四元数；q_n′n,k为第k时刻北东地坐标系到准北东地坐标系的旋转四元数。

c.基于加表计算的加速度，利用最小二乘估计失准角

在修正后的准北东地坐标系基于加表计算加速度：

由q_n′n,k＝[q₀ q₁ q₂ q₃]^T求A_n′n,k

式中，A_n′n,k为第k时刻北东地坐标系到准北东地坐标系的转换矩阵。

计算加速度

式中，a_k为第k时刻北东地坐标系总的加速度。a_N为第k时刻北向加速度；a_E第k时刻东向加速度；a_D第k时刻地向加速度。

第一步初始化：

式中，(a_N)_k-1为第k-1时刻北向加速度；(a_E)_k-1为第k-1时刻东向加速度；(b_N)_k-1为第k-1时刻北向加速度滤波系数；(b_E)_k-1为第k-1时刻东向加速度滤波系数。

第二步开始迭代计算

计算失准角

d.基于失准角和粗对准姿态角，利用四元数乘法计算精对准角度

基于失准角和粗对准姿态角，利用四元数乘法计算精对准四元数：

步骤4.1，基于失准角和粗对准姿态角，利用四元数乘法计算精对准四元数：

由

求得：

式中，dq_φ为准北东地坐标系到北东地坐标系的误差四元数；q_bn,k为第k时刻北东地坐标系到本体系转换四元数。

由q_bn,k求北东地坐标系到本体系转换矩阵A_bn；

再由A_bn按3-2-1转序求取三轴姿态角。

姿态按3-2-1转序表示的姿态转换矩阵如下：

将A_bn表示为矩阵形式：

姿态四元数按3-2-1转序求三轴姿态角，如果|a₁₃|≤0.99999，则：

sinθ＝-a₁₃，θ＝asin(-a₁₃)

否则，即|a₁₃|＞0.99999，则

γ＝0

θ＝asin(-a₁₃)

本实施例提供的避免欧拉转换奇异导致精度下降的地面静态对准方法，提供了一种避免欧拉转换奇异导致精度下降的地面静态对准方法，该方法将惯组陀螺角速率和加表比力转换到基于粗对准建立的准北东地坐标系，基于陀螺计算的角增量，利用四元数乘法修正准北东地坐标系，基于加表计算的加速度，利用最小二乘估计失准角，基于失准角和粗对准姿态角，利用四元数乘法计算精对准角度；基于粗对准姿态角建立的准北东地坐标系进行精对准，避免欧拉转换奇异导致精度下降。

假设飞行器本体系相对于北东地坐标系的姿态角为[0；89；20]°，惯组陀螺的测量精度为0.36°/h，惯组加表的测量精度为0.0001m/s²。采用粗对准得到姿态角为[0；90；19.86]°，存在1°的测量误差；根据粗对准姿态角建立准北东地坐标系n′，进而得到本体系到准北东地坐标系的转换矩阵A_n′b：

将惯组陀螺角速率和加表比力转换到准北东地坐标系下，利用四元数乘法修正准北东地坐标系，利用最小二乘法计算失准角计算精对准姿态角为[0.03；89；20.14]°，对准精度为0.14°。

本实施例基于粗对准姿态角建立的准北东地坐标系进行精对准，并基于四元数乘法计算精对准四元数，生成高精度对准角度，避免了欧拉转换奇异导致精度下降的问题。该地面静态对准算法简单，易于工程应用。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变形或修改，这并不影响本发明的实质内容。

Claims (2)

1.一种避免欧拉转换奇异导致精度下降的地面静态对准方法，其特征在于包括如下步骤：

步骤1，根据惯组陀螺测量的角速度信息和加表测量的比力信息采用粗对准的方法得到粗对准姿态角，将惯组陀螺角速度和加表比力转换到准北东地坐标系；

步骤2，根据惯组陀螺计算角增量，利用四元数乘法修正准北东地坐标系；

步骤3，根据加表计算加速度，利用最小二乘估计失准角；

步骤4，根据步骤3得到的失准角和步骤1得到的粗对准姿态角，利用四元数乘法计算精对准四元数，再基于精对准四元数按3-2-1转序求精对准角度；

所述步骤1中，准北东地坐标系的建立方法为：

计算重力加速度g

式中，R_e为地球赤道半径；L为当地纬度；h为当地高度；

计算本体相对准北东地初始姿态角，3-2-1转序：

如果

则

γ＝0

式中，

为加表测量的比力；

为惯组陀螺测量的角速度；γ为横滚角、θ为俯仰角、ψ为偏航角；

否则即

时，

根据粗对准姿态角，建立准北东地坐标系，准NED系，在准NED系建立n′导航坐标系；

计算3-2-1转序姿态四元数：

q_n′b＝q_bn′ ^*

式中，q_bn′为准北东地坐标系到本体系的转换四元数；q_n′b本体系到准北东地坐标系的转换四元数；

计算姿态转换阵：

A_n′b＝A_bn′ ^T

式中，A_bn′为准北东地坐标系到本体系的转换矩阵；A_n′b为本体系到准北东地坐标系的转换矩阵；

所述步骤1中，将惯组陀螺角速度和加表比力转换到准北东地坐标系，方法为：

将惯组陀螺角速率和加表比力转换到准NED系：

f^n′＝A_n′b·f^b

ω^n′＝A_n′b·ω^b

式中，f^n′为准北东地系的比力；ω^n′为准北东地系的角速度；

预处理：

式中，

为第k时刻准北东地系比力的滤波值；

为第k-1时刻准北东地系比力的滤波值；

第k时刻准北东地系角速度的滤波值；

第k-1时刻准北东地系角速度的滤波值；

所述步骤2中，根据惯组陀螺计算角增量的方法为：

计算地球自转角速率在导航系分量

式中，ω_ei为地球自转角速度；

为地球自转角速率在北东地坐标系的分量；

为北东地坐标系的角速度；

计算角度增量：

q_n′n,0＝[1 0 0 0]^T

式中，q_n′n,0为初始时刻北东地坐标系到准北东地坐标系的旋转四元数；A_n′n,k-1为第k-1时刻北东地坐标系到准北东地坐标系的转换矩阵；T为计算周期；

所述步骤2中，利用四元数乘法修正准北东地坐标系的方法为：

式中，dq_ω为北东地坐标系到准北东地坐标系的误差四元数；q_n′n,k-1为第k-1时刻北东地坐标系到准北东地坐标系的旋转四元数；q_n′n,k为第k时刻北东地坐标系到准北东地坐标系的旋转四元数；

所述步骤3中，根据加表计算加速度的方法为：

由q_n′n,k＝[q₀ q₁ q₂ q₃]^T求A_n′n,k

式中，A_n′n,k为第k时刻北东地坐标系到准北东地坐标系的转换矩阵；

加速度计算：

式中，

为第k时刻北东地坐标系速度的变化率；

所述步骤3中，利用最小二乘估计失准角的方法为：

计算加速度

式中，a_k为第k时刻北东地坐标系总的加速度；a_N为第k时刻北向加速度；a_E第k时刻东向加速度；a_D第k时刻地向加速度；

第一步初始化：

式中，(a_N)_k-1为第k-1时刻北向加速度；(a_E)_k-1为第k-1时刻东向加速度；(b_N)_k-1为第k-1时刻北向加速度滤波系数；(b_E)_k-1为第k-1时刻东向加速度滤波系数；

第二步开始迭代计算

计算失准角

所述步骤4中，基于失准角和粗对准姿态角，利用四元数乘法计算精对准四元数的方法为：

由

求得：

式中，dq_φ为准北东地坐标系到北东地坐标系的误差四元数；q_bn,k为第k时刻北东地坐标系到本体系转换四元数。

2.根据权利要求1所述的一种避免欧拉转换奇异导致精度下降的地面静态对准方法，其特征在于：

所述步骤4中，基于精对准四元数，按3-2-1转序求精对准角度的方法为：

由q_bn,k求北东地坐标系到本体系转换矩阵A_bn；

再由A_bn按3-2-1转序求取三轴姿态角；

姿态按3-2-1转序表示的姿态转换矩阵如下：

将A_bn表示为矩阵形式：

姿态四元数按3-2-1转序求三轴姿态角，如果|a₁₃|≤0.99999，则：

sinθ＝-a₁₃，θ＝asin(-a₁₃)

否则，即|a₁₃|＞0.99999，则

γ＝0

θ＝a sin(-a₁₃)

所述的地面静态对准方法基于粗对准建立的准北东地坐标系计算角增量和估计失准角，避免欧拉转换奇异导致精度下降：

对于3-2-1转序姿态运动学方程如下：

Images