一种阵列迭代优化信标位置和圆形轨迹的无人机定位方法

摘要: 本发明属于无源定位,具体涉及一种阵列迭代优化信标位置和圆形轨迹的无人机定位方法。背景技术:1、无人机定位是信号处理研究中的重要方向,传统的无人机定位方式主要有卫...
一种阵列迭代优化信标位置和圆形轨迹的无人机定位方法

本发明属于无源定位,具体涉及一种阵列迭代优化信标位置和圆形轨迹的无人机定位方法。

背景技术:

1、无人机定位是信号处理研究中的重要方向,传统的无人机定位方式主要有卫星导航定位、惯性导航定位、视觉导航定位、无线电导航定位。卫星导航定位通过利用多颗在轨卫星实现无人机位置的确定,具有高精度、低时延、全球覆盖的优点。但是卫星导航信号基于合作收发的方式,容易受到障碍物遮挡造成信号的丢失。惯性导航定位是唯一不依赖外界环境的无人机定位方法,仅利用加速度积分信息实现轨迹的确定,具有抗干扰能力强、隐蔽性高的优点,但是惯性导航定位也存在着误差累积的问题。视觉导航定位是依靠摄像头捕捉周围环境画面实现自身位置确定的定位技术,这种方法能够获取丰富的物体形状、颜色等特性,但是也存在着计算复杂度高、环境光线要求高、实时性不高的缺点。无线电导航是利用电磁波信号的传播特性进行定位的方式,具有全天候工作、定位距离远、建设成本低的优势,但是这一定位方式对信标位置精度要求较高。因此研究能够实现信标误差下的无人机定位方法具有重要的意义。

技术实现思路

1、本发明所要解决的技术问题是针对上述现有技术的不足,提供一种阵列迭代优化信标位置和圆形轨迹的无人机定位方法,基于阵列测向结果,匹配优化信标位置,同时匹配惯性导航结果,优化圆形轨迹的定位结果。

2、为实现上述技术目的,本发明采取的技术方案为:

3、一种阵列迭代优化信标位置和圆形轨迹的无人机定位方法,包括:

4、步骤1:利用速度信息结合对应的时间推断相对坐标系下无人机圆形飞行轨迹并利用圆的一般方程求解轨迹半径、圆心位置;

5、步骤2:利用相对坐标系下无人机圆形飞行轨迹和定位区域内信号接收阵列的测向值估计相对坐标系中信标位置,并将相对坐标系中信标位置估计值与测量信标位置匹配,得到绝对坐标系中信标位置估计值;

6、步骤3:利用信标位置估计值和测向值的几何关系初步定位无人机在绝对坐标系中的轨迹并确定绝对坐标系下圆心位置;

7、步骤4:利用相对坐标系和绝对坐标系中圆心、航向角的配对关系,将相对坐标系下圆形飞行轨迹映射到绝对坐标系中并根据起点位置直线信息校正映射后绝对坐标系中轨迹位置,得到校正后的绝对坐标系中轨迹位置;

8、步骤5:利用校正后的绝对坐标系中轨迹位置和测向值重新确定绝对坐标系中信标位置估计值;

9、步骤6:重复步骤3-步骤5进行有限次数的迭代优化,得到优化后的信标位置和圆形轨迹。

10、为优化上述技术方案,采取的具体措施还包括:

11、上述的步骤1具体包括:

12、利用速度信息结合对应的时间推断相对坐标系下无人机圆形飞行轨迹,得到相对坐标系中无人机飞行轨迹位置估计值为,公式如下:

13、

14、

15、其中,、为绝对坐标系中第时刻无人机飞行速度的x轴、y轴直角分量,,为时刻数量,,,为采样点间的时间间隔。

16、基于无人机飞行轨迹位置估计值求解得到圆的一般方程参数、、:

17、

18、根据参数、、确定相对坐标系下的轨迹半径和圆心:

19、, , 。

20、上述的步骤2中,定位区域内分布有个信源,信源发射信号的波长为,其测量位置坐标为,为测量信标位置;接收信号的阵列为个阵元的圆阵,圆阵半径为,第时刻阵列接收到第个信源的测向值为,,是第时刻以阵列参考阵元为基准的相对测向值,是第时刻无人机航向角。

21、上述的步骤2中利用无人机圆形飞行轨迹和定位区域内信号接收阵列的测向值估计相对坐标系中信标位置,具体包括:

22、将q个时刻的测向值分成组,每组有个元素,其中,为第个信源测向值的数量,第组中第个测向值为,相应的无人机圆形飞行轨迹位置为,;

23、对于每一组中的元素计算交点,得到组元素所有交点集合,其中,为第组第个测向值和第个测向值形成的交点,

24、

25、

26、计算交点与周围其余交点的距离,其中,表示集合中第个元素;

27、对于集合中的每一个交点,计算其周围点分布密度,其中,中的元素由集合元素升序排列构成,是集合中的第个元素,是集合元素求和数量阈值,满足;

28、采用下式选取出第组中测向值异常的序号集合,

29、

30、其中,、表示第组中元素的序号,是判断密度的阈值,表示第、个测向值形成的交点;

31、将中出现较多的个元素从集合中剔除,剩余的元素构成新的集合;

32、将中第个元素表示成,中元素的个数;

