CN117455960B - 时变观测噪声条件下机载光电***对地无源定位滤波方法 - Google Patents

Abstract

本发明涉及一种时变观测噪声条件下机载光电***对地无源定位滤波算法，针对存在时变观测噪声的无人机载光电***，本发明提出一种联合自适应扩展卡尔曼滤波算法来实现多个目标点的无源定位。本发明针对观测噪声的渐变性，根据动态残差的大小平衡观测模型的估计量和预测量，从而实时调整观测协方差矩阵；针对观测噪声的突变性，在观测协方差预测中引入了自适应遗忘因子，以减小无人机工况变化对光电***地理位置估计值的影响。最后，仿真实验和实测飞行实验结果表明，该算法具有高精度、高实时性和强鲁棒性的特点，能够克服光电***非线性扰动带来的影响，有效提高多个目标点地理位置的定位精度。

Description

时变观测噪声条件下机载光电***对地无源定位滤波方法

技术领域

本发明涉及定位滤波算法领域，具体涉及的是一种时变观测噪声条件下机载光电***对地无源定位滤波算法。

背景技术

随着机载测量***、信息交换技术和计算机视觉的迅猛发展，搭载着高精度机载光电平台的新一代无人机正朝着模块化、轻量化和智能化迈进。机载光电***作为无人机进行态势感知的重要手段之一，在军事侦察、航空遥感、资源探测等重要领域发挥了越来越关键的作用。利用机载光电***对区域内的目标点进行精准定位，成为无人机的关键技术之一。

目前，大多数传统算法依靠距离进行目标点定位，通过激光测距仪获取无人机和目标点之间的距离，进而解算出目标点的三维地理坐标。檀立刚等借助地面目标点相对于无人机的激光测距值，结合无人机自身位姿和视轴信息，对机载光电测量平台目标点自主定位技术的定位误差进行了分析研究。Liu等利用距离信息，解决了单观测者进行无源定位与跟踪的非线性估计问题。张赫等提出了一种基于激光测距设备的直接对地目标点定位算法，不需要准确的光电平台姿态信息即可实现高精度的目标点定位。陈晨等提出了在受限观测条件下对连续观测数据建立加权误差方程，有效提高了对地面目标点定位的精度。

然而，在实际使用过程中，激光测距仪的有效作用距离有限，测距时往往需要良好的气象条件。另外，随着无人机隐蔽性能和自身安全需求的提高，要求其在执行任务过程中不与外界发生光、电联系，降低被发现和拦截的风险。因此，在无法获取目标点距离信息的条件下，利用可见光电成像***和无人机自身的传感信息对多个目标点进行高精度的无源实时定位，成为目前无人机感知技术发展过程中亟待解决的关键问题。

国内外学者针对目标点无源定位方法进行了广泛而深入地研究，以期提高光电平台在无源条件下的目标点定位精度。孙辉通过不同的坐标系之间的线性变换，成功构建了从光电平台成像***像面坐标到大地地理坐标的目标点定位数学模型。白冠冰等根据数字高程模型构建基于目标点的椭球模型，最终利用卡尔曼滤波实现了对同一视场中多个目标点的实时定位。上述算法采用卡尔曼滤波(Kalman filter，KF)这类状态估计算法，由于机载光电平台进行目标点定位时***的量测方程极为复杂，KF算法无法处理这类非线性问题。因此，穆绍硕等在KF算法的基础上提出了扩展卡尔曼滤波(extended Kalman filter，EKF)算法，对地面目标点的地理位置进行估计。刘志明等通过对目标点区域的多次扫描成像，利用立体成像算法和扩展卡尔曼滤波器完成区域内的像素配准和连续定位。然而，EKF算法需要合理地设置***噪声矩阵和测量噪声来抑制***的噪声，一般通过大量的试验来进行标定，而且需要将其设置为固定值。当飞机运行工况的变化导致光电***存在非线性扰动时，EKF无法自适应地调整包含时变特性的噪声矩阵，从而导致目标点定位精度下降。

