CN109682397B - 一种不受历史数据影响快速收敛的地面静态对准方法 - Google Patents

Abstract

本发明公开了一种不受历史数据影响快速收敛的地面静态对准方法，包括如下步骤：步骤1，将惯组陀螺角速率和加表比力转换到准北东地坐标系及预处理；步骤2，基于准北东地坐标系北向和东向比力估计水平失准角及分段处理；步骤3，基于准北东地坐标系东向角速率估计方位失准角及分段处理；步骤4，基于水平和方位失准角，利用四元数乘法计算精对准角度。本发明基于动态修正的准北东地坐标系进行精对准，惯组姿态变化后不受历史数据影响，能够快速收敛到新对准姿态。该地面静态对准算法简单，收敛速度快，易于工程应用。

Description

一种不受历史数据影响快速收敛的地面静态对准方法

技术领域

本发明涉及惯导初始对准技术领域，具体是一种不受历史数据影响快速收敛的地面静态对准方法，用于惯导地面对准。

背景技术

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

现有地面对准一般直接基于本体建立导航坐标系，并在此坐标系基于加表测得的比力和最小二乘估计失准角，基于最小二乘估计失准角的常规算法如下：

加速度计算：

其中，g为重力加速度；f^b为加速度计测量值，A_bn为地理系到本体系的转换矩阵。

其中，a_n为地理系加速度，a_N为北向加速度，a_E为东向加速度，a_D为地向加速度。

第一步初始化

第二步开始迭代计算

计算失准角

其中，φ为失准角，φ_N为北向失准角，φ_E为东向失准角，φ_D为方位失准角。

(a_N)_n-1＝(a_N)_n

(a_E)_n-1＝(a_E)_n

(b_N)_n-1＝(b_N)_n

(b_E)_n-1＝(b_E)_n

直接基于本体坐标系和最小二乘进行精对准，受历史数据影响收敛速度较慢。对准姿态变化后受历史数据影响，很难收敛到新对准姿态。

发明内容

本发明针对现有技术中存在的上述不足，提供了一种不受历史数据影响快速收敛的地面静态对准方法，该方法基于动态修正的准北东地坐标系进行精对准，惯组姿态变化后不受历史数据影响，能够快速收敛到新对准姿态。该地面静态对准算法简单，收敛速度快，易于工程应用。

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

一种不受历史数据影响快速收敛的地面静态对准方法，包括如下步骤：

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

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

在准北东地坐标系对角速率和比力滤波；

基于准北东地坐标系地向比力，判别是否重赋初始对准角；

步骤2，基于准北东地坐标系北向和东向比力估计水平失准角及分段处理：

基于准北东地坐标系北向和东向比力估计水平失准角；

对水平失准角进行限幅处理和平滑处理；

步骤3，基于准北东地坐标系东向角速率估计方位失准角及分段处理：

基于准北东地坐标系东向角速率估计方位失准角；

对方位失准角进行限幅处理和平滑处理；

步骤4，基于水平失准角和方位失准角，利用四元数乘法计算精对准角度：

基于水平失准角和方位失准角，利用四元数乘法计算精对准四元数；

基于精对准四元数，按3-2-1转序计算精对准角度。

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

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

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

A_n′b,0＝I_3×3

f^n′＝A_n′b,k-1·f^b

ω^n′＝A_n′b,k-1·ω^b

步骤1.2，在准北东地坐标系对角速率和比力滤波：

k_lv＝0.02

步骤1.3，基于准北东地坐标系地向比力，判别是否重赋初始对准角：

如果

dθ＝0°、dψ＝0°

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

步骤2.1，基于准北东地坐标系北向和东向比力估计水平失准角：

k_a＝0.01°/(m/s²)

步骤2.2，对水平失准角进行限幅处理和平滑处理：

水平失准角进行10°限幅；水平失准角小于0.1°，比例作用减弱50％。

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

步骤3.1，基于准北东地坐标系东向角速率估计方位失准角：

k_ω＝1600°/(°/s)

步骤3.2，对方位失准角进行限幅处理和平滑处理：

方位失准角进行10°限幅；方位失准角小于0.1°，比例作用减弱50％。

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

步骤4.1，基于水平失准角和方位失准角，利用四元数乘法计算精对准四元数：

失准角转换成弧度，求误差四元数：

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

由q_bn,k＝q_bn′,k＝[q₀ q₁ q₂ q₃]^T

求A_bn,k：

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

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

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

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

否则：

θ＝asin(-a₁₃)

本发明提供的不受历史数据影响快速收敛的地面静态对准方法，提供了一种不受历史数据影响快速收敛的地面静态对准方法，该方法基于动态修正的准北东地坐标系进行精对准，惯组姿态变化后不受历史数据影响，能够快速收敛到新对准姿态。该地面静态对准算法简单，收敛速度快，易于工程应用。

本发明提供的不受历史数据影响快速收敛的地面静态对准方法，与现有技术相比，具有以下优点和有益效果：

1、基于动态修正的准北东地坐标系进行精对准，能够快速收敛；

