基于系统噪声估计的光电吊舱自适应EKF目标定位方法与流程

基于系统噪声估计的光电吊舱自适应ekf目标定位方法
技术领域
1.本发明涉及基于系统噪声估计的光电吊舱自适应ekf目标定位方法，属于机载光电吊舱目标定位技术领域。


背景技术：

2.机载光电吊舱集可见光摄像机、红外热像仪和激光测距仪等设备于一体，是无人机装备的重要任务载荷。随着机载光电吊舱向多功能、智能化方向发展，其被广泛应用于海上搜救、目标侦察监视、战场态势评估等一系列领域，其中，地面关键目标的高精度定位，对野外救援、激光制导打击等具有重要意义。
3.目前，基于激光测距和惯组姿态角度的有源定位方法精度较高，将卡尔曼滤波及其扩展算法用于光电吊舱的目标定位之中，能以线性最小方差状态估计方法给出地面目标的最优估计坐标。然而，对于在卡尔曼滤波器应用过程中有关滤波器参数的设定，如系统噪声方差与测量噪声方差，将对滤波结果产生巨大影响。
4.一般测量噪声取决于传感器自身的测量误差，可从传感器性能手册中获取测量噪声方差的数据；而对于系统噪声而言，常常根据经验来选取，具有一定的随机性。不同的系统噪声方差取值对滤波结果影响较大。在实际机载光电吊舱对地面目标进行激光测距定位过程中，系统噪声会受到目标测距距离、光轴稳定性、图像目标跟踪精度的影响，存在时变的特性，现有遗忘因子、期望最大化和遗传迭代等方法，虽能实现对系统噪声方差的调整，但这些方法计算过程均繁琐费时，无法满足实际应用中的实时性要求。


技术实现要素：