针对无人机载光电***进行多目标点无源定位时，***观测噪声存在时变特性的问题，本发明提出一种联合自适应扩展卡尔曼滤波算法(Joint Adaptive ExtendedKalman Filtering, JAEKF)用于无人机载光电***对目标点位置的动态估计，主要开展以下研究：1）对于定位中存在观测噪声的问题，本发明研究了基于EKF的目标点地理位置估计策略，减小视轴测量误差对定位精度的影响；2）对于观测噪声的渐变性，根据动态残差协方差矩阵的大小平衡观测模型的估计量和预测量，从而实时调整观测协方差矩阵；3）进一步，对于观测噪声的突变性，在观测协方差预测中引入了自适应遗忘因子，从而减小无人机工况变化对光电***地理位置估计值的影响。

发明内容

本发明要解决的技术问题是针对以上问题和要求，提供一种时变观测噪声条件下机载光电***对地无源定位滤波算法。

为解决以上技术问题，本发明采用以下技术方案：

时变观测噪声条件下机载光电***对地无源定位滤波算法，包括以下步骤：

确定目标点在大地坐标系中的初始位置坐标，借助自适应扩展卡尔曼滤波，根据上一时刻目标点的位置先验值，对目标点进行实时定位迭代更新，得到k时刻目标点经滤波后的更新位置和协方差矩阵/>，/>的求解公式为：

；

式中，为目标点在k时刻的位置观测量，通过图像***获得,/>为k时刻的新息向量；/>为由k-1时刻目标点位置的观测值,/>为由k-1时刻到k时刻的目标点位置的观测值,/>为k-1时刻非线性观测函数的线性化矩阵，/>为k-1时刻目标点位置的先验值，根据目标点的初始位置多次迭代更新后得到；/>为k-1时刻到k时刻的目标点位置的预测值；/>为由k-1时刻到k时刻目标点的状态转移矩阵；/>为k时刻目标点位置的先验误差协方差矩阵；/>为由k-1时刻到k时刻目标点位置的误差协方差矩阵预测值；为k-1时刻到k时刻目标点的状态转移矩阵的转置；/>为k时刻的观测噪声协方差；n为计算窗口长度；k时刻下的残差向量/>；/>为中间变量，/>为k时刻的卡尔曼增益矩阵，/>为k-1时刻的***噪声W _k-1的协方差矩阵，/>为单位矩阵；

λ _k 为自适应遗忘因子，计算公式为；

式中，α为遗忘因子的最大值；γ为放大系数；为k时刻的开窗新息协方差，/>的具体计算公式为：

。

进一步的，h(·)为光电平台观测的非线性函数，h(x)的具体公式为：

x为目标点在大地坐标系下的坐标,设x=[L,B,H]^T，L为经度信息，B为纬度信息，H为高程信息，则目标点在摄像机坐标系中的坐标为[x _c ,y _c ,z _c ]^T，则：

；

（x _c ,y _c ,z _c ）的计算公式为：

；

式中，等效焦距、/>分别为摄像机焦距/>与像元的横、纵尺寸/>、/>之比；/>为机体坐标系到摄像机坐标系的变换矩阵，/>为导航坐标系到机体坐标系的变换矩阵；/>为地心地固坐标系到导航坐标系的变换矩阵; />为地球椭球子午椭圆的第一偏心率；/>为卯酉圈的曲率半径，其中/>为椭球长半轴，为椭球短半轴，单位均为米。

本发明采用以上技术方案后，与现有技术相比，具有以下优点：

针对无人机载光电***观测噪声的时变特性，本发明提出一种联合自适应扩展卡尔曼滤波（JAEKF）算法用于光电***对目标点地理位置的动态估计。对于观测噪声的渐变性，JAEKF算法根据动态残差协方差矩阵的大小平衡观测模型的估计量和预测量，从而实时调整观测协方差矩阵；对于观测噪声的突变性，在观测协方差预测中引入了自适应遗忘因子，从而减小无人机工况变化对光电***地理位置估计值的影响。仿真和实测试验结果表明，该算法具有高精度、高实时性和强鲁棒性的特点，当无人机进行快速机动而导致工况发生显著变化时，该算法依然能够克服光电***非线性扰动带来的影响，具有更加良好的观测噪声跟踪能力和误差收敛性能，有效提高多个目标点地理位置的估计精度。下面结合附图和实施例对本发明进行详细说明。