2、惯组姿态变化后不受历史数据影响，能够快速收敛到新对准姿态；

3、基于四元数乘法计算精对准四元数，生成高精度对准角度；

4、地面静态对准算法简单，收敛速度快，易于工程应用。

附图说明

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

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

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

具体实施方式

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

请同时参阅图1至图2。

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

一种不受历史数据影响快速收敛的地面静态对准方法，包括如下步骤：

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

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

在准北东地坐标系对角速率和比力滤波；

基于准北东地坐标系地向比力，判别是否重赋初始对准角；

步骤2，基于准北东地坐标系北向和东向比力估计水平失准角及分段处理：

基于准北东地坐标系北向和东向比力估计水平失准角；

对水平失准角进行限幅处理和平滑处理；

步骤3，基于准北东地坐标系东向角速率估计方位失准角及分段处理：

基于准北东地坐标系东向角速率估计方位失准角；

对方位失准角进行限幅处理和平滑处理；

步骤4，基于水平失准角和方位失准角，利用四元数乘法计算精对准角度：

基于水平失准角和方位失准角，利用四元数乘法计算精对准四元数；

基于精对准四元数，按3-2-1转序计算精对准角度。

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

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

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

A_n′b,0＝I_3×3

f^n′＝A_n′b,k-1·f^b

ω^n′＝A_n′b,k-1·ω^b

其中，q_n′b,0为地理系相对于本体系姿态四元数的初始值；A_n′b,0为本体系到地理系的转换矩阵的初始值；A_n′b,k-1为k-1时刻本体系到地理系的转换矩阵；f^b为加速度计的测量值；ω^b为陀螺的测量值；

为地理系加速度；

为地理系角速度。

步骤1.2，在准北东地坐标系对角速率和比力滤波：

k_lv＝0.02

其中，

为k时刻北向加速度滤波值；

为k时刻东向加速度滤波值；

为k-1时刻北向加速度滤波值；

为k-1时刻东向加速度滤波值；

为k时刻东向角速度滤波值。

步骤1.3，基于准北东地坐标系地向比力，判别是否重赋初始对准角：

如果

dθ＝0°、dψ＝0°

其中，

为北向失准角，dθ为东向失准角，dψ为方位失准角。

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

步骤2.1，基于准北东地坐标系北向和东向比力估计水平失准角：

k_a＝0.01°/(m/s²)

步骤2.2，对水平失准角进行限幅处理和平滑处理：

水平失准角进行10°限幅；水平失准角小于0.1°，比例作用减弱50％。

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

步骤3.1，基于准北东地坐标系东向角速率估计方位失准角：

k_ω＝1600°/(°/s)

步骤3.2，对方位失准角进行限幅处理和平滑处理：

方位失准角进行10°限幅；方位失准角小于0.1°，比例作用减弱50％。

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

步骤4.1，基于水平失准角和方位失准角，利用四元数乘法计算精对准四元数：

失准角转换成弧度，求误差四元数：

其中，q_n′b,k为k时刻地理系相对于本体系的姿态四元数；q_n′b,k-1为k-1地理系相对于本体系的姿态四元数；q_bn′,k为k时刻本体系相对于地理系的姿态四元数；q_bn′,k-1为k-1时刻本体系相对于地理系的姿态四元数。

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

由q_bn,k＝q_bn′,k＝[q₀ q₁ q₂ q₃]^T

求A_bn,k：

其中，A_bn,k为地理系到本体系的转换矩阵。

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

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

其中，

为横滚角，θ为俯仰角，ψ为方位角。

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

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

否则：

θ＝asin(-a₁₃)

具体为：

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

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

A_n′b,0＝I_3×3

f^n′＝A_n′b,k-1·f^b

ω^n′＝A_n′b,k-1·ω^b

在准北东地坐标系对角速率和比力滤波：

k_lv＝0.02

基于准北东地坐标系地向比力，判别是否重赋初始对准角：

如果

dθ＝0°、dψ＝0°

b.基于准北东地坐标系北向和东向比力估计水平失准角及分段处理

基于准北东地坐标系北向和东向比力估计水平失准角：

k_a＝0.01°/(m/s²)

对水平失准角进行限幅处理和平滑处理：

水平失准角进行10°限幅；水平失准角小于0.1°，比例作用减弱50％。

c.基于准北东地坐标系东向角速率估计方位失准角及分段处理

基于准北东地坐标系东向角速率估计方位失准角：

k_ω＝1600°/(°/s)

对方位失准角进行限幅处理和小平滑处理：

方位失准角进行10°限幅；方位失准角小于0.1°，比例作用减弱50％。

d.基于水平失准角和方位失准角，利用四元数乘法计算精对准角度

基于水平失准角和方位失准角，利用四元数乘法计算精对准四元数：

失准角转换成弧度，求误差四元数：

基于精对准四元数，按3-2-1转序计算精对准角度：

由q_bn,k＝q_bn′,k＝[q₀ q₁ q₂ q₃]^T

求A_bn,k：

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

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

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

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

