CN109752690A

CN109752690A - 无人机定位nlos的消除算法、系统、装置及存储介质

Info

Publication number: CN109752690A
Application number: CN201910037325.XA
Authority: CN
Inventors: 相征; 齐庆凯; 任鹏
Original assignee: Xidian University
Current assignee: Xidian University
Priority date: 2019-01-15
Filing date: 2019-01-15
Publication date: 2019-05-14
Anticipated expiration: 2039-01-15
Also published as: CN109752690B

Abstract

本发明属于通信技术领域，具体涉及一种无人机定位NLOS的消除算法、系统、装置及存储介质，算法步骤如下：获取无人机初始位置、飞行信息以及测量值；根据所述无人机初始位置及所述飞行信息识别所述测量值中具有非视距误差的值；根据所述具有非视距误差的测量值进行非视距误差消除得到实际位置。本发明实施例通过先获取具有非视距误差的测量值，在对具有非视距误差的测量值消除处理，可以更加准确的对测量值进行处理，使得定位结果更加准确，并且本发明所使用的消除非视距误差的方法可以大大缩减定位时间。

Description

无人机定位NLOS的消除算法、系统、装置及存储介质

技术领域

本发明属于通信技术领域，具体涉及一种无人机定位NLOS的消除算法、系统、装置及存储介质。

背景技术

近年来，随着无人机相关技术的不断发展，无人机被应用于越来越多的领域：灾害急救，目标跟踪，农业领域甚至战争领域。对于无人机的定位以前总是借助于全球卫星导航系统，但是GPS有很大的缺陷：在恶劣的环境下会出现很大的误差，在一些偏远的地区会出现收不到GPS信号的情况。因此关于无人机的无线定位越来越受到人们的关注，也得到了越来越多的研究人员的研究。

由于高楼和障碍物的存在，无线信号通过折射和反射在无人机和基准站之间传播，从而形成了非视距NLOS(Non-Line-Of-Sight)误差，极大降低其定位精度，使其定位结果产生很大的误差。针对这种误差一般处理方法有两种：1.通过算法识别出无人机与基准站的信号传输是NLOS传输，抛弃这个数据；2.不抛弃信号传输是NLOS的数据，通过算法去降低NLOS误差，使其达到很好的精度。

在第一种方法中早期Wylie在考虑移动跟踪题时，在一段时间内对目标进行测距，利用NLOS传播路径测量值的样本方差大于比LOS传播测量值的样本方差的特性，对NLOS进行识别，但是该方法需要很大计算复杂度，可实施性不高。由于在LOS和NLOS环境下测量误差有所不同，那么在不同场景下其测量距离有着不同的统计分布。LOS环境下接收信号的测量距离通常呈高斯分布，而NLOS环境下的接收信号的测量距离可能呈指数分布或其它统计分布。因此，在未知LOS/NLOS传播先验概率的情况下，可以通过检验测量值是否服从高斯分布来识别NLOS传播。近年来先后出现了K-S、A-D、Chi-Square、格鲁斯检验、偏斜度和峭度检验等检验方法。

在第二种方法中早期Chen P C提出了一种残差加权(RWGH)算法，该算法利用任意三个及以上的基站TOA(time of arrive，到达时间)测量值组合，通过最小二乘法求解每一种组合中移动台的估计位置坐标和对应的归一化残差，再利用各归一化残差对估计值进行加权求和，得到最终的估计位置。该算法能在一定程度抑制NLOS误差，但是需要计算每种组合的估计位置和对应的残差，导致计算复杂度较高。Li X提出了迭代最小残差算法，该算法通过迭代每种组合中的残差大小，其值小于预定的门限值，选取最小残差组合的估计结果作为MS最终估计位置。虽然该方法比第一种方法有所改进，但是同样计算量比较大。同时上述两种算法都是至少需要三个基站以上才能完成定位。

发明内容