附图说明

图1为GCF和ECEF坐标系示意图；

图2为ECEF和NED坐标系示意图；

图3为HRD、C和I坐标系示意图；

图4为JAEKF算法流程；

图5为仿真的飞行轨迹与测量目标点示意图；

图6为仿真的姿态角示意图；

图7为定位点及其误差频数分布图，其中（a）图为定位点分布图；（b）图为经度误差频数分布图；（c）图为纬度误差频数分布图；

图8为圆概率误差仿真结果示意图；

图9 定位均方根误差示意图。

具体实施方式

以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。

针对无人机载光电***观测噪声的时变特性，本发明提出一种联合自适应扩展卡尔曼滤波（JAEKF）算法用于光电***对目标点地理位置的动态估计，本发明的方法的主要内容如下：

首先建立无人机载光电成像模型：

在目标点定位过程中主要涉及6个基本惯性坐标系：大地（GCF）坐标系、地心地固（ECEF）坐标系、导航（NED）坐标系、机体（HRD）坐标系、摄像机坐标系（C）、图像（I）坐标系。将A坐标系下某一坐标点变换到B坐标系的公式如下：

（1）；

式中：为从A坐标系到B坐标系的变换矩阵，（x _A ,y _A ,z _A ）^T和（x _B ,y _B ,z _B ）^T分别为同一点在不同坐标系下的坐标。

如图1所示，已知空间中任意一点K在GCF坐标系O_gcf-LBH的位置信息包括经度信息L，纬度信息B，高程信息H，则大地坐标（L,B,H）^T到地心地固坐标（x _ecef ,y _ecef ,z _ecef ）^T的坐标变换数学关系式为

（2）；

式中：为地球椭球子午椭圆的第一偏心率；为卯酉圈的曲率半径。其中a=6378137为椭球长半轴，b=6356752.31为椭球短半轴，单位均为米。

NED坐标系的原点为无人机的位置，X _ned 轴和Y _ned 轴分别指向正北和正东，Z _ned 轴垂直于地球表面向下。由图2可知，地心地固坐标（x _ecef ,y _ecef ,z _ecef ）^T到导航坐标（x _ned ,y _ned , z _ned ）^T的变换矩阵为：

（3）；

NED坐标系O _ned - X _ned Y _ned Z _ned 的原点与HRD坐标系O _hrd - X _hrd Y _hrd Z _hrd 的原点重合，X _hrd 轴和Y _hrd 轴分别指向无人机的机头和右翼方向，Z _hrd 轴垂直无人机水平面向下。由图3可知，导航坐标（x _ecef ,y _ecef ,z _ecef ）^T到机体坐标

（x _ned ,y _ned ,z _ned ）^T的变换矩阵为:

（4）；

C坐标系O _c - X _c Y _c Z _c 的原点为光电平台的光心O _c ，X _c 轴和Y _c 轴与后面介绍的图像坐标系的x轴和y轴平行，Z _c 轴与光电平台的光轴重合。由于光电成像平台安装在两轴框架内，只需考虑平台中摄像机内框架的高低角β和外框架的方位角α，则机体坐标（x _hrd ,y _hrd ,z _hrd ）^T到摄像机坐标(x _c ,y _c ,z _c )^T的变换矩阵为:

（5）；

式中：t=[t _x ,t _y ,t _z ]为HRD坐标系原点到C坐标系原点的位置向量，被称为平移向量。

I坐标系I-xy以图像的左上角顶点为原点，x轴和y轴分别与图像的长边和短边重合。为了便于对像点p和对应物点P进行空间位置的相互换算，在此坐标变换中引入图像物理坐标系o-x _w y _w 。图像物理坐标系的原点为光轴和像面的交点o，x _w 轴和y _w 轴分别与x轴和y轴平行。由图3可知，空间某点P在C坐标系中的坐标为(x _c ,y _c ,z _c )，与图像物理坐标系中的坐标（x _w ，y _w ）之间的关系如下：

（6）；

式中：f为摄像机的焦距。

