CN117990112B - 基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法 - Google Patents
基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法 Download PDFInfo
- Publication number
- CN117990112B CN117990112B CN202410403488.6A CN202410403488A CN117990112B CN 117990112 B CN117990112 B CN 117990112B CN 202410403488 A CN202410403488 A CN 202410403488A CN 117990112 B CN117990112 B CN 117990112B
- Authority
- CN
- China
- Prior art keywords
- matrix
- state
- coordinate
- target
- time
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 100
- 238000001914 filtration Methods 0.000 title claims abstract description 27
- 239000011159 matrix material Substances 0.000 claims abstract description 102
- 230000008569 process Effects 0.000 claims abstract description 49
- 238000005070 sampling Methods 0.000 claims abstract description 45
- 238000004364 calculation method Methods 0.000 claims abstract description 25
- 239000013598 vector Substances 0.000 claims description 46
- 230000009466 transformation Effects 0.000 claims description 20
- 238000012545 processing Methods 0.000 claims description 14
- 230000007704 transition Effects 0.000 claims description 10
- 239000000203 mixture Substances 0.000 claims description 9
- 238000005516 engineering process Methods 0.000 claims description 8
- 239000006185 dispersion Substances 0.000 claims description 6
- 230000026676 system process Effects 0.000 claims description 6
- 230000003044 adaptive effect Effects 0.000 claims description 5
- 230000001131 transforming effect Effects 0.000 claims description 5
- 239000002131 composite material Substances 0.000 claims description 3
- 230000000694 effects Effects 0.000 abstract description 6
- 230000002411 adverse Effects 0.000 abstract description 3
- 230000008859 change Effects 0.000 description 7
- 238000003384 imaging method Methods 0.000 description 7
- 238000002474 experimental method Methods 0.000 description 5
- 238000005259 measurement Methods 0.000 description 5
- 230000005693 optoelectronics Effects 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 238000004458 analytical method Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 239000000306 component Substances 0.000 description 2
- 238000007689 inspection Methods 0.000 description 2
- 238000012544 monitoring process Methods 0.000 description 2
- 239000002245 particle Substances 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 239000008358 core component Substances 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 230000035772 mutation Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000004044 response Effects 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法，为了适应无人机载光电***对实时目标定位的需求，本发明针对受到复杂环境干扰的***扰动场景，融合了无迹卡尔曼滤波（UKF）与稳健估计理论，通过动态残差的实时调整来降低***干扰对目标定位的不利影响。该方法通过各时刻不同的残差来动态调整自适应权重因子，进而实时调整预测矩阵与误差协方差的加权值，以减轻外部环境变化对***稳定性的影响，并且提升了***的定位准确度与鲁棒性。此外，算法还通过优化采样过程来降低计算负荷，从而提高了***的实时性能。
Description
技术领域
本发明涉及定位滤波算法领域，具体涉及的是一种基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法。
背景技术
在经济成本效益、机动性、时效性和适应性方面表现出色的无人机技术，经历了快速的技术迭代发展，并在多个领域获得了广泛的应用。伴随着机载光电***的普及，无人机在实施目标侦测、追踪及定位方面展现出巨大的潜力，尤其是在精确获取目标位置信息和运动特征方面，已引起许多行业的兴趣。当前，无人机展现出广泛的应用潜力，涵盖了场地监控、气象侦测、道路巡查、地质探索、水灾监测、航空摄影、交通管控、电力设施检查以及森林防火等多个领域。
机载光电平台构成了无人机执行任务的核心组件，其通过集成的光电***和光电传感器技术，能够对地面以及空中的多种目标进行精确的侦测和定位。随着光电技术及数字化技术的持续进步，在机载***的小型化与高度集成化趋势推动下，预期无人机搭载的光电***性能将显著提升。这一进步预示着探测范围的显著扩大、灵敏度与分辨率的提高，以及***质量与体积的进一步减小，这些因素共同为实现无人机目标的高精度定位提供了坚实的硬件支持。
通过机载侦察***，可获得包括飞行姿态（俯仰、侧倾、航向角）、成像***的方位、仰俯角度及传感器的焦距等参数，然而，这些数据并不足以直接确定目标的精确地理位置。为了解析目标在地理坐标系中的三维位置，必须获取该时刻无人机的飞行姿态数据、全球定位***（GPS）坐标以及机载光电平台的视轴方位等关键信息。接着，通过一系列的坐标转换过程，可以计算得到目标的三维空间位置。基于无人机搭载光电平台的目标定位方法的工作原理和步骤，如图1所示。
目标无源定位方法的优劣，核心是目标地理位置估计算法的设计。目标定位的复杂性主要由于传感器精度、测量工具、负载承载平台的稳定性、航空器的导航精度以及定位准确性的多重因素共同作用所致。在目标地理位置的精确估计方法方面，国际研究者广泛关注于多种滤波技术，包括卡尔曼滤波（Kalman filter, KF）、扩展卡尔曼滤波（ExtendedKalman Filter, EKF）、容积卡尔曼滤波（Cubature Kalman Filter, CKF）、无迹卡尔曼滤波（Unscented Kalman Filter, UKF）和粒子滤波（Particle Filter, PF）算法。例如，Luo及其同事开发的三级EKF算法，通过建立目标横向位置与其相对高度的几何联系，显著增强了定位精确度。Xu等人采用数字高度模型大致预测目标起始位置，随后应用CKF算法细化位置估计，特别是在复杂地形中提升了地面目标的定位准确性。进一步，唐大全等人通过迭代UKF算法对地面目标进行定位，有效克服了跟踪速度缓慢和滤波分散的难题。
然而，EKF、UKF和CKF算法需要合理地设置***噪声矩阵和观测噪声来抑制***的噪声，一般通过大量的试验来进行标定，而且需要将其设置为固定值。其次，当飞机运行工况的变化导致光电***存在非线性扰动时，会产生随着时间变化、不具有恒定统计特性的时变噪声。时变噪声主要分为两种：一种是渐变噪声，其统计特性在较长的时间内缓慢变化；另一种是突变噪声，通常由飞行条件的急剧变化引起，在短时间内统计特性发生急速变化。上述这些算法无法自适应地调整其参数来处理包含时变特性的噪声，难以适应无人机飞行过程中变化的工作环境，从而导致光电***目标定位精度下降。
发明内容
本发明要解决的技术问题是针对以上问题和要求，提供一种基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法。
为解决以上技术问题，本发明采用以下技术方案：
基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法，其特征在于,包括以下步骤：
步骤1、确定目标点在大地坐标系中的初始位置，并确定坐标***状态向量初值和误差协方差矩阵初值；
步骤2、根据上一时刻目标的位置先验值，对目标进行实时定位更新，求解t时刻目标的状态估计值向量时，对t-1时刻的***状态向量中的元素及其均值和协方差采样，生成n+1个采样点集合；
步骤3、通过状态变换获得t-1时刻***状态的后验估计，具体公式为：
；
式中，为第i个采样点时刻由时刻状态更新后的***状态向量，是t-1时刻的***过程噪声；为状态变换函数；
对状态函数均值和方差进行加权处理，获得t时刻***状态量的加权均值和加权协方差矩阵：
；
式中，为由加权均值组成的***状态量均值向量；为t时刻的过程噪声的协方差矩阵，为第i个采样点的权重；
步骤4、对t时刻***状态量进行变换后，对第i个采样点的观测值进行加权变换，获得***观测均值向量，其中各元素的计算公式为：
；
其中，为 非线性观测函数；
步骤5、利用新息向量对状态量的均值矩阵和协方差矩阵进行更新：
；
；
；
；
；
式中，为实际观测向量，计算机视觉技术对目标进行图像跟踪获得；为观测噪声；为观测量的加权协方差矩阵；为状态量和观测量的加权协方差矩阵；为时刻的卡尔曼增益矩阵；为更新后的***状态量的估计向量；为更新后的***状态量的估计协方差矩阵；
步骤5、计算动态残差：
；
；
；
；
；
式中，为***状态矩阵，由状态量估计均值向量和观测量矩阵组成；为***观测状态转移矩阵，由观测矩阵和单位矩阵组成，是非线性观测函数的雅可比矩阵；为***噪声矩阵，由过程噪声矩阵和***状态量的估计协方差矩阵组成；为中间变量；为t时刻的动态残差；
步骤6、鲁棒性处理：
计算自适应比例因子
式中，为的等价权矩阵；i和j为比例因子的计算上限和下限；
利用抗差估计理论对状态量的估计值与估计协方差进行鲁棒性处理，得到t时刻的最终状态估计值向量：
；
；
式中，为t时刻经鲁棒性处理的最终状态估计值向量，能用于下一时刻的目标的***状态迭代估计；为t时刻的最终***估计值协方差矩阵，经过鲁棒性处理的会在时刻对状态估计值进行修正。
进一步的，步骤1中，
式中，、、分别为光电平台***在L坐标、B坐标、H坐标的初始方差，、和分别为目标的初始经度、纬度和大地高。
进一步的，步骤2中的具体采样过程如下：
式中，为采样点初始权重；为缩放参数，根据计算得到，用于控制采样点的分散程度；n为采样点数量；为一个n行n列的矩阵，每个元素都是1；为n维单位矩阵；是通过和计算得到的一个矩阵，用于调整采样点的分散；为对角元素，是权重的对角矩阵，用于采样点的更新；为时刻的状态协方差矩阵；为一个行1列的列向量，每个元素都是；为时刻采样点集合的第i个元素；为仅有一个采样点时的均值向量，显然；,为第i个采样点的均值向量，其计算公式为：
。
进一步的，步骤3中状态变换函数的公式为：
；
式中： 为t-1时刻到t时刻的状态转移矩阵；为t-1时刻到t时刻的噪声驱动矩阵；是t-1时刻的***过程噪声。
进一步的, h(·)为光电平台观测的非线性函数，h(x)的具体公式为：
x为目标点在GCF坐标系下的坐标,设x=[ L,B,H]T，L为经度信息，B为纬度信息，H为高程信息，则目标点在C坐标系中的坐标为[ x c ,y c ,z c ]T，则：
（x c ,y c ,z c ）的计算公式为：
；
式中，u’是指图像跟踪仪所测目标在像素坐标系x方向的坐标，v’是指图像跟踪仪所测目标在像素坐标系y方向的坐标，
c x 为像素平面中心在x方向的坐标，c y 为像素平面中心在y方向的坐标；等效焦距、分别为摄像机焦距与像元的横、纵尺寸、之比； 为HRD坐标系到C坐标系的坐标变换矩阵，为NED坐标系到HRD坐标系的坐标变换矩阵；为ECEF坐标系到NED坐标系的坐标变换矩阵; 为地球椭球子午椭圆的第一偏心率； 为卯酉圈的曲率半径，其中为椭球长半轴，为椭球短半轴，单位均为米。
本发明采用以上技术方案后，与现有技术相比，具有以下优点：
本发明针对受到复杂环境干扰的***扰动场景，融合了无迹卡尔曼滤波（UKF）与稳健估计理论，通过动态残差的实时调整来降低***干扰对目标定位的不利影响。该方法通过各时刻不同的残差来动态调整自适应权重因子，进而实时调整预测矩阵与误差协方差的加权值，以减轻外部环境变化对***稳定性的影响，并且提升了***的定位准确度与鲁棒性。此外，本发明还通过优化采样过程来降低计算负荷，从而提高了***的实时性能。
附图说明
图1为基于无人机载光电平台的目标定位工作流程；
图2为GCF、ECEF和NED坐标系示意图；
图3为光电平台成像模型；
图4为实验无人机飞行轨迹规划示意图；
图5为不同航点无人机的成像图像；
图6为轨迹W1到W2的目标定位结果图；
图7为轨迹W3到W4的目标定位结果图。
具体实施方式
以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
为了适应无人机载光电***对实时目标定位的需求，本研究提出了一种基于动态残差反馈的自适应鲁棒无迹卡尔曼滤波方法（Adaptive Robust Unscented KalmanFilter, ARUKF）。本方法针对受到复杂环境干扰的***扰动场景，融合了无迹卡尔曼滤波（UKF）与稳健估计理论，通过动态残差的实时调整来降低***干扰对目标定位的不利影响。该方法通过各时刻不同的残差来动态调整自适应权重因子，进而实时调整预测矩阵与误差协方差的加权值，以减轻外部环境变化对***稳定性的影响，并且提升了***的定位准确度与鲁棒性。此外，算法还通过优化采样过程来降低计算负荷，从而提高了***的实时性能。
在目标定位过程中主要涉及6个基本惯性坐标系：
（1）大地坐标系GCF
（2）地心坐标系ECEF
（3）导航坐标系NED
（4）机体坐标系HRD
（5）摄像机坐标系C
（6）图像物理坐标系O
（7）图像坐标系I
GCF坐标系、ECEF坐标系和NED坐标系如图2所示。
光电平台成像模型如图3所示，主要涉及HRD、C、O和I坐标系。
假定无人机搭载的光电***对视野内的多个目标执行连续的多重观测活动，由于该***主要估计的是目标的地理位置信息，故而***状态量被设定为目标的地理坐标。
（1）
式中：、和分别为时刻目标的经度、纬度和大地高。则***的状态方程可表示为
（2）
式中：下标k和k-1分别代表k时刻和k-1时刻；为从k-1时刻到k时刻的状态转移矩阵；为噪声驱动矩阵；是***过程噪声协方差矩阵。对于静止不动的目标，显然和取值为
（3）
式中：为***噪声的协方差矩阵。
图3所示的光电平台成像模型中，机体坐标系HRD为，将摄像机的光心定义为无人机的成像中心，建立光点平台的摄像机坐标系C。
目标在C坐标系中的坐标为，由光电平台成像模型可知：
（4）
式中，u’是指图像跟踪仪所测目标在像素坐标系x方向的坐标，v’是指图像跟踪仪所测目标在像素坐标系y方向的坐标，c x 为像素平面中心在x方向的坐标，c y 为像素平面中心在y方向的坐标；等效焦距、分别为摄像机焦距与像元的横、纵尺寸、之比；
假设拍摄目标区域的大地高为，则可由下式求得：
（5）；
式中， 为HRD坐标系到C坐标系的坐标变换矩阵，为NED坐标系到HRD坐标系的坐标变换矩阵；为ECEF坐标系到NED坐标系的坐标变换矩阵;为地球椭球子午椭圆的第一偏心率； 为卯酉圈的曲率半径，其中为椭球长半轴， 为椭球短半轴，单位均为米。
为了满足目标高精度定位的要求，将摄像机作为外部测量传感器。则观测量为目标在采集图像的像素坐标，数学表达式为:
（6）
则观测方程为 （7）；
式中：为测量噪声；为光电平台观测的非线性函数。另外，的协方差矩阵是。
无人机在自主飞行期间会遭遇剧烈的非线性姿态变化，而通过采用无迹卡尔曼滤波(UKF)算法，借助无迹变换技术可以有效解决在光电***目标定位过程中常见的非线性观测问题，具有对复杂动态行为的出色处理能力。UKF算法的应用要求预先设定***及观察噪声参数，然而，由于无人机操作环境的不确定性和风速的波动性，加之机体在飞行中的持续震动，准确预测噪声误差的分布或类型变得极为困难，这导致无法构建精确的函数或随机模型。因此，在滤波过程中，减少参数更新的影响显得尤为重要。
针对上述问题，本发明通过计算***观测值与预测值之间的动态残差，来确定自适应权重因子。这些权重因子被用于对每一时刻的预测状态向量及其预测误差协方差进行加权调整，以此实现动态模型与测量数据之间的优化平衡。该方法允许对观测噪声矩阵进行实时调整，以减少预设噪声参数与实际情况不符的影响，有效降低在滤波过程中预测位置信息发散的风险，从而提升了算法的准确度和鲁棒性。此外，通过采用最小化策略采样，本研究进一步减少了***的计算需求，提高了其实时处理能力。具体算法实施主要分为6个步骤：
（1）设定***状态向量初值和误差协方差矩阵初值，为后续***状态空间迭代提供确定性的***响应依据，具体公式为
（8）；
式中，、、分别为光电平台***在L坐标、B坐标、H坐标的初始方差。
（2）***状态量迭代采样和加权变换。光电平台对地面目标进行观测时对算法的实时性要求极高，所以UKF在生成sigma采样点时，借助最小采样策略能够大幅减小计算量，提高运算效率和定位实时性。首先，根据***状态向量和协方差，生成个采样点集合。具体采样过程如下：
（9）；
式中，为采样点初始权重；为缩放参数，根据计算得到，用于控制Sigma采样点的分散程度；n为采样点数量；
（10）
式中，为一个n行n列的矩阵，每个元素都是1；为n维单位矩阵；是通过和计算得到的一个矩阵，用于调整Sigma点的分散；
（11）；
式中，为对角元素，是权重的对角矩阵，用于Sigma点的更新；第i个对角元素的计算公式为
（12）;
（13）；
式中，为时刻的状态协方差矩阵；为一个行1列的列向量，每个元素都是；为时刻采样点集合的第i个元素；为仅有一个采样点时的均值向量，显然；,为第i个采样点的均值向量，其计算公式为：
（14）；
式（9）到式（14）描述了如何从状态均值向量和协方差矩阵生成Sigma采样点的详细步骤，主要用于捕获预测状态分布的非线性特性，而后通过状态变换获得***状态的后验估计。具体公式为
（15）；
式中，为时刻***状态量的第个元素，是t-1时刻的***过程噪声；为状态变换函数；
对状态函数均值和方差进行加权处理，获得t时刻***状态量的加权均值和加权协方差矩阵：
（16）
（17）；
式中，为第i个Sigma点时刻由时刻状态更新后的***状态向量；为由加权均值组成的***状态量均值向量；为t时刻的过程噪声的协方差矩阵；
对状态函数均值和方差进行加权处理，获得***状态量的加权均值和加权协方差矩阵：
（3）观测量加权变换
对t时刻***状态量进行变换后，对第i个观测值进行加权变换，获得***观测均值向量，其中各元素的计算公式为：
（18）；
（4）***状态量更新
利用新息向量对状态量的均值矩阵和协方差矩阵进行更新：
（19）；
（20）；
（21）；
（22）；
（23）；
式中，为实际观测向量，通过计算机视觉技术对目标进行图像跟踪获得；为观测噪声；为观测量的加权协方差矩阵；为状态量和观测量的加权协方差矩阵；为时刻的卡尔曼增益矩阵；为更新后的***状态量的估计向量；为更新后的***状态量的估计协方差矩阵；
（5）计算动态残差：
（24）；
（25）；
（26）；
（27）；
（28） ；
式中，为***状态矩阵，由状态量估计均值向量和观测量矩阵组成；为***观测状态转移矩阵，由观测矩阵和单位矩阵组成，是非线性观测函数的雅可比矩阵；为***噪声矩阵，由过程噪声矩阵和***状态量的估计协方差矩阵组成；为中间变量；为t时刻的动态残差；
（6）鲁棒性处理
计算自适应比例因子
（29）
式中，为的等价权矩阵；i和j为比例因子的计算上限和下限；
为减小不准确的预设噪声对***的影响，利用抗差估计理论对状态量的估计值与估计协方差进行鲁棒性处理，得到t时刻的最终状态估计值向量：
（30）；
（31）；
式中，为t时刻经鲁棒性处理的最终状态估计值向量；为t时刻的最终***估计值协方差矩阵，经过鲁棒性处理的会在t+1时刻对状态估计值进行修正。其次，最终输出的***状态估计值可以通过迭代求得。
光电***目标定位实验
本实验无人机的飞行高度分别为750m和1500m，飞行轨迹规划示意图如图4所示。无人机在航点W1处开始稳定直线飞行，历经3次快速转弯后到达W4，完成对目标区域的连续多次成像观测，观测频率为30帧/秒。在飞行轨迹中设置了2个途经点，分别为W2和W3，当无人机在红色航迹进行快速转弯机动时，飞行工况会发生显著的改变。无人机光电***采样频率为5帧/秒，从航点W1飞行到航点W4共计用时35秒，7-9秒是无人机进行小范围转弯的时间范围。实验中不同航点无人机对目标区域的成像图像如图5所示。由图5可知，实验共在地面布置了7个目标，具***置关系如图5最后一幅图像所示。在光电***的观测过程中，摄像机始终确保所有目标点均处于观测范围之内。因此，从T1至T7，目标点相对于成像中心的距离逐渐增大。采用差分定位***对3个目标的GPS坐标进行精确测量，结果如表1所示。该***的测量误差标准差小于0.1m，因此测量结果可作为3个目标地理位置的真值使用。
表1 目标的真实地理位置
为了研究无人机飞行工况与所提算法目标定位精度之间的关系，选取飞行轨迹W1到W2和飞行轨迹W3到W4的观测数据进行结果对比分析，其飞行高度为750m。以目标T1为例，采用轨迹W1到W2得到的定位结果如图6所示，采用轨迹W3到W4得到的定位结果如图7所示。图6和图7中红色五角星为目标地理位置真值，黄色圆点为滤波初始值，黑色曲线为不同时间的地理位置测量值，渐变色曲线为不同时间的地理位置估计值，颜色由蓝色变换为红色代表采样时间的增大。
由图6和图7可知，ARUKF算法得到的距离滤波值随着时间逐渐趋近于真实值。其中，飞行轨迹W1到W2在机体坐标系下北方向的目标距离估计误差为7.37m，在东方向的目标距离估计误差为7.92m；飞行轨迹W3到W4在机体坐标系下北方向的目标距离估计误差为5.52m，在东方向的目标距离估计误差为3.72m。
对于飞行轨迹W1到W2，在7-9秒时无人机进行快速机动转弯，东方向的测量值发生小范围抖动，且之后距离滤波值略微偏离真实值。其原因是机动转弯导致无人机工况发生显著变化，加剧了光电***的非线性扰动，增大了目标位置的估计误差。但ARUKF算法依然能够在10s快速收敛至真实值，表明该算法能够快速且准确地跟踪观测噪声的变化，有效克服观测噪声的突变性，从而提高了光电平台的定位精度。由图6和图7可知，ARUKF算法得到的地位位置估计值同样不断趋近于真实值，验证了算法的可行性。对于飞行轨迹W3到W4，无人机全程稳定飞行，ARUKF算法能够在10s快速收敛至真实值，且定位精度更高。
为了研究所提算法进行多目标无源定位的性能优势，在条件设定为飞行高度750m、飞行轨迹W1到W2和实验次数20次的实验情景下，分别利用EKF算法、UKF算法、AEKF算法、PF算法和所提出的ARUKF算法，对所有目标的平均定位误差、定位均方根误差和每个采样周期内的平均运算时间进行计算，结果如表2所示。
表2 不同算法定位精度和运算时间
从表2中可以得出以下结论：（1）EKF因其无法自适应地调整噪声矩阵，导致其平均定位误差较大，介于46.32m至63.10m之间，表现出最低的定位精度；UKF通过采用无损变换处理非线性问题，在一定程度上提高了定位精度，但也无法有效克服时变噪声的影响；AEKF能够自适应地调整噪声矩阵，从而使得定位精度获得了进一步提升；PF和ARUKF在所有目标上的平均定位误差相差仅0.59m，二者定位精度总体相差不大；（2）ARUKF的计算时间为0.039ms，与AEKF相近，比EKF稍高，但远低于PF和UKF，其运算效率比PF提升了51%，这一结果与引言中的分析以及理论计算复杂度[32]相一致；（3）在无人机进行转弯机动的条件下，ARUKF在定位均方根误差上表现出较强的鲁棒性，误差范围在2.24米至6.21米之间，与PF的定位效果相当；（4）目标T5、T6和T7的定位误差相较于T1、T2、T3和T4普遍更大，其原因是目标T5、T6和T7距离成像中心更远，吊舱镜头的畸变导致定位误差更大。
针对无人机载光电***观测噪声的时变特性，本发明提出了一种自适应鲁棒卡尔曼滤波算法用于光电平台对目标地理位置的动态估计方法。通过实测飞行实验对算法进行多方面验证与测试，实验结果表明：
（1）机动转弯导致无人机工况发生显著变化，加剧了光电***的非线性扰动，增大了目标位置的估计误差。但ARUKF算法依然能够在10s快速收敛至真实值，表明该算法能够快速且准确地跟踪时变观测噪声的变化，有效克服观测噪声的突变性；
（2）在飞行轨迹相同的条件下，随着飞行高度的增大，距离测量值显示出更加显著的波动性，同时算法进行目标定位的误差亦随之增加；
（3）由5种不同算法精度、鲁棒性以及效率的对比分析可知，所提算法在精度和稳定性方面可达到PF的效果，而效率比PF提升了51%。
以上所述为本发明最佳实施方式的举例，其中未详细述及的部分均为本领域普通技术人员的公知常识。本发明的保护范围以权利要求的内容为准，任何基于本发明的技术启示而进行的等效变换，也在本发明的保护范围之内。
Claims (5)
1.基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法，其特征在于,包括以下步骤：
步骤1、确定目标点在大地坐标系中的初始位置，并确定坐标***状态向量初值和误差协方差矩阵初值；
步骤2、根据上一时刻目标的位置先验值，对目标进行实时定位更新，求解t时刻目标的状态估计值向量时，对t-1时刻的***状态向量中的元素及其均值和协方差采样，生成n+1个采样点集合；
步骤3、通过状态变换获得t-1时刻***状态的后验估计，具体公式为：
；
式中，为第i个采样点时刻由时刻状态更新后的***状态向量，是t-1时刻的***过程噪声；为状态变换函数；
对状态函数均值和方差进行加权处理，获得t时刻***状态量的加权均值和加权协方差矩阵：
；
式中，为由加权均值组成的***状态量均值向量；为t时刻的过程噪声的协方差矩阵，为第i个采样点的权重；
步骤4、对t时刻***状态量进行变换后，对第i个采样点的观测值进行加权变换，获得***观测均值向量，其中各元素的计算公式为：
；
其中，为 非线性观测函数；
步骤5、利用新息向量对状态量的均值矩阵和协方差矩阵进行更新：
；
；
；
；
；
式中，为实际观测向量，计算机视觉技术对目标进行图像跟踪获得；为观测噪声；为观测量的加权协方差矩阵；为状态量和观测量的加权协方差矩阵；为时刻的卡尔曼增益矩阵；为更新后的***状态量的估计向量；为更新后的***状态量的估计协方差矩阵；
步骤6、计算动态残差：
；
；
；
；
；
式中，为***状态矩阵，由状态量估计均值向量和观测量矩阵组成；为***观测状态转移矩阵，由观测矩阵和单位矩阵组成，是非线性观测函数的雅可比矩阵；为***噪声矩阵，由过程噪声矩阵和***状态量的估计协方差矩阵组成；为中间变量；为t时刻的动态残差；
步骤7、鲁棒性处理：
计算自适应比例因子
式中，为的等价权矩阵；i和j为比例因子的计算上限和下限；
利用抗差估计理论对状态量的估计值与估计协方差进行鲁棒性处理，得到t时刻的最终状态估计值向量：
；
；
式中，为t时刻经鲁棒性处理的最终状态估计值向量，能用于下一时刻的目标的***状态迭代估计；为t时刻的最终***估计值协方差矩阵，经过鲁棒性处理的会在时刻对状态估计值进行修正。
2.根据权利要求1所述的基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法，其特征在于，步骤1中，
式中，、、分别为光电平台***在L坐标、B坐标、H坐标的初始方差，、和分别为目标的初始经度、纬度和大地高。
3.根据权利要求1所述的基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法，其特征在于，步骤2中的具体采样过程如下：
式中，为采样点初始权重；为缩放参数，根据计算得到，用于控制采样点的分散程度；n为采样点数量；为一个n行n列的矩阵，每个元素都是1；为n维单位矩阵；是通过和计算得到的一个矩阵，用于调整采样点的分散；为对角元素，是权重的对角矩阵，用于采样点的更新；为时刻的状态协方差矩阵；为一个行1列的列向量，每个元素都是；为时刻采样点集合的第i个元素；为仅有一个采样点时的均值向量，显然；,为第i个采样点的均值向量，其计算公式为：
。
4.根据权利要求1所述的基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法，其特征在于，步骤3中状态变换函数的公式为：
；
式中： 为t-1时刻到t时刻的状态转移矩阵；为t-1时刻到t时刻的噪声驱动矩阵；是t-1时刻的***过程噪声。
5.根据权利要求1所述的基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法，其特征在于, h(·)为光电平台观测的非线性函数，h(x)的具体公式为：
x为目标点在GCF坐标系下的坐标,设x=[ L,B,H]T，L为经度信息，B为纬度信息，H为高程信息，则目标点在C坐标系中的坐标为[ x c ,y c ,z c ]T，则：
（x c ,y c ,z c ）的计算公式为：
；
式中，u’是指图像跟踪仪所测目标在像素坐标系x方向的坐标，v’是指图像跟踪仪所测目标在像素坐标系y方向的坐标，
c x 为像素平面中心在x方向的坐标，c y 为像素平面中心在y方向的坐标；等效焦距、分别为摄像机焦距与像元的横、纵尺寸、之比； 为HRD坐标系到C坐标系的坐标变换矩阵，为NED坐标系到HRD坐标系的坐标变换矩阵；为ECEF坐标系到NED坐标系的坐标变换矩阵; 为地球椭球子午椭圆的第一偏心率；为卯酉圈的曲率半径，其中为椭球长半轴，为椭球短半轴，单位均为米。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410403488.6A CN117990112B (zh) | 2024-04-03 | 2024-04-03 | 基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410403488.6A CN117990112B (zh) | 2024-04-03 | 2024-04-03 | 基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117990112A CN117990112A (zh) | 2024-05-07 |
| CN117990112B true CN117990112B (zh) | 2024-07-02 |
Family
ID=90896388
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410403488.6A Active CN117990112B (zh) | 2024-04-03 | 2024-04-03 | 基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117990112B (zh) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109990786A (zh) * | 2019-02-28 | 2019-07-09 | 深圳大学 | 机动目标跟踪方法及装置 |
| CN110196443A (zh) * | 2019-06-06 | 2019-09-03 | 中国人民解放军战略支援部队信息工程大学 | 一种飞行器的容错组合导航方法及*** |
Family Cites Families (14)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| FR2961897B1 (fr) * | 2010-06-25 | 2012-07-13 | Thales Sa | Filtre de navigation pour un systeme de navigation par correlation de terrain |
| JP2013061309A (ja) * | 2011-09-15 | 2013-04-04 | Yamaha Corp | カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム |
| JP2015219714A (ja) * | 2014-05-16 | 2015-12-07 | 日本電信電話株式会社 | 状態推定装置、方法、及びプログラム |
| US20200132775A1 (en) * | 2017-06-14 | 2020-04-30 | Mitsubishi Electric Corporation | State estimation device |
| CN110728716B (zh) * | 2019-09-04 | 2023-11-17 | 深圳市道通智能航空技术股份有限公司 | 一种标定方法、装置及飞行器 |
| CN111896008A (zh) * | 2020-08-20 | 2020-11-06 | 哈尔滨工程大学 | 一种改进的鲁棒无迹卡尔曼滤波组合导航方法 |
| US20220065587A1 (en) * | 2020-08-31 | 2022-03-03 | Raytheon Company | System and method of hypersonic object tracking |
| WO2022126397A1 (zh) * | 2020-12-15 | 2022-06-23 | 深圳市大疆创新科技有限公司 | 传感器的数据融合方法、设备及存储介质 |
| CN112683274A (zh) * | 2020-12-22 | 2021-04-20 | 西安因诺航空科技有限公司 | 一种基于无迹卡尔曼滤波的无人机组合导航方法和*** |
| CN113048984B (zh) * | 2021-04-01 | 2023-10-03 | 江苏科技大学 | 一种用于水下无人机器人集群的动态定位信息融合方法 |
| CN114609912B (zh) * | 2022-03-18 | 2023-10-03 | 电子科技大学 | 基于伪线性最大相关熵卡尔曼滤波的仅测角目标追踪方法 |
| CN115981148A (zh) * | 2022-12-07 | 2023-04-18 | 哈尔滨工业大学 | 无人机地面移动目标跟踪方法 |
| CN116520380A (zh) * | 2023-04-28 | 2023-08-01 | 南京邮电大学 | 一种基于自适应卡尔曼滤波的动态目标组合定位方法 |
| CN117455960B (zh) * | 2023-12-22 | 2024-03-08 | 中国人民解放军海军工程大学 | 时变观测噪声条件下机载光电***对地无源定位滤波方法 |
-
2024
- 2024-04-03 CN CN202410403488.6A patent/CN117990112B/zh active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109990786A (zh) * | 2019-02-28 | 2019-07-09 | 深圳大学 | 机动目标跟踪方法及装置 |
| CN110196443A (zh) * | 2019-06-06 | 2019-09-03 | 中国人民解放军战略支援部队信息工程大学 | 一种飞行器的容错组合导航方法及*** |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117990112A (zh) | 2024-05-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110243358B (zh) | 多源融合的无人车室内外定位方法及*** | |
| AU2012328156B2 (en) | Identification and analysis of aircraft landing sites | |
| Kong et al. | Autonomous landing of an UAV with a ground-based actuated infrared stereo vision system | |
| CN107192375B (zh) | 一种基于航拍姿态的无人机多帧图像自适应定位校正方法 | |
| CN117455960B (zh) | 时变观测噪声条件下机载光电***对地无源定位滤波方法 | |
| CN111238469B (zh) | 一种基于惯性/数据链的无人机编队相对导航方法 | |
| Goppert et al. | Invariant Kalman filter application to optical flow based visual odometry for UAVs | |
| CN112947569A (zh) | 基于预设性能四旋翼无人机视觉伺服目标跟踪控制方法 | |
| CN117308938B (zh) | 一种基于多次激光测距的惯导寻北收敛误差快速补偿方法 | |
| Mostafa et al. | Optical flow based approach for vision aided inertial navigation using regression trees | |
| CN117990112B (zh) | 基于鲁棒无迹卡尔曼滤波的无人机光电平台目标定位方法 | |
| CN115618749B (zh) | 一种大型无人机实时定位的误差补偿方法 | |
| CN112268558B (zh) | 一种基于无人机自主式光电平台的有源目标定位测速方法 | |
| CN115388890A (zh) | 基于视觉的多无人机协同对地目标定位方法 | |
| CN115525062A (zh) | 面向返回式航天器测量任务无人机实时在线航迹规划方法 | |
| CN113189585A (zh) | 基于无人机双基地sar***的运动误差补偿算法 | |
| Popov et al. | UAV navigation on the basis of video sequences registered by onboard camera | |
| Duo et al. | Uav autonomous navigation system for gnss invalidation | |
| Ishii et al. | Autonomous UAV flight using the Total Station Navigation System in Non-GNSS Environments | |
| CN114322943B (zh) | 一种基于无人机前视图像的目标距离测量方法及装置 | |
| Wu et al. | Integrated navigation algorithm based on vision-inertial extended Kalman filter for low-cost unmanned aerial vehicle | |
| Han | Optical flow/ins navigation system in four-rotor | |
| Zahedi et al. | Neural network based approaches to mobile target localization and tracking using unmanned aerial vehicles | |
| Zhang et al. | Robust Autonomous Navigation Method for High-Precision UAV Based on Inertial/Machine Vision Fusion | |
| Jeong et al. | Interacting Multiple Model Filter Based Autonomous Landing Considering Camera Model Uncertainty |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant |