一种无人机状态估计方法、系统、设备及可读存储介质与流程

1.本发明属于无人机技术领域，特别涉及一种无人机状态估计方法、系统、设备及可读存储介质。


背景技术：

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


技术实现要素：

5.为了解决无人机的扩展卡尔曼算法中误差项对状态估计的稳定性和准确性的影响，本着易于实现、稳定性好且计算量增加可控的原则，提供一种基于迭代扩展卡尔曼滤波的无人机状态估计方法，从而提高无人机飞行状态的稳定性和可靠性，进一步促进无人机行业的发展与普及。
6.以下为本发明的技术方案：
7.本发明提供了一种无人机状态估计方法，所述方法包括：
8.建立系统模型；
9.根据系统模型，在指定时刻，预测标准扩展卡尔曼的状态估计以及初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计；
10.执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，并进行量测的指定次数的迭代更新计算。
11.进一步地，
12.所述建立系统模型包括建立系统状态方程和系统量测方程。
13.进一步地，所述系统状态方程如下：
[0014][0015]
其中，q
k+1
为k+1时刻的单位四元数值；qk为k时刻的单位四元数值；δθ
mx
为x轴的角度变化量的量测值；δθ
my
为y轴的角度变化量的量测值2δθ
mz
表示z轴角度变化量的量测值；δθ
bx
表示x轴的角度变化量的偏差值；δθ
by
表示y轴的角度变化量的偏差值；δθ
bz
表示z轴角度变化量的偏差值；δθ
nx
表示x轴角度变化量的噪声值；δθ
ny
表示y轴角度变化量的噪声值；δθ
nz
表示z轴角度变化量的噪声值。
[0016]
进一步地，所述系统量测方程如下：
[0017]zmag
＝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为姿态四元数值，mn导航坐标系北方的磁场值，me为导航坐标系东方的磁场值，md为导航坐标系大地的磁场值，m为机体坐标系下的磁场量测值，m
ned
导航坐标系下的北东地方向的磁场向量，q为四元数向量，为磁场量测值关于姿态四元数的雅克比矩阵，mb为磁场量测偏差值，rb←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-(hk(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次迭代的状态值，hk(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]
迭代模块，用于循环指定次数，执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，进行量测的指定次数的迭代更新计算。
[0046]
进一步地，所述系统还包括：
[0047]
循环模块，用于在执行完最后一次的迭代更新计算后，将最后一次的计算结果输出，并用于指定时刻的下一刻的状态参数和协方差矩阵的初始化。
[0048]
本发明还提供一种设备，包括存储器、处理器及存储在存储器上并能够在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述所述方法的步骤。
[0049]
另一方面，本发明还提供一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现上述所述方法的步骤。
[0050]
本发明提供的一种无人机状态估计方法及系统，通过迭代方式对新估计点重新进行泰勒级数展开来降低线性误差，实现扩展卡尔曼的高阶性能，降低线性化误差项对状态估计准确性的影响。
[0051]
本发明的其它特征和优点将在随后的说明书中阐述，并且，部分地从说明书中变得显而易见，或者通过实施本发明而了解。本发明的目的和其他优点可通过在说明书、权利要求书以及附图中所指出的结构来实现和获得。
附图说明
[0052]
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一个简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0053]
图1示出了根据本发明实施例的无人机状态估计方法的流程图。
[0054]
图2示出了根据本发明实施例的无人机状态估计系统的结构示意图。
[0055]
图3示出了根据本发明实施例的设备结构示意图。
[0056]
图4示出了根据本发明实施例的可读存储介质的结构示意图。
具体实施方式
[0057]
为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地说明，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0058]
具体的，如图1所述的，本发明提供了一种无人机状态估计方法，所述方法包括：
[0059]
建立系统模型；
[0060]
根据系统模型，在指定时刻，进行标准扩展卡尔曼的状态估计的预测以及初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计；
[0061]
执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，并进行量测的指定次数的迭代更新计算。
[0062]
下面进行详细地说明。
[0063]
1、建立系统模型
[0064]
为了充分展现迭代计算公式的具体算法模型，采用姿态估计作为建模对象，对姿态估计的问题进行求解。
[0065]
在本实施例中，系统模型的建立包括建立系统状态方程和系统量测方程，其中：
[0066]
系统的系统状态方程如下：
[0067][0068]
其中，q
k+1
为k+1时刻的单位四元数值；qk为k时刻的单位四元数值；δθ
mx
为x轴的角度变化量的量测值；δθ
my
为y轴的角度变化量的量测值；δθ
mz
表示z轴角度变化量的量测值；δθ
bx
表示x轴的角度变化量的偏差值；δθ
by
表示y轴的角度变化量的偏差值；δθ
bz
表示z轴角度变化量的偏差值；δθ
nx
表示x轴角度变化量的噪声值；δθ
ny
表示y轴角度变化量的噪声值；δθ
nz
表示z轴角度变化量的噪声值。
[0069]
系统的系统量测方程如下：
[0070]zmag
＝h
mag
·
x+r
mag
；
[0071]
其中，z
mag
为磁力计传感器的采集获取数据；h
mag
为系统量测方程雅克比矩阵值；r
mag
为磁力计的噪声系数矩阵，x表示系统状态变量(磁场值)。
[0072]
另外，本实施例的系统模型的高斯系统的噪声符合正态分布，即本实施例的系统模型具有限制条件，该限制条件通过如下公式确定：
[0073]
wk～(0，qk)
[0074]
vk～(0，rk)；
[0075]
其中，w为过程噪声，v为测量噪声，k为指定时刻值；qk为wk的方差矩阵，rk为vk的方差矩阵。
[0076]
在系统模型中，还需要初始化滤波器，即需要对系统模型中的状态参数和协方差矩阵分别赋予初值：
[0077][0078][0079]
其中，为赋予初值后的状态参数，为赋予初值后的协方差矩阵，x0为初始状态值，为初始状态估计值，协方差矩阵是根据旋转向量方差0.1计算对应的四元数的协方差。
[0080]
2、进行标准扩展卡尔曼的状态估计的预测
[0081]
在本实施例中，可根据系统模型，进行标准扩展卡尔曼的状态估计的预测。其中，标准扩展卡尔曼的状态估计的预测公式如下：
[0082][0083]
其中，f为系统状态方程的雅克比矩阵，t为矩阵转置运算符，和p
k+1k
均为k+1时刻的协方差，p
k|k
为k时刻的协方差值，q
k+1|k
为k+1时刻的噪声矩阵。
[0084]qk+1k
可通过以下公式确定：
[0085]qk+1|k
＝g
·qk|k
·gt
；
[0086]
其中，g为噪声转移矩阵。
[0087]
f可通过如下公式确定：
[0088][0089]
g可通过如下公式确定：
[0090][0091]
综上，可得，q
k+1|k
＝f
·qk|k
·ft
+g
·qk|k
。为k时刻角度变化量的噪声。
[0092]
3、初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计
[0093]
在k时刻，初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计的输出值，包括初始迭代状态参数和协方差矩阵。通过量测模块执行迭代扩展卡尔曼滤波的状态估计。
[0094]
在本实施例中：
[0095]
相当于q
k+1|k，0
＝q
k+1|k
；
[0096]
相当于p
k+1|k，0
＝p
k+1|k
；
[0097]
其中，q
k+1|k
和p
k+1|k
为标准扩展卡尔曼滤波器的每次计算的输出，作为迭代部分的初值设置。为k时刻第一次迭代更新的状态，为k时刻第一次迭代的协方差值。
[0098]
4、执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开
[0099]
设i为迭代控制量，循环i＝1，
…
，n(此处n为量测更新期望迭代次数)，执行在新估计点(标准扩展卡尔曼滤波估计的估计点)的泰勒展开，即执行以下方程：
[0100]
第i次的系统量测方程的雅可比矩阵(h
k，i
)如下：
[0101][0102]
其中，q0，q1，q2，q3为姿态四元数值，mn导航坐标系北方的磁场值，me为导航坐标系东方的磁场值，md为导航坐标系大地的磁场值，m为机体坐标系下的磁场量测值，m
ned
导航坐标系下的北东地方向的磁场向量，q为四元数向量，为磁场量测值关于姿态四元数的雅克比矩阵，rb←n为大地坐标系到机体坐标系的旋转矩阵，mb为磁场量测偏差值。
[0103]
观测噪声的转移矩阵(m
k，i
)如下：
[0104][0105]
5、进行量测的指定次数的迭代更新计算
[0106]
量测的迭代更新计算公式包括：
[0107]
[0108][0109][0110]
x
k+1|k+1，i+1
＝x
k+1|k
+h
k，i
[z
mag-(hk(x
k|k，i
)+h
k，i
(x
k|k-1-x
k|k，i
))]；
[0111]
其中，s为中间计算变量，k
k，i
为卡尔曼增益值，为k+1时刻的第i+1次迭代的协方差值，x
k+1|k+1，i+1
为k+1时刻的第i+1次迭代的状态值，hk(x
k|k，i
)+h
k，i
(x
k|k-1-x
k|k，i
)为先验估计在x
k|k，i
处的值；i为单位矩阵。
[0112]
6、状态更新输出
[0113]
在执行完最后一次的迭代更新计算后，将最后一次的计算结果输出，并用于指定时刻的下一刻(k+1时刻)的状态参数和协方差矩阵的初始化。
[0114]
其中，在执行完最后一次的迭代更新计算后，最后一次的计算结果可通过如下的状态估计(x
k|k
)和估计误差协方差(p
k|k
)的更新公式表示：
[0115]
x
k|k
＝x
k|k，n+1
；
[0116]
p
k|k
＝p
k|k，n+1
。
[0117]
综上所述，最大迭代次数n为可调节参数，首先根据硬件平台计算能力设置，以及通过测试出选定自驾平台的最优迭代次数，实现计算量不超界。通过选定平台可控的计算量调节参数(n)，提升了无人机的自驾平台的飞行控制系统的稳定性和可靠性。
[0118]
如图2所示的，本实施例还提供一种无人机状态估计系统，所述系统包括：
[0119]
模型建立模块，用于建立系统模型；
[0120]
预测模块，用于根据系统模型，在指定时刻，进行标准扩展卡尔曼的状态估计的预测；
[0121]
初始化模块，用于根据系统模型在指定时刻，初始化迭代扩展卡尔曼滤波估计为标准扩展卡尔曼滤波估计；
[0122]
迭代模块，用于循环指定次数，执行在标准扩展卡尔曼滤波估计的估计点的泰勒展开，进行量测的指定次数的迭代更新计算。
[0123]
所述系统还包括：
[0124]
循环模块，用于在执行完最后一次的迭代更新计算后，将最后一次的计算结果输出，并用于指定时刻的下一刻的状态参数和协方差矩阵的初始化，其中，在执行完最后一次的迭代更新计算后，状态估计和估计误差协方差的更新公式如下：
[0125]
x
k|k
＝x
k|k，n+1
；
[0126]
p
k|k
＝p
k|k，n+1
。
[0127]
本发明的一种无人机状态估计方法的各步骤实现的功能以及实现方式与本发明的一种无人机状态估计系统中各模块的其他功能以及实现方式对应一致，因此，此处不再赘述。
[0128]
另外，本发明还提供一种设备(图3)以及一种计算机可读存储介质(图4)，其中：
[0129]
所述设备包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述方法的步骤。
[0130]
所述可读存储介质上存储有计算机程序，所述计算机程序被处理器执行时实现上述方法的步骤。
[0131]
另外，本发明主要适用于无人机自驾平台，要求无人机自驾平台具备至少一组陀螺仪和磁力计传感器，其中，陀螺仪用于获取角度变化量以及中的δθ
mx
项值，磁力计传感器用于磁场数据采集(即获取z
mag
)。
[0132]
以上所述仅是本发明的较佳实施例而已，并非对本发明做任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专业的技术人员，在不脱离本发明技术方案的范围内，当可利用上述揭示的技术内容作出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。