则像点p的图像坐标表示为（u,v），与对应图像物理坐标（x _w ,y _w ）的关系为：

（7）；

从而，像点p的像素坐标（u,v）与物点p的摄像机坐标(x _c ,y _c ,z _c )之间的关系为：

（8）；

式中：等效焦距f _x ，f _y 分别为焦距f与像元的横、纵尺寸d _x ，d _y 之比。

将式(8)转化为矩阵形式，可得：

（9）；

假设无人机载光电***对视场内多个目标点进行连续多次测量，由于估计对象是目标点的地理位置信息，因此选择目标点导航坐标为***的状态量，即

x _k =[L _k ,B _k ,H _k ]^T （10）；

则***的状态方程为：

（11）；

式中：下标k和k-1分别代表k时刻和k-1时刻；为从k-1时刻到k时刻的状态转移矩阵；/>为噪声驱动矩阵；/>是***过程噪声。对于静止不动的目标点，显然有：

（12）；

式中：Q _k-1为***噪声W _k-1的协方差矩阵。

***观测量即目标点在图像坐标系下的像素坐标，可通过图像***得到，数学表达式为：

（13）；

则观测方程为：

（14）；

式中：V _k-1为测量噪声；h(·)为光电平台观测的非线性函数。另外，V _k-1的协方差矩阵是R _k-1。

以下是h(x _k )的求解过程：

假设目标点P在GCF坐标系下的坐标x _k =[L _k ,B _k ,H _k ]^T，将C坐标系中的坐标[x _c ,y _c , z _c ]^T作为中间变量，由式(9)可得：

（15）；

其次，式(15)中的参数(x _c ,y _c ,z _c )可通过式(16)计算得到：

（16）；

EKF算法通过对非线性函数进行泰勒展开来得到线性近似解，能够在每个时间步骤中使用状态和观测方程的线性化版本来更新滤波器的状态估计。然而，EKF算法需要合理地设置***噪声矩阵和测量噪声来抑制***的噪声，当飞机的运行工况发生变化时，该算法难以自适应地调整噪声矩阵，从而导致目标点定位精度下降。因此，本发明提出一种基于联合自适应扩展卡尔曼滤波算法，通过观测噪声矩阵和遗忘因子的联合自适应算法，实现观测噪声自适应更新的同时保证算法的稳态性能，从而提高目标点定位精度和噪声突变时的误差收敛速度。

根据前向欧拉法，可得***状态方程和测量方程的离散形式：

（17）；

式中：k-1时刻非线性观测函数的线性化矩阵。

式(17)将无人机的目标点定位问题演变成通过多次测量对目标点位置进行最优估计的问题。其中，***的状态方程是线性的，但测量方程是非线性的，为此采用EKF递推地进行目标点位置估计，其计算过程分为以下两步：

（1）预测

利用上一周期的定位结果对状态和协方差进行预测：

（18）；

（2）更新

利用新息向量对预测的状态和协方差进行修正：

（19）；

式中，为目标点在k时刻的位置观测量，通过图像***获得,/>为k时刻的新息向量；/>为由k-1时刻到k时刻的目标点位置的观测值,/>为k-1时刻非线性观测函数的线性化矩阵，/>为k-1时刻目标点位置的先验值，根据目标点的初始位置多次迭代更新后得到；/>为k-1时刻到k时刻的目标点位置的预测值；/>为由k-1时刻到k时刻目标点的状态转移矩阵；/>为k时刻目标点位置的先验误差协方差矩阵；/>为由k-1时刻到k时刻目标点位置的误差协方差矩阵预测值；/>为k-1时刻到k时刻目标点的状态转移矩阵的转置；/>为k时刻的观测噪声协方差；/>为中间变量，/>为k时刻的卡尔曼增益矩阵，/>为单位矩阵。

针对光电***观测噪声的渐变特性，在EKF算法的基础上，根据***观测量和预测量之间动态残差的大小实时调整观测矩阵，即通过在窗口内每个离散时刻的观测残差序列加权处理来实现观测噪声协方差的估计。其计算公式为：

（20）；

式中，n为计算窗口长度；k时刻下的残差向量。

