CN114111770B - 一种水平姿态测量方法、***、处理设备及存储介质 - Google Patents
一种水平姿态测量方法、***、处理设备及存储介质 Download PDFInfo
- Publication number
- CN114111770B CN114111770B CN202111373775.XA CN202111373775A CN114111770B CN 114111770 B CN114111770 B CN 114111770B CN 202111373775 A CN202111373775 A CN 202111373775A CN 114111770 B CN114111770 B CN 114111770B
- Authority
- CN
- China
- Prior art keywords
- carrier
- angle
- attitude
- accelerometer
- mimu
- 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
- 238000012545 processing Methods 0.000 title claims abstract description 20
- 238000000691 measurement method Methods 0.000 title claims abstract description 17
- 238000003860 storage Methods 0.000 title claims abstract description 17
- 238000000034 method Methods 0.000 claims abstract description 42
- 230000003068 static effect Effects 0.000 claims abstract description 33
- 238000004364 calculation method Methods 0.000 claims abstract description 30
- 239000011159 matrix material Substances 0.000 claims abstract description 25
- 238000005259 measurement Methods 0.000 claims abstract description 13
- 238000001914 filtration Methods 0.000 claims description 19
- 238000004590 computer program Methods 0.000 claims description 11
- 230000000295 complement effect Effects 0.000 claims description 9
- 230000004927 fusion Effects 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 5
- 230000001133 acceleration Effects 0.000 claims description 3
- 230000036544 posture Effects 0.000 description 12
- 230000008569 process Effects 0.000 description 4
- 230000008901 benefit Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 3
- 238000009826 distribution Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 238000012821 model calculation Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process 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
- 230000003287 optical effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/183—Compensation of inertial measurements, e.g. for temperature effects
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Gyroscopes (AREA)
Abstract
本发明涉及一种水平姿态测量方法、***、处理设备及存储介质，其特征在于，包括：将MIMU放置于载体上，根据载体的初始姿态，对MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角；进行载体的静止状态判断，并根据MIMU内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵；根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角；根据载体的噪声动态观测方差阵和失准角，确定MIMU的陀螺零偏，并得到更新后的载体姿态角；对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角，本发明可以广泛应用于惯性技术领域中。
Description
技术领域
本发明涉及惯性技术领域，特别是关于一种水平姿态测量方法、***、处理设备及存储介质。
背景技术
MIMU(Micro Inertial Measurement Unit，微惯性测量单元)由于其自身小体积、低功耗、低成本等特点，已经成为小型无人机、无人车、微型稳定平台以及人体可穿戴式智能化设备的首选器件，应用于姿态测量及导航定位。目前，应用于MIMU测姿的方法主要是利用MIMU内加速度计的精度和稳定性远高于陀螺器件水准的特点，在考虑载体运动过程中的静止或准静止状态下，通过互补滤波等方法融合陀螺的解算姿态和加速度计的估计姿态，达到提升载体水平姿态的测量精度。
但是，目前采用互补滤波方法进行融合时，对于不同姿态的权重设置仅依靠经验值，或者基于加速度计幅值的一阶线性关系，不能更为准确地评估加速度计水平姿态估计的精度。因此，需要一种能够实时动态地计算加速度计水平姿态的估计精度的方法，从而更为准确的进行姿态融合解算。
发明内容
针对上述问题，本发明的目的是提供一种水平姿态测量方法、***、处理设备及存储介质，能够实时动态地计算加速度计水平姿态的估计精度。
为实现上述目的，本发明采取以下技术方案：第一方面，提供一种水平姿态测量方法，包括：
将MIMU放置于载体上，根据载体的初始姿态，对MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角；
根据MIMU内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵；
根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角；
根据载体的噪声动态观测方差阵和失准角，确定MIMU的陀螺零偏，并得到更新后的载体姿态角；
对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角。
进一步地，所述将MIMU放置于载体上，根据载体的初始姿态，对MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角，包括：
将MIMU放置于载体上，获取MEMS陀螺的初始零偏；
采用四元数姿态更新方法，根据载体的初始姿态，对MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角包括横滚角/>俯仰角/>和航向角/>
进一步地，所述根据MIMU内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵，包括：
根据MIMU内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断；
当载体进入静止状态时，根据加速度计的输出数据，实时计算载体的水平姿态角；
根据加速度计输出的水平姿态方差，实时确定噪声动态观测方差阵。
进一步地，所述根据MIMU内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断，包括：
设定三轴加速度计的输出为其中，b为载体坐标系，/>和分别为x轴、y轴、z轴加速度计的输出，取前右下坐标投影，导航系n系选择北东地坐标；
设定加速度计输出幅值的阈值范围：
其中，g为重力加速度的值；k为阈值系数；为加速度计的输出含有误差；/>为三轴加速度计的输出模值。
进一步地，所述根据加速度计输出的水平姿态方差，实时确定噪声动态观测方差阵，包括：
根据加速度计输出的噪声方差，采用下述模型，实时动态确定水平姿态角的方差：
其中，D(φN)、D(φE)为水平姿态角的方差；为加速度计输出的噪声方差；
根据实时动态确定的水平姿态角的方差，实时确定噪声动态观测方差阵
其中，Vk为k时刻的噪声；Vj为j时刻的噪声；δkj为狄拉克函数，Rk为航向失准角的方差D(φD)以及实时动态确定的水平姿态角的方差D(φN)和D(φE)：
进一步地，所述根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角，包括：
根据载体的姿态角和进入静止状态的载体水平姿态角，确定水平参考姿态的欧拉角；
采用旋转矢量计算方法，根据水平参考姿态的欧拉角和载体的姿态角，获取载体的水平失准角；
将获取的载体姿态角中的航向角和判断静止初始时刻的航向角的差值作为载体的航向失准角。
进一步地，所述根据噪声动态观测方差阵和载体的失准角，确定MIMU的陀螺零偏，并得到更新后的载体姿态角，包括：
采用KF滤波方法，根据噪声动态观测方差阵和载体的失准角，确定MIMU的陀螺零偏；
采用四元数姿态更新方法，根据MIMU的陀螺零偏，得到更新后的载体姿态角。
第二方面，提供一种水平姿态测量***，包括：
姿态解算模块，用于根据载体的初始姿态，对放置于载体上的MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角；
噪声动态观测方差阵计算模块，用于根据MIMU内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵；
失准角计算模块，用于根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角；
陀螺零偏计算模块，用于根据载体的噪声动态观测方差阵和失准角，确定MIMU的陀螺零偏，并得到更新后的载体姿态角；
融合模块，用于对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角。
第三方面，提供一种处理设备，包括计算机程序指令，其中，所述计算机程序指令被处理设备执行时用于实现上述水平姿态测量方法对应的步骤。
第四方面，提供一种计算机可读存储介质，所述计算机可读存储介质上存储有计算机程序指令，其中，所述计算机程序指令被处理器执行时用于实现上述水平姿态测量方法对应的步骤。
本发明由于采取以上技术方案，其具有以下优点：
1、本发明建立实时动态方差估计模型，可以准确评估由加速度计进行水平姿态估计的精度，可以为加速度计估计的水平姿态应用提供精度衡量。
2、本发明通过实时动态方差估计，将KF滤波和互补滤波有效结合，实现两层互补，且互补系数的设定不需要再依据经验值或是简单的线性估计，而是更为精准的实时权重分配，能够进一步提升水平姿态的估计精度以及减弱环境震动所来的干扰影响。
综上所述，本发明可以广泛应用于惯性技术领域中。
附图说明
通过阅读下文优选实施方式的详细描述，各种其他的优点和益处对于本领域普通技术人员将变得清楚明了。附图仅用于示出优选实施方式的目的，而并不认为是对本发明的限制。在整个附图中，用相同的附图标记表示相同的部件。在附图中：
图1是本发明一实施例提供的方法流程示意图；
图2是本发明一实施例提供的加速度计动态方差模型验证效果图，其中，图2(a)为模型计算横滚角误差，图2(b)为理论统计横滚角误差，图2(c)为模型计算俯仰角误差，图2(d)为理论统计俯仰角误差。
具体实施方式
下面将参照附图更详细地描述本发明的示例性实施方式。虽然附图中显示了本发明的示例性实施方式，然而应当理解，可以以各种形式实现本发明而不应被这里阐述的实施方式所限制。相反，提供这些实施方式是为了能够更透彻地理解本发明，并且能够将本发明的范围完整的传达给本领域的技术人员。
应理解的是，文中使用的术语仅出于描述特定示例实施方式的目的，而无意于进行限制。除非上下文另外明确地指出，否则如文中使用的单数形式“一”、“一个”以及“所述”也可以表示包括复数形式。术语“包括”、“包含”、“含有”以及“具有”是包含性的，并且因此指明所陈述的特征、步骤、操作、元件和/或部件的存在，但并不排除存在或者添加一个或多个其它特征、步骤、操作、元件、部件、和/或它们的组合。文中描述的方法步骤、过程、以及操作不解释为必须要求它们以所描述或说明的特定顺序执行，除非明确指出执行顺序。还应当理解，可以使用另外或者替代的步骤。
本发明实施例提供的水平姿态测量方法、***、处理设备及存储介质，基于动态方差估计，能够解决目前基于MIMU进行姿态测量时姿态融合权重系数设定不精确的问题，给出精确的加速度计估计水平姿态的动态方差估计模型，有效融合两种滤波方法，提升水平姿态测量精度。本发明通过建立的实时动态方差估计模型，针对加速度计进行水平姿态估计给与了实时动态方差评估，以此为纽带，有效地将KF滤波和互补滤波进行结合，实现两层互补处理，并通过实时精准的权重分配，进一步提升水平姿态的估计精度，在工程上具有广泛的应用价值。
实施例1
如图1所示，本实施例提供一种水平姿态测量方法，包括以下步骤：
1)将MIMU放置于载体上，获取MEMS陀螺的初始零偏。
具体地，将MIMU放置于载体上，上电后，对初始静止时若干秒(例如3秒)的MIMU的陀螺输出数据取平均值，并作为MEMS(微机电***)陀螺的初始零偏。
2)采用四元数姿态更新方法，根据载体的初始姿态，对MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角包括横滚角/>俯仰角/>和航向角/>
3)根据MIMU内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断。
具体地，由于MEMS陀螺的精度较差，且容易受到温度的影响，特别是开机预热过程中的零偏不稳定性很大，此时仅依靠MEMS陀螺进行水平姿态求取会产生较大误差，因此，本发明通过MIMU内加速度计的输出数据和加速度计输出幅值阈值范围进行载体的静止状态判断，具体过程为：
3.1)设定三轴加速度计的输出为其中，b为载体坐标系，/> 和/>分别为x轴、y轴、z轴加速度计的输出，取前右下坐标投影，导航系n系选择北东地(N-E-D)坐标。
3.2)取三轴加速度计的输出模值：
3.3)设定加速度计输出幅值的阈值范围：
其中，g为当地重力加速度的值；k为阈值系数，一般可以取0.5％～2％；为加速度计的输出含有误差，有/>f为加速度计的理论输出(在静止或准静止状态下f＝gb)；ba为加速度计的零偏，且ba＝[bx by bz]T；ε为器件输出噪声及环境影响噪声，一般考虑为白噪声，且ε＝[εx by bz]T，εx、by和bz分别为x轴、y轴、z轴加速度计的输出的白噪声。
3.4)根据MIMU内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断。
进一步地，当载体进入静止状态时，此时，由单纯的四元数姿态解算进入到姿态融合估计阶段，该阶段包括两层并行滤波处理。
4)当载体进入静止状态时，采用矢量观测方法，根据MIMU内加速度计的输出数据，实时计算载体的水平姿态角，包括横滚角和俯仰角，具体为：
4.1)将MIMU内加速度计的输出数据进行单位化：
其中，()u表示矢量的单位化；为单位化后的加速度计的输出数据；/> 和/>分别为加速度计测量值单位化后的x轴、y轴、z轴的输出。
4.2)根据单位化后加速度计的输出数据，实时计算载体的水平姿态角：
其中，为由加速度计估计的横滚角；/>为由加速度计估计的俯仰角。
5)根据MIMU内加速度计的输出数据，实时确定噪声动态观测方差阵，具体为：
5.1)根据加速度计输出的噪声方差，采用下述模型(6)，实时动态确定水平姿态角的方差：
其中，D(DN)、D(φE)为水平姿态角的方差；为加速度计输出的噪声方差。对于该模型正确性的验证如图2所示，通过与理论统计特征相比，该模型的计算结果与真实值很吻合。
5.2)对于航向失准角的方差D(φD)的估计，由于是采用伪观测量的方式，即观测量是真实的误差增量，从而可以将航向失准角的方差D(φD)设定成为与水平姿态角的方差D(φN)或D(φE)同量级的数值，而不影响水平姿态精度的估计。
5.3)根据实时动态确定的水平姿态角的方差，实时确定噪声动态观测方差阵
其中，Vk为k时刻的噪声；Vj为j时刻的噪声；δkj为狄拉克函数，当k等于j时，为1；否则，为0；Rk为航向失准角的方差D(φD)以及实时动态确定的水平姿态角的方差D(φN)和D(φE)，即：
6)根据步骤2)中获取的载体的姿态角和步骤4)中计算的载体的水平姿态角/>实时确定载体的失准角，包括水平失准角和航向失准角，具体为：
6.1)根据步骤2)中获取的载体的姿态角和步骤4)中计算的载体的水平姿态角/>确定水平参考姿态的欧拉角/>
6.2)采用旋转矢量计算方法，根据水平参考姿态的欧拉角和步骤2)中获取的载体的姿态角/>获取载体的水平失准角φN和φE。
6.3)将步骤2)中获取的载体姿态角中的航向角和判断静止初始时刻的航向角的差值作为载体的航向失准角φD。
7)采用KF滤波(卡尔曼滤波)方法，根据噪声动态观测方差阵和载体的失准角，确定MIMU的陀螺零偏。
常规的KF滤波方法，其状态量为姿态误差和陀螺零偏，观测量为失准角，通过MIMU内加速度计的水平姿态和航向静止状态下的增量误差获得：
其中，X(t)为***状态变量；F(t)为状态转移矩阵；G(t)为***噪声矩阵；u(t)为***噪声阵；Z(t)为***观测量；H(t)为***观测矩阵；V(t)为观测噪声，其方差参数设定就是[φN φE φD]T为失准角即姿态误差角，φN、φE和φD分别为水平失准角和航向失准角；[bwx bwy bwz]T为三轴陀螺的输出零偏；/>为三轴陀螺输出噪声在导航坐标系(n系)下的投影，ωD为地球自转角速度在D向的投影，ωD＝-ωiesinL，ωie为地球自转角速度，L为位置纬度；ωN为地球自转角速度在N向的投影，ωN＝ωiecosL；/>为b系到n系的旋转矩阵。
进一步地，进行KF滤波，估计出三轴陀螺的零偏及失准角，一方面可以修正当前陀螺姿态解算的结果，同时可以实时更新三轴陀螺的零偏估值，用于下个运动阶段姿态解算，可以提升解算精度。
8)采用四元数姿态更新方法，根据MIMU的陀螺零偏，得到更新后的载体姿态角。
9)采用互补滤波方法，对更新后的载体姿态角和步骤4)中计算的水平姿态角 进行融合，确定载体的水平姿态角/>
其中，为更新后的载体姿态角；l和m为融合权重系数，根据实时估计的方差选取，即：
其中，Pk(i,i)为对于状态量估计的方差矩阵的第i个对角元素，Pk为KF滤波递推公式中的***状态估计方差矩阵。
实施例2
本实施例提供一种水平姿态测量***，包括：
姿态解算模块，用于根据载体的初始姿态，对放置于载体上的MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角。
噪声动态观测方差阵计算模块，用于根据MIMU内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵。
失准角计算模块，用于根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角。
陀螺零偏计算模块，用于根据载体的噪声动态观测方差阵和失准角，确定MIMU的陀螺零偏，并得到更新后的载体姿态角。
融合模块，用于对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角。
实施例3
本实施例提供一种与本实施例1所提供的水平姿态测量方法对应的处理设备，处理设备可以是用于客户端的处理设备，例如手机、笔记本电脑、平板电脑、台式机电脑等，以执行实施例1的方法。
所述处理设备包括处理器、存储器、通信接口和总线，处理器、存储器和通信接口通过总线连接，以完成相互间的通信。存储器中存储有可在处理设备上运行的计算机程序，处理设备运行计算机程序时执行本实施例1所提供的水平姿态测量方法。
在一些实现中，存储器可以是高速随机存取存储器(RAM：Random AccessMemory)，也可能还包括非不稳定的存储器(non-volatile memory)，例如至少一个磁盘存储器。
在另一些实现中，处理器可以为中央处理器(CPU)、数字信号处理器(DSP)等各种类型通用处理器，在此不做限定。
实施例4
本实施例提供一种与本实施例1所提供的水平姿态测量方法对应的计算机程序产品，计算机程序产品可以包括计算机可读存储介质，其上载有用于执行本实施例1所述的水平姿态测量方法的计算机可读程序指令。
计算机可读存储介质可以是保持和存储由指令执行设备使用的指令的有形设备。计算机可读存储介质例如可以是但不限于电存储设备、磁存储设备、光存储设备、电磁存储设备、半导体存储设备或者上述的任意组合。
上述各实施例仅用于说明本发明，其中各部件的结构、连接方式和制作工艺等都是可以有所变化的，凡是在本发明技术方案的基础上进行的等同变换和改进，均不应排除在本发明的保护范围之外。
Claims (8)
1.一种水平姿态测量方法，其特征在于，包括：
将MIMU放置于载体上，根据载体的初始姿态，对MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角；
根据MIMU内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵，包括：
根据MIMU内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断；
当载体进入静止状态时，根据加速度计的输出数据，实时计算载体的水平姿态角；
根据加速度计输出的水平姿态方差，实时确定噪声动态观测方差阵；
根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角；
根据载体的噪声动态观测方差阵和失准角，确定MIMU的陀螺零偏，并得到更新后的载体姿态角，包括：
采用KF滤波方法，根据噪声动态观测方差阵和载体的失准角，确定MIMU的陀螺零偏；
采用四元数姿态更新方法，根据MIMU的陀螺零偏，得到更新后的载体姿态角；
采用互补滤波方法，对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角。
2.如权利要求1所述的一种水平姿态测量方法，其特征在于，所述将MIMU放置于载体上，根据载体的初始姿态，对MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角，包括：
将MIMU放置于载体上，获取MEMS陀螺的初始零偏；
采用四元数姿态更新方法，根据载体的初始姿态，对MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角包括横滚角/>俯仰角/>和航向角/>
3.如权利要求1所述的一种水平姿态测量方法，其特征在于，所述根据MIMU内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断，包括：
设定三轴加速度计的输出为其中，b为载体坐标系，/>和/>分别为x轴、y轴、z轴加速度计的输出，取前右下坐标投影，导航系n系选择北东地坐标；
设定加速度计输出幅值的阈值范围：
其中，g为重力加速度的值；k为阈值系数；为加速度计的输出含有误差；/>为三轴加速度计的输出模值。
4.如权利要求3所述的一种水平姿态测量方法，其特征在于，所述根据加速度计输出的水平姿态方差，实时确定噪声动态观测方差阵，包括：
根据加速度计输出的噪声方差，采用下述模型，实时动态确定水平姿态角的方差：
其中，D(φN)、D(φE)为水平姿态角的方差；为加速度计输出的噪声方差；
根据实时动态确定的水平姿态角的方差，实时确定噪声动态观测方差阵
其中，Vk为k时刻的噪声；Vj为j时刻的噪声；δkj为狄拉克函数，Rk为航向失准角的方差D(φD)以及实时动态确定的水平姿态角的方差D(φN)和D(φE)：
5.如权利要求2所述的一种水平姿态测量方法，其特征在于，所述根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角，包括：
根据载体的姿态角和进入静止状态的载体水平姿态角，确定水平参考姿态的欧拉角；
采用旋转矢量计算方法，根据水平参考姿态的欧拉角和载体的姿态角，获取载体的水平失准角；
将获取的载体姿态角中的航向角和判断静止初始时刻的航向角的差值作为载体的航向失准角。
6.一种水平姿态测量***，其特征在于，包括：
姿态解算模块，用于根据载体的初始姿态，对放置于载体上的MIMU的陀螺输出数据进行姿态解算，获取载体的姿态角；
噪声动态观测方差阵计算模块，用于根据MIMU内加速度计的输出数据，实时计算进入静止状态的载体水平姿态角和载体的噪声动态观测方差阵，包括：
根据MIMU内加速度计的输出数据和加速度计输出幅值阈值范围，进行载体的静止状态判断；
当载体进入静止状态时，根据加速度计的输出数据，实时计算载体的水平姿态角；
根据加速度计输出的水平姿态方差，实时确定噪声动态观测方差阵；
失准角计算模块，用于根据获取的姿态角和实时计算的水平姿态角，实时确定载体的失准角；
陀螺零偏计算模块，用于根据载体的噪声动态观测方差阵和失准角，确定MIMU的陀螺零偏，并得到更新后的载体姿态角，包括：
采用KF滤波方法，根据噪声动态观测方差阵和载体的失准角，确定MIMU的陀螺零偏；
采用四元数姿态更新方法，根据MIMU的陀螺零偏，得到更新后的载体姿态角；
融合模块，用于采用互补滤波方法，对更新后的载体姿态角和进入静止状态的载体水平姿态角进行融合，确定载体的水平姿态角。
7.一种处理设备，其特征在于，包括计算机程序指令，其中，所述计算机程序指令被处理设备执行时用于实现权利要求1-5中任一项所述的水平姿态测量方法对应的步骤。
8.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质上存储有计算机程序指令，其中，所述计算机程序指令被处理器执行时用于实现权利要求1-5中任一项所述的水平姿态测量方法对应的步骤。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111373775.XA CN114111770B (zh) | 2021-11-19 | 2021-11-19 | 一种水平姿态测量方法、***、处理设备及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111373775.XA CN114111770B (zh) | 2021-11-19 | 2021-11-19 | 一种水平姿态测量方法、***、处理设备及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114111770A CN114111770A (zh) | 2022-03-01 |
| CN114111770B true CN114111770B (zh) | 2024-04-09 |
Family
ID=80396530
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111373775.XA Active CN114111770B (zh) | 2021-11-19 | 2021-11-19 | 一种水平姿态测量方法、***、处理设备及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114111770B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108444467A (zh) * | 2017-11-17 | 2018-08-24 | 西北工业大学 | 一种基于反馈互补滤波和代数逼近的行人定位方法 |
| WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| CN112629538A (zh) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 |
| WO2021090352A1 (ja) * | 2019-11-05 | 2021-05-14 | 楽天株式会社 | 航空機の飛行制御を行う制御装置、及び制御方法 |
| CN113175926A (zh) * | 2021-04-21 | 2021-07-27 | 哈尔滨工程大学 | 一种基于运动状态监测的自适应水平姿态测量方法 |
-
2021
- 2021-11-19 CN CN202111373775.XA patent/CN114111770B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108444467A (zh) * | 2017-11-17 | 2018-08-24 | 西北工业大学 | 一种基于反馈互补滤波和代数逼近的行人定位方法 |
| WO2020253854A1 (zh) * | 2019-06-21 | 2020-12-24 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| WO2021090352A1 (ja) * | 2019-11-05 | 2021-05-14 | 楽天株式会社 | 航空機の飛行制御を行う制御装置、及び制御方法 |
| CN112629538A (zh) * | 2020-12-11 | 2021-04-09 | 哈尔滨工程大学 | 基于融合互补滤波和卡尔曼滤波的舰船水平姿态测量方法 |
| CN113175926A (zh) * | 2021-04-21 | 2021-07-27 | 哈尔滨工程大学 | 一种基于运动状态监测的自适应水平姿态测量方法 |
Non-Patent Citations (2)
| Title |
|---|
| He, GY ,et al..Improving the Position Accuracy of the ZUPT-Aided Pedestrian Inertial Navigation by Using a Differential Layout MIMU Array.IEEE SENSORS JOURNAL.第23卷(第19期),23420-23430. * |
| 级联卡尔曼滤波在初始对准中的应用;张复建;单斌;杨波;薛亮;腾红磊;;计算机测量与控制;20180919(第09期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114111770A (zh) | 2022-03-01 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN105300379B (zh) | 一种基于加速度的卡尔曼滤波姿态估计方法及*** | |
| WO2022160391A1 (zh) | 磁力计信息辅助的mems陀螺仪标定方法及标定*** | |
| US20030028340A1 (en) | Hybrid inertial navigation method and device | |
| CN110988926B (zh) | 一种在松gnss/ins组合导航模式下实现位置精确定点欺骗偏移方法 | |
| CN112562077B (zh) | 一种融合pdr和先验地图的行人室内定位方法 | |
| CN107764261B (zh) | 一种分布式pos传递对准用模拟数据生成方法和*** | |
| CN106153069B (zh) | 自主导航***中的姿态修正装置和方法 | |
| JP2012173190A (ja) | 測位システム、測位方法 | |
| JP7025215B2 (ja) | 測位システム及び測位方法 | |
| CN111121766A (zh) | 一种基于星光矢量的天文与惯性组合导航方法 | |
| Amirsadri et al. | Practical considerations in precise calibration of a low-cost MEMS IMU for road-mapping applications | |
| JP3726884B2 (ja) | 慣性計測装置を用いた姿勢推定装置及び方法並びにプログラム | |
| CN114485641A (zh) | 一种基于惯导卫导方位融合的姿态解算方法及装置 | |
| Li et al. | Low-cost MEMS sensor-based attitude determination system by integration of magnetometers and GPS: A real-data test and performance evaluation | |
| CN110645976B (zh) | 一种移动机器人的姿态估计方法及终端设备 | |
| CN116007620A (zh) | 一种组合导航滤波方法、***、电子设备及存储介质 | |
| US11150090B2 (en) | Machine learning zero-rate level calibration | |
| CN117589163A (zh) | 多传感器组合导航方法及装置 | |
| CN110375773B (zh) | Mems惯导***姿态初始化方法 | |
| Ding et al. | A quaternion based error state Kalman filter for attitude estimation using low-cost MEMS MARG sensors | |
| CN114111770B (zh) | 一种水平姿态测量方法、***、处理设备及存储介质 | |
| JP2013061309A (ja) | カルマンフィルタ、状態推定装置、カルマンフィルタの制御方法、及びカルマンフィルタの制御プログラム | |
| CN110160530B (zh) | 一种基于四元数的航天器姿态滤波方法 | |
| CN109916399B (zh) | 一种阴影下的载体姿态估计方法 | |
| CN114964214B (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 | ||
| CB03 | Change of inventor or designer information | ||
| CB03 | Change of inventor or designer information |
Inventor after: Guo Meifeng Inventor after: Yang Haijun Inventor after: Wang Chengbin Inventor after: Li Xin Inventor after: Chen Jianqiang Inventor after: Shi Gengxiu Inventor after: Liang Hongyi Inventor before: Guo Meifeng Inventor before: Yang Haijun Inventor before: Wang Chengbin Inventor before: Li Xin Inventor before: Chen Jianqiang Inventor before: Shi Gengxiu Inventor before: Liang Hongyi |