一种无人机状态估计方法、系统、设备及可读存储介质技术方案

本发明专利技术提供了一种无人机状态估计方法，所述方法包括：建立系统模型；根据系统模型，在指定时刻，进行标准扩展卡尔曼的状态估计的预测以及初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计；执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，并进行量测的指定次数的迭代更新计算。本发明专利技术提高了无人机飞行状态的稳定性和可靠性，进一步促进无人机行业的发展与普及。发展与普及。发展与普及。

【技术实现步骤摘要】
一种无人机状态估计方法、系统、设备及可读存储介质


[0001]本专利技术属于无人机
，特别涉及一种无人机状态估计方法、系统、设备及可读存储介质。

技术介绍

[0002]无人机是指无人驾驶的飞行器或航空器，各种类型的无人机在不同领域发挥着作用。状态估计是实现无人机自主导航控制的核心手段，是指实时确定航空器在轨位置、速度及姿态等导航参数，是无人机导航控制技术的重要发展方向之一。传统状态估计的方法主要采用最小二乘法、卡尔曼滤波算法等。为了提高自主导航控制性能，状态估计方法不断发展，目前主流算法为标准扩展卡尔曼滤波算法。
[0003]目前，标准扩展卡尔曼滤波算法是在卡尔曼估计点进行泰勒的一阶展开，将非线性系统转换成线性系统进行状态估计，但是在模型转换中引入了误差项。误差在一般的非线性系统中可以不被考虑，在高度非线性系统中，误差项对状态估计的准确性和稳定性至关重要。二阶扩展卡尔曼可以有效降低误差项影响，但是需要计算二阶偏微分项，计算量较大。
[0004]由此，需要设计一种无人机状态估计方案，以解决无人机的扩展卡尔曼算法中误差项对状态估计的稳定性和准确性影响的问题。

技术实现思路