5.本发明解决的技术问题是：克服现有技术的不足，提出基于系统噪声估计的光电吊舱自适应ekf目标定位方法，实现目标运动状态准确估计，用于无人机目标侦查、机载光电吊舱视觉伺服跟踪。
6.本发明解决技术的方案是：
7.基于系统噪声估计的光电吊舱自适应ekf目标定位方法，包括：
8.无人机挂载光电吊舱进行巡航飞行，光电吊舱的基座上安装惯导，通过地面站操作光电吊舱，使视场中心锁定地面目标，进行激光测距获得光电吊舱与地面目标之间的距离l；
9.根据光电吊舱图像跟踪目标时的水平脱靶量、垂直脱靶量与光电吊舱与地面目标之间的距离l，计算目标定位模型的系统噪声q；
10.建立目标定位的状态方程与观测方程，根据光电吊舱自身的经纬度坐标、光电吊舱与地面目标之间的距离l与系统噪声q，采用ekf滤波算法计算地面目标的地理坐标值，用于对光电吊舱进行目标跟踪。
11.进一步的，通过地面站操作光电吊舱的方法包括：
12.通过地面站摇杆搜寻并锁定地面目标，使吊舱处于稳定的目标图像跟踪状态；吊
舱激光测距仪上电，对图像锁定的地面目标进行激光测距，记录光电吊舱与地面目标之间的距离l、吊舱基座惯组的gps坐标以及图像跟踪的水平、垂直脱靶量。
13.进一步的，目标定位模型中的系统噪声q的计算方法包括：
14.基于载荷视轴与地面目标的偏差角度、光电吊舱与地面目标之间的距离，计算第k时刻，激光测距点偏离实际目标的距离为：
[0015][0016]
其中lk为k时刻光电吊舱与地面目标之间的距离，μ
xk
为视轴水平偏差角，μ
yk
为视轴垂直偏差角；
[0017]
统计计算一段时间内偏离目标距离[δγ1，δγ2...δγk]的方差值，即可得到目标定位模型中的系统噪声q。
[0018]
进一步的，在跟踪地面目标的过程中，采样点以滑窗的形式随时间后移更新，进而重新统计与计算滑窗内的偏离目标距离数据，实时更新相应系统噪声方差。
[0019]
进一步的，通过k时刻查询可见光相机焦距值，获得当前图像水平视场角为κk，垂直视场角为λk，图像的水平与垂直方向的总像素数分别为m、n，根据水平脱靶量mk与垂直脱靶量nk计算载荷视轴偏离目标的角度为：
[0020][0021]
进一步的，建立目标定位的观测方程的方法包括：
[0022]
在k时刻获取目标相对于光电吊舱的方位姿态角α、俯仰姿态角β，以无人机吊舱为原点，目标在机体坐标系下的位置为(x
bk
,y
bk
,z
bk
)，以α，β，l的测量值为观测量，建立目标定位系统的观测方程为：
[0023][0024]
进一步的，所建立的状态与观测方程具有非线性特性，采用扩展卡尔曼滤波算法对目标位置进行估计，将h(xk)在处进行一阶taylor级数展开，省略二阶及以上项，得到线性化后的观测方程为：
[0025][0026]
其中，υk为第k时刻的观测噪声，为线性化后的观测矩阵。
[0027]
进一步的，基于线性化后的状态与观测方程，具体迭代过程如下：
[0028]
状态一步预测：
[0029][0030]
一步预测均方误差：
[0031][0032]
滤波增益更新：
[0033][0034]
状态估计更新：
[0035][0036]
状态估计误差更新：
[0037][0038]
其中，φ
k|k-1
表示t
k-1
时刻系统的一步转移矩阵，γ
k|k-1
表示t
k-1
时刻系统的噪声驱动矩阵，hk表示线性化后的观测矩阵，q
k-1
表示t
k-1
时刻的系统噪声方差矩阵，rk表示tk时刻的系统的噪声驱动矩阵。
[0039]
进一步的，光电吊舱在进行目标定位时，采集到的吊舱基座经纬度坐标为(n0,w0,h0)，按照下式计算地面目标的经度、纬度、高程坐标(n
tar
,w
tar
,h
tar
)为：
[0040][0041][0042][0043]
式中，re为地球半径。
[0044]
进一步的，状态方程的表达式为：
[0045]
xk＝φx
k-1
+w
k-1
[0046]
其中，xk＝[xk,yk,zk]
t
为tk时刻的目标状态，φ为单位矩阵，w
k-1
为系统噪声。
[0047]
基于系统噪声估计的光电吊舱自适应ekf目标定位系统，包括：
[0048]
光电吊舱与地面目标之间距离获得模块：无人机挂载光电吊舱进行巡航飞行，光电吊舱的基座上安装惯导，通过地面站操作光电吊舱，使视场中心锁定地面目标，进行激光测距获得光电吊舱与地面目标之间的距离l；
[0049]
目标定位模型的系统噪声确定模块：根据光电吊舱图像跟踪目标时的水平脱靶量、垂直脱靶量与光电吊舱与地面目标之间的距离l，计算目标定位模型的系统噪声q；
[0050]
地面目标地理坐标值确定模块：建立目标定位的状态与观测方程，根据光电吊舱自身的经纬度坐标、光电吊舱与地面目标之间的距离l与系统噪声q，采用ekf滤波算法计算地面目标的地理坐标值，用于对光电吊舱进行目标跟踪。
[0051]
本发明与现有技术相比的有益效果是：
[0052]
(1)对于目前ekf滤波器应用过程中的系统噪声方差，一般是通过经验选取的固定数值，这在一定程度上存在随机性，不同的选取值会对ekf的滤波结果产生巨大影响；同时，
在实际机载光电吊舱对地面目标进行激光测距定位过程中，系统噪声会受到目标测距距离、光轴稳定性、图像目标跟踪精度的影响，是动态变化的过程。采用本发明方法能够始终保持系统噪声统计量自适应的逼近真实值，减少传统卡尔曼滤波算法中，因系统噪声选用常值可能导致结果发散的风险，从而实现机载光电吊舱对地面目标的高精度定位；
[0053]
(2)现有遗忘因子、期望最大化和遗传算法等加入扩展卡尔曼滤波模型中，虽能实现对系统噪声方差的调整，但这些方法计算过程均繁琐费时，无法满足实际应用中的实时性要求。本发明所提出的系统噪声估计算法，仅需激光测距值与图像跟踪脱靶量进行统计计算，运算量小，便于部署在嵌入式系统中，具有较高的实用性。
附图说明
[0054]
图1为本发明机载光电吊舱与地面目标激光测距“测点”相对位置示意图；
[0055]
图2本发明方法的流程图。
具体实施方式
[0056]
下面结合实施例对本发明作进一步阐述。
[0057]
本发明实施的光电吊舱在其基座内安装有惯组传感器。基于系统噪声估计的光电吊舱自适应ekf目标定位方法，包括以下步骤，如图1、2所示：
[0058]
步骤1、无人机在起飞前，需对吊舱上电完成初始化。
[0059]
(1)安装于光电吊舱基座的惯组需完成上电对准初始化。同时，光电吊舱伺服系统俯仰轴与方位轴旋转，转动至框架角度的零位。
[0060]
(2)无人机巡航飞行至地面目标区域，通过地面站操作台摇杆，将图像视场中“锁定窗”对准地面目标，使吊舱进入视觉伺服跟踪状态。当图像“锁定窗”稳定的跟踪上目标后，吊舱激光测距仪上电，对图像锁定的地面目标进行激光测距，记录一段时间的激光测距值l，吊舱基座惯组的gps坐标以及图像跟踪的水平、垂直脱靶量x和y。
[0061]
步骤2、根据光电吊舱图像跟踪目标时的水平脱靶量x、垂直脱靶量y与激光测距值l，统计计算这一时间段内的目标定位模型的系统噪声q。
[0062]
(1)在k时刻时，采样得到水平脱靶量xk、垂直脱靶量yk与激光测距值lk，通过查询可见光相机焦距值，可知当前图像水平视场角为κk，垂直视场角为λk，图像的水平与垂直方向的总像素数分别为m、n。则根据水平脱靶量mk与垂直脱靶量nk计算载荷视轴偏离目标的角度为：
[0063][0064][0065]
式中，μ
xk
为视轴水平偏差角，μ
yk
为视轴垂直偏差角。
[0066]
(2)根据载荷视轴与地面目标的偏差角度值与激光测距值，由于视轴偏离实际目标的角度很小，一般为微弧量级，即可计算第k时刻，激光测距点偏离实际目标的距离为：
[0067]
[0068]
统计计算一段时间内偏离目标距离[δγ1，δγ2...δγk]的方差值，即可得到目标定位模型中的系统噪声qk。在跟踪地面目标的过程中，采样点将以滑窗的形式随时间后移更新，进而重新统计与计算滑窗内的偏离目标距离数据，实时更新相应系统噪声方差。
[0069]
步骤3、建立目标定位的状态方程与观测方程，根据光电吊舱自身的经纬度坐标、激光测距值l与系统噪声q，采用ekf滤波算法计算地面目标的地理坐标值。
[0070]
(1)选取目标点在大地直角坐标系下的位置为系统状态变量，建立目标定位系统的状态方程与观测方程。
[0071]
状态方程的表达式为
[0072]
xk＝φx
k-1
+w
k-1
[0073]
其中，xk＝[xk,yk,zk]
t
为tk时刻的目标状态，φ为单位矩阵，w
k-1
为系统噪声。
[0074]
机载光电成像系统对固定目标进行多次测量，在k时刻可获取目标相对于光电平台的方位姿态角α、俯仰姿态角β。目标在机体坐标系下的位置为(x
bk
,y
bk
,z
bk
)。以α，β，l的测量值为观测量，则观测方程为：
[0075]
zk＝h(xk)+υk[0076]
其中，vk为测量噪声，是均值为0的高斯白噪声。
[0077][0078]
(2)根据系统的状态和观测方程，无人机在飞行运动中采用光电吊舱对地面目标进行激光测距，进而完成定位计算。所建立的系统观测方程具有非线性特性，为此采用扩展卡尔曼滤波算法对目标位置进行估计。因此，将h(xk)在处进行一阶taylor级数展开，省略二阶及以上项，得到线性化后的观测方程为：
[0079][0080]
其中，其中，为线性化后的观测矩阵，表达式为jacobian矩阵。
[0081]
基于线性化后的观测方程和状态方程，具体迭代过程如下：
[0082]
状态一步预测：
[0083][0084]
一步预测均方误差：
[0085][0086]
滤波增益更新：
[0087][0088]
状态估计更新：
[0089][0090]
状态估计误差更新：
[0091][0092]
其中，φ
k|k-1
表示t
k-1
时刻系统的一步转移矩阵，γ
k|k-1
表示t
k-1
时刻系统的噪声驱动矩阵，q
k-1
表示t
k-1
时刻的系统噪声方差矩阵，rk表示tk时刻的系统的噪声驱动矩阵。
[0093]
(3)光电吊舱在进行目标定位时，采集到的吊舱基座经纬度坐标为(n0,w0,h0)，按照下式计算地面目标的经度、纬度、高程坐标(n
tar
,w
tar
,h
tar
)为：
[0094][0095][0096][0097]
式中，re为地球半径。
[0098]
基于系统噪声估计的光电吊舱自适应ekf目标定位系统，其特征在于，包括：
[0099]
光电吊舱与地面目标之间距离获得模块：无人机挂载光电吊舱进行巡航飞行，光电吊舱的基座上安装惯导，通过地面站操作光电吊舱，使视场中心锁定地面目标，进行激光测距获得光电吊舱与地面目标之间的距离l；
[0100]
目标定位模型的系统噪声确定模块：根据光电吊舱图像跟踪目标时的水平脱靶量x、垂直脱靶量y与光电吊舱与地面目标之间的距离l，计算目标定位模型的系统噪声q；
[0101]
地面目标地理坐标值确定模块：建立目标定位的状态与观测方程，根据光电吊舱自身的经纬度坐标、光电吊舱与地面目标之间的距离l与系统噪声q，采用ekf滤波算法计算地面目标的地理坐标值。
[0102]
本发明提出在对地面目标进行定位时，采用激光测距值与图像脱靶量相结合的方式，在一段时间内对扩展卡尔曼滤波算法(ekf)中的系统噪声q进行统计估计。传统ekf滤波算法始终采用固定不变的系统噪声经验值，会对滤波器的输出结果带来较大误差，甚至导致滤波器发散。而采用本方法估算目标定位中的系统噪声q，使得ekf自适应选取逼近真实的滤波参数，参与ekf滤波后能够输出较高的地面目标定位精度，且在嵌入式系统中计算量小，具有较高的实用性。
[0103]
对于目前ekf滤波器应用过程中的系统噪声方差，一般是通过经验选取的固定数值，这在一定程度上存在随机性，不同的选取值会对ekf的滤波结果产生巨大影响；同时，在实际机载光电吊舱对地面目标进行激光测距定位过程中，系统噪声会受到目标测距距离、光轴稳定性、图像目标跟踪精度的影响，是动态变化的过程。采用本发明方法能够始终保持系统噪声统计量自适应的逼近真实值，减少传统卡尔曼滤波算法中，因系统噪声选用常值可能导致结果发散的风险，从而实现机载光电吊舱对地面目标的高精度定位。
[0104]
本发明虽然已以较佳实施例公开如上，但其并不是用来限定本发明，任何本领域技术人员在不脱离本发明的精神和范围内，都可以利用上述揭示的方法和技术内容对本发明技术方案做出可能的变动和修改，因此，凡是未脱离本发明技术方案的内容，依据本发明
的技术实质对以上实施例所作的任何简单修改、等同变化及修饰，均属于本发明技术方案的保护范围。

