CN115685278B - 基于kf的低空无人机航迹定位修正方法 - Google Patents
基于kf的低空无人机航迹定位修正方法 Download PDFInfo
- Publication number
- CN115685278B CN115685278B CN202211331541.3A CN202211331541A CN115685278B CN 115685278 B CN115685278 B CN 115685278B CN 202211331541 A CN202211331541 A CN 202211331541A CN 115685278 B CN115685278 B CN 115685278B
- Authority
- CN
- China
- Prior art keywords
- state
- value
- matrix
- observation
- unmanned aerial
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 34
- 238000012937 correction Methods 0.000 title claims abstract description 19
- 239000013598 vector Substances 0.000 claims abstract description 34
- 238000001914 filtration Methods 0.000 claims abstract description 25
- 230000001133 acceleration Effects 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 79
- 230000008569 process Effects 0.000 claims description 13
- 230000007704 transition Effects 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 6
- 238000012935 Averaging Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000005259 measurement Methods 0.000 claims description 3
- 238000012546 transfer Methods 0.000 claims description 3
- 238000010586 diagram Methods 0.000 description 10
- 230000000694 effects Effects 0.000 description 5
- 230000008901 benefit Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000012545 processing Methods 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000005530 etching Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
Landscapes
- Navigation (AREA)
- Feedback Control In General (AREA)
Abstract
本发明公开了基于KF(卡尔曼滤波)的低空无人机航迹定位修正方法，包括：步骤1，从ADS‑B广播获取北斗无人机定位信息数据，包括x,y,z方向坐标与速度和加速度向量；步骤2，将步骤1中的数据作为原始数据，考虑UAV运动状态为xyz三个方向的匀速直线运动，定义其状态向量与观测向量，将原始和向量数据通过线性卡尔曼滤波器得到滤后序列；步骤3，基于步骤2得到的序列，将线性卡尔曼滤波器改进为无迹卡尔曼滤波器，将步骤1中的数据作为原始数据，考虑UAV运动状态为xyz三个方向的匀加速直线运动，进行无迹卡尔曼滤波的更新，得到修正后的定位信息。定位精度显著提高，且可以减小在城市等复杂环境下定位精度的偏差。
Description
技术领域
本发明属于无人机定位技术领域，具体涉及基于KF(卡尔曼滤波)的低空无人机航迹定位修正方法。
背景技术
当前，无人机正处在一个日趋繁荣的发展形式之中，在许多领域诸如植保、电力巡检和通信等场景中得到了广泛的应用。当前低空空域内无人机的数量越来越多，因此对无人机实行空中交通管制是无人机未来各种应用场景实现的必要基础。在各类场景中，对无人机实时位置的获取往往是任务规划、安全性保证等方面的必需手段。
但受限于制造技术等原因，无人机搭载的各类传感器精度往往有限，有时接收的定位信息会产生较大误差。为了解决以上难题，具有信息更新速度快、地面站建设成本低等突出优点，且大量应用于民航客机的位置信息获取的ADS-B是可选的技术手段之一。无人机载ADS-B OUT系统按照一定的时间间隔向所在空域广播自身的态势信息。同时，卡尔曼滤波可以结合预设的真实轨迹与实际的观测轨迹，在接收信息的基础上滤去噪声，得到一个较好的轨迹信息，以此实现高精度定位偏差修正的效果。
发明内容
本发明所要解决的技术问题是针对上述现有技术的不足，提供基于KF的低空无人机航迹定位修正方法，实现低空无人机的跟踪以及实时高精度定位信息获取，相较于不经处理的传统定位方法，定位精度显著提高，且可以减小在城市等复杂环境下定位精度的偏差。
为实现上述技术目的，本发明采取的技术方案为：
基于KF的低空无人机航迹定位修正方法，包括：
步骤1，从ADS-B广播获取北斗无人机定位信息数据，包括x,y,z方向坐标与速度和加速度向量；将发送的信息，按照无人机的编号将定位信息数据按ADS-B广播时刻的先后按序存储为定位数据DUg(k)，等待后续信息。
DUg(k)＝(Ug(k)1,Ug(k)2,…,Ug(k)t)
步骤2，将步骤1中的数据作为原始数据，考虑UAV运动状态为xyz三个方向的匀速直线运动，定义其状态向量与观测向量，将原始和向量数据通过线性卡尔曼滤波器得到滤后序列；
步骤3，基于步骤2得到的序列，将线性卡尔曼滤波器改进为无迹卡尔曼滤波器，将步骤1中的数据作为原始数据，考虑UAV运动状态为xyz三个方向的匀加速直线运动，进行无迹卡尔曼滤波的更新，得到修正后的定位信息。
为优化上述技术方案，采取的具体措施还包括：
上述的步骤1包括：
步骤1-1：利用四颗北斗卫星获取无人机的定位信息数据：
其中，ρi表示伪距，(X，Y，Z)表示接收机的三维坐标，δi表示伪距测量的误差(i＝1,2,3,4)；
步骤1-2：将步骤11中定位信息传给装载在无人机上的GNSS传感器；
步骤1-3：无人机通过ADS-B OUT利用数据链以DF17格式将信息广播给负责ADS-B信息接收的地面基站。
上述的步骤2中，所有运动系统的状态空间模型表示为：
X(k+1)＝FX(k)+γW(k)
A(k)＝HX(k)+V(k)
其中，X(k)表示k时刻的真实状态向量，F称为状态转移矩阵，γ称为过程噪声驱动矩阵，而W(k)则是k时刻输入的噪声，服从高斯分布，均值为0，方差记为Q；
A(k)表示k时刻的观测状态向量，H称为观测矩阵，V(k)是传感器本身产生的观测噪声，均值为0，方差为R；
卡尔曼滤波器最核心的性能指标是求取状态值X(k)的最小方差估计值，由射影定理推知所求估计值即为X(k)在观测值A(1)、A(2)……A(k)序列方向上的投影；
在初始状态X(0)预先知道，不依赖于W(k)和V(k)的前提下，线性卡尔曼滤波器被归结为如下公式：
状态预测：
状态更新：
滤波增益矩阵：K(k+1)＝P(k+1|k)HT[HP(k+1)HT+R]-1
协方差阵预测：P(k+1|k)＝FP(k|k)FT+γQγT
协方差阵更新：P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)
其中，表示X(k+1)在X(k)方向的投影，等效看成基于k时刻的状态值对k+1时刻状态的预测值,ε(k+1)表示k+1时刻观测值与根据实际状态预测值的残差，HT表示矩阵的转置，I表示单位矩阵；
初始值的设置P(0|0)＝P0；
K(k+1)表示k+1时刻的Kalman增益，其值表征预测的优劣，与协方差阵更新优劣P(k+1|k)，观测质量优劣R以及观测矩阵H共同决定，目的是合理地使用观测值序列A(k)。
上述的步骤2具体包括：
步骤2-1：根据预先设定的轨迹模型确定状态转移矩阵F和观测矩阵H，并且进行各噪声值的设定；
步骤2-2：接收由无人机飞行过程中发送的位置信息作为状态观测值序列A(k)存储，即：
A(k)＝DUg(k)+V(k)
其中，DUg(k)表示k时刻接收到的BDS的观测状态值。
步骤2-3：根据卡尔曼滤波器方程，结合当前时刻的预测值对下一时刻进行状态预测：
其中，表示/>在/>方向的投影，等效看成基于k时刻的状态值对k+1时刻状态的预测值；/>
步骤2-4：进行协方差阵的预测：
P(k+1|k)＝FP(k|k)FT+γQγT
其中，P(k|k)表示k时刻的协方差阵，初始值P(0|0)＝P0；
步骤2-5：计算本次迭代中的卡尔曼增益矩阵；
K(k+1)＝P(k+1|k)HT[HP(k+1)HT+R]-1
其中，HT表示矩阵的转置，P(k+1|k)表示基于k时刻对k+1时刻协方差阵进行的预测，K(k+1)表示k+1时刻的Kalman增益，其值表征预测的优劣；
步骤2-6：根据预测值进行状态更新：
其中，ε(k+1)表示k+1时刻观测值与根据实际状态预测值的残差；
步骤2-7：对下一时刻的协方差阵进行更新，并将所有预测值作为下一个时刻的输入继续迭代；
P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)
其中，I表示单位矩阵；
最终的定位精度与协方差阵更新优劣P(k+1|k)，观测质量优劣R以及观测矩阵H共同决定，目的是合理地使用观测值序列A(k)。
上述的步骤3中，按照某种规律在原状态中取得一系列点集，使得均值和协方差相同，将点集带入非线性函数，得到一组新的点集，根据得到的新的点集进行无迹卡尔曼滤波的更新过程，求取均值和协方差，更新状态，得到精度更高误差更小的定位信息。
上述的步骤3中无迹卡尔曼滤波的更新具体包括：
步骤3-1:进行无迹变换UT，首先将非线性的模型方程改写为如下形式
X(k+1)＝f(X(k),W(k))
A(k)＝h(X(k),V(k))
其中，f为非线性的状态转移函数，h是非线性的观测方程函数；
步骤3-2：获取2L+1个sigma点以及对应权值：
X0＝μ
其中表示协方差矩阵P开方后的第i列，μ表示序列X的均值；
在后续步骤中，用Xi(k|k)进行全新sigma点集的表示；
其中可以理解为k时刻的状态向量；
步骤3-3：对于Xi(k|k)中的每一个元素，其权值如下计算：
其中，下标m,c分别表示均值和协方差的对应权重，参数λ＝α2(L+k)-L是一个缩放比例参数，用来减小误差；α为正数；β是一个非负系数；参数k＝0；
Sigma点集如下表示：
其中，为k时刻的状态向量；
步骤3-4：将得到的sigma点集代入状态方程进行预测，即：
Xi(k+1|k)＝f(k,Xi(k|k)),i＝1,…2L+1
步骤3-5:UKF状态向量的最终确定需要对每个sigma点的预测进行加权求均值，权重由步骤33给出；
同时协方差矩阵进行加权求和；
步骤3-6：根据预测值继续UT变换，得到新的sigma点集：
步骤3-7：将新的sigma点集代入观测方程，得到预测值的观测值：
Ai(k+1|k)＝h(Xi(k+1|k)),i＝1,…2L+1
步骤3-8：对新的观测值进行均值和协方差阵加权求和，权重系数不变，此处的协方差阵考虑X与X以及X与Z，即Pxz和Pzz。.
步骤3-9：计算卡尔曼增益：
K(k+1)＝Pxz×Pzz -1
步骤3-10：更新系统状态和协方差，作为下一个时刻的输入值，如此迭代；
还包括步骤4：将修正后的定位信息传递给后台监管部门。
本发明具有以下有益效果：
本发明充分利用了ADS-B在获取无人机飞信态势信息中的灵活性与实时性优势。ADS-B是应用在通航飞机上的一种通用位置信息获取手段，具有信息更新频率快，建站成本低，定位精度高等技术优势。在本发明中，利用ADS-B作为无人机的定位数据源，因此，地面基站对无人机飞行状态的实时性掌控能力将得到提高。
本发明充分利用卡尔曼滤波及其改进形式对不同模型的不同处理效果，达到了较好的滤波和提升定位精度的效果。原始形态的卡尔曼滤波能够有效结合观测状态与真实状态，减小传感器噪声带来的误差；经过改进的无迹卡尔曼滤波则得益于其无迹变换UT的统计特性，能够保留原模型统计特征的前提下，较好地应对非线性模型场景，并且相较于EKF等算法具有更高的精度，达到了Taylor展开的二阶精度。
附图说明
图1为本发明中低空空域内基于ADS-B和卡尔曼滤波的无人机高精度定位系统模型。
图2为本发明提出的算法流程图。
图3为线性模型下本发明进行的轨迹仿真效果对比，箭头指示表示取出了一段时间的路径放大观察。
图4为线性模型下本发明滤波前后的绝对误差对比图。
图5为线性模型下本发明滤波前后的相对误差对比图。
图6为非线性模型下本发明进行的轨迹仿真效果对比，箭头指示表示取出了一段时间的路径放大观察。
图7为非线性模型下本发明滤波前后的绝对误差对比图。
图8为非线性模型下本发明滤波前后的相对误差对比图。
具体实施方式
以下结合附图对本发明的实施例作进一步详细描述。
图1为低空空域内基于ADS-B和卡尔曼滤波的无人机定位偏差修正系统模型。该系统的运行方式如下：
通过无人机载ADS-B OUT发送定位信息数据，地面基站的ADS-B IN接收定位信息数据后将其发送给数据处理中心，数据处理中心利用卡尔曼滤波对收到的无人机定位信息进行偏差修正，通过内核为卡尔曼滤波的程序对定位数据进行处理，糅合先验了解的轨迹信息得到更逼近真实情况的飞行轨迹，达到精确定位的目的，将修正后的定位信息传递给后台监管部门，如图2所示。
本发明实施例提供的基于KF的低空无人机航迹定位修正方法，具体步骤如下：
步骤1，从ADS-B广播获取北斗无人机定位信息数据，包括x,y,z方向坐标与速度和加速度向量；将发送的信息，按照无人机的编号将定位信息数据按ADS-B广播时刻的先后按序存储为定位数据DUg(k)，等待后续信息。
DUg(k)＝(Ug(k)1,Ug(k)2,…,Ug(k)t)
步骤2，将步骤1中的数据作为原始数据，考虑UAV运动状态为xyz三个方向的匀速直线运动，定义其状态向量与观测向量，将原始和向量数据通过线性卡尔曼滤波器得到滤后序列；
步骤3，将线性卡尔曼滤波器改进为无迹卡尔曼滤波器，将步骤1中的数据作为原始数据，考虑UAV运动状态为xyz三个方向的匀加速直线运动，进行无迹卡尔曼滤波的更新，得到修正后的定位信息。
还包括步骤4：将修正后的定位信息传递给后台监管部门。
上述的步骤1包括：
步骤1-1：利用四颗北斗卫星获取无人机的定位信息数据：
其中，ρi表示伪距，(X，Y，Z)表示接收机的三维坐标，δi表示伪距测量的误差，包括接收机钟差和大气折射等(i＝1,2,3,4)；
步骤1-2：将步骤11中GNSS的定位信息传给装载在无人机上的GNSS传感器；
步骤1-3：无人机通过ADS-B OUT利用数据链(1090ES，UAT，VDL4)以DF17格式将航空器的位置、速度、高度、姿态等信息广播给负责ADS-B信息接收的地面基站接收。
上述的步骤2中，所有运动系统的状态空间模型(或部分运动)都可以用如下表示：
X(k+1)＝FX(k)+γW(k)
A(k)＝HX(k)+V(k)
其中，X(k)表示k时刻的真实状态向量，F称为状态转移矩阵，γ称为过程噪声驱动矩阵，而W(k)则是k时刻输入的噪声，服从高斯分布，均值为0，方差记为Q；
A(k)表示k时刻的观测状态向量，H称为观测矩阵，V(k)是传感器本身产生的观测噪声，均值为0，方差为R；
由于传感器精度、环境噪声等因素的影响，观测值与真实状态之间必然存在误差。
卡尔曼滤波器最核心的性能指标是求取状态值X(k)的最小方差估计值，由射影定理(证明略)可以推知所求估计值即为X(k)在观测值A(1)、A(2)……A(k)序列方向上的投影；
在初始状态X(0)预先知道，不依赖于W(k)和V(k)的前提下，线性卡尔曼滤波器可以被归结为如下公式：
状态预测：
状态更新：
滤波增益矩阵：K(k+1)＝P(k+1|k)HT[HP(k+1)HT+R]-1
协方差阵预测：P(k+1|k)＝FP(k|k)FT+γQγT
协方差阵更新：P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)
其中，表示X(k+1)在X(k)方向的投影，等效看成基于k时刻的状态值对k+1时刻状态的预测值,ε(k+1)表示k+1时刻观测值与根据实际状态预测值的残差，HT表示矩阵的转置，I表示单位矩阵；
初始值的设置P(0|0)＝P0；
K(k+1)表示k+1时刻的Kalman增益，其值表征预测的优劣，与协方差阵更新优劣P(k+1|k)，观测质量优劣R以及观测矩阵H共同决定，目的是合理地使用观测值序列A(k)。
综上所述，步骤2实施过程为：
首先，所有运动系统的状态空间模型(或部分运动)都可以用如下方程组表示：
X(k+1)＝FX(k)+γW(k)
A(k)＝HX(k)+V(k)
其中，X(k)表示k时刻的真实状态向量，F称为状态转移矩阵，γ称为过程噪声驱动矩阵，而W(k)则是k时刻输入的噪声，服从高斯分布，均值为0，方差记为Q。
A(k)表示k时刻的观测状态向量，H称为观测矩阵，V(k)是传感器本身产生的观测噪声，均值为0，方差为R。
首先考虑一个线性、匀速的飞行轨迹模型：
步骤2-1：根据预先设定的轨迹模型确定状态转移矩阵F和观测矩阵H，并且进行各噪声值的设定。
步骤2-2：接收由无人机飞行过程中发送的位置信息作为状态观测值序列A(k)存储，即：
A(k)＝DUg(k)+V(k)
其中DUg(k)表示k时刻接收到的BDS的观测状态值。
步骤2-3：根据卡尔曼滤波器方程，结合当前时刻的预测值对下一时刻进行状态预测过程。
其中，表示/>在/>方向的投影，等效看成基于k时刻的状态值对k+1时刻状态的预测值。初始值的设置/>即给定的起始位置。
步骤2-4：进行协方差阵的预测。
其中，P(k|k)表示k时刻的协方差阵，初始值P(0|0)＝P0。
步骤2-5：计算本次迭代中的卡尔曼增益矩阵。
K(k+1)＝P(k+1|k)HT[HP(k+1)HT+R]-1
其中，HT表示矩阵的转置，P(k+1|k)表示基于k时刻对k+1时刻协方差阵进行的预测。K(k+1)表示k+1时刻的Kalman增益，其值表征预测的优劣。
步骤2-6：根据预测值进行状态更新。
其中，ε(k+1)表示k+1时刻观测值与根据实际状态预测值的残差。
步骤2-7：对下一时刻的协方差阵进行更新，并将所有预测值作为下一个时刻的输入继续迭代。
P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)
其中，I表示单位矩阵。
最终的定位精度与协方差阵更新优劣P(k+1|k)，观测质量优劣R以及观测矩阵H共同决定，目的是合理地使用观测值序列A(k)。
接着执行步骤3，考虑具有加速度的模型，此时模型为非线性，使用的是结合无迹变换改进的无迹卡尔曼滤波。
所述步骤3中，按照某种规律在原状态中取得一系列点集，使得均值和协方差相同，将点集带入非线性函数(状态转移方程)，得到一组新的点集，根据得到的新的点集进行无迹卡尔曼滤波的更新过程，求取均值和协方差，更新状态，得到精度更高误差更小的定位信息。
所述步骤3中无迹卡尔曼滤波的更新具体包括：
步骤3-1:进行无迹变换UT。
为此，不失一般性地，将非线性的模型方程改写为如下形式
X(k+1)＝f(X(k),W(k))
A(k)＝h(X(k),V(k))
其中，f为非线性的状态转移函数，h是非线性的观测方程函数；
步骤3-2：需要先行获取2L+1个sigma点以及对应权值：
X0＝μ
其中表示协方差矩阵P开方后的第i列，μ表示序列X的均值。在后续步骤中，用Xi(k|k)进行全新sigma点集的表示。
其中可以理解为k时刻的状态向量(在k时刻预测k时刻)。
步骤3-3：对于Xi(k|k)中的每一个元素，其权值如下计算：
其中下标m,c分别表示均值和协方差的对应权重，参数λ＝α2(L+k)-L是一个缩放比例参数，用来减小误差；
α的选取可以控制分布密集程度，一般选取一个较小的正数降低高阶项的影响；
而β是一个非负系数，，可以合并高阶项的动差，将其影响纳入考虑之中；
参数k的调节可以在采样点的相同分布情况下调整权重的分布。此外，还需要满足(L+λ)P是一个半正定矩阵。工程应用中通常取k＝0。
Sigma点集如下表示：
其中，可以理解为k时刻的状态向量(在k时刻预测k)；
步骤3-4：将得到的sigma点集代入状态方程进行预测，即：
Xi(k+1|k)＝f(k,Xi(k|k)),i＝1,…2L+1
步骤3-5:不同于线性卡尔曼滤波的一步预测，UKF状态向量的最终确定需要对每个sigma点的预测进行加权求均值，权重由步骤33给出。
同时协方差矩阵也需要进行加权求和步骤，过程类似，不多赘述。
步骤3-6：根据第一步预测值继续UT变换，得到新的sigma点集。
步骤3-7：将新的sigma点集代入观测方程，得到预测值的观测值。
Ai(k+1|k)＝h(Xi(k+1|k)),i＝1,…2L+1
步骤3-8：对新的观测值进行类似步骤三的均值和协方差阵加权求和，权重系数不变。此处的协方差阵需要考虑X与X以及X与Z的，即Pxz和Pzz。.
步骤3-9：计算卡尔曼增益。
K(k+1)＝Pxz×Pzz -1
步骤3-10：更新系统状态和协方差，作为下一个时刻的输入值，如此迭代。
P(k+1|k+1)＝P(k+1|k)-K(k+1)PzzKT(k+1)
可以看出，UT变换实际上是一种统计意义上的近似估计。正是由于这种特性，其可以较好地处理非线性的模型。
图3利用了卡尔曼滤波对一条先验已知的线性运动航迹进行定位修正，利用前一时刻的状态向量对该时刻的观测值进行糅合修正，并且随着时刻的前进进行更新直到航行任务结束。五角星表示修正过后的轨迹。图3右表示选取(40,50)时间段内的路径放大观察。
图4表示线性运动模型下仿真得出的绝对误差示意图，左图为直接观测值误差，右图为经过滤波的观测误差。可以看出绝大部分误差都有较大下降，最大偏差下降仅到4.2m左右，约为滤波前的一半。
图5表示线性运动模型下仿真得出的相对误差示意图(滤波前/滤波后)，左图为直接观测值误差，右图为经过滤波的观测误差。与100％线相比，绝大部分误差都有较大下降。个别点的误差上升是可以接受的。
图6利用了无迹卡尔曼滤波对一条先验已知的非线性运动航迹进行定位修正，利用前一时刻的状态向量对该时刻的观测值进行糅合修正，并且随着时刻的前进进行更新直到航行任务结束。五角星表示修正过后的轨迹。图6右表示选取(40,50)时间段内的路径放大观察。
图7表示非线性运动模型下仿真得出的绝对误差示意图，左图为直接观测值误差，右图为经过滤波的观测误差。可以看出绝大部分误差都有较大下降。最大偏差下降仅到6.5m左右，约为滤波前的一半。
图8表示非线性运动模型下仿真得出的相对误差示意图(滤波前/滤波后)，左图为直接观测值误差，右图为经过滤波的观测误差。与100％线相比，绝大部分误差都有较大下降。部分点误差明显增大，是可以接受的。
以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。
Claims (6)
1.基于KF的低空无人机航迹定位修正方法，其特征在于，包括：
步骤1，从ADS-B广播获取北斗无人机定位信息数据，包括x,y,z方向坐标与速度和加速度向量；
步骤2，将步骤1中的数据作为原始数据，考虑UAV运动状态为xyz三个方向的匀速直线运动，定义其状态向量与观测向量，将原始数据和向量数据通过线性卡尔曼滤波器得到滤后序列；
步骤3，基于步骤2得到的序列，将线性卡尔曼滤波器改进为无迹卡尔曼滤波器，将步骤1中的数据作为原始数据，考虑UAV运动状态为xyz三个方向的匀加速直线运动，进行无迹卡尔曼滤波的更新，得到修正后的定位信息；
步骤2中，所有运动系统的状态空间模型表示为：
X(k+1)＝FX(k)+γW(k)
A(k)＝HX(k)+V(k)
其中，X(k)表示k时刻的真实状态向量，F称为状态转移矩阵，γ称为过程噪声驱动矩阵，而W(k)则是k时刻输入的噪声，服从高斯分布，均值为0，方差记为Q；
A(k)表示k时刻的观测状态向量，H称为观测矩阵，V(k)是传感器本身产生的观测噪声，均值为0，方差为R；
卡尔曼滤波器最核心的性能指标是求取状态值X(k)的最小方差估计值，由射影定理推知所求估计值即为X(k)在观测值A(1)、A(2)……A(k)序列方向上的投影；
在初始状态X(0)预先知道，不依赖于W(k)和V(k)的前提下，线性卡尔曼滤波器被归结为如下公式：
状态预测：
状态更新：
滤波增益矩阵：K(k+1)＝P(k+1|k)HT[HP(k+1)HT+R]-1
协方差阵预测：P(k+1|k)＝FP(k|k)FT+γQγT
协方差阵更新：P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)
其中，表示X(k+1)在X(k)方向的投影，等效看成基于k时刻的状态值对k+1时刻状态的预测值,ε(k+1)表示k+1时刻观测值与根据实际状态预测值的残差，HT表示矩阵的转置，I表示单位矩阵；
初始值的设置P(0|0)＝P0；
K(k+1)表示k+1时刻的Kalman增益，其值表征预测的优劣，与协方差阵更新优劣P(k+1|k)，观测质量优劣R以及观测矩阵H共同决定，目的是合理地使用观测值序列A(k)。
2.根据权利要求1所述的基于KF的低空无人机航迹定位修正方法，其特征在于，所述步骤1包括：
步骤1-1：利用四颗北斗卫星获取无人机的定位信息数据：
其中，ρi表示伪距，(X，Y，Z)表示接收机的三维坐标，δi表示伪距测量的误差(i＝1,2,3,4)；
步骤1-2：将步骤11中定位信息传给装载在无人机上的GNSS传感器；
步骤1-3：无人机通过ADS-B OUT利用数据链以DF17格式将信息广播给负责ADS-B信息接收的地面基站。
3.根据权利要求1所述的基于KF的低空无人机航迹定位修正方法，其特征在于，
所述步骤2具体包括：
步骤2-1：根据预先设定的轨迹模型确定状态转移矩阵F和观测矩阵H，并且进行各噪声值的设定；
步骤2-2：接收由无人机飞行过程中发送的位置信息作为状态观测值序列A(k)存储，即：
A(k)＝DUg(k)+V(k)
其中，DUg(k)表示k时刻接收到的BDS的观测状态值；
步骤2-3：根据卡尔曼滤波器方程，结合当前时刻的预测值对下一时刻进行状态预测：
其中，表示/>在/>方向的投影，等效看成基于k时刻的状态值对k+1时刻状态的预测值；/>
步骤2-4：进行协方差阵的预测：
P(k+1|k)＝FP(k|k)FT+γQγT
其中，P(k|k)表示k时刻的协方差阵，初始值P(0|0)＝P0；
步骤2-5：计算本次迭代中的卡尔曼增益矩阵；
K(k+1)＝P(k+1|k)HT[HP(k+1)HT+R]-1
其中，HT表示矩阵的转置，P(k+1|k)表示基于k时刻对k+1时刻协方差阵进行的预测，K(k+1)表示k+1时刻的Kalman增益，其值表征预测的优劣；
步骤2-6：根据预测值进行状态更新：
其中，ε(k+1)表示k+1时刻观测值与根据实际状态预测值的残差；
步骤2-7：对下一时刻的协方差阵进行更新，并将所有预测值作为下一个时刻的输入继续迭代；
P(k+1|k+1)＝[I-K(k+1)H]P(k+1|k)
其中，I表示单位矩阵；
最终的定位精度与协方差阵更新优劣P(k+1|k)，观测质量优劣R以及观测矩阵H共同决定，目的是合理地使用观测值序列A(k)。
4.根据权利要求1所述的基于KF的低空无人机航迹定位修正方法，其特征在于，所述步骤3中，按照某种规律在原状态中取得一系列点集，使得均值和协方差相同，将点集带入非线性函数，得到一组新的点集，根据得到的新的点集进行无迹卡尔曼滤波的更新过程，求取均值和协方差，更新状态，得到精度更高误差更小的定位信息。
5.根据权利要求1所述的基于KF的低空无人机航迹定位修正方法，其特征在于，所述步骤3中无迹卡尔曼滤波的更新具体包括：
步骤3-1:进行无迹变换UT，首先将非线性的模型方程改写为如下形式
X(k+1)＝f(X(k),W(k))
A(k)＝h(X(k),V(k))
其中，f为非线性的状态转移函数，h是非线性的观测方程函数；
步骤3-2：获取2L+1个sigma点以及对应权值：
X0＝μ
其中表示协方差矩阵P开方后的第i列，μ表示序列X的均值；
在后续步骤中，用Xi(k|k)进行全新sigma点集的表示；
其中可以理解为k时刻的状态向量；
步骤3-3：对于Xi(k|k)中的每一个元素，其权值如下计算：
其中，下标m,c分别表示均值和协方差的对应权重，参数λ＝α2(L+k)-L是一个缩放比例参数，用来减小误差；α为正数；β是一个非负系数；参数k＝0；
Sigma点集如下表示：
其中，为k时刻的状态向量；
步骤3-4：将得到的sigma点集代入状态方程进行预测，即：
Xi(k+1|k)＝f(k,Xi(k|k)),i＝1,…2L+1
步骤3-5:UKF状态向量的最终确定需要对每个sigma点的预测进行加权求均值，权重由步骤3-3给出；
同时协方差矩阵进行加权求和；
步骤3-6：根据预测值继续UT变换，得到新的sigma点集：
步骤3-7：将新的sigma点集代入观测方程，得到预测值的观测值：
Ai(k+1|k)＝h(Xi(k+1|k)),i＝1,…2L+1
步骤3-8：对新的观测值进行均值和协方差阵加权求和，权重系数不变，此处的协方差阵考虑X与X以及X与Z，即Pxz和Pzz；
步骤3-9：计算卡尔曼增益：
K(k+1)＝Pxz×Pzz -1
步骤3-10：更新系统状态和协方差，作为下一个时刻的输入值，如此迭代；
P(k+1|k+1)＝P(k+1|k)-K(k+1)PzzKT(k+1)。
6.根据权利要求1所述的基于KF的低空无人机航迹定位修正方法，其特征在于，所述方法还包括步骤4：将修正后的定位信息传递给后台监管部门。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211331541.3A CN115685278B (zh) | 2022-10-28 | 2022-10-28 | 基于kf的低空无人机航迹定位修正方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211331541.3A CN115685278B (zh) | 2022-10-28 | 2022-10-28 | 基于kf的低空无人机航迹定位修正方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115685278A CN115685278A (zh) | 2023-02-03 |
| CN115685278B true CN115685278B (zh) | 2023-11-24 |
Family
ID=85046476
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211331541.3A Active CN115685278B (zh) | 2022-10-28 | 2022-10-28 | 基于kf的低空无人机航迹定位修正方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115685278B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116182949B (zh) * | 2023-02-23 | 2024-03-19 | 中国人民解放军91977部队 | 一种海洋环境水质监测系统及方法 |
| CN117408084B (zh) * | 2023-12-12 | 2024-04-02 | 江苏君立华域信息安全技术股份有限公司 | 一种用于无人机航迹预测的增强卡尔曼滤波方法及系统 |
| CN117409150A (zh) * | 2023-12-12 | 2024-01-16 | 江苏君立华域信息安全技术股份有限公司 | 一种改善三维显示无人机航迹平滑的方法及系统 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005331382A (ja) * | 2004-05-20 | 2005-12-02 | Hidenori Hashimoto | Ｇｐｓ受信装置 |
| CN105911567A (zh) * | 2016-05-14 | 2016-08-31 | 四川中卫北斗科技有限公司 | 一种gnss系统完好性评估方法和装置 |
| CN106443733A (zh) * | 2016-08-26 | 2017-02-22 | 广州极飞科技有限公司 | 一种无人机的定位系统和方法 |
| CN106707235A (zh) * | 2017-03-08 | 2017-05-24 | 南京信息工程大学 | 一种基于改进的无迹卡尔曼滤波的室内测距定位方法 |
| CN111351482A (zh) * | 2020-03-19 | 2020-06-30 | 南京理工大学 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
| CN112683274A (zh) * | 2020-12-22 | 2021-04-20 | 西安因诺航空科技有限公司 | 一种基于无迹卡尔曼滤波的无人机组合导航方法和系统 |
-
2022
- 2022-10-28 CN CN202211331541.3A patent/CN115685278B/zh active Active
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2005331382A (ja) * | 2004-05-20 | 2005-12-02 | Hidenori Hashimoto | Ｇｐｓ受信装置 |
| CN105911567A (zh) * | 2016-05-14 | 2016-08-31 | 四川中卫北斗科技有限公司 | 一种gnss系统完好性评估方法和装置 |
| CN106443733A (zh) * | 2016-08-26 | 2017-02-22 | 广州极飞科技有限公司 | 一种无人机的定位系统和方法 |
| CN106707235A (zh) * | 2017-03-08 | 2017-05-24 | 南京信息工程大学 | 一种基于改进的无迹卡尔曼滤波的室内测距定位方法 |
| CN111351482A (zh) * | 2020-03-19 | 2020-06-30 | 南京理工大学 | 基于误差状态卡尔曼滤波的多旋翼飞行器组合导航方法 |
| CN112683274A (zh) * | 2020-12-22 | 2021-04-20 | 西安因诺航空科技有限公司 | 一种基于无迹卡尔曼滤波的无人机组合导航方法和系统 |
Non-Patent Citations (1)
| Title |
|---|
| 张倩云.缺星情况下无人机紧组合导航系统滤波技术研究.硕士电子期刊,2021,第07期卷(第2021年版),第28页第3段-第45页第2段. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115685278A (zh) | 2023-02-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN115685278B (zh) | 基于kf的低空无人机航迹定位修正方法 | |
| CN108134640B (zh) | 一种基于节点运动状态约束的协作定位系统和方法 | |
| WO2023134666A1 (zh) | 终端定位方法、装置、设备以及介质 | |
| Kaniewski et al. | Estimation of UAV position with use of smoothing algorithms | |
| Peng et al. | UAV positioning based on multi-sensor fusion | |
| CN112083457B (zh) | 一种神经网络优化的imm卫星定位导航方法 | |
| Deep et al. | Application of Kalman filter in GPS position estimation | |
| CA2699137A1 (en) | Hybrid inertial system with non-linear behaviour and associated method of hybridization by multi-hypothesis filtering | |
| Gao et al. | Maximum likelihood-based measurement noise covariance estimation using sequential quadratic programming for cubature Kalman filter applied in INS/BDS integration | |
| CN112577496A (zh) | 一种基于自适应选权的多源融合定位方法 | |
| CN111208544A (zh) | 一种用于无人机蜂群协同导航的完好性保护水平优化方法 | |
| CN109781374A (zh) | 一种实时在线快速估计飞行器推力的方法 | |
| Rahman et al. | Earth-centered earth-fixed (ecef) vehicle state estimation performance | |
| CN114063122B (zh) | 电推进转移轨道航天器星载gnss在轨实时定轨方法 | |
| CN115859839A (zh) | 基于rcs序列阶跃效应的抛物面天线载荷指向估计方法 | |
| CN115759498A (zh) | 基于双向长短期记忆网络的无人机飞行路径实时预测方法 | |
| CN111816005A (zh) | 基于ads-b的远程驾驶飞机环境监视优化方法 | |
| Jiang et al. | Extended Kalman filter with input detection and estimation for tracking manoeuvring satellites | |
| CN114740497A (zh) | 基于ukf多源融合探测的无人机欺骗方法 | |
| CN113189585A (zh) | 基于无人机双基地sar系统的运动误差补偿算法 | |
| Thevenon et al. | Estimation of the base station position error in a RTK receiver using state augmentation in a Kalman filter | |
| Qi et al. | Maneuvering target tracking algorithm based on adaptive markov transition probabilitiy matrix and IMM-MGEKF | |
| Bottino et al. | System Architecture Design of a UAV for Automated Cinematography in GNSS-Challenging Scenarios | |
| CN115346401B (zh) | 一种低空无人机监视和航迹预测方法 | |
| CN112348228B (zh) | 一种基于高度-航程剖面的飞行器速度预测方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |