CN111366148B - 适用于机载光电观瞄系统多次观察的目标定位方法 - Google Patents

Abstract

本发明属于机载光电侦察技术领域，具体涉及一种适用于机载光电观瞄系统多次观察的目标定位方法，该方法基于机载光电观瞄系统的预先校靶误差参数，机载光电观瞄系统位置和姿态测量单元的位置误差和姿态误差统计信息，机载光电观瞄系统的姿态误差统计信息，机载光电观瞄系统激光测距单元的测距误差统计信息，通过建立合适的卡尔曼滤波模型，可用于实时修正目标定位过程中上述各项参数误差对目标定位结果的影响，该方法可实现对静止目标的高精度地理定位。本发明对目标定位结果实时滤波，在现有光电系统的基础上不需要增加任何硬件资源，只需要增加适用于机载光电观瞄系统多次观察的目标定位软件包便可提升机载光电观瞄系统对静止目标的定位精度。

Description

适用于机载光电观瞄系统多次观察的目标定位方法

技术领域

本发明属于机载光电侦察技术领域，具体涉及一种适用于机载光电观瞄系统多次观察的目标定位方法，该方法基于机载光电观瞄系统预先校靶参数建立健全的卡尔曼滤波模型，通过对目标多次观察过程中各项输入参数的随机误差进行修正，可实时结算滤波后的目标地理位置信息。

背景技术

当前的信息化战争对目标侦察提出了更高要求，在对感兴趣目标进行自动跟踪的同时，还需提供目标高精度的位置信息，如何能够准确的求解目标信息成为实施精确打击的首要解决问题。

机载光电观瞄系统目标定位过程中存在如下误差源：1)现有机载光电观瞄系统大多配有位置和姿态测量单元，并采取位置和姿态测量单元与光电平台固连的方式进行联接，这种方式与传统光电观瞄系统(未配有位置和姿态测量单元)通过减震器与载机组合导航系统联接相比，可消除载机组合导航系统与光电观瞄系统数据通信间的非同步误差，并有效减小有减震器带来的随机安装误差角，但同时引入了位置和姿态测量单元与光电平台的固定安装误差角；2)机载光电观瞄系统多采用两轴多框架的系统结构，光机状态无法完全保证两个转轴的垂直度，其将导致系统存在固定的转轴非正交误差角；3)系统对目标多次观察过程中存在各项输入参数的随机测量误差。

以上各误差源是机载光电观瞄系统目标定位误差的主要来源。

中国专利CN201410590112《系统无人机搜索目标快速定位方法》中公开了一种无人机搜索目标快速定位方法。该方法采用一架搜索无人机搜查旷野中目标，通过自身无线系统传输可视图像进行目标锁定，通过特定航行轨迹配合无人机多点视角切换和所处的姿态信息、高度信息和GPS定位信息进行多点定位迭代计算来快速获得旷野目标的具体定位信息。该专利可快速获取目标点定位信息，但并未考虑修正定位过程中存在的各种误差源，其降导致目标定位结果存在固定偏差和随机偏差，即最终获取的定位信息精度较低。

期刊《基于EKF的机载光电吊舱目标定位研究》为提高机载光电吊舱目标定位的精度，研究了光电吊舱测距、高低角和方位角的测量误差对目标定位精度的影响，推导并建立了目标定位的扩展卡尔曼滤波模型，通过该滤波模型抑制光电吊舱测距误差、高低角误差和方位角误差对目标点定位结果的影响来实现对目标定位精度的提升。该文献考虑了光电吊舱测距、高低角和方位角的测量误差对目标定位结果的影响，切定位滤波结果收敛速度较快，但其并未考虑载机姿态和位置的测量误差，同时忽略了光电吊舱与载机之间的安装误差和光电吊舱高低角与方位角之间的非正交误差，这些因素都想导致该文献提出的滤波模型与实际使用环境差异较大，模型的不准确性将直接导致滤波后的定位结果极易发散。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何提出一种适用于机载光电观瞄系统多次观察的目标定位方法。

(二)技术方案

为解决上述技术问题，本发明提供一种适用于机载光电观瞄系统多次观察的目标定位方法，所述方法包括以下步骤：

步骤1：建立地心坐标系e、地理坐标系g、光电平台坐标系b、瞄线坐标系s、瞄线俯仰零位坐标系s’；