针对光电***观测噪声的突变特性，在观测协方差预测中引入了自适应遗忘因子，提高算法对突变噪声的瞬态响应速度，克服不变遗忘因子降低算法稳定性的缺陷，从而减小无人机工况变化对光电***地理位置估计值的影响。自适应遗忘因子的计算公式为：

（21）；

式中，α为遗忘因子的最大值；γ为放大系数；为k时刻的开窗新息协方差，具体计算公式为：

（22）；

则误差协方差矩阵为

（23）；

由式(22)可知，当***进入稳态后自适应遗忘因子约等于1，因此所提算法在快速处理突变噪声的同时兼具良好的稳定性。将式(19)的替换为式(23)，并结合式(19)和式(20)，即为所构建的联合自适应扩展卡尔曼滤波算法，具体流程图如图4所示。

本发明的仿真实验具体数据如下：

为了验证以上所提出的无人机载光电平台无源目标点定位算法的精度和有效性，采用蒙特卡洛统计试验法进行仿真测试。假设目标点的地理位置为（31.000000°N，114.000000°E，50.00m），无人机在1000m的高度且以577m的盘旋半径围绕目标点顺时针飞行，滚转角为45°，俯仰角为0°，其飞行轨迹和测量目标点如图5所示。无人机光电平台的成像次数定为200次，光轴的俯角为60°，成像距离约为1200m。

无人机装有可测量自身精确地理位置和姿态的GPS传感器和惯性导航传感器，其测量误差仿真参数如表1所示。根据误差仿真参数以及无人机的飞行路径，通过蒙特卡罗方法仿真分析无人机和摄像机的姿态角、以及误差概率分布情况，仿真数据的变化规律如图6所示。

表1 测量误差仿真参数

仿真过程中光电平台的焦距为50mm，像元尺寸为2.9μm，参考表1的参数设置，并取n=100，对位于像面中心的目标点进行定位，其蒙特卡洛仿真分析结果如图7所示。图7的（a）图中蓝色散点表示蒙特卡洛仿真的全部GPS坐标点，共计20000个；图7的（b）图和（c）图分别为经度计算值与真实值、纬度计算值与真实值的误差频数分布图，红色曲线为频数拟合曲线，呈正态分布的形式。

假设（L _i ,B _i ）为第i次GPS定位结果，（L _t ,B _t ）为目标点位置，则经度和纬度的定位标准差分别为：

根据图7中的数据，计算出经度定位标准差为2.5601×10^-4°，纬度定位标准差为2.1735×10^-4°。由WGS-84定义的地球椭球模型可知，可反映目标点地理位置定位精度的圆概率误差为

式中，和/>分别为卯酉圈和子午圈的曲率半径，H _t 为目标点高度，B _t 为目标点纬度。

假设目标点区域高度的误差为25m，则上述算法的协方差矩阵P ₁ 设置为[ diag（0.0001,0.00014,25）]，观测过程噪声设置为[ diag（1，5）]。为了验证所提算法对目标点定位精度的改善，分别使用EKF、UKF和JAEKF进行对比分析，得到的圆概率误差仿真结果如图8所示。

由图8可知，EKF在解决光电平台目标点定位问题时无法有效地对观测噪声进行准确认识和处理，其圆概率误差仿真结果最大，且波动较为剧烈。UKF对非线性和非高斯问题有更好的性能表现，误差减小，但在飞机工况剧烈变化时无法有效跟踪观测噪声，算法结果稳定性较差，波动同样较大。AEKF通过引入自适应控制来动态调整观测噪声统计属性，误差和波动进一步降低。而JAEKF在观测协方差预测中引入了自适应遗忘因子，误差和波动最小，在滤波后期误差更为收敛，表明本发明算法进一步提升了对观测噪声估计的稳定性。

均方根误差是反映算法定位性能的指标，计算公式为：

式中，m为信息序列的个数，为定位信息的估计值，/>为定位信息的真实值。针对上述算法各进行100次蒙特卡洛仿真计算，并对不同算法的均方根误差进行比较（RootMean Square Error, RMSE），如图9所示。