[0005]为了解决无人机的扩展卡尔曼算法中误差项对状态估计的稳定性和准确性的影响，本着易于实现、稳定性好且计算量增加可控的原则，提供一种基于迭代扩展卡尔曼滤波的无人机状态估计方法，从而提高无人机飞行状态的稳定性和可靠性，进一步促进无人机行业的发展与普及。
[0006]以下为本专利技术的技术方案：
[0007]本专利技术提供了一种无人机状态估计方法，所述方法包括：
[0008]建立系统模型；
[0009]根据系统模型，在指定时刻，预测标准扩展卡尔曼的状态估计以及初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计；
[0010]执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，并进行量测的指定次数的迭代更新计算。
[0011]进一步地，
[0012]所述建立系统模型包括建立系统状态方程和系统量测方程。
[0013]进一步地，所述系统状态方程如下：
[0014][0015]其中，q
k+1
为k+1时刻的单位四元数值；q
k
为k时刻的单位四元数值；Δθ
mx
为X轴的角度变化量的量测值；Δθ
my
为Y轴的角度变化量的量测值2Δθ
mz
表示Z轴角度变化量的量测值；Δθ
bx
表示X轴的角度变化量的偏差值；Δθ
by
表示Y轴的角度变化量的偏差值；Δθ
bz
表示Z轴角度变化量的偏差值；Δθ
nx
表示X轴角度变化量的噪声值；Δθ
ny
表示Y轴角度变化量的噪声值；Δθ
nz
表示Z轴角度变化量的噪声值。
[0016]进一步地，所述系统量测方程如下：
[0017]z
mag
＝H
Mag
·
x+R
Mag
；
[0018]其中，z
mag
为磁力计传感器的采集获取数据；H
Mag
为系统量测方程雅克比矩阵值；R
Mag
为磁力计的噪声系数矩阵，x表示系统状态变量。
[0019]进一步地，标准扩展卡尔曼的状态估计的预测公式如下：
[0020][0021]其中，F为系统状态方程的雅克比矩阵，T为矩阵转置运算符，和P
k+1|k
均为k+1时刻的协方差，P
k|k
为k时刻的协方差值，Q
k+1|k
为k+1时刻的噪声矩阵。
[0022]进一步地，循环指定次数，执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，包括：执行系统量测方程的雅可比矩阵和观测噪声的转移矩阵。
[0023]进一步地，系统量测方程的雅可比矩阵如下：
[0024][0025]其中，i为迭代控制量，i＝1，
…
，N；q0，q1，q2，q3为姿态四元数值，m
N
导航坐标系北方的磁场值，m
E
为导航坐标系东方的磁场值，m
D
为导航坐标系大地的磁场值，m为机体坐标系下的磁场量测值，m
NED
导航坐标系下的北东地方向的磁场向量，q为四元数向量，为磁场量测值关于姿态四元数的雅克比矩阵，m
b
为磁场量测偏差值，R
B
←
N
为大地坐标系到机体坐标系的旋转矩阵。
[0026]进一步地，观测噪声的转移矩阵如下：
[0027][0028]其中v为测量噪声。
[0029]进一步地，量测的指定次数的迭代更新计算公式包括：
[0030][0031][0032][0033]x
k+l|k+1，i+1
＝x
k+1|k
+K
k，i
[z
Mag
‑
(h
k
(x
k|k，i
)+H
k，i
(x
k|k
‑1‑
x
k|k，i
))]；
[0034]其中，S为中间计算变量，K
k，i
为卡尔曼增益值，为k+1时刻的第i+1次迭代的协方差值，x
k+1|k+1，i+1
为k+1时刻的第i+1次迭代的状态值，h
k
(x
k|k，i
)+H
k，i
(x
k|k
‑1‑
x
k|k，i
)为先验估计在x
k|k，i
处的值；I为单位矩阵。
[0035]进一步地，所述方法还包括：
[0036]在执行完最后一次的迭代更新计算后，将最后一次的计算结果输出，并用于指定时刻的下一刻的状态参数和协方差矩阵的初始化。
[0037]进一步地，在执行完最后一次的迭代更新计算后，状态估计和估计误差协方差的更新公式如下：
[0038]x
k|k
＝x
k|k,N+1
；
[0039]P
k|k
＝P
k|k,N+1
；
[0040]其中，x
k|k
为状态估计，P
k|k
为估计误差协方差，k表示时刻，N为最大迭代次数。
[0041]一方面，本专利技术还提供一种无人机状态估计系统，所述系统包括：
[0042]模型建立模块，用于建立系统模型；
[0043]预测模块，用于根据系统模型，在指定时刻，进行标准扩展卡尔曼的状态估计的预测；
[0044]初始化模块，用于根据系统模型，在指定时刻，初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计；
[0045本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种无人机状态估计方法，其特征在于，所述方法包括：建立系统模型；根据系统模型，在指定时刻，预测标准扩展卡尔曼的状态估计以及初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计；执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，并进行量测的指定次数的迭代更新计算。2.根据权利要求1所述的一种无人机状态估计方法，其特征在于，所述建立系统模型包括建立系统状态方程和系统量测方程。3.根据权利要求2所述的一种无人机状态估计方法，其特征在于，所述系统状态方程如下：其中，q
k+1
为k+1时刻的单位四元数值；q
k
为k时刻的单位四元数值；Δθ
mx
为X轴的角度变化量的量测值；Δθ
my
为Y轴的角度变化量的量测值；Δθ
mz
表示Z轴角度变化量的量测值；Δθ
bx
表示X轴的角度变化量的偏差值；Δθ
by
表示Y轴的角度变化量的偏差值；Δθ
bz
表示Z轴角度变化量的偏差值；Δθ
nx
表示X轴角度变化量的噪声值；Δθ
ny
表示Y轴角度变化量的噪声值；Δθ
nz
表示Z轴角度变化量的噪声值。4.根据权利要求3所述的一种无人机状态估计方法，其特征在于，所述系统量测方程如下：z
mg
＝H
Mag
·
x+R
Mag
；其中，z
mag
为磁力计传感器的采集获取数据；H
Mag
为系统量测方程雅克比矩阵值；R
Mag
为磁力计的噪声系数矩阵，x表示系统状态变量。5.根据权利要求4所述的一种无人机状态估计方法，其特征在于，标准扩展卡尔曼的状态估计的预测公式如下：其中，F为系统状态方程的雅克比矩阵，T为矩阵转置运算符，和P
k+1|k
均为k+1时刻的协方差，P
k|k
为k时刻的协方差值，Q
k+1|k
为k+1时刻的噪声矩阵。6.根据权利要求5所述的一种无人机状态估计方法，其特征在于，循环指定次数，执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，包括：执行系统量测方程的雅可比矩阵和观测噪声的转移矩阵。7.根据权利要求6所述的一种无人机状态估计方法，其特征在于，系统量测方程的雅可比矩阵如下：
其中，i为迭代控制量，i＝1，
…
，N；q0，q1，q2，q3为姿态四元数值，m
N
导航坐标系北方的磁场值，m
E
为导航坐标系东方的磁场值，m
D
为导航坐标系大地的磁场值，m为机体坐标系下的磁场量测值，m
NED
导航坐标系下的北东地方向的磁场向量，q为四元数向量，为磁场量测值关于姿态四元数的雅克比矩阵，m
b
为磁场量测偏差值，R
B
←
N
为大地坐标系到机体坐标系的...

【专利技术属性】
技术研发人员：黄桃丽，辛艳峰，李琳琳，候洁明，
申请(专利权)人：合肥赛为智能有限公司，
类型：发明
国别省市：