步骤2：实时采集当前测距点数据组(α_P，β_P，γ_P，λ_P，L_P，h_P，

R)；其中：α_P、β_P、γ_P分别为在光电平台坐标系b下，位置和姿态测量单元输出的当前测距点载机俯仰角、横滚角和航向角；λ_P、L_P、h_P分别为在地心坐标系下，位置和姿态测量单元输出的当前测距点载机经度、纬度和高度信息；

分别为在瞄线坐标系s下，机载光电观瞄系统在当前测距点输出的目标瞄线俯仰角和横滚角；R为机载光电观瞄系统在当前测距点输出的目标至光电观瞄系统的激光测距值；

步骤3：采用以下公式计算瞄线坐标系到瞄线俯仰零位坐标系的转换矩阵

瞄线俯仰零位坐标系到光电平台坐标系的转换矩阵

光电平台坐标系到地理坐标系的转换矩阵

和地理坐标系到地心坐标系的变换矩阵

上式中，

为校靶测量所得俯仰轴与横滚轴的非正交误差角；θ_AZ、θ_EL分别为校靶测量所得横滚框架零位与光电平台的俯仰向安装误差角和方位向安装误差角；

步骤4：采用以下公式计算目标T在地心坐标系下的坐标

并将其作为首次卡尔曼滤波的中间参数：

其中：

上式中，

为载机P在地心坐标系下的坐标，

为目标T在瞄线坐标系下的坐标；R_N为机载光电观瞄系统所在点的地理垂线与WGS-84参考旋转椭球体交点处的主曲率半径，R_e为WGS-84参考旋转椭球体的半长轴，e为WGS-84参考旋转椭球体的扁率，e＝1/298.257；

步骤5：采用以下公式计算激光测距值的估计值

和瞄线坐标系下目标瞄线俯仰角的估计值

上式中，

分别为利用步骤2采集的当前时刻数据组数据计算所得的各旋转矩阵和载机P在地心坐标系下的坐标，

为当前时刻采用的目标T在地心坐标系下的坐标；

为目标T在瞄线俯仰零位坐标系s’下的估算值，x_PT、y_PT、z_PT分别为向量

的坐标值；

步骤6：采用以下公式定义卡尔曼滤波测量状态方程，选取地面点目标在地球直角坐标系下的位置误差作为状态变量，则对于地面点静止目标，其状态方程可表示为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

其中：

Φ_k/k-1＝I_3×3，Γ_k-1＝ΔtI_3×3

上式中，X_k、X_k-1分别为t_k和t_k-1时刻的目标点在地心坐标系下的位置误差；Δt为t_k和t_k-1时刻的时间差，即滤波周期；Φ_k/k-1为t_k-1时刻至t_k时刻的一步转移阵；Γ_k-1为系统噪声驱动阵；W_k-1为系统激励噪声序列，其方差用Q_k表示；

步骤7：采用以下公式定义卡尔曼滤波测量等式：

上式中，

分别为步骤2采集的t_k时刻数据组数据，

分别为步骤5求取的t_k时刻估计值；Z_k为t_k时刻求取的卡尔曼滤波测量输出值；

步骤8：采用以下公式定义如下变量M₁、M₂、M₃、M₄，并利用步骤2采集的当前时刻数据组数据计算结果：

其中：

步骤9：采用以下公式对步骤7进行泰勒级数展开，并去掉高此项：

其中：

G_k＝[G₁ G₂ G₃ G₄]^T

G₁＝H₁M₁,

G₄＝I_2×2

上式中，H_k为量测阵；G_k为量测噪声驱动阵；V_k为量测噪声序列，其方差阵用R_k表示；Δα_P、Δβ_P、Δγ_P分别为位置和姿态测量单元输出的俯仰误差角、横滚误差角和航向误差角；Δλ_P、ΔL_P、Δh_P分别为位置和姿态测量单元输出的经度误差、纬度误差和高度误差；

分别为目标瞄线俯仰误差角和横滚误差角；ΔR为激光测距误差值；

步骤10：采用以下公式对状态变量进行递推估计：

X_k/k-1＝Φ_k/k-1X_k-1

P_k＝(I-K_kH_k)P_k/k-1

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