否则：

θ＝asin(-a₁₃)

本发明公开了一种不受历史数据影响快速收敛的地面静态对准方法，包括如下步骤：步骤1，将惯组陀螺角速率和加表比力转换到准北东地坐标系及预处理；步骤2，基于准北东地坐标系北向和东向比力估计水平失准角及分段处理；步骤3，基于准北东地坐标系东向角速率估计方位失准角及分段处理；步骤4，基于水平失准角和方位失准角，利用四元数乘法计算精对准角度。本发明基于动态修正的准北东地坐标系进行精对准，惯组姿态变化后不受历史数据影响，能够快速收敛到新对准姿态。该地面静态对准算法简单，收敛速度快，易于工程应用。

本实施例提供的不受历史数据影响快速收敛的地面静态对准方法，提供了一种不受历史数据影响快速收敛的地面静态对准方法，该方法将惯组陀螺角速率和加表比力转换到准北东地坐标系及预处理；基于准北东地坐标系北向和东向比力估计水平失准角及分段处理；基于准北东地坐标系东向角速率估计方位失准角及分段处理；基于水平失准角和方位失准角，利用四元数乘法计算精对准角度。

本实施例基于动态修正的准北东地坐标系进行精对准，惯组姿态变化后不受历史数据影响，能够快速收敛到新对准姿态。该地面静态对准算法简单，收敛速度快，易于工程应用。

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

Claims (4)

1.一种不受历史数据影响快速收敛的地面静态对准方法，其特征在于，包括如下步骤：

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

预处理：在准北东地坐标系对角速率和比力滤波；

基于准北东地坐标系地向比力，判别是否重赋初始对准角；

步骤2，基于准北东地坐标系北向和东向比力估计水平失准角；

分段处理：对水平失准角进行限幅处理和平滑处理；

步骤3，基于准北东地坐标系东向角速率估计方位失准角；

分段处理：对方位失准角进行限幅处理和平滑处理；

步骤4，基于水平失准角和方位失准角，利用四元数乘法计算精对准四元数；

基于精对准四元数，按3-2-1转序计算精对准角度；

其中，所述步骤1包括如下步骤：

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

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

A_n′b,0＝I_3×3

f^n′＝A_n′b,k-1·f^b

ω^n′＝A_n′b,k-1·ω^b

其中，q_n′b,0为地理系相对于本体系姿态四元数的初始值；A_n′b,0为本体系到地理系的转换矩阵的初始值；A_n′b,k-1为k-1时刻本体系到地理系的转换矩阵；f^b为加速度计的测量值；ω^b为陀螺的测量值；

为地理系加速度；

为地理系角速度；

步骤1.2，在准北东地坐标系对角速率和比力滤波：

k_lv＝0.02

其中，

为k时刻北向加速度滤波值；

为k时刻东向加速度滤波值；

为k-1时刻北向加速度滤波值；

为k-1时刻东向加速度滤波值；

为k时刻东向角速度滤波值；

步骤1.3，基于准北东地坐标系地向比力，判别是否重赋初始对准角：

如果f_z ^n′＞g_n/2，则重赋初始对准角：

dθ＝0°、dψ＝0°

其中，

为北向失准角，dθ为东向失准角，dψ为方位失准角。

2.根据权利要求1所述不受历史数据影响快速收敛的地面静态对准方法，其特征在于，所述步骤2包括如下步骤：

步骤2.1，基于准北东地坐标系北向和东向比力估计水平失准角：

k_a＝0.01°/(m/s²)

步骤2.2，对水平失准角进行限幅处理和平滑处理：

将水平失准角限幅到[-10°，10°]，若水平失准角小于0.1°，将比例作用减弱50％。

3.根据权利要求2所述不受历史数据影响快速收敛的地面静态对准方法，其特征在于，所述步骤3包括如下步骤：

步骤3.1，基于准北东地坐标系东向角速率估计方位失准角：

k_ω＝1600°/(°/s)

步骤3.2，对方位失准角进行限幅处理和平滑处理：

方位失准角进行10°限幅；方位失准角小于0.1°，比例作用减弱50％。

4.根据权利要求3所述不受历史数据影响快速收敛的地面静态对准方法，其特征在于，所述步骤4包括如下步骤：

步骤4.1，基于水平失准角和方位失准角，利用四元数乘法计算精对准四元数：

失准角转换成弧度，求误差四元数：

其中，q_n′b,k为k时刻地理系相对于本体系的姿态四元数；q_n′b,k-1为k-1地理系相对于本体系的姿态四元数；q_bn′,k为k时刻本体系相对于地理系的姿态四元数；q_bn′,k-1为k-1时刻本体系相对于地理系的姿态四元数；

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

由q_bn,k＝q_bn′,k＝[q₀ q₁ q₂ q₃]^T

求A_bn,k：

其中，A_bn,k为地理系到本体系的转换矩阵；

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

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

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

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

否则：

θ＝asin(-a₁₃)

其中，

为横滚角，θ为俯仰角，ψ为方位角。

Images