为了解决现有技术中存在的上述问题，本发明提供了一种无人机定位NLOS的消除算法、系统、装置及存储介质。本发明要解决的技术问题通过以下技术方案实现：

本发明实施例提供了一种无人机定位NLOS的消除算法，步骤如下：

获取无人机初始位置、飞行信息以及测量值；

根据所述无人机初始位置及所述飞行信息识别所述测量值中具有非视距误差的值；

根据所述具有非视距误差的测量值进行非视距误差消除得到实际位置。

在本发明的一个实施例中，所述根据所述无人机初始位置及所述飞行信息识别所述测量值中具有非视距误差的值，包括：

根据所述初始位置和所述飞行信息得到预测值；

根据所述预测值和所述测量值得到第一估计值；

根据所述第一估计值和所述测量值得到具有非视距误差的测量值。

在本发明的一个实施例中，所述根据所述具有非视距误差的测量值进行非视距误差消除得到实际位置，包括：

根据所述具有非视距误差的测量值得到最终测量值；

根据所述最终测量值得到所述实际位置。

在本发明的一个实施例中，在进行根据具有非视距误差的测量值进行消除非视距误差得到实际位置之前，还包括：

对所述具有非视距误差的测量值进行扩展卡尔曼滤波。

本发明还提供了一种无人机定位NLOS的消除系统，包括：

信息采集模块，用于获取无人机初始位置、飞行信息以及测量值；

判断模块，用于根据所述无人机初始位置及所述飞行信息识别所述测量值中具有非视距误差的值；

误差消除模块，用于根据所述具有非视距误差的测量值进行非视距误差消除得到实际位置。

在本发明的一个实施例中，判断模块包括：

一级计算单元，用于根据所述初始位置和所述飞行信息得到预测值；

二级计算单元，用于根据所述预测值和所述测量值得到第一估计值；

三级计算单元，用于根据所述第一估计值和所述测量值得到具有非视距误差的测量值。

在本发明的一个实施例中，误差消除模块包括：消除单元，用于根据所述具有非视距误差的测量值得到最终测量值；坐标运算单元，用于根据所述最终测量值得到所述实际位置。

在本发明的一个实施例中，还包括：抛弃单元，用于对所述具有非视距误差的测量值进行扩展卡尔曼滤波。

本发明还提供了一种无人机定位NLOS的消除装置，包括：基站、处理器和存储器，所述存储器中存储有计算机程序，所述处理器执行所述计算机程序的步骤为：

获取无人机初始位置、飞行信息以及测量值；

根据所述具有非视距误差的测量值进行消除非视距误差得到实际位置。

本发明还包括一种存储介质，所述计算机程序被处理器执行时实现权利要求1至4中任一项所述的方法步骤。

与现有技术相比，本发明的有益效果：

1.本发明实施例通过先获取具有非视距误差的测量值，在对具有非视距误差的测量值消除处理，可以更加准确的对测量值进行处理，使得定位结果更加准确，并且本发明所使用的消除非视距误差的方法可以大大缩减定位时间。

2.本发明实施例在NLOS消除时使用的两步卡尔曼滤波。相比于其他算法，本算法可以只有一个基站的工况下就能精准定位且容易实现。

3.本发明实施例在NLOS识别时，根据的是卡尔曼滤波在非高斯白噪声下估计值为有偏估计来识别，卡尔曼滤波迭代只与前一个时刻有关，因此计算步骤简单很容易就能在计算机上实现。同时卡尔曼滤波回归很快，能够少量的测量值就能回归到真实值附近，因此能够更好地实现实时快速的识别。

4.本发明实施例在定位的同时还实现了追踪。在第二次卡尔曼滤波中实现了对无人机的实时追踪，因此在每次的定位中不需要再通过复杂的算法计算出迭代的初始值。还将第二次卡尔曼滤波的追踪值用于初始值，大大减少了定位复杂度，提高了实时准确性。