上式中，X_k/k-1为状态一步预测值；X_k为t_k时刻的状态估计值；X_k-1为t_k-1时刻的状态估计值；K_k为卡尔曼滤波增益；P_k/k-1为一步预测均方误差；P_k为估计均方误差；

步骤11：采用以下公式对目标T在地心坐标系下的坐标进行更新：

上式中，

分别为t_k-1时刻、t_k时刻目标T在地心坐标系下的坐标估计值。

其中，所述地心坐标系根据描述的不同分为地心直角坐标系和地心球面坐标系。

其中，地心直角坐标系用o-x_ey_ez_e表示，原点o位于地心，x_e、y_e轴在地球赤道平面内，x_e指向零度本初子午线，z_e轴沿地球自转轴，y_e轴穿越东经90°子午线与赤道的交点。

其中，地心球面坐标系的原点位于旋转椭球体中心，位置用经度、纬度和高度表示，即地球球面坐标系内坐标的表示方法为(λ,L,h)。

其中，所述地理坐标系g：用o-x_gy_gz_g表示，原点o位于机载光电观瞄系统位置和姿态测量单元质心，x_g轴指向正东方向，y_g轴指向正北方向，z_g轴指向天向。

其中，所述光电平台坐标系b：用o-x_by_bz_b表示，原点o位于机载光电观瞄系统位置和姿态测量单元质心，x_b轴沿位置和姿态测量单元横轴向右，y_b轴沿位置和姿态测量单元纵轴向前，z_b轴沿位置和姿态测量单元立轴向上。

其中，所述瞄线坐标系s：用o-x_sy_sz_s表示，原点o位于光电平台回转中心，x_s轴沿光电平台俯仰机构转轴向前，z_s轴沿瞄线轴向上，y_s轴与x_s轴和z_s轴构成右手坐标系。

其中，所述瞄线俯仰零位坐标系s’：用o-x_s’y_s’z_s’表示，原点o位于光电平台回转中心，y_s’轴沿光电平台俯仰机构转轴向前，z_s’轴垂直于光电平台俯仰机构转轴和横滚机构转轴所在平面向上，x_s’轴与y_s’轴和z_s’轴构成右手坐标系。

(三)有益效果

与现有技术相比较，本发明具备如下有益效果：

(1)本发明基于机载光电观瞄系统预先校靶所得的位置和姿态测量单元与光电平台的固定安装误差角、转轴非正交误差角，并基于机载光电观瞄系统位置和姿态测量单元的位置随机误差和姿态随机误差统计信息，机载光电观瞄系统的姿态随机误差统计信息，机载光电观瞄系统激光测距单元的测距随机误差统计信息，通过建立完善的卡尔曼滤波模型，可用于修正校靶所得的各固定误差对目标定位结果的影响，同时减弱上述各项随机误差对目标定位结果的影响，向用户实时提供高精度的目标定位结果。

(2)本发明通过软件算法对目标定位结果实时滤波，在现有光电系统的基础上不需要增加任何硬件资源，只需要增加适用于机载光电观瞄系统多次观察的目标定位软件包便可提升机载光电观瞄系统对静止目标的定位精度。

附图说明

图1是本发明目标定位方法工作流程图。

图2a-图2c是本发明滤波前后目标定位误差对比图。

具体实施方式

为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。

为解决现有技术存在的问题，本发明提出一种适用于机载光电观瞄系统多次观察的目标定位方法，由目标定位软件模块实现，当机载光电观瞄系统跟踪上地面静止目标，且收到上级系统发出的目标定位指令时，该模块执行具体操作，并输出目标定位结果。

具体而言，本发明提供一种适用于机载光电观瞄系统多次观察的目标定位方法，如图1所示，所述方法包括以下步骤：

步骤1：建立地心坐标系e、地理坐标系g、光电平台坐标系b、瞄线坐标系s、瞄线俯仰零位坐标系s’；

步骤2：实时采集当前测距点数据组(α_P，β_P，γ_P，λ_P，L_P，h_P，

R)；其中：α_P、β_P、γ_P分别为在光电平台坐标系b下，位置和姿态测量单元输出的当前测距点载机俯仰角、横滚角和航向角；λ_P、L_P、h_P分别为在地心坐标系下，位置和姿态测量单元输出的当前测距点载机经度、纬度和高度信息；