33、对于第组元素,利用信标、无人机和测向值之间几何关系,通过以下关系求解信标位置估计值,其中表示伪逆操作,表示关于测向值的测量矩阵,表示关于测向值和无人机轨迹位置的观测矩阵;

34、

35、

36、通过对所有组元素对应的信标位置估计值求解结果取平均得到相对坐标系中第个信标位置估计值。

37、上述的步骤2将相对坐标系中信标位置估计值和测量信标位置匹配,得到绝对坐标系中第个信标位置估计值,其中为定位区域内信源数量。

38、上述的步骤3具体包括:

39、(1)对测向值采用流式处理的方式进行异常值初步剔除,过程如下:

40、第1、2时刻的测向值不进行异常值初步剔除,输出为、;

41、第3时刻测向值开始采用以下公式进行异常值初步求解:

42、

43、其中,,;、表示异常值,表示输出的正常值;

44、初步剔除异常值后的测向输出值为;

45、所述异常值初步求解采用遍历搜索的方式并采用如下的矩阵运算形式实现:

46、

47、其中,是矩阵的第行、第列元素,矩阵为:

48、;

49、(2)在异常值初步剔除的基础上,使用阈值法进行异常值的二次过滤,过程如下:

50、第1~时刻不进行异常值二次过滤,输出为,,,;表示时间窗口阈值,用于限制下面异常值判断需要的测向值范围;

51、从第时刻开始,如果,,则输出,否则输出;表示异常判断阈值;

52、(3)利用二次过滤后的测向值,初步定位无人机在绝对坐标系中轨迹:

53、

54、其中,表示关于二次过滤测向值的测量矩阵,表示关于二次过滤测向值和信标位置估计值的观测矩阵;

55、

56、

57、其中,为绝对坐标系中第个信标位置估计值;

58、(4)采用阈值法对轨迹异常值进行滤除,包括:

59、定义轨迹初步定位每一点异常程度为:;

60、定义满足阈值条件的索引点升序集合;其中,中元素个数为,元素索引为,;如果,则;表示判断轨迹初步定位位置的异常值阈值;表示限制中元素个数的阈值;

61、(5)对第时刻初步定位轨迹对应的圆心位置进行求解:

62、

63、

64、(6)剔除初步定位轨迹对应的圆心位置中的离群值后,得到剩余圆心所对应的新圆心索引集合:

65、

66、其中,

67、

68、(7)基于新圆心索引集合求解绝对坐标系下圆心位置:

69、

70、

71、其中,是的第个元素,表示数量限制阈值。

72、上述的步骤4具体包括:

73、计算相对坐标系下圆形飞行轨迹和圆心的坐标关系:

74、

75、且

76、其中,是相对坐标系中第时刻轨迹点和圆心的距离,是相对坐标系中从圆心到轨迹点射线相对于x轴的夹角,为相对坐标系下的圆心位置;

77、将相对坐标系下圆形飞行轨迹映射到绝对坐标系中,得到,:

78、

79、

80、计算无人机进入圆形飞行轨迹前直线的解析方程:

81、

82、其中,是第时刻无人机航向角,为无人机沿圆形轨迹的切线方向直线飞行进入轨迹时的切入直线起点;表示选取航向角的时刻阈值;

83、如果起点位置直线信息,利用如下公式对映射后的轨迹进行平移校正:

84、

85、

86、如果起点位置直线信息,利用如下公式对映射后的轨迹进行平移校正:

87、

88、。

89、上述的步骤5具体包括:

90、个时刻的测向值被分成组,每组有个元素,其中,表示时刻测向值的总个数,第组中第个测向值为,相应的校正后的绝对坐标系中无人机轨迹位置为,其中,;

91、对于每一组中的元素计算交点,第组元素交点表示为,

92、

93、

94、将组元素所有交点表示为;

95、对于第组元素中的交点,计算交点与周围其余点的距离:

96、

97、其中,表示集合中第个元素;

98、对于集合中的每一个交点,其周围点分布密度计算公式如下,

99、

100、式中,中的元素由集合元素升序排列构成,是集合中的第个元素,是集合元素求和数量阈值,满足。

101、使用以下公式选出第组中测向值异常的序号集合,

102、

103、其中,、表示第组中元素的序号,是判断密度的阈值,表示第、个测向值形成的交点;

104、将中出现较多的个元素从集合中剔除,剩余的元素构成新的集合;

105、将中第个元素表示成,中元素的个数记作;

106、对于第组元素,利用信标、无人机和测向值之间几何关系,通过以下关系求解信标位置估计值:

107、

108、其中,表示关于测向值的测量矩阵,表示关于测向值和无人机轨迹位置的观测矩阵;

109、

110、

111、通过对所有组元素求解结果取平均得到绝对坐标系中第个信标位置估计值:

112、。

113、上述的步骤6中,令,重复步骤3至步骤5进行次迭代,得到优化后的信标位置和圆形轨迹,,,为定位区域内信源数量。

114、本发明具有以下有益效果:

115、本发明通过惯性导航建立相对坐标系下的无人机轨迹,同时借助于测向值得到相对坐标系下的信标位置,借助于相对坐标系和绝对坐标系的配对关系,优化绝对坐标系下的信标位置和无人机轨迹,能够有效克服信标位置误差带来的无人机定位偏差问题,实现信标偏差和轨迹偏差的联合校正。