5.本发明实施例同时相对于传统的卡尔曼算法本专利提出了测量值丢弃法和改进卡尔曼增益法，使得很好地消除了NLOS误差，定位更精确。

附图说明

图1为本发明实施例提供的一种无人机定位NLOS的消除算法的流程示意图；

图2为本发明实施例提供的一种无人机定位NLOS的消除装置的功能原理示意图；

图3为本发明实施例提供的一种无人机定位NLOS的消除算法的另一种流程示意图；

图4为本发明实施例提供的一种无人机定位NLOS的消除算法的第一次卡尔曼滤波的仿真效果；

图5为本发明实施例提供的一种无人机定位NLOS的消除算法的第二次卡尔曼仿真效果图。

具体实施方式

下面结合具体实施例对本发明做进一步详细的描述，但本发明的实施方式不限于此。

获取无人机初始位置、飞行信息以及测量值；

具体的，通过先获取具有非视距误差的测量值，在对具有非视距误差的测量值消除处理，可以更加准确的对测量值进行处理，使得定位结果更加准确，并且本发明所使用的消除非视距误差的方法可以大大缩减定位时间。

根据所述初始位置和所述飞行信息得到预测值；

根据所述预测值和所述测量值得到第一估计值；

根据所述具有非视距误差的测量值得到最终测量值；

根据所述最终测量值得到所述实际位置。

对所述具有非视距误差的测量值进行扩展卡尔曼滤波。

具体的，在进行消除非视距误差之前，还增加了测量值丢弃法和改进卡尔曼增益法，使得在进行非视距误差消除时，可以更加全面，进而使得定位更准确。

本发明还提供了一种无人机定位NLOS的消除系统，包括：

在本发明的一个实施例中，判断模块包括：

获取无人机初始位置、飞行信息以及测量值；

具体的，由于初始位置的准确性直接影响无人机定位非视距误差消除的准确性。为了提高初始位置定位的准确性采用圆周定位法，故而根据非线性最小二乘法得到以下公式：

解得初始无人机坐标位置，其中(x，y)是被定位无人机的坐标，(x_i,y_i)是基站坐标，b_i是基站到无人机测量距离。

具体的，由于测量噪声和NLOS误差的存在，使得圆周定位法所得到的圆形不能相交于一点。因此要对已经获得的初始无人机坐标进行进一步处理。根据泰勒级数得到d_i(x，y)≈d_i(x₀，y₀)+H₀(x-x₀，y-y₀)，根据分布在(x₀，y₀)周边的基站坐标d_i(x，y)得到雅克比矩阵如下：

并使用高斯牛顿迭代求解，得到最终无人机坐标。

具体的，通过无人机上的舵机向基站不断发送无人机速度、加速度、运行方向等信息，基站结合上述信息与最终无人机坐标得到预测值，基站同时对无人机进行测量得到测量值。

通过线性随机微分方程得到：X(k)＝A X(k-1)+B*U(k)+W(k)，测量值为Z(k)＝H*X(k)+V(k)，X(k)是k时刻的系统状态，U(k)是k时刻对系统的控制量，A和B是系统参数(A和B均为矩阵)，A,H分别是状态方程和测量方程的雅可比矩阵，W(k)和V(k)分别表示过程和测量的噪声，且W(k)和V(k)均为高斯白噪声。

根据卡尔曼滤波器的五个基本公式：

X(k|k-1)＝AX(k-1|k-1)+BU(k|k-1)..........(1)

P(k|k-1)＝AP(k-1|k-1)A'+Q..........(2)

X(k|k)＝X(k|k-1)+K_g(k)(Z(k)-HX(k|k-1))..........(3)

K_g(k)＝P(k|k-1)H'/(HP(k|k-1)H'+R).............(4)

P(k|k)＝(I-K_g(k)H)P(k|k-1)..............(5)