技术特征：
1.基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，包括：无人机挂载光电吊舱进行巡航飞行，光电吊舱的基座上安装惯导，通过地面站操作光电吊舱，使视场中心锁定地面目标，进行激光测距获得光电吊舱与地面目标之间的距离l；根据光电吊舱图像跟踪目标时的水平脱靶量、垂直脱靶量与光电吊舱与地面目标之间的距离l，计算目标定位模型的系统噪声q；建立目标定位的状态方程与观测方程，根据光电吊舱自身的经纬度坐标、光电吊舱与地面目标之间的距离l与系统噪声q，采用ekf滤波算法计算地面目标的地理坐标值，用于对光电吊舱进行目标跟踪。2.根据权利要求1所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，通过地面站操作光电吊舱的方法包括：通过地面站摇杆搜寻并锁定地面目标，使吊舱处于稳定的目标图像跟踪状态；吊舱激光测距仪上电，对图像锁定的地面目标进行激光测距，记录光电吊舱与地面目标之间的距离l、吊舱基座惯组的gps坐标以及图像跟踪的水平、垂直脱靶量。3.根据权利要求1所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，目标定位模型中的系统噪声q的计算方法包括：基于载荷视轴与地面目标的偏差角度、光电吊舱与地面目标之间的距离，计算第k时刻，激光测距点偏离实际目标的距离为：其中l
k
为k时刻光电吊舱与地面目标之间的距离，μ
xk
为视轴水平偏差角，μ
yk
为视轴垂直偏差角；统计计算一段时间内偏离目标距离[δγ1，δγ2...δγ
k
]的方差值，即可得到目标定位模型中的系统噪声q。4.根据权利要求3所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，在跟踪地面目标的过程中，采样点以滑窗的形式随时间后移更新，进而重新统计与计算滑窗内的偏离目标距离数据，实时更新相应系统噪声方差。5.根据权利要求3所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，通过k时刻查询可见光相机焦距值，获得当前图像水平视场角为κ
k
，垂直视场角为λ
k
，图像的水平与垂直方向的总像素数分别为m、n，根据水平脱靶量m
k
与垂直脱靶量n
k
计算载荷视轴偏离目标的角度为：6.根据权利要求1所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，建立目标定位的观测方程的方法包括：在k时刻获取目标相对于光电吊舱的方位姿态角α、俯仰姿态角β，以无人机吊舱为原点，目标在机体坐标系下的位置为(x
bk
,y
bk
,z
bk
)，以α，β，l的测量值为观测量，建立目标定位系统的观测方程为：
7.根据权利要求6所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，所建立的状态与观测方程具有非线性特性，采用扩展卡尔曼滤波算法对目标位置进行估计，将h(x
k
)在处进行一阶taylor级数展开，省略二阶及以上项，得到线性化后的观测方程为：其中，υ
k
为第k时刻的观测噪声，为线性化后的观测矩阵。8.根据权利要求7所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，基于线性化后的状态与观测方程，具体迭代过程如下：状态一步预测：一步预测均方误差：滤波增益更新：状态估计更新：状态估计误差更新：其中，φ
k|k-1
表示t
k-1
时刻系统的一步转移矩阵，γ
k|k-1
表示t
k-1
时刻系统的噪声驱动矩阵，h
k
表示线性化后的观测矩阵，q
k-1
表示t
k-1
时刻的系统噪声方差矩阵，r
k
表示t
k
时刻的系统的噪声驱动矩阵。9.根据权利要求1所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，光电吊舱在进行目标定位时，采集到的吊舱基座经纬度坐标为(n0,w0,h0)，按照下式计算地面目标的经度、纬度、高程坐标(n
tar
,w
tar
,h
tar
)为：
式中，re为地球半径。10.根据权利要求1所述的基于系统噪声估计的光电吊舱自适应ekf目标定位方法，其特征在于，状态方程的表达式为：x
k
＝φx
k-1
+w
k-1
其中，x
k
＝[x
k
,y
k
,z
k
]
t
为t
k
时刻的目标状态，φ为单位矩阵，w
k-1
为系统噪声。11.基于系统噪声估计的光电吊舱自适应ekf目标定位系统，其特征在于，包括：光电吊舱与地面目标之间距离获得模块：无人机挂载光电吊舱进行巡航飞行，光电吊舱的基座上安装惯导，通过地面站操作光电吊舱，使视场中心锁定地面目标，进行激光测距获得光电吊舱与地面目标之间的距离l；目标定位模型的系统噪声确定模块：根据光电吊舱图像跟踪目标时的水平脱靶量、垂直脱靶量与光电吊舱与地面目标之间的距离l，计算目标定位模型的系统噪声q；地面目标地理坐标值确定模块：建立目标定位的状态与观测方程，根据光电吊舱自身的经纬度坐标、光电吊舱与地面目标之间的距离l与系统噪声q，采用ekf滤波算法计算地面目标的地理坐标值，用于对光电吊舱进行目标跟踪。

技术总结
本发明涉及基于系统噪声估计的光电吊舱自适应EKF目标定位方法，包括：无人机挂载光电吊舱进行巡航飞行，光电吊舱的基座上安装惯导，通过地面站操作光电吊舱，使视场中心锁定地面目标，进行激光测距获得光电吊舱与地面目标之间的距离L；根据光电吊舱图像跟踪目标时的水平脱靶量、垂直脱靶量与光电吊舱与地面目标之间的距离L，计算目标定位模型的系统噪声Q；建立目标定位的状态方程与观测方程，根据光电吊舱自身的经纬度坐标、光电吊舱与地面目标之间的距离L与系统噪声Q，采用EKF滤波算法计算地面目标的地理坐标值，用于对光电吊舱进行目标跟踪。本发明实现了对目标运动状态准确估计，用于无人机目标侦查、机载光电吊舱视觉伺服跟踪。服跟踪。服跟踪。


技术研发人员：赵丰 郭镇净 孙洁洁 袁鑫 卢锦明 平铎
受保护的技术使用者：北京航天控制仪器研究所
技术研发日：2023.04.25
技术公布日：2023/8/13