图9中左侧纵轴代表经度和纬度的均方根误差，右侧纵轴代表圆概率的均方根误差，实线框对应EKF方法，粗虚线框对应UKF方法，点划线框对应AEKF方法，细虚线框对应本发明的JAEKF方法，图中，可以看出，本发明所提算法的均方根误差最小，具有更强的鲁棒性。对于定位精度，JAEKF相较于EKF、UKF和AEKF分别平均提高43.9%、30.3%和14.8%。

针对无人机载光电***观测噪声的时变特性，本发明提出一种联合自适应扩展卡尔曼滤波（JAEKF）算法用于光电***对目标点地理位置的动态估计。对于观测噪声的渐变性，JAEKF算法根据动态残差协方差矩阵的大小平衡观测模型的估计量和预测量，从而实时调整观测协方差矩阵；对于观测噪声的突变性，在观测协方差预测中引入了自适应遗忘因子，从而减小无人机工况变化对光电***地理位置估计值的影响。仿真和实测试验结果表明，该算法具有高精度、高实时性和强鲁棒性的特点，当无人机进行快速机动而导致工况发生显著变化时，该算法依然能够克服光电***非线性扰动带来的影响，具有更加良好的观测噪声跟踪能力和误差收敛性能，有效提高多个目标点地理位置的估计精度。

以上所述为本发明最佳实施方式的举例，其中未详细述及的部分均为本领域普通技术人员的公知常识。本发明的保护范围以权利要求的内容为准，任何基于本发明的技术启示而进行的等效变换，也在本发明的保护范围之内。

Claims (2)

1.时变观测噪声条件下机载光电***对地无源定位滤波方法，其特征在于,包括以下步骤：

确定目标点在大地坐标系中的初始位置坐标，借助自适应扩展卡尔曼滤波，根据上一时刻目标点的位置先验值，对目标点进行实时定位迭代更新，得到k时刻目标点经滤波后的更新位置和协方差矩阵/>，/>的求解公式为：

；

式中，为目标点在k时刻的位置观测量，通过图像***获得,/>为k时刻的新息向量；/>为由k-1时刻目标点位置的观测值,/>为由k-1时刻到k时刻的目标点位置的观测值,/>为k-1时刻非线性观测函数的线性化矩阵，/>为k-1时刻目标点位置的先验值，根据目标点的初始位置多次迭代更新后得到；/>为k-1时刻到k时刻的目标点位置的预测值；/>为由k-1时刻到k时刻目标点的状态转移矩阵；/>为k时刻目标点位置的先验误差协方差矩阵；/>为由k-1时刻到k时刻目标点位置的误差协方差矩阵预测值；为k-1时刻到k时刻目标点的状态转移矩阵的转置；/>为k时刻的观测噪声协方差；n为计算窗口长度；k时刻下的残差向量/>；/>为中间变量，/>为k时刻的卡尔曼增益矩阵，/>为k-1时刻的***噪声W _k-1的协方差矩阵，/>为单位矩阵；

λ _k 为自适应遗忘因子，计算公式为；

式中，α为遗忘因子的最大值；γ为放大系数；为k时刻的开窗新息协方差，/>的具体计算公式为：

。

2.根据权利要求1所述的时变观测噪声条件下机载光电***对地无源定位滤波方法，其特征在于, h(·)为光电平台观测的非线性函数，h(x)的具体公式为：

x为目标点在大地坐标系下的坐标,设x=[ L,B,H]^T，L为经度信息，B为纬度信息，H为高程信息，则目标点在摄像机坐标系中的坐标为[ x _c ,y _c ,z _c ]^T，则：

；

（x _c ,y _c ,z _c ）的计算公式为：

；

式中，等效焦距、/>分别为摄像机焦距/>与像元的横、纵尺寸/>、/>之比；/>为机体坐标系到摄像机坐标系的变换矩阵，/>为导航坐标系到机体坐标系的变换矩阵；/>为地心地固坐标系到导航坐标系的变换矩阵; />为地球椭球子午椭圆的第一偏心率；/>为卯酉圈的曲率半径，其中/>为椭球长半轴，为椭球短半轴，单位均为米。