e_k＝Z(k)-HX(k|k-1)....................(6)

具体的，无人机定位系统一般模型为其中t_i是信号传输的时间，c是光速，是无人机与基准站的测量距离，d_i是无人机与基准站的真实距离，n_i服从均值为0，b_i为由于直接路径阻碍(非视距)而引入的正距离偏差，

具体的，本实施例中非视距误差服从指数分布，且非视距误差的均值和方差远大于服从均值n_i。由于使用的卡尔曼滤波器只有在高斯白噪声(LOS)情况下才能很好地得到无偏估值，所以观测值与第一估计值差值的方差(D(Z(k)-X(k|k))，Z(k)是测量值，X(k|k)是第一估计值)等于LOS噪声方差。但当无人机与基站处于非视距误差情况下，包含了指数分布的噪声，那么卡尔曼滤波的到的第一估计值就是有偏估值，同时由于非视距误差的均值和方差较大。进而得到：

其中η门限(η的值随不同工况而变化)。通过上式，对已经获得的测量值进行判断，得到该时间节点具有非视距误差的测量值。

具体的，误差消除阶段主要使用了两次卡尔曼滤波。

具体的，第一次卡尔曼滤波采用标准卡尔曼滤波，第一次卡尔曼滤波用于对多个具有非视距误差的测量值进行滤波得到的第二估计值。

具体的，在进行第一次卡尔曼滤波时，由于可能出现迭代的初始位置和真实位置相差较大，先不采取任何措施进行滤波，可以将第二估计值聚集在基站与无人机的真实距离附近，同时对不可靠的测量值进行丢弃。由于卡尔曼滤波器在进行TOA估计的常规方法中，由于具有因非视距误差的测量值使得当前TOA估计发生较大偏差，并且由于卡尔曼滤波器的记忆性，使后续的TOA估计都受到影响。

具体的，在对影响估计值准确的具有非视距误差的测量值进行抛弃时，结合卡尔曼滤波器五个基本公式中的(1)、(2)和(5)，计算出估计值X(k|k-1)、预测协方差P(k|k-1)和新息值e_k。e_k越小说明结果测量值受到的噪声影响越小。所以设置一个预设门限值。当新息值大于预设门限值，则卡尔曼滤波器五个基本公式的公式(4)中K_g(k)＝0，继而得到：

即当e_k＝Z(k)-HX(k|k-1)＞ε时把具有非视距误差的测量值中受噪声影响较大的值丢弃，不参与迭代，反之这次测量值有效，参与第一次卡尔曼滤波器的迭代。

具体的，由于非视距误差服从指数分布，在仿真图上则显示为真实结果的整体上移，即估计值整体大于真实值。故而要将估计值减小。由卡尔曼滤波器基本公式中的公式(3)和公式(5)可得：

当e_k大于0，缩小K_g(k)的值，反之增大K_g(k)的值。为了减少迭代次数，当e_k＞0，K_g(k)＝0。为了防止估计值过小，进而小于真实值，e_k的绝对值越大τ就越大，τ的值大于1并且是在不断的变化的。

具体的，第二次卡尔曼滤波为扩展卡尔曼滤波。

具体的，基站接收来自无人机发送的飞行信息，基站的相对坐标为(0,0)，基站可以测的与无人机的距离d(k)和角度α(k)。根据无人机运动参数得到第二步卡尔曼滤波的非线状态方程及观测方程：

状态方程：

观测方程：其中Δ为卡尔曼采样间隔，q为系统噪声，v₁,v₂为观测噪声，X(k),Y(k)分别为无人机的横坐标和纵坐标，a_x,a_y分别为无人机在x轴，y轴方向上的加速度，V_x,V_y分别为无人机在x轴，y轴方向上的速度。根据状态方程和观测方程计算雅可比矩阵X(k)＝[x(k) v_x(k) y(k) v_y(k)]，