分别为在瞄线坐标系s下，机载光电观瞄系统在当前测距点输出的目标瞄线俯仰角和横滚角；R为机载光电观瞄系统在当前测距点输出的目标至光电观瞄系统的激光测距值；

步骤3：采用以下公式计算瞄线坐标系到瞄线俯仰零位坐标系的转换矩阵

瞄线俯仰零位坐标系到光电平台坐标系的转换矩阵

光电平台坐标系到地理坐标系的转换矩阵

和地理坐标系到地心坐标系的变换矩阵

上式中，

为校靶测量所得俯仰轴与横滚轴的非正交误差角；θ_AZ、θ_EL分别为校靶测量所得横滚框架零位与光电平台的俯仰向安装误差角和方位向安装误差角；

步骤4：采用以下公式计算目标T在地心坐标系下的坐标

并将其作为首次卡尔曼滤波的中间参数：

其中：

上式中，

为载机P在地心坐标系下的坐标，

为目标T在瞄线坐标系下的坐标；R_N为机载光电观瞄系统所在点的地理垂线与WGS-84参考旋转椭球体交点处的主曲率半径，R_e为WGS-84参考旋转椭球体的半长轴，e为WGS-84参考旋转椭球体的扁率，e＝1/298.257；

步骤5：采用以下公式计算激光测距值的估计值

和瞄线坐标系下目标瞄线俯仰角的估计值

上式中，

分别为利用步骤2采集的当前时刻数据组数据计算所得的各旋转矩阵和载机P在地心坐标系下的坐标，

为当前时刻采用的目标T在地心坐标系下的坐标；

为目标T在瞄线俯仰零位坐标系s’下的估算值，x_PT、y_PT、z_PT分别为向量

的坐标值；

步骤6：采用以下公式定义卡尔曼滤波测量状态方程，选取地面点目标在地球直角坐标系下的位置误差作为状态变量，则对于地面点静止目标，其状态方程可表示为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

其中：

Φ_k/k-1＝I_3×3，Γ_k-1＝ΔtI_3×3

上式中，X_k、X_k-1分别为t_k和t_k-1时刻的目标点在地心坐标系下的位置误差；Δt为t_k和t_k-1时刻的时间差，即滤波周期；Φ_k/k-1为t_k-1时刻至t_k时刻的一步转移阵；Γ_k-1为系统噪声驱动阵；W_k-1为系统激励噪声序列，其方差用Q_k表示；

步骤7：采用以下公式定义卡尔曼滤波测量等式：

上式中，

分别为步骤2采集的t_k时刻数据组数据，

分别为步骤5求取的t_k时刻估计值；Z_k为t_k时刻求取的卡尔曼滤波测量输出值；

步骤8：采用以下公式定义如下变量M₁、M₂、M₃、M₄，并利用步骤2采集的当前时刻数据组数据计算结果：

其中：

步骤9：采用以下公式对步骤7进行泰勒级数展开，并去掉高此项：

其中：

G_k＝[G₁ G₂ G₃ G₄]^T

G₁＝H₁M₁,

G₄＝I_2×2

上式中，H_k为量测阵；G_k为量测噪声驱动阵；V_k为量测噪声序列，其方差阵用R_k表示；Δα_P、Δβ_P、Δγ_P分别为位置和姿态测量单元输出的俯仰误差角、横滚误差角和航向误差角；Δλ_P、ΔL_P、Δh_P分别为位置和姿态测量单元输出的经度误差、纬度误差和高度误差；

分别为目标瞄线俯仰误差角和横滚误差角；ΔR为激光测距误差值；

步骤10：采用以下公式对状态变量进行递推估计：

X_k/k-1＝Φ_k/k-1X_k-1

P_k＝(I-K_kH_k)P_k/k-1

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

上式中，X_k/k-1为状态一步预测值；X_k为t_k时刻的状态估计值；X_k-1为t_k-1时刻的状态估计值；K_k为卡尔曼滤波增益；P_k/k-1为一步预测均方误差；P_k为估计均方误差；

步骤11：采用以下公式对目标T在地心坐标系下的坐标进行更新：

上式中，

分别为t_k-1时刻、t_k时刻目标T在地心坐标系下的坐标估计值。

其中，所述地心坐标系根据描述的不同分为地心直角坐标系和地心球面坐标系。

