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

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

背景技术：

1、机载光电吊舱集可见光摄像机、红外热像仪和激光测距仪等设备于一体，是无人机装备的重要任务载荷。随着机载光电吊舱向多功能、智能化方向发展，其被广泛应用于海上搜救、目标侦察监视、战场态势评估等一系列领域，其中，地面关键目标的高精度定位，对野外救援、激光制导打击等具有重要意义。

2、目前，基于激光测距和惯组姿态角度的有源定位方法精度较高，将卡尔曼滤波及其扩展算法用于光电吊舱的目标定位之中，能以线性最小方差状态估计方法给出地面目标的最优估计坐标。然而，对于在卡尔曼滤波器应用过程中有关滤波器参数的设定，如系统噪声方差与测量噪声方差，将对滤波结果产生巨大影响。

3、一般测量噪声取决于传感器自身的测量误差，可从传感器性能手册中获取测量噪声方差的数据；而对于系统噪声而言，常常根据经验来选取，具有一定的随机性。不同的系统噪声方差取值对滤波结果影响较大。在实际机载光电吊舱对地面目标进行激光测距定位过程中，系统噪声会受到目标测距距离、光轴稳定性、图像目标跟踪精度的影响，存在时变的特性，现有遗忘因子、期望最大化和遗传迭代等方法，虽能实现对系统噪声方差的调整，但这些方法计算过程均繁琐费时，无法满足实际应用中的实时性要求。

技术实现思路

1、本发明解决的技术问题是：克服现有技术的不足，提出基于系统噪声估计的光电吊舱自适应ekf目标定位方法，实现目标运动状态准确估计，用于无人机目标侦查、机载光电吊舱视觉伺服跟踪。

2、本发明解决技术的方案是：

3、基于系统噪声估计的光电吊舱自适应ekf目标定位方法，包括：

4、无人机挂载光电吊舱进行巡航飞行，光电吊舱的基座上安装惯导，通过地面站操作光电吊舱，使视场中心锁定地面目标，进行激光测距获得光电吊舱与地面目标之间的距离l；

5、根据光电吊舱图像跟踪目标时的水平脱靶量、垂直脱靶量与光电吊舱与地面目标之间的距离l，计算目标定位模型的系统噪声q；

6、建立目标定位的状态方程与观测方程，根据光电吊舱自身的经纬度坐标、光电吊舱与地面目标之间的距离l与系统噪声q，采用ekf滤波算法计算地面目标的地理坐标值，用于对光电吊舱进行目标跟踪。

7、进一步的，通过地面站操作光电吊舱的方法包括：

8、通过地面站摇杆搜寻并锁定地面目标，使吊舱处于稳定的目标图像跟踪状态；吊舱激光测距仪上电，对图像锁定的地面目标进行激光测距，记录光电吊舱与地面目标之间的距离l、吊舱基座惯组的gps坐标以及图像跟踪的水平、垂直脱靶量。

9、进一步的，目标定位模型中的系统噪声q的计算方法包括：

10、基于载荷视轴与地面目标的偏差角度、光电吊舱与地面目标之间的距离，计算第k时刻，激光测距点偏离实际目标的距离为：

11、

12、其中lk为k时刻光电吊舱与地面目标之间的距离，μxk为视轴水平偏差角，μyk为视轴垂直偏差角；

13、统计计算一段时间内偏离目标距离[δγ1，δγ2...δγk]的方差值，即可得到目标定位模型中的系统噪声q。

14、进一步的，在跟踪地面目标的过程中，采样点以滑窗的形式随时间后移更新，进而重新统计与计算滑窗内的偏离目标距离数据，实时更新相应系统噪声方差。

15、进一步的，通过k时刻查询可见光相机焦距值，获得当前图像水平视场角为κk，垂直视场角为λk，图像的水平与垂直方向的总像素数分别为m、n，根据水平脱靶量mk与垂直脱靶量nk计算载荷视轴偏离目标的角度为：

16、

17、进一步的，建立目标定位的观测方程的方法包括：

18、在k时刻获取目标相对于光电吊舱的方位姿态角α、俯仰姿态角β，以无人机吊舱为原点，目标在机体坐标系下的位置为(xbk,ybk,zbk)，以α，β，l的测量值为观测量，建立目标定位系统的观测方程为：

19、

20、进一步的，所建立的状态与观测方程具有非线性特性，采用扩展卡尔曼滤波算法对目标位置进行估计，将h(xk)在处进行一阶taylor级数展开，省略二阶及以上项，得到线性化后的观测方程为：

21、

22、其中，υk为第k时刻的观测噪声，为线性化后的观测矩阵。

23、进一步的，基于线性化后的状态与观测方程，具体迭代过程如下：

24、状态一步预测：

25、

26、一步预测均方误差：

27、

28、滤波增益更新：

29、

30、状态估计更新：

31、

32、状态估计误差更新：

33、

34、其中，φk|k-1表示tk-1时刻系统的一步转移矩阵，γk|k-1表示tk-1时刻系统的噪声驱动矩阵，hk表示线性化后的观测矩阵，qk-1表示tk-1时刻的系统噪声方差矩阵，rk表示tk时刻的系统的噪声驱动矩阵。

35、进一步的，光电吊舱在进行目标定位时，采集到的吊舱基座经纬度坐标为(n0,w0,h0)，按照下式计算地面目标的经度、纬度、高程坐标(ntar,wtar,htar)为：

36、

37、

38、

39、式中，re为地球半径。

40、进一步的，状态方程的表达式为：

41、xk＝φxk-1+wk-1

42、其中，xk＝[xk,yk,zk]t为tk时刻的目标状态，φ为单位矩阵，wk-1为系统噪声。

43、基于系统噪声估计的光电吊舱自适应ekf目标定位系统，包括：

44、光电吊舱与地面目标之间距离获得模块：无人机挂载光电吊舱进行巡航飞行，光电吊舱的基座上安装惯导，通过地面站操作光电吊舱，使视场中心锁定地面目标，进行激光测距获得光电吊舱与地面目标之间的距离l；

45、目标定位模型的系统噪声确定模块：根据光电吊舱图像跟踪目标时的水平脱靶量、垂直脱靶量与光电吊舱与地面目标之间的距离l，计算目标定位模型的系统噪声q；

46、地面目标地理坐标值确定模块：建立目标定位的状态与观测方程，根据光电吊舱自身的经纬度坐标、光电吊舱与地面目标之间的距离l与系统噪声q，采用ekf滤波算法计算地面目标的地理坐标值，用于对光电吊舱进行目标跟踪。

47、本发明与现有技术相比的有益效果是：

48、(1)对于目前ekf滤波器应用过程中的系统噪声方差，一般是通过经验选取的固定数值，这在一定程度上存在随机性，不同的选取值会对ekf的滤波结果产生巨大影响；同时，在实际机载光电吊舱对地面目标进行激光测距定位过程中，系统噪声会受到目标测距距离、光轴稳定性、图像目标跟踪精度的影响，是动态变化的过程。采用本发明方法能够始终保持系统噪声统计量自适应的逼近真实值，减少传统卡尔曼滤波算法中，因系统噪声选用常值可能导致结果发散的风险，从而实现机载光电吊舱对地面目标的高精度定位；

49、(2)现有遗忘因子、期望最大化和遗传算法等加入扩展卡尔曼滤波模型中，虽能实现对系统噪声方差的调整，但这些方法计算过程均繁琐费时，无法满足实际应用中的实时性要求。本发明所提出的系统噪声估计算法，仅需激光测距值与图像跟踪脱靶量进行统计计算，运算量小，便于部署在嵌入式系统中，具有较高的实用性。