第二估计值作为第二次卡尔曼滤波的测量值，且第二次卡尔曼滤波的估计值作为第一次卡尔曼滤波的初始位置。

具体的，在对测量值进行是否具有非视距误差判断时，当判断某一测量值为视距误差时，则将具有视距误差的测量值作为第二次卡尔曼滤波器的输入值。

具体的，本实施例在NLOS消除时使用的两步卡尔曼滤波。相比于其他算法，本算法可以只有一个基站的工况下就能精准定位且容易实现。

具体的，本实施例在NLOS识别时，根据的是卡尔曼滤波在非高斯白噪声下估计值为有偏估计来识别，卡尔曼滤波迭代只与前一个时刻有关，因此计算步骤简单很容易就能在计算机上实现。同时卡尔曼滤波回归很快，能够少量的测量值就能回归到真实值附近，因此能够更好地实现实时快速的识别。

具体的，本发明在定位的同时还实现了追踪。在第二次卡尔曼滤波中实现了对无人机的实时追踪，因此在每次的定位中不需要再通过复杂的算法计算出迭代的初始值。还将第二次卡尔曼滤波的追踪值用于初始值，大大减少了定位复杂度，提高了实时准确性。

具体的，本实施例中对测量值进行识别时进行的卡尔曼滤波与在对具有非视距误差的测量值进行非视距误差消除时的第一次卡尔曼滤波的矩阵设置相同，其中，X(k)＝[d(k) d'(k)]，W(k)＝[0 u_n]，H＝[1 0]，V(k)＝n_m。d(k)为一次的测量值，d'(k)为d(k)的一阶导，Δ为卡尔曼滤波器的采样间隔，u_n为过程噪声分量，n_m为测量误差。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。

Claims

1.一种无人机定位NLOS的消除算法，其特征在于：步骤如下：

获取无人机初始位置、飞行信息以及测量值；

2.根据权利要求1所述的一种基于卡尔曼滤波的无人机定位非视距误差的识别与消除算法，其特征在于：所述根据所述无人机初始位置及所述飞行信息识别所述测量值中具有非视距误差的值，包括：

根据所述初始位置和所述飞行信息得到预测值；

根据所述预测值和所述测量值得到第一估计值；

3.根据权利要求1所述的一种基于卡尔曼滤波的无人机定位非视距误差的识别与消除算法，其特征在于：所述根据所述具有非视距误差的测量值进行非视距误差消除得到实际位置，包括：

根据所述具有非视距误差的测量值得到最终测量值；

根据所述最终测量值得到所述实际位置。

4.根据权利要求1所述的一种基于卡尔曼滤波的无人机定位非视距误差的识别与消除算法，其特征在于：在进行根据具有非视距误差的测量值进行消除非视距误差得到实际位置之前，还包括：

对所述具有非视距误差的测量值进行扩展卡尔曼滤波。

5.一种无人机定位NLOS的消除系统，其特征在于：包括：

6.根据权利要求5所述的一种无人机定位非视距误差识别与消除系统，其特征在于：判断模块包括：

7.根据权利要求5所述的一种无人机定位非视距误差识别与消除系统，其特征在于：误差消除模块包括：

消除单元，用于根据所述具有非视距误差的测量值得到最终测量值；

坐标运算单元，用于根据所述最终测量值得到所述实际位置。

8.根据权利要求5所述的一种无人机定位非视距误差识别与消除系统，其特征在于：还包括：

抛弃单元，用于对所述具有非视距误差的测量值进行扩展卡尔曼滤波。

9.一种无人机定位NLOS的消除装置，其特征在于；包括：基站、处理器和存储器，所述存储器中存储有计算机程序，所述处理器执行所述计算机程序的步骤为：

获取无人机初始位置、飞行信息以及测量值；

10.一种存储介质，其上存储有计算机程序，其特征在于：所述计算机程序被处理器执行时实现权利要求1至4中任一项所述的方法步骤。