其中，地心直角坐标系用o-x_ey_ez_e表示，原点o位于地心，x_e、y_e轴在地球赤道平面内，x_e指向零度本初子午线，z_e轴沿地球自转轴，y_e轴穿越东经90°子午线与赤道的交点。

其中，地心球面坐标系的原点位于旋转椭球体中心，位置用经度、纬度和高度表示，即地球球面坐标系内坐标的表示方法为(λ,L,h)。

其中，所述地理坐标系g：用o-x_gy_gz_g表示，原点o位于机载光电观瞄系统位置和姿态测量单元质心，x_g轴指向正东方向，y_g轴指向正北方向，z_g轴指向天向。

其中，所述光电平台坐标系b：用o-x_by_bz_b表示，原点o位于机载光电观瞄系统位置和姿态测量单元质心，x_b轴沿位置和姿态测量单元横轴向右，y_b轴沿位置和姿态测量单元纵轴向前，z_b轴沿位置和姿态测量单元立轴向上。

其中，所述瞄线坐标系s：用o-x_sy_sz_s表示，原点o位于光电平台回转中心，x_s轴沿光电平台俯仰机构转轴向前，z_s轴沿瞄线轴向上，y_s轴与x_s轴和z_s轴构成右手坐标系。

其中，所述瞄线俯仰零位坐标系s’：用o-x_s’y_s’z_s’表示，原点o位于光电平台回转中心，y_s’轴沿光电平台俯仰机构转轴向前，z_s’轴垂直于光电平台俯仰机构转轴和横滚机构转轴所在平面向上，x_s’轴与y_s’轴和z_s’轴构成右手坐标系。

实施例1

本实施例是针对机载光电观瞄系统多次观察的目标定位方法，该方法通过机载光电观瞄系统中的计算机实现。计算机带有存储器并装有目标定位软件模块。在载机飞行时，当机载光电观瞄系统跟踪上地面静止目标且收到上级系统发出的目标定位指令时，目标定位软件模块根据图1所示的流程执行以下操作步骤。

步骤1：建立地球坐标系e：所述地心坐标系根据描述的不同分为地心直角坐标系和地心球面坐标系。其中地心直角坐标系用o-x_ey_ez_e表示，原点o位于地心，x_e、y_e轴在地球赤道平面内，x_e指向零度本初子午线，z_e轴沿地球自转轴，y_e轴穿越东经90°子午线与赤道的交点；地心球面坐标系的原点位于旋转椭球体中心，位置用经度、纬度和高度表示，即地球球面坐标系内坐标的表示方法为(λ,L,h)；地理坐标系g：用o-x_gy_gz_g表示，原点o位于机载光电观瞄系统位置和姿态测量单元质心，x_g轴指向正东方向，y_g轴指向正北方向，z_g轴指向天向。光电平台坐标系b：用o-x_by_bz_b表示，原点o位于机载光电观瞄系统位置和姿态测量单元质心，x_b轴沿位置和姿态测量单元横轴向右，y_b轴沿位置和姿态测量单元纵轴向前，z_b轴沿位置和姿态测量单元立轴向上。瞄线坐标系s：用o-x_sy_sz_s表示，原点o位于光电平台回转中心，x_s轴沿光电平台俯仰机构转轴向前，z_s轴沿瞄线轴向上，y_s轴与x_s轴和z_s轴构成右手坐标系。瞄线俯仰零位坐标系s’：用o-x_s’y_s’z_s’表示，原点o位于光电平台回转中心，y_s’轴沿光电平台俯仰机构转轴向前，z_s’轴垂直于光电平台俯仰机构转轴和横滚机构转轴所在平面向上，x_s’轴与y_s’轴和z_s’轴构成右手坐标系。

步骤2：实时采集当前测距点数据组(α_P，β_P，γ_P，λ_P，L_P，h_P，

R)；其中：α_P、β_P、γ_P分别为在光电平台坐标系b下，位置和姿态测量单元输出的当前测距点载机俯仰角、横滚角和航向角；λ_P、L_P、h_P分别为在地球球面坐标系下，位置和姿态测量单元输出的当前测距点载机经度、纬度和高度信息；

分别为在瞄线坐标系s下，机载光电观瞄系统在当前测距点输出的目标瞄线俯仰角和横滚角；R为机载光电观瞄系统在当前测距点输出的目标至光电观瞄系统的激光测距值。

