[发明专利]无人机定位NLOS的消除方法、系统、装置及存储介质

权利要求书

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

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

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

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

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

对所述具有非视距误差的测量值进行第一次的标准卡尔曼滤波，得到第二估计值；

接收来自无人机发送的飞行信息，根据所述飞行信息得到无人机运动参数，并根据所述无人机运动参数得到第二次的扩展卡尔曼滤波的非线状态方程及观测方程：

状态方程：

观测方程：

其中，Δ为卡尔曼采样间隔，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)]，

将所述第二估计值作为第二次的扩展卡尔曼滤波的测量值，执行第二次的扩展卡尔曼滤波，得到第二次的扩展卡尔曼滤波的估计值；

将所述第二次的扩展卡尔曼滤波的估计值作为第一次的标准卡尔曼滤波的初始位置，得到非视距误差消除后的实际位置。

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

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

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

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

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

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

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

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

其中，所述误差消除模块，根据所述具有非视距误差的测量值进行非视距误差消除得到实际位置，包括：

对所述具有非视距误差的测量值进行第一次的标准卡尔曼滤波，得到第二估计值；

接收来自无人机发送的飞行信息，根据所述飞行信息得到无人机运动参数，并根据所述无人机运动参数得到第二次的扩展卡尔曼滤波的非线状态方程及观测方程：

状态方程：

观测方程：

其中，Δ为卡尔曼采样间隔，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)]，

将所述第二估计值作为第二次的扩展卡尔曼滤波的测量值，执行第二次的扩展卡尔曼滤波，得到第二次的扩展卡尔曼滤波的估计值；

将所述第二次的扩展卡尔曼滤波的估计值作为第一次的标准卡尔曼滤波的初始位置，得到非视距误差消除后的实际位置。

4.根据权利要求3所述的无人机定位NLOS的消除系统，其特征在于：判断模块包括：

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

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

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