一种PSO-SITAN双模式重力匹配组合导航方法

本发明涉及飞行器导航领域，具体涉及一种pso-sitan双模式重力匹配组合导航方法。

背景技术：

1、重力匹配导航是通过重力敏感器探测地球附近单点或多点重力场特征，在预先制作的重力场特征地图上寻找与敏感器探测的特征最匹配的点，确定探测点在地图上的位置，实现载体定位的一种导航方法。重力匹配导航具备导航信息源稳定，隐蔽性高，不需要向外界发送或接收无线电信号等优点。目前已在潜艇、舰船上得到应用，未来随着设备小型化和测量精度提高，在航空器和临近空间飞行器上具有广阔的应用前景。

2、重力特征地图总体上是多极值、强非线性的，传统的线性化方法，以sitan算法为例，计算速度快且精度高，但在地图非线性度高时匹配可靠性显著降低。集群智能搜索算法，以粒子群算法(particle swarm optimization，pso)为例，则面临计算复杂，时效性差的问题。

技术实现思路

1、针对现有技术中的上述不足，本发明提供了一种pso-sitan双模式重力匹配组合导航方法。

2、为了达到上述发明目的，本发明采用的技术方案为：

3、一种pso-sitan双模式重力匹配组合导航方法，包括如下步骤：

4、s1、惯导系统初始化并进行姿态、速度和位置更新；组合导航系统进行时间更新，输出载体的位置以及组合导航系统对载体位置误差方差的估计；

5、s2、构造拟合平面判断载体所处的地图线性程度是否可以使用sitan线性化方法进行重力匹配定位，若是则利用sitan引力梯度匹配算法对载体进行位置跟踪，若否则利用pso引力梯度匹配算法对载体进行位置捕获，并分别输出匹配位置；

6、s3、匹配点和采样点的误差判断匹配的正确性，隔离并舍弃错误匹配结果；

7、s4、利用正确匹配结果进行组合导航系统的量测更新，校正惯性导航位置。

8、进一步的，所述s2中判断组合导航系统地图的线性度采用九点平面拟合法判断：

9、s201、以惯导指示的位置位(x0,y0)为中心，组合导航误差方差pk阵指示位置误差1倍标准差为(δx,δy)，以(δx,δy)为步长，取九个采样点拟合一个平面，采样点表示为：

10、b1＝b(x0-δx,y0-δy)

11、b2＝b(x0,y0-δy)

12、b3＝b(x0+δx,y0-δy)

13、b4＝b(x0-δx,y0)

14、b5＝b(x0,y0)

15、b6＝b(x0+δx,y0)

16、b7＝b(x0-δx,y0+δy)

17、b8＝b(x0,y0+δy)

18、b9＝b(x0+δx,y0+δy)

19、其中b(x,y)表示地图上的点(x,y)对应的引力梯度值。

20、s202、根据9个采样点的数据构造拟合平面，根据所拟合的平面预测采样点附近的引力梯度变化趋势，其中拟合平面表示为：

21、b(x,y)＝a+hx(x-x0)+hy(y-y0)

22、其中，其中hx、hy通过采样点b1到b9的引力梯度计算得到：

23、

24、s203、以x、y两个方向上的引力梯度γxx、γyy作为s202中的引力梯度分别构造平面，表示为：

25、

26、s204、判断该区域地图是否适合平面拟合，判据如下

27、判据1：

28、δx>10km或δy>10km

29、其中，(x0,y0)为惯性导航指示的位置，(δx,δy)为误差方差阵给出的位置误差的1倍标准差；

30、判据2：

31、

32、b(x,y)表示地图上的点(x,y)对应的引力梯度值；rb为平面极差：

33、rb＝max{bi}-min{bi},(i＝1,2,…,9)

34、bi为第i个采样点，max{bi}为b1、b2…b9中最大值，max{bi}为b1、b2…b9中最小值；

35、判据3：

36、

37、其中，|vb|为引力梯度测量噪声、为飞行器实际位置观测到的引力梯度值；

38、若上述判据公式任意一项成立，则利用引力梯度匹配算法对载体进行位置捕获；若均不成立，则利用引力梯度匹配算法对载体进行位置跟踪。

39、进一步的，所述s1中利用sitan引力梯度匹配算法对载体进行位置跟踪具体方式为：

40、所述s1中利用引力梯度匹配算法对载体进行位置追踪时采用sitan算法进行位置追踪，具体方式为：

41、s211、以s203中构建的解析的拟合平面代替地图，作为sitan线性化算法需要的线性化平面。

42、s212、根据惯性导航指示位置的平面中心引力梯度值和引力梯度系统在飞行器实际位置观测到的引力梯度扩展卡尔曼滤波的观测量，表示为：

43、

44、其中，zk为卡尔曼滤波的观测量，b为惯性导航指示位置的平面中心的引力梯度且b(x0,y0)＝(γxx,γzz)；为引力梯度系统在飞行器实际位置观测到的引力梯度且t为矩阵转置；

45、s213、构建量测方程，选取状态量进行卡尔曼滤波并进行量测方程更新并输出匹配位置。

46、进一步的，所述s2中利用pso引力梯度匹配算法对载体进行位置捕获的具体方式为：

47、s221、初始化数量为n的粒子，每个粒子包含载体的速度和位置属性，其中，粒子的位置属性包括经度和纬度，表示为：pi＝(b l)，式中，pi为第i个粒子，b为经度，l为纬度；粒子的速度属性表示为：vi＝[vn ve]；

48、s222、粒子群算法迭代依次，粒子在空间移动一次，并记录粒子位置变化和速度；

49、s223、通过跟踪单个粒子历史最优位置和适应度、粒子群的全局历史最优位置指导粒子运动，记录自身经过的位置中适应度最小的点并计算运动过程中的速度；

50、s224、记录单个粒子的适应度并按照s223所计算速度运动，运动过程中重新计算适应度，并以适应度作为下次运动的依据直至误差小于阈限后输出位置匹配结果。

51、进一步的，所述s222中粒子位置变化表示为：

52、pi+1＝pi+δpi＝pi+viδt

53、其中，t为世代数，δt为每次迭代的世代数变化，δpi位移量。

54、进一步的，所述s223中适应度的计算方式为：

55、

56、其中，为重力传感器测量值且为在x、y方向上的分量；γi为根据粒子位置pi查找地图得到的理论值，为γi在x、y方向上的分量。

57、进一步的，所述s223中粒子运动过程中的速度表示为：

58、vi＝ω×vi-1+c1×rand(1)×(pbesti-pi)+c2×rand(1)×(gbest-pi)

59、其中，vi-1为上一时刻粒子速度，ω为惯性权重，当i＝1时先取一组随机数作为v0；pbest为个体历史最优位置，即粒子本身所找到的fi最小的点，c1为跟踪个体最优位置的权重；gbest为群体历史最优位置，即整个种群目前找到的fi最小的点，c2为跟踪群体最优位置的权重。

60、进一步的，所述s3中对输出的匹配位置进行检测隔离并舍弃错误匹配结果的判据为：

61、

62、其中，为匹配点的引力梯度，为引力梯度系统在飞行器实际位置观测到的引力梯度。

63、本发明具有以下有益效果：

64、可以在载体位置误差较大的情况下，满足匹配算法在复杂地图下搜索定位的要求，在pso使导航误差减小后，切换sitan算法使运算速度显著提高。最终使导航系统综合了大范围捕获定位、高效、高精度等优点。