在本优选实施例中，设定当前测距点真值为：

设定上述各项参数误差序列均为白噪声，其均值为0，均方根误差分别如下：

滤波次数：100次。

步骤3：采用以下算法计算瞄线坐标系到瞄线俯仰零位坐标系的转换矩阵

瞄线俯仰零位坐标系到光电平台坐标系的转换矩阵

光电平台坐标系到地理坐标系的转换矩阵

和地理坐标系到地球直角坐标系的变换矩阵

上式中，

为校靶测量所得俯仰轴与横滚轴的非正交误差角；θ_AZ、θ_EL分别为校靶测量所得横滚框架零位与光电平台的俯仰向安装误差角和方位向安装误差角；

在本优选实施例中，设定当前系统校靶参数为：

步骤4：采用以下算法计算目标T在地球直角坐标系下的坐标

并将其作为首次卡尔曼滤波的中间参数：

其中：

上式中，

为载机P在地球直角坐标系下的坐标，

为目标T在瞄线坐标系下的坐标；R_N为机载光电观瞄系统所在点的地理垂线与WGS-84参考旋转椭球体交点处的主曲率半径，R_e为WGS-84参考旋转椭球体的半长轴，e为WGS-84参考旋转椭球体的扁率，e＝1/298.257；

在本优选实施例中，计算得到的目标T在地球直角坐标系下的坐标真值为：

XT_real＝[-1589994.96m 5050247.22m 3544416.88m]T，其对应地球球面坐标系下的坐标为：(107.4758303°，33.9779274°，0.62m)；

将步骤二中的各项参数真值分别叠加其误差序列后，选取序列的第一组参数所求得的目标T在地球直角坐标系下的坐标作为卡尔曼滤波的首次输入值。

步骤5：采用以下算法计算激光测距值的估计值

和瞄线坐标系下目标瞄线俯仰角的估计值

上式中，

分别为利用步骤2采集的当前时刻数据组数据计算所得的各旋转矩阵和载机P在地球直角坐标系下的坐标，对于首次滤波，

为选取各项输入参数叠加误差序列的第一组参数所求得的目标T在地球直角坐标系下的坐标，后续滤波中，

为上一次滤波更新后的目标T在地球直角坐标系下的坐标值。

步骤6：采用以下算法定义卡尔曼滤波测量状态方程，选取地面点目标在地球直角坐标系下的位置误差作为状态变量，则对于地面点静止目标，其状态方程可表示为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

其中：

Φ_k/k-1＝I_3×3，Γ_k-1＝ΔtI_3×3

上式中，X_k、X_k-1分别为t_k和t_k-1时刻的目标点在地心坐标系下的位置误差；Δt为t_k和t_k-1时刻的时间差，即滤波周期；Φ_k/k-1为t_k-1时刻至t_k时刻的一步转移阵；Γ_k-1为系统噪声驱动阵；W_k-1为系统激励噪声序列，其方差用Q_k表示；

在本优选实施例中，设定滤波状态变量初值

滤波周期Δt和系统噪声序列方差阵Qk分别为：

Δt＝1s，

步骤7：采用以下算法定义卡尔曼滤波测量等式输入：

上式中，

分别为步骤2采集的t_k时刻数据组数据，

分别为步骤5求取的t_k时刻估计值；Z_k为t_k时刻求取的卡尔曼滤波测量输出值；

步骤8：采用以下算法定义如下变量M₁、M₂、M₃、M₄，并利用步骤2采集的当前时刻数据组数据计算结果：

其中：

步骤9：采用以下算法对步骤7进行泰勒级数展开，并去掉高此项：

其中：

G_k＝[G₁ G₂ G₃ G₄]^T

G₁＝H₁M₁,

G₄＝I_2×2

上式中，H_k为量测阵；G_k为量测噪声驱动阵；V_k为量测噪声序列，其方差阵用R_k表示；Δα_P、Δβ_P、Δγ_P分别为位置和姿态测量单元输出的俯仰误差角、横滚误差角和航向误差角；Δλ_P、ΔL_P、Δh_P分别为位置和姿态测量单元输出的经度误差、纬度误差和高度误差；

分别为目标瞄线俯仰误差角和横滚误差角；ΔR为激光测距误差值；

