一种水平姿态测量方法、系统、处理设备及存储介质与流程

1.本发明涉及惯性技术领域，特别是关于一种水平姿态测量方法、系统、处理设备及存储介质。


背景技术：

2.mimu(micro inertial measurement unit，微惯性测量单元)由于其自身小体积、低功耗、低成本等特点，已经成为小型无人机、无人车、微型稳定平台以及人体可穿戴式智能化设备的首选器件，应用于姿态测量及导航定位。目前，应用于mimu测姿的方法主要是利用mimu内加速度计的精度和稳定性远高于陀螺器件水准的特点，在考虑载体运动过程中的静止或准静止状态下，通过互补滤波等方法融合陀螺的解算姿态和加速度计的估计姿态，达到提升载体水平姿态的测量精度。
3.但是，目前采用互补滤波方法进行融合时，对于不同姿态的权重设置仅依靠经验值，或者基于加速度计幅值的一阶线性关系，不能更为准确地评估加速度计水平姿态估计的精度。因此，需要一种能够实时动态地计算加速度计水平姿态的估计精度的方法，从而更为准确的进行姿态融合解算。


技术实现要素：

4.针对上述问题，本发明的目的是提供一种水平姿态测量方法、系统、处理设备及存储介质，能够实时动态地计算加速度计水平姿态的估计精度。
5.为实现上述目的，本发明采取以下技术方案：第一方面，提供一种水平姿态测量方法，包括：
6.将mimu放置于载体上，根据载体的初始姿态，对mimu的陀螺输出数据进行姿态解算，获取载体的姿态角；
7.根据mimu内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵；
8.根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角；
9.根据载体的噪声动态观测方差阵和失准角，确定mimu的陀螺零偏，并得到更新后的载体姿态角；
10.对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角。
11.进一步地，所述将mimu放置于载体上，根据载体的初始姿态，对mimu的陀螺输出数据进行姿态解算，获取载体的姿态角，包括：
12.将mimu放置于载体上，获取mems陀螺的初始零偏；
13.采用四元数姿态更新方法，根据载体的初始姿态，对mimu的陀螺输出数据进行姿态解算，获取载体的姿态角包括横滚角俯仰角和航向角
14.进一步地，所述根据mimu内加速度计的输出数据，实时计算进入静止状态的载体
水平姿态角和载体的噪声动态观测方差阵，包括：
15.根据mimu内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断；
16.当载体进入静止状态时，根据加速度计的输出数据，实时计算载体的水平姿态角；
17.根据加速度计输出的水平姿态方差，实时确定噪声动态观测方差阵。
18.进一步地，所述根据mimu内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断，包括：
19.设定三轴加速度计的输出为其中，b为载体坐标系，和分别为x轴、y轴、z轴加速度计的输出，取前右下坐标投影，导航系n系选择北东地坐标；
20.设定加速度计输出幅值的阈值范围：
[0021][0022]
其中，g为重力加速度的值；k为阈值系数；为加速度计的输出含有误差；为三轴加速度计的输出模值。
[0023]
进一步地，所述根据加速度计输出的水平姿态方差，实时确定噪声动态观测方差阵，包括：
[0024]
根据加速度计输出的噪声方差，采用下述模型，实时动态确定水平姿态角的方差：
[0025][0026]
其中，d(φn)、d(φe)为水平姿态角的方差；为加速度计输出的噪声方差；
[0027]
根据实时动态确定的水平姿态角的方差，实时确定噪声动态观测方差阵
[0028][0029]
其中，vk为k时刻的噪声；vj为j时刻的噪声；δ
kj
为狄拉克函数，rk为航向失准角的方差d(φd)以及实时动态确定的水平姿态角的方差d(φn)和d(φe)：
[0030][0031]
进一步地，所述根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角，包括：
[0032]
根据载体的姿态角和进入静止状态的载体水平姿态角，确定水平参考姿态的欧拉角；
[0033]
采用旋转矢量计算方法，根据水平参考姿态的欧拉角和载体的姿态角，获取载体的水平失准角；
[0034]
将获取的载体姿态角中的航向角和判断静止初始时刻的航向角的差值作为载体的航向失准角。
[0035]
进一步地，所述根据噪声动态观测方差阵和载体的失准角，确定mimu的陀螺零偏，并得到更新后的载体姿态角，包括：
[0036]
采用kf滤波方法，根据噪声动态观测方差阵和载体的失准角，确定mimu的陀螺零偏；
[0037]
采用四元数姿态更新方法，根据mimu的陀螺零偏，得到更新后的载体姿态角。
[0038]
第二方面，提供一种水平姿态测量系统，包括：
[0039]
姿态解算模块，用于根据载体的初始姿态，对放置于载体上的mimu的陀螺输出数据进行姿态解算，获取载体的姿态角；
[0040]
噪声动态观测方差阵计算模块，用于根据mimu内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵；
[0041]
失准角计算模块，用于根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角；
[0042]
陀螺零偏计算模块，用于根据载体的噪声动态观测方差阵和失准角，确定mimu的陀螺零偏，并得到更新后的载体姿态角；
[0043]
融合模块，用于对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角。
[0044]
第三方面，提供一种处理设备，包括计算机程序指令，其中，所述计算机程序指令被处理设备执行时用于实现上述水平姿态测量方法对应的步骤。
[0045]
第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序指令，其中，所述计算机程序指令被处理器执行时用于实现上述水平姿态测量方法对应的步骤。
[0046]
本发明由于采取以上技术方案，其具有以下优点：
[0047]
1、本发明建立实时动态方差估计模型，可以准确评估由加速度计进行水平姿态估计的精度，可以为加速度计估计的水平姿态应用提供精度衡量。
[0048]
2、本发明通过实时动态方差估计，将kf滤波和互补滤波有效结合，实现两层互补，且互补系数的设定不需要再依据经验值或是简单的线性估计，而是更为精准的实时权重分配，能够进一步提升水平姿态的估计精度以及减弱环境震动所来的干扰影响。
[0049]
综上所述，本发明可以广泛应用于惯性技术领域中。
附图说明
[0050]
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。在整个附图中，用相同的附图标记表示相同的部件。在附图中：
[0051]
图1是本发明一实施例提供的方法流程示意图；
[0052]
图2是本发明一实施例提供的加速度计动态方差模型验证效果图，其中，图2(a)为
模型计算横滚角误差，图2(b)为理论统计横滚角误差，图2(c)为模型计算俯仰角误差，图2(d)为理论统计俯仰角误差。
具体实施方式
[0053]
下面将参照附图更详细地描述本发明的示例性实施方式。虽然附图中显示了本发明的示例性实施方式，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。
[0054]
应理解的是，文中使用的术语仅出于描述特定示例实施方式的目的，而无意于进行限制。除非上下文另外明确地指出，否则如文中使用的单数形式“一”、“一个”以及“所述”也可以表示包括复数形式。术语“包括”、“包含”、“含有”以及“具有”是包含性的，并且因此指明所陈述的特征、步骤、操作、元件和/或部件的存在，但并不排除存在或者添加一个或多个其它特征、步骤、操作、元件、部件、和/或它们的组合。文中描述的方法步骤、过程、以及操作不解释为必须要求它们以所描述或说明的特定顺序执行，除非明确指出执行顺序。还应当理解，可以使用另外或者替代的步骤。
[0055]
本发明实施例提供的水平姿态测量方法、系统、处理设备及存储介质，基于动态方差估计，能够解决目前基于mimu进行姿态测量时姿态融合权重系数设定不精确的问题，给出精确的加速度计估计水平姿态的动态方差估计模型，有效融合两种滤波方法，提升水平姿态测量精度。本发明通过建立的实时动态方差估计模型，针对加速度计进行水平姿态估计给与了实时动态方差评估，以此为纽带，有效地将kf滤波和互补滤波进行结合，实现两层互补处理，并通过实时精准的权重分配，进一步提升水平姿态的估计精度，在工程上具有广泛的应用价值。
[0056]
实施例1
[0057]
如图1所示，本实施例提供一种水平姿态测量方法，包括以下步骤：
[0058]
1)将mimu放置于载体上，获取mems陀螺的初始零偏。
[0059]
具体地，将mimu放置于载体上，上电后，对初始静止时若干秒(例如3秒)的mimu的陀螺输出数据取平均值，并作为mems(微机电系统)陀螺的初始零偏。
[0060]
2)采用四元数姿态更新方法，根据载体的初始姿态，对mimu的陀螺输出数据进行姿态解算，获取载体的姿态角包括横滚角俯仰角和航向角
[0061]
3)根据mimu内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断。
[0062]
具体地，由于mems陀螺的精度较差，且容易受到温度的影响，特别是开机预热过程中的零偏不稳定性很大，此时仅依靠mems陀螺进行水平姿态求取会产生较大误差，因此，本发明通过mimu内加速度计的输出数据和加速度计输出幅值阈值范围进行载体的静止状态判断，具体过程为：
[0063]
3.1)设定三轴加速度计的输出为其中，b为载体坐标系，其中，b为载体坐标系，和分别为x轴、y轴、z轴加速度计的输出，取前右下坐标投影，导航系n系选择北东地(n-e-d)坐标。
[0064]
3.2)取三轴加速度计的输出模值：
[0065][0066]
3.3)设定加速度计输出幅值的阈值范围：
[0067][0068]
其中，g为当地重力加速度的值；k为阈值系数，一般可以取0.5％～2％；为加速度计的输出含有误差，有f为加速度计的理论输出(在静止或准静止状态下f＝gb)；ba为加速度计的零偏，且ba＝[b
x b
y bz]
t
；ε为器件输出噪声及环境影响噪声，一般考虑为白噪声，且ε＝[ε
x b
y bz]
t
，ε
x
、by和bz分别为x轴、y轴、z轴加速度计的输出的白噪声。
[0069]
3.4)根据mimu内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断。
[0070]
进一步地，当载体进入静止状态时，此时，由单纯的四元数姿态解算进入到姿态融合估计阶段，该阶段包括两层并行滤波处理。
[0071]
4)当载体进入静止状态时，采用矢量观测方法，根据mimu内加速度计的输出数据，实时计算载体的水平姿态角，包括横滚角和俯仰角，具体为：
[0072]
4.1)将mimu内加速度计的输出数据进行单位化：
[0073][0074]
其中，()u表示矢量的单位化；为单位化后的加速度计的输出数据；为单位化后的加速度计的输出数据；和分别为加速度计测量值单位化后的x轴、y轴、z轴的输出。
[0075]
4.2)根据单位化后加速度计的输出数据，实时计算载体的水平姿态角：
[0076][0077]
其中，为由加速度计估计的横滚角；为由加速度计估计的俯仰角。
[0078]
5)根据mimu内加速度计的输出数据，实时确定噪声动态观测方差阵，具体为：
[0079]
5.1)根据加速度计输出的噪声方差，采用下述模型(6)，实时动态确定水平姿态角的方差：
[0080][0081]
其中，d(dn)、d(φe)为水平姿态角的方差；为加速度计输出的噪声方差。对于该模型正确性的验证如图2所示，通过与理论统计特征相比，该模型的计算结果
与真实值很吻合。
[0082]
5.2)对于航向失准角的方差d(φd)的估计，由于是采用伪观测量的方式，即观测量是真实的误差增量，从而可以将航向失准角的方差d(φd)设定成为与水平姿态角的方差d(φn)或d(φe)同量级的数值，而不影响水平姿态精度的估计。
[0083]
5.3)根据实时动态确定的水平姿态角的方差，实时确定噪声动态观测方差阵
[0084][0085]
其中，vk为k时刻的噪声；vj为j时刻的噪声；δ
kj
为狄拉克函数，当k等于j时，为1；否则，为0；rk为航向失准角的方差d(φd)以及实时动态确定的水平姿态角的方差d(φn)和d(φe)，即：
[0086][0087]
6)根据步骤2)中获取的载体的姿态角和步骤4)中计算的载体的水平姿态角实时确定载体的失准角，包括水平失准角和航向失准角，具体为：
[0088]
6.1)根据步骤2)中获取的载体的姿态角和步骤4)中计算的载体的水平姿态角确定水平参考姿态的欧拉角
[0089]
6.2)采用旋转矢量计算方法，根据水平参考姿态的欧拉角和步骤2)中获取的载体的姿态角获取载体的水平失准角φn和φe。
[0090]
6.3)将步骤2)中获取的载体姿态角中的航向角和判断静止初始时刻的航向角的差值作为载体的航向失准角φd。
[0091]
7)采用kf滤波(卡尔曼滤波)方法，根据噪声动态观测方差阵和载体的失准角，确定mimu的陀螺零偏。
[0092]
常规的kf滤波方法，其状态量为姿态误差和陀螺零偏，观测量为失准角，通过mimu内加速度计的水平姿态和航向静止状态下的增量误差获得：
[0093][0094]
[0095][0096]
其中，x(t)为系统状态变量；f(t)为状态转移矩阵；g(t)为系统噪声矩阵；u(t)为系统噪声阵；z(t)为系统观测量；h(t)为系统观测矩阵；v(t)为观测噪声，其方差参数设定就是[φ
n φ
e φd]
t
为失准角即姿态误差角，φn、φe和φd分别为水平失准角和航向失准角；[b
wx b
wy b
wz
]
t
为三轴陀螺的输出零偏；为三轴陀螺输出噪声在导航坐标系(n系)下的投影，ωd为地球自转角速度在d向的投影，ωd＝-ω
ie
sinl，ω
ie
为地球自转角速度，l为位置纬度；ωn为地球自转角速度在n向的投影，ωn＝ω
ie
cosl；为b系到n系的旋转矩阵。
[0097]
进一步地，进行kf滤波，估计出三轴陀螺的零偏及失准角，一方面可以修正当前陀螺姿态解算的结果，同时可以实时更新三轴陀螺的零偏估值，用于下个运动阶段姿态解算，可以提升解算精度。
[0098]
8)采用四元数姿态更新方法，根据mimu的陀螺零偏，得到更新后的载体姿态角。
[0099]
9)采用互补滤波方法，对更新后的载体姿态角和步骤4)中计算的水平姿态角9)采用互补滤波方法，对更新后的载体姿态角和步骤4)中计算的水平姿态角进行融合，确定载体的水平姿态角
[0100][0101]
其中，为更新后的载体姿态角；l和m为融合权重系数，根据实时估计的方差选取，即：
[0102][0103][0104]
其中，pk(i,i)为对于状态量估计的方差矩阵的第i个对角元素，pk为kf滤波递推公式中的系统状态估计方差矩阵。
[0105]
实施例2
[0106]
本实施例提供一种水平姿态测量系统，包括：
[0107]
姿态解算模块，用于根据载体的初始姿态，对放置于载体上的mimu的陀螺输出数据进行姿态解算，获取载体的姿态角。
[0108]
噪声动态观测方差阵计算模块，用于根据mimu内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵。
[0109]
失准角计算模块，用于根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角。
[0110]
陀螺零偏计算模块，用于根据载体的噪声动态观测方差阵和失准角，确定mimu的陀螺零偏，并得到更新后的载体姿态角。
[0111]
融合模块，用于对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角。
[0112]
实施例3
[0113]
本实施例提供一种与本实施例1所提供的水平姿态测量方法对应的处理设备，处理设备可以是用于客户端的处理设备，例如手机、笔记本电脑、平板电脑、台式机电脑等，以执行实施例1的方法。
[0114]
所述处理设备包括处理器、存储器、通信接口和总线，处理器、存储器和通信接口通过总线连接，以完成相互间的通信。存储器中存储有可在处理设备上运行的计算机程序，处理设备运行计算机程序时执行本实施例1所提供的水平姿态测量方法。
[0115]
在一些实现中，存储器可以是高速随机存取存储器(ram：random access memory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。
[0116]
在另一些实现中，处理器可以为中央处理器(cpu)、数字信号处理器(dsp)等各种类型通用处理器，在此不做限定。
[0117]
实施例4
[0118]
本实施例提供一种与本实施例1所提供的水平姿态测量方法对应的计算机程序产品，计算机程序产品可以包括计算机可读存储介质，其上载有用于执行本实施例1所述的水平姿态测量方法的计算机可读程序指令。
[0119]
计算机可读存储介质可以是保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意组合。
[0120]
上述各实施例仅用于说明本发明，其中各部件的结构、连接方式和制作工艺等都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变换和改进，均不应排除在本发明的保护范围之外。