步骤10：采用以下算法对状态变量进行递推估计：

X_k/k-1＝Φ_k/k-1X_k-1

P_k＝(I-K_kH_k)P_k/k-1

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

上式中，X_k/k-1为状态一步预测值；X_k为t_k时刻的状态估计值；X_k-1为t_k-1时刻的状态估计值；K_k为卡尔曼滤波增益；P_k/k-1为一步预测均方误差；P_k为估计均方误差；

在本优选实施例中，设定滤波估计均方误差初值P0为：

步骤11：采用以下算法在每一次滤波完成后，利用状态变量的估计值对目标T在地球直角坐标系下的坐标进行更新：

上式中，

分别为t_k-1时刻、t_k时刻目标T在地心坐标系下的坐标估计值。

在本优选实施例中，未经本方法滤波前的目标点水平圆概率误差CEPT0和三维定位均方根误差σT0分别为：

CEPT0＝59.84m；

σT0＝93.03m；

经100次滤波后，目标点水平圆概率误差CEPT1和三维定位均方根误差σT1分别收敛至：

CEPT1＝44.78m；

σT1＝66.52m。

在本优选实施例中，滤波前后目标点在经度方向、纬度方向和高度方向误差对比图如图2a-图2c所示。由滤波结果和滤波前后定位误差对比图可知，本方法可有效修正目标定位过程中各项输入参数的随机误差项导致的定位误差，模型准确可靠。

以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (6)

1.一种适用于机载光电观瞄系统多次观察的目标定位方法，其特征在于，所述方法包括以下步骤：

步骤1：建立地心坐标系e、地理坐标系g、光电平台坐标系b、瞄线坐标系s、瞄线俯仰零位坐标系s’；

步骤2：实时采集当前测距点数据组

其中：α_P、β_P、γ_P分别为在光电平台坐标系b下，位置和姿态测量单元输出的当前测距点载机俯仰角、横滚角和航向角；λ_P、L_P、h_P分别为在地心坐标系下，位置和姿态测量单元输出的当前测距点载机经度、纬度和高度信息；

分别为在瞄线坐标系s下，机载光电观瞄系统在当前测距点输出的目标瞄线俯仰角和横滚角；R为机载光电观瞄系统在当前测距点输出的目标至光电观瞄系统的激光测距值；

步骤3：采用以下公式计算瞄线坐标系到瞄线俯仰零位坐标系的转换矩阵

瞄线俯仰零位坐标系到光电平台坐标系的转换矩阵

光电平台坐标系到地理坐标系的转换矩阵

和地理坐标系到地心坐标系的变换矩阵

上式中，

为校靶测量所得俯仰轴与横滚轴的非正交误差角；θ_AZ、θ_EL分别为校靶测量所得横滚框架零位与光电平台的俯仰向安装误差角和方位向安装误差角；

步骤4：采用以下公式计算目标T在地心坐标系下的坐标

并将其作为首次卡尔曼滤波的中间参数：

其中：

上式中，

为载机P在地心坐标系下的坐标，

为目标T在瞄线坐标系下的坐标；R_N为机载光电观瞄系统所在点的地理垂线与WGS-84参考旋转椭球体交点处的主曲率半径，R_e为WGS-84参考旋转椭球体的半长轴，e为WGS-84参考旋转椭球体的扁率，e＝1/298.257；

步骤5：采用以下公式计算激光测距值的估计值

和瞄线坐标系下目标瞄线俯仰角的估计值

上式中，

分别为利用步骤2采集的当前时刻数据组数据计算所得的各旋转矩阵和载机P在地心坐标系下的坐标，

为当前时刻采用的目标T在地心坐标系下的坐标；

为目标T在瞄线俯仰零位坐标系s’下的估算值，x_PT、y_PT、z_PT分别为向量

的坐标值；

步骤6：采用以下公式定义卡尔曼滤波测量状态方程，选取地面点目标在地球直角坐标系下的位置误差作为状态变量，则对于地面点静止目标，其状态方程可表示为：

X_k＝Φ_k/k-1X_k-1+Γ_k-1W_k-1

其中：

Φ_k/k-1＝I_3×3，Γ_k-1＝ΔtI_3×3

上式中，X_k、X_k-1分别为t_k和t_k-1时刻的目标点在地心坐标系下的位置误差；Δt为t_k和t_k-1时刻的时间差，即滤波周期；Φ_k/k-1为t_k-1时刻至t_k时刻的一步转移阵；Γ_k-1为系统噪声驱动阵；W_k-1为系统激励噪声序列，其方差用Q_k表示；

步骤7：采用以下公式定义卡尔曼滤波测量等式：

上式中，

分别为步骤2采集的t_k时刻数据组数据，

分别为步骤5求取的t_k时刻估计值；Z_k为t_k时刻求取的卡尔曼滤波测量输出值；

步骤8：采用以下公式定义如下变量M₁、M₂、M₃、M₄，并利用步骤2采集的当前时刻数据组数据计算结果：

其中：

步骤9：采用以下公式对步骤7进行泰勒级数展开，并去掉高此项：

其中：

G_k＝[G₁ G₂ G₃ G₄]^T

G₁＝H₁M₁,

G₄＝I_2×2

上式中，H_k为量测阵；G_k为量测噪声驱动阵；V_k为量测噪声序列，其方差阵用R_k表示；Δα_P、Δβ_P、Δγ_P分别为位置和姿态测量单元输出的俯仰误差角、横滚误差角和航向误差角；Δλ_P、ΔL_P、Δh_P分别为位置和姿态测量单元输出的经度误差、纬度误差和高度误差；

分别为目标瞄线俯仰误差角和横滚误差角；ΔR为激光测距误差值；

步骤10：采用以下公式对状态变量进行递推估计：

X_k/k-1＝Φ_k/k-1X_k-1

P_k＝(I-K_kH_k)P_k/k-1

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

上式中，X_k/k-1为状态一步预测值；X_k为t_k时刻的状态估计值；X_k-1为t_k-1时刻的状态估计值；K_k为卡尔曼滤波增益；P_k/k-1为一步预测均方误差；P_k为估计均方误差；

步骤11：采用以下公式对目标T在地心坐标系下的坐标进行更新：

上式中，

分别为t_k+1时刻、t_k时刻目标T在地心坐标系下的坐标估计值；

其中，所述瞄线坐标系s：用o-x_sy_sz_s表示，原点o位于光电平台回转中心，x_s轴沿光电平台俯仰机构转轴向前，z_s轴沿瞄线轴向上，y_s轴与x_s轴和z_s轴构成右手坐标系；

所述瞄线俯仰零位坐标系s’：用o-x_s’y_s’z_s’表示，原点o位于光电平台回转中心，y_s’轴沿光电平台俯仰机构转轴向前，z_s’轴垂直于光电平台俯仰机构转轴和横滚机构转轴所在平面向上，x_s’轴与y_s’轴和z_s’轴构成右手坐标系。

2.如权利要求1所述的适用于机载光电观瞄系统多次观察的目标定位方法，其特征在于，所述地心坐标系根据描述的不同分为地心直角坐标系和地心球面坐标系。

3.如权利要求2所述的适用于机载光电观瞄系统多次观察的目标定位方法，其特征在于，地心直角坐标系用o-x_ey_ez_e表示，原点o位于地心，x_e、y_e轴在地球赤道平面内，x_e指向零度本初子午线，z_e轴沿地球自转轴，y_e轴穿越东经90°子午线与赤道的交点。

4.如权利要求3所述的适用于机载光电观瞄系统多次观察的目标定位方法，其特征在于，地心球面坐标系的原点位于旋转椭球体中心，位置用经度、纬度和高度表示，即地球球面坐标系内坐标的表示方法为(λ,L,h)。

5.如权利要求4所述的适用于机载光电观瞄系统多次观察的目标定位方法，其特征在于，所述地理坐标系g：用o-x_gy_gz_g表示，原点o位于机载光电观瞄系统位置和姿态测量单元质心，x_g轴指向正东方向，y_g轴指向正北方向，z_g轴指向天向。

6.如权利要求5所述的适用于机载光电观瞄系统多次观察的目标定位方法，其特征在于，所述光电平台坐标系b：用o-x_by_bz_b表示，原点o位于机载光电观瞄系统位置和姿态测量单元质心，x_b轴沿位置和姿态测量单元横轴向右，y_b轴沿位置和姿态测量单元纵轴向前，z_b轴沿位置和姿态测量单元立轴向上。

Images