CN115452003A - 一种微机电陀螺非线性偏置在线估计方法 - Google Patents

一种微机电陀螺非线性偏置在线估计方法 Download PDF

Info

Publication number
CN115452003A
CN115452003A CN202211165045.5A CN202211165045A CN115452003A CN 115452003 A CN115452003 A CN 115452003A CN 202211165045 A CN202211165045 A CN 202211165045A CN 115452003 A CN115452003 A CN 115452003A
Authority
CN
China
Prior art keywords
attitude
bias
estimation
gyroscope
gyro
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.)
Pending
Application number
CN202211165045.5A
Other languages
English (en)
Inventor
富立
常杰灵
王玲玲
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202211165045.5A priority Critical patent/CN115452003A/zh
Publication of CN115452003A publication Critical patent/CN115452003A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
    • 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

Abstract

本发明公开了一种微机电陀螺非线性偏置在线估计方法，包括步骤：S1，利用MEMS惯性传感器和外部姿态传感器获取陀螺角速度和非惯性姿态测量值，并对陀螺角速度测量值进行静态偏置校正、去噪及映射预处理，对非惯性姿态测量值进行映射预处理；S2，构建在线观测陀螺仪偏置的非线性观测器，以步骤S1得到的传感器信息为输入进行带偏置校正的姿态估计和陀螺偏置估计。该方法对模型的假设更符合实际情况，使用范围更加广泛，且较现有RHO、IEKF方法具有更高的准确性、鲁棒性和稳定性。

Description

一种微机电陀螺非线性偏置在线估计方法
技术领域
本发明涉及组合导航领域、信息技术领域和自动控制领域的综合应用，尤其涉及一种利用组合导航系统姿态信息，在线估计微机电陀螺非线性偏置的方法。
背景技术
基于微机电系统技术(Micro Electro-Mechanical systems-MEMS)的惯性元件(陀螺和加速度计)与外界辅助信息(如卫星导航信息、视觉信息等)构成微机电组合导航系统(MEMS-based integrated navigation system)，广泛应用于各类运动体(如车辆、行人、机器人、无人机、飞艇等)的姿态和位置测量之中。随着MEMS技术的快速发展，已出现了导航级MEMS惯性元件。然而在低成本应用场合中，工业级微机电组合导航系统精度仍受制于MEMS陀螺测量值中的偏置误差。MEMS陀螺偏置误差中的随机性误差包含较大的逐次启动偏置和环境耦合误差。当外界辅助信息失效时，这些随机性误差将导致MEMS组合导航系统精度快速下降。因此，在线校正MEMS陀螺偏置，成为提高低成本应用场合中MEMS组合导航系统可靠性和实用性的关键。
多传感器信息融合方法是目前广泛应用的MEMS陀螺偏置在线校正方法之一。该方法中假设已知MEMS陀螺偏置的线性动态模型及其噪声特性，采用非线性卡尔曼滤波等随机状态滤波方法，利用外界辅助信息实现偏置的在线估计和校正，在很多研究中已经取得了不小的成果。有研究针对姿态初始对准误差、加速度和磁场传感器测量值的不确定性，提出了基于协方差膨胀-乘法扩展卡尔曼滤波器的姿态和陀螺偏置估计器。有研究提出了一种基于两步测量更新卡尔曼滤波策略的鲁棒姿态和陀螺偏置估计器。此外，还有研究针对火星星表约束环境下火星无人机振动耦合的微惯性陀螺随机性偏置，提出一种微惯性/视觉组合导航算法。在火星星表为平面的假设条件下，提取视觉图像中的无人机姿态测量信息，基于扩张卡尔曼滤波实现姿态和陀螺偏置估计。然而，组合滤波方法受运动体动力学特性和任务轨迹的限制，难以始终保证MEMS陀螺偏置的可观性。与此同时，当MEMS陀螺偏置的线性模型和量测噪声特性不准确时，很难获得令人满意的在线校正结果。
近年来，针对量测噪声特性不准确，抗干扰能力差等问题，非线性观测器被引入到MEMS陀螺偏置在线校正过程中，已经在诸多研究中表现出良好的性能。有人针对陀螺偏置动态线性模型参数变化和量测噪声特性不准确等问题，提出了基于运动体动力学和运动学模型的姿态滑模观测器，提高了陀螺偏置估计的鲁棒性。然而，姿态滑模观测器存在着运动体动力学模型精度要求高等问题。还有专家在全状态可观和轨迹加速度约束条件下，基于UWB和运动捕捉系统输出的距离和姿态信息，提出了一种非线性导航观测器，实现了半全局渐进稳定地估计线性陀螺逐次启动偏置。此外，还有人提出了一种半全局稳定的非线性观测器，利用视觉方位、速度等测量信息，实现了陀螺偏置和姿态的估计。在此基础上，有专家还为视觉辅助惯性导航系统开发了一种非线性观测器，该观测器保证了几乎全局渐近稳定性和局部指数稳定性。研究表明，在已知MEMS陀螺偏置动态线性模型、高精度观测信息、特定飞行轨迹等约束条件下，基于非线性观测器的陀螺偏置在线校正方法具有计算量小、李雅普诺夫稳定和对不确定干扰具有鲁棒性等优势。然而，有专家指出实际应用中，环境因素耦合的MEMS陀螺偏置特性难以用特定线性模型描述。据我们所知，在MEMS陀螺偏置动态模型未知情况下，基于非线性观测器的陀螺偏置在线校正方法仍然未被讨论。
发明内容
为了解决上述已有技术存在的不足，提高低成本组合导航系统的精度，本发明提出了一种微机电陀螺非线性偏置在线估计方法，该方法利用商用级MEMS惯性传感器和外部姿态传感器获取陀螺角速度和非惯性姿态测量值，提出了一种在线观测陀螺仪偏置的非线性观测器，以获取的传感器信息为输入进行带偏置校正的姿态估计和陀螺偏置估计，并应用于微惯性组合导航平台，在动态实验中测试该估计方法的性能。本发明的具体技术方案如下：
一种微机电陀螺非线性偏置在线估计方法，包括以下步骤：
S1：利用MEMS惯性传感器和外部姿态传感器获取陀螺角速度和非惯性姿态测量值，并对陀螺角速度测量值进行静态偏置校正、去噪及映射预处理，对非惯性姿态测量值进行映射预处理；
S2：构建在线观测陀螺仪偏置的非线性观测器，以步骤S1得到的传感器信息为输入进行带偏置校正的姿态估计和陀螺偏置估计。
进一步，所述步骤S1包括以下子步骤：
S1-1:搭建安装有MEMS惯性传感器和外部姿态传感器的微惯性组合导航平台；
S1-2:设计期望轨迹，完成动态实验，利用MEMS惯性传感器和外部姿态传感器，实时采集陀螺角速度和非惯性姿态测量值；
S1-3:根据运动起始的时刻，截取步骤S1-2陀螺角速度中的陀螺仪初始静止阶段输出，将该阶段陀螺仪输出的均值视为MEMS陀螺的初始确定性偏置，对运动阶段的陀螺角速度进行静态偏置校正；
S1-4:利用滑动平均滤波器去除步骤S1-3的处理结果的振动噪声；
S1-5:定义超复映射R＝H(r)，其中H:
Figure BDA0003860953470000031
Figure BDA0003860953470000032
为三维欧几里得空间，r＝[rx ry rz]T为任意三维空间向量，R＝0+irx+jry+krz为超复映射后的四元数，i,j,k为虚数单位；使用超复映射H将步骤S1-2的非惯性姿态λc和步骤S1-4的陀螺角速度
Figure BDA0003860953470000033
分别映射为非惯性姿态测量四元数Λc和陀螺仪测量值四元数
Figure BDA0003860953470000034
进一步，所述步骤S2包括以下子步骤：
S2-1：将运动起始的时刻作为初始时刻，根据步骤S1-2数据的最小采样时刻确定积分时刻t，其中t＝1,2,…，其中初始0时刻姿态估计四元数Λo0取值等于初始0时刻非惯性测量四元数Λc0，初始0时刻陀螺偏置估计值满足
Figure BDA0003860953470000035
S2-2：提取步骤S1-5中t时刻的非惯性姿态测量四元数Λct，将(t-1)时刻的姿态估计四元数Λo(t-1)取逆，根据下式计算出t时刻的姿态估计反馈项et：
Figure BDA0003860953470000036
其中，右下角标t或(t-1)代表积分时刻，
Figure BDA0003860953470000037
为姿态估计误差四元数，
Figure BDA0003860953470000038
为
Figure BDA0003860953470000039
的实数部分，
Figure BDA00038609534700000310
为
Figure BDA00038609534700000311
的向量部分，
Figure BDA00038609534700000312
代表姿态估计反馈项；
S2-3：将步骤S2-2的处理结果放大得到姿态估计修正项ket，同时提取t时刻步骤S1-4中的处理结果
Figure BDA00038609534700000313
和(t-1)时刻陀螺偏置估计值
Figure BDA00038609534700000314
t时刻的姿态估计修正项ket、t时刻的陀螺仪测量值
Figure BDA00038609534700000315
与(t-1)时刻的陀螺偏置估计值
Figure BDA00038609534700000316
三者共同驱动姿态估计泊松方程，如下式
Figure BDA00038609534700000317
从而实现带偏置校正的姿态估计，得到t时刻的姿态估计四元数Λot；
S2-4：定义映射
Figure BDA00038609534700000318
其中h:
Figure BDA00038609534700000319
Γ为任意四元数Γ＝τ0+iτ1+jτ2+kτ3，
Figure BDA00038609534700000320
为该四元数对应的列向量
Figure BDA00038609534700000321
使用映射h将步骤S2-3的处理结果Λot映射为t时刻姿态估计四元数向量λot，经过一阶非线性前馈放大环节，如下式
Figure BDA0003860953470000041
得到t时刻陀螺偏置乘积耦合分量的估计
Figure BDA0003860953470000042
其中，
Figure BDA0003860953470000043
是一阶非线性前馈放大环节t时刻的增益矩阵；
S2-5：提取t时刻步骤S1-5中的陀螺仪测量值四元数
Figure BDA0003860953470000044
步骤S2-4中的姿态估计四元数向量λot与陀螺偏置乘积耦合分量
Figure BDA0003860953470000045
共同合成t时刻姿态-角速度驱动项ut，输入到一阶非线性反馈系统中，得到t时刻陀螺偏置积分耦合分量的估计
Figure BDA0003860953470000046
如下式
Figure BDA0003860953470000047
其中，
Figure BDA0003860953470000048
为t时刻一阶非线性反馈系统的增益矩阵，ut∈R4；
S2-6：提取t时刻步骤S2-4和步骤S2-5的处理结果，陀螺偏置乘积耦合分量的估计
Figure BDA0003860953470000049
与陀螺偏置积分耦合分量的估计
Figure BDA00038609534700000410
相加得到t时刻陀螺偏置估计值四元数
Figure BDA00038609534700000411
使用h和H复合后的逆映射得到t时刻陀螺偏置估计值
Figure BDA00038609534700000412
即满足
Figure BDA00038609534700000413
S2-7：重复步骤S2-2至S2-6，直到完成所有时刻带偏置校正的姿态估计和陀螺偏置估计。
本发明与现有技术相比所具有的有益效果：
1.本发明设计了一种使用外部姿态传感器在线观测商用级MEMS陀螺偏置的观测器。相较于鲁棒混合观测器(Robust Hybrid Attitude and Gyro-Bias Observer-RHO)、迭代扩展卡尔曼滤波(Iterated Extended Kalman Filter-IEKF)方法对于运动体姿态和陀螺漂移线性相关的假设，本发明提出的NRBO(Nonlinear Robust Bias Observer)方法放宽了陀螺漂移的模型要求，即不再认为陀螺偏置为常值，而是认为陀螺漂移模型为运动体姿态的非线性函数。这使得NRBO方法对模型的假设更符合真实情况，适用范围更加广泛。
2.本发明将陀螺漂移的非线性模型设计为陀螺偏置乘积耦合分量与陀螺偏置积分耦合分量两部分，通过调整前馈增益矩阵和反馈增益矩阵便可实现陀螺非线性偏置的在线估计。相较于以往的RHO、IEKF方法的效果，NRBO方法在准确性、鲁棒性和稳定性上都有更高的表现。
3.本发明提出的NRBO方法可用于加速度计非线性偏置的在线估计，从而进一步提高低成本微机电组合导航系统的精度。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，通过参考附图会更加清楚的理解本发明的特征和优点，附图是示意性的而不应理解为对本发明进行任何限制，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，可以根据这些附图获得其他的附图。其中：
图1是本发明的一种微机电陀螺非线性偏置在线估计方法流程图；
图2是根据本发明的一个实施例的索并联机器人平台的配置图；
图3是根据本发明的一个实施例的索并联机器人的末端执行器；
图4是根据本发明的一个实施例的外部传感器采集的末端执行器运动轨迹图；
图5是根据本发明的一个实施例的外部传感器采集的末端执行器姿态图；
图6是根据本发明的一个实施例的MEMS惯性传感器采集图；
图7是根据本发明的一个实施例的GI、RHO、IEKF和NRBO四种姿态估计方法姿态对比结果；
图8是根据本发明的一个实施例的四种方法姿态误差的均方根误差结果；
图9是根据本发明的一个实施例的RHO方法估计的陀螺偏置；
图10是根据本发明的一个实施例的IEKF方法估计的陀螺偏置；
图11是根据本发明的一个实施例的NRBO方法估计的陀螺偏置；
图12是根据本发明的一个实施例的RHO_GI、IEKF_GI和NRBO_GI方法姿态误差的均方根误差结果；
具体实施方式
为了能够更清楚地理解本发明的上述目的、特征和优点，下面结合附图和具体实施方式对本发明进行进一步的详细描述。需要说明的是，在不冲突的情况下，本发明的实施例及实施例中的特征可以相互组合。
在下面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，本发明的保护范围并不受下面公开的具体实施例的限制。
本发明提供的一种利用非惯性姿态测量值在线观测微机电陀螺仪偏置的非线性观测器，以提高精度和鲁棒性为动机，对影响陀螺仪偏置的因素分类建模，完成“高质量”陀螺偏置校正，从而提高低成本组合导航系统的可靠性和实用性。非惯性姿态测量值可来源于视觉导航系统、MARG(Magnetic,Angular Rate,and Gravity)或组合导航系统。非惯性姿态测量四元数与陀螺仪测量角速度共同作为MEMS陀螺仪偏置非线性观测器的输入，首先完成带偏置校正的姿态估计。而后估计姿态输入到非线性放大环节的前馈环路，完成陀螺偏置乘积耦合分量的估计。设计该环节的放大增益，即可保证了载体运动过程中陀螺仪偏置的不稳定性和姿态估计误差，不影响陀螺偏置观测结果的鲁棒性。接着将MEMS陀螺测量的角速度，估计姿态以及陀螺偏置乘积耦合分量输入到一阶非线性反馈系统中，得到陀螺偏置积分耦合分量。调整一阶非线性反馈增益可以保证状态观测器的全局渐近稳定性。
具体的，如图1所示为本发明的一种微机电陀螺非线性偏置在线估计方法流程图，图2为根据本发明的一个实施例的索并联机器人平台的配置图，一种微机电陀螺非线性偏置在线估计方法，包括以下步骤：
S1：利用商用级MEMS惯性传感器和外部姿态传感器获取陀螺角速度和非惯性姿态测量值，并对陀螺角速度测量值进行静态偏置校正、去噪及映射预处理，对非惯性姿态测量值进行映射预处理。
优选的，所述步骤S1的具体步骤为：
S1-1:设计一个安装有MEMS惯性传感器和外部姿态传感器的微惯性组合导航平台。
S1-2:设计期望轨迹，完成动态实验，利用MEMS惯性传感器和外部姿态传感器，实时采集陀螺角速度和非惯性姿态测量值。
S1-3:根据运动起始的时刻，截取步骤S1-2陀螺角速度中的陀螺仪初始静止阶段输出，将该阶段陀螺仪输出的均值视为MEMS陀螺的初始确定性偏置，对运动阶段的陀螺角速度进行静态偏置校正。
S1-4:将步骤S1-3的处理结果利用窗口长度为10的滑动平均滤波器去除振动噪声。
S1-5:定义超复映射R＝H(r)，其中H:
Figure BDA0003860953470000061
Figure BDA0003860953470000062
为三维欧几里得空间，r＝[rxry rz]T为任意三维空间向量，R＝0+irx+jry+krz为超复映射后的四元数，i,j,k为虚数单位。使用超复映射H将步骤S1-2的非惯性姿态λc和步骤S1-4的陀螺角速度
Figure BDA0003860953470000063
分别映射为非惯性姿态测量四元数Λc和陀螺仪测量值四元数
Figure BDA0003860953470000064
S2：构建在线观测陀螺仪偏置的非线性观测器，以步骤S1得到的传感器信息为输入进行带偏置校正的姿态估计和陀螺偏置估计。
优选的，所述步骤S2的具体步骤为：
S2-1：将运动起始的时刻作为初始时刻，根据步骤S1-2数据的最小采样时刻确定积分时刻t，其中t＝1,2,…，其中初始0时刻姿态估计四元数Λo0取值等于初始0时刻非惯性测量四元数Λc0，初始0时刻陀螺偏置估计值满足
Figure BDA0003860953470000071
下面从1时刻开始逐一时刻地估计姿态和陀螺偏置。
S2-2：提取步骤S1-5中t时刻的非惯性姿态测量四元数Λct，将(t-1)时刻的姿态估计四元数Λo(t-1)取逆，根据下式计算出t时刻的姿态估计反馈项et：
Figure BDA0003860953470000072
其中，右下角标t或(t-1)代表步骤S2-1中划分的积分时刻，
Figure BDA0003860953470000073
为姿态估计误差四元数，是姿态估计四元数的逆
Figure BDA0003860953470000074
和非惯性测量四元数Λc的四元数乘积，
Figure BDA0003860953470000075
为
Figure BDA0003860953470000076
的实数部分，
Figure BDA0003860953470000077
为
Figure BDA0003860953470000078
的向量部分，
Figure BDA0003860953470000079
代表姿态估计反馈项。
S2-3：将步骤S2-2的处理结果放大得到姿态估计修正项ket，同时提取t时刻步骤S1-4中的处理结果
Figure BDA00038609534700000710
和(t-1)时刻陀螺偏置估计值
Figure BDA00038609534700000711
t时刻的姿态估计修正项ket、t时刻的陀螺仪测量值
Figure BDA00038609534700000712
与(t-1)时刻的陀螺偏置估计值
Figure BDA00038609534700000713
三者共同驱动姿态估计泊松方程，如下式
Figure BDA00038609534700000714
从而实现带偏置校正的姿态估计，得到t时刻的姿态估计四元数Λot。
S2-4：定义映射
Figure BDA00038609534700000715
其中h:
Figure BDA00038609534700000716
Γ为任意四元数Γ＝τ0+iτ1+jτ2+kτ3，
Figure BDA00038609534700000717
为该四元数对应的列向量
Figure BDA00038609534700000718
使用映射h将步骤S2-3的处理结果Λot映射为t时刻姿态估计四元数向量λot，经过一阶非线性前馈放大环节，如下式
Figure BDA00038609534700000719
得到t时刻陀螺偏置乘积耦合分量的估计
Figure BDA00038609534700000720
其中，
Figure BDA00038609534700000721
是一阶非线性前馈放大环节t时刻的增益矩阵。
S2-5：提取t时刻步骤S1-5中的陀螺仪测量值四元数
Figure BDA00038609534700000722
步骤S2-4中的姿态估计四元数向量λot与陀螺偏置乘积耦合分量
Figure BDA00038609534700000723
共同合成t时刻姿态-角速度驱动项ut，输入到一阶非线性反馈系统中，得到t时刻陀螺偏置积分耦合分量的估计
Figure BDA0003860953470000081
具体公式如下，
Figure BDA0003860953470000082
其中，
Figure BDA0003860953470000083
为t时刻一阶非线性反馈系统的增益矩阵，该矩阵有一个为零的特征值和三个正特征值，ut∈R4。
S2-6：提取t时刻步骤S2-4和步骤S2-5的处理结果，陀螺偏置乘积耦合分量的估计
Figure BDA0003860953470000084
与陀螺偏置积分耦合分量的估计
Figure BDA0003860953470000089
相加得到t时刻陀螺偏置估计值四元数
Figure BDA0003860953470000085
使用h和H复合后的逆映射得到t时刻陀螺偏置估计值
Figure BDA0003860953470000086
即满足
Figure BDA0003860953470000087
其中，
Figure BDA0003860953470000088
表示映射的复合。
S2-7：重复步骤S2-2至S2-6，直到完成所有时刻带偏置校正的姿态估计和陀螺偏置估计。
在此基础上，本发明利用微惯性组合导航平台进行载体自身振动环境影响下的动态实验，进一步验证了该非线性观测器的观测精度。采用的具体方法为：利用安装了MEMS惯性传感器和外部姿态传感器的微惯性组合导航平台进行动态试验。以高精度外部姿态传感器的输出值作为姿态参考值，利用步骤S1-2的原始数据评估四种姿态估计方法的精度，包括陀螺积分(Gyro Integrating-GI)、鲁棒混合观测器(RHO)、迭代扩展卡尔曼滤波(IEKF)和本发明提出的方法(NRBO)。由于没有陀螺偏置的真实值，难以直接判断RHO、IEKF和NRBO方法的陀螺偏置估计精度。然而，考虑到陀螺输出经过真实的陀螺偏置校正后，输入到GI算法中可以得到精确的姿态。故使用RHO、IEKF和NRBO方法估计的陀螺偏置作为GI方法陀螺输入的实时校正，根据RHO_GI、IEKF_GI和NRBO_GI方法解算的姿态间接判断RHO、IEKF和NRBO方法陀螺偏置的精度。
下面通过一个具体的实施例来具体说明本发明的微机电陀螺非线性偏置在线估计方法及其精度验证。
微机电陀螺非线性偏置在线估计方法依赖于高精度的外部姿态传感器测量值，故设计了一个安装有MEMS惯性传感器和OptiTrack运动捕捉系统的索并联机器人平台，进行载体自身振动影响下的动态试验。选择索并联机器人实验平台为本实施例的微惯性组合导航平台，该平台由电机、柔索、末端执行器等组成，如图2所示。索并联机器人的运动控制执行单元是埃斯顿ESTUN伺服电机，四个电机位于4.8m×3.8m的矩形的顶点处，距地面高度为2.5m。每个电机都通过柔索与末端执行器相连，如图3所示，在末端执行器上安装了清华微机械陀螺实验室研制的WCG-1型号IMU。相邻电机之间各安装两个运动捕捉相机Flex 13，共计八个相机组成OptiTrack运动捕捉系统。其中，WCG-1型号IMU和OptiTrack运动捕捉系统的规格见表1。
表1 WCG-1和OptiTrack系统的技术参数
Figure BDA0003860953470000091
利用电机控制柔索，可以使末端执行器在工作空间内完成任意轨迹的运动，在本实验中，控制索并联机器人末端执行器完成了近似“Z”形轨迹的运动。末端执行器运动总共持续了347.465s，前18秒末端执行器处于静止状态，18-25s末端执行器竖直向上运动，25-168s末端执行器完成“Z”形轨迹，168-320s末端执行器沿“Z”形轨迹返回起始点，在起始点停留27秒，结束实验。其中在25s、70s、121s、168s、215s、265s、311s、320s时刻末端执行器到达“Z”形轨迹上的不同拐弯点，各有2至3秒的停留。末端执行器运动过程中，OptiTrack系统以120Hz的帧率采集数据，捕获到的末端执行器位置和姿态信息分别如图4、图5所示；WCG-1型号IMU以200Hz的频率采集数据，输出的角速度信息如图6所示。
由图4可以看出，A点为起始点，B、C、D、E各点为拐弯点，末端执行器精确地完成了“Z”形轨迹的运动。由图5可以看出，在18s、120s、200s、230s等时刻姿态值有轻微跳变，对其做频谱分析可知存在6Hz左右的低频噪声，这是由于索并联机器人是欠约束的、并且柔索有弹性，说明了末端执行器的运动处于振动环境中。由图6可以看出，在25s、70s、121s、168s、215s、265s、311s、320s等时刻MEMS传感器输出几乎为0，这是由于末端执行器分别在B、C、D、E、D、C、B、A等拐弯点停留。对图6的惯性信息做频谱分析可知，MEMS传感器输出中也存在6Hz左右的低频噪声，印证了安装着MEMS传感器的末端执行器是在振动环境下完成预定轨迹的。
利用实验获取的数据评估了四种姿态估计算法，包括GI、RHO、IEKF和NRBO。考虑到补偿静态陀螺偏置可以有效地提升这四种算法的收敛速度，将0-18s静基座下陀螺输出的均值作为陀螺偏置静态补偿值，并使用窗口长度为10的滑动平均滤波器作预处理。
选取RHO算法的增益参数k为2，选取NRBO算法的非线性参数为
Figure BDA0003860953470000101
Figure BDA0003860953470000102
其中
Figure BDA0003860953470000103
和
Figure BDA0003860953470000104
为陀螺仪测量值
Figure BDA0003860953470000105
预处理后的三轴角速度平均值。
在此参数下，四种算法估计的末端执行器姿态对比如图7所示，姿态的均方根误差(RMSE)如图8所示。由图7、图8可以看出，GI算法估计的三轴姿态都出现了漂移，最严重的偏航角RMSE甚至超过2°。RHO、IEKF和NRBO算法的三轴姿态能够较好地跟踪参考值，三种算法三轴姿态RMSE都不超过0.5°。这是由于载体运动过程中存在陀螺漂移，证明了RHO、IEKF和NRBO算法姿态精度都要高于GI算法。此外，由图7的局部放大图、图8可以看出，与RHO、IEKF算法相比，NRBO算法估计的三轴姿态更接近参考值，三轴姿态的RMSE也更小，这证明了在RHO、IEKF和NRBO算法的姿态估计都比较精确的情况下，NRBO算法的效果要更好一些。
RHO、IEKF和NRBO这三种算法在估计出较为精确姿态的同时，还能够在线估计出陀螺偏置，分别如图9、图10和图11所示。由图9-图11可以看出，在末端执行器上升过程中，柔索的弹性导致末端执行器产生较大的晃动，此阶段估计出的陀螺偏置有较大抖动，说明陀螺偏置对载体的运动状态敏感，验证了有专家指出“商用级MEMS表现出动力学敏感性”的观点。另外，在末端执行器的整个运动过程中，陀螺偏置时时变化，与RHO算法中陀螺偏置关于时间导数为零的假设相悖，因此难以用线性模型描述陀螺漂移。
由于没有陀螺偏置的真实值，难以直接根据图9-图11判断RHO、IEKF和NRBO算法的陀螺偏置估计精度。然而，有专家指出陀螺输出经过真实的陀螺偏置校正后，输入到GI算法中可以得到精确的姿态。故使用RHO、IEKF和NRBO算法估计的陀螺偏置对陀螺输出实时校正，将校正后的陀螺输出积分得到姿态的过程分别称为RHO_GI、IEKF_GI和NRBO_GI算法。通过比较RHO_GI、IEKF_GI和NRBO_GI算法估计的姿态误差，可以间接说明RHO、IEKF和NRBO算法陀螺偏置的精度。以Optitrack系统捕捉的高精度末端执行器姿态为参考，由RHO_GI、IEKF_GI和NRBO_GI算法估计的姿态均方根误差(RMSE)如图12所示。
由图12可以看出，RHO_GI算法三轴姿态的均方根误差都超过了10°，姿态精度反而不如图8的GI算法，说明在陀螺输出的校正过程产生了过度补偿，也就是说RHO算法在线估计陀螺偏置的精度有较大的提升空间。IEKF_GI算法三轴姿态的均方根误差在0.6°到3°之间，其中末端执行器滚转角和俯仰角精度反而不如图8中的GI算法，说明IEKF在这两个轴向估计的陀螺偏置不够准确，产生了过度补偿。对比而言，NRBO_GI算法三轴姿态的均方根误差都小于0.6°，平均比RHO_GI算法提升了约70倍，比IEKF_GI算法提升了3倍，比图8中的GI算法提升了1.2倍，这证明了在陀螺漂移模型未知的情况下，NRBO算法在线估计陀螺偏置的优越性，验证了本发明的假设。

Claims (3)

1.一种微机电陀螺非线性偏置在线估计方法，其特征在于，包括以下步骤：
S1：利用MEMS惯性传感器和外部姿态传感器获取陀螺角速度和非惯性姿态测量值，并对陀螺角速度测量值进行静态偏置校正、去噪及映射预处理，对非惯性姿态测量值进行映射预处理；
S2：构建在线观测陀螺仪偏置的非线性观测器，以步骤S1得到的传感器信息为输入进行带偏置校正的姿态估计和陀螺偏置估计。
2.根据权利要求1所述的一种微机电陀螺非线性偏置在线估计方法，其特征在于，所述步骤S1包括以下子步骤：
S1-1:搭建安装有MEMS惯性传感器和外部姿态传感器的微惯性组合导航平台；
S1-2:设计期望轨迹，完成动态实验，利用MEMS惯性传感器和外部姿态传感器，实时采集陀螺角速度和非惯性姿态测量值；
S1-3:根据运动起始的时刻，截取步骤S1-2陀螺角速度中的陀螺仪初始静止阶段输出，将该阶段陀螺仪输出的均值视为MEMS陀螺的初始确定性偏置，对运动阶段的陀螺角速度进行静态偏置校正；
S1-4:利用滑动平均滤波器去除步骤S1-3的处理结果的振动噪声；
S1-5:定义超复映射R＝H(r)，其中H:
Figure FDA0003860953460000011
Figure FDA0003860953460000012
为三维欧几里得空间，r＝[rx ryrz]T为任意三维空间向量，R＝0+irx+jry+krz为超复映射后的四元数，i,j,k为虚数单位；使用超复映射H将步骤S1-2的非惯性姿态λc和步骤S1-4的陀螺角速度
Figure FDA0003860953460000013
分别映射为非惯性姿态测量四元数Λc和陀螺仪测量值四元数
Figure FDA0003860953460000014
3.根据权利要求2所述的一种微机电陀螺非线性偏置在线估计方法，其特征在于，所述步骤S2包括以下子步骤：
S2-1：将运动起始的时刻作为初始时刻，根据步骤S1-2数据的最小采样时刻确定积分时刻t，其中t＝1,2,…，其中初始0时刻姿态估计四元数Λo0取值等于初始0时刻非惯性测量四元数Λc0，初始0时刻陀螺偏置估计值满足
Figure FDA0003860953460000015
S2-2：提取步骤S1-5中t时刻的非惯性姿态测量四元数Λct，将(t-1)时刻的姿态估计四元数Λo(t-1)取逆，根据下式计算出t时刻的姿态估计反馈项et：
Figure FDA0003860953460000016
其中，右下角标t或(t-1)代表积分时刻，
Figure FDA0003860953460000021
为姿态估计误差四元数，
Figure FDA0003860953460000022
为
Figure FDA0003860953460000023
的实数部分，
Figure FDA0003860953460000024
为
Figure FDA0003860953460000025
的向量部分，
Figure FDA0003860953460000026
代表姿态估计反馈项；
S2-3：将步骤S2-2的处理结果放大得到姿态估计修正项ket，同时提取t时刻步骤S1-4中的处理结果
Figure FDA0003860953460000027
和(t-1)时刻陀螺偏置估计值
Figure FDA0003860953460000028
t时刻的姿态估计修正项ket、t时刻的陀螺仪测量值
Figure FDA0003860953460000029
与(t-1)时刻的陀螺偏置估计值
Figure FDA00038609534600000210
三者共同驱动姿态估计泊松方程，如下式
Figure FDA00038609534600000211
从而实现带偏置校正的姿态估计，得到t时刻的姿态估计四元数Λot；
S2-4：定义映射
Figure FDA00038609534600000212
其中h:
Figure FDA00038609534600000213
Γ为任意四元数Γ＝τ0+iτ1+jτ2+kτ3，
Figure FDA00038609534600000214
为该四元数对应的列向量
Figure FDA00038609534600000215
使用映射h将步骤S2-3的处理结果Λot映射为t时刻姿态估计四元数向量λot，经过一阶非线性前馈放大环节，如下式
Figure FDA00038609534600000216
得到t时刻陀螺偏置乘积耦合分量的估计
Figure FDA00038609534600000217
其中，
Figure FDA00038609534600000218
是一阶非线性前馈放大环节t时刻的增益矩阵；
S2-5：提取t时刻步骤S1-5中的陀螺仪测量值四元数
Figure FDA00038609534600000219
步骤S2-4中的姿态估计四元数向量λot与陀螺偏置乘积耦合分量
Figure FDA00038609534600000220
共同合成t时刻姿态-角速度驱动项ut，输入到一阶非线性反馈系统中，得到t时刻陀螺偏置积分耦合分量的估计
Figure FDA00038609534600000221
如下式
Figure FDA00038609534600000222
其中，
Figure FDA00038609534600000223
为t时刻一阶非线性反馈系统的增益矩阵，ut∈R4；
S2-6：提取t时刻步骤S2-4和步骤S2-5的处理结果，陀螺偏置乘积耦合分量的估计
Figure FDA00038609534600000224
与陀螺偏置积分耦合分量的估计
Figure FDA00038609534600000225
相加得到t时刻陀螺偏置估计值四元数
Figure FDA00038609534600000226
使用h和H复合后的逆映射得到t时刻陀螺偏置估计值
Figure FDA00038609534600000227
即满足
Figure FDA00038609534600000228
S2-7：重复步骤S2-2至S2-6，直到完成所有时刻带偏置校正的姿态估计和陀螺偏置估计。
CN202211165045.5A 2022-09-23 2022-09-23 一种微机电陀螺非线性偏置在线估计方法 Pending CN115452003A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211165045.5A CN115452003A (zh) 2022-09-23 2022-09-23 一种微机电陀螺非线性偏置在线估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211165045.5A CN115452003A (zh) 2022-09-23 2022-09-23 一种微机电陀螺非线性偏置在线估计方法

Publications (1)

Publication Number Publication Date
CN115452003A true CN115452003A (zh) 2022-12-09

Family

ID=84307514

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211165045.5A Pending CN115452003A (zh) 2022-09-23 2022-09-23 一种微机电陀螺非线性偏置在线估计方法

Country Status (1)

Country Link
CN (1) CN115452003A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116608852A (zh) * 2023-07-19 2023-08-18 齐鲁空天信息研究院 一种用于农机惯导设备的陀螺仪温度漂移补偿方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116608852A (zh) * 2023-07-19 2023-08-18 齐鲁空天信息研究院 一种用于农机惯导设备的陀螺仪温度漂移补偿方法
CN116608852B (zh) * 2023-07-19 2023-09-29 齐鲁空天信息研究院 一种用于农机惯导设备的陀螺仪温度漂移补偿方法

Similar Documents

Publication Publication Date Title
CN108731670B (zh) 基于量测模型优化的惯性/视觉里程计组合导航定位方法
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN109764880B (zh) 紧耦合车辆轮子编码器数据的视觉惯性测程方法及系统
Kelly et al. Visual-inertial simultaneous localization, mapping and sensor-to-sensor self-calibration
JP4876204B2 (ja) 小型姿勢センサ
CN110567461B (zh) 一种考虑无陀螺仪的非合作航天器姿态和参数估计方法
CN105698765A (zh) 双imu单目视觉组合测量非惯性系下目标物位姿方法
CN108731676B (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN111707261A (zh) 一种微型无人机高速感知和定位方法
CN110702113B (zh) 基于mems传感器的捷联惯导系统数据预处理和姿态解算的方法
CN107014376A (zh) 一种适用于农业机械精准作业的姿态倾角估计方法
CN110672078B (zh) 一种基于地磁信息的高旋弹丸姿态估计方法
Fusini et al. Nonlinear observers for GNSS-and camera-aided inertial navigation of a fixed-wing UAV
KR20050065710A (ko) 이동체의 회전 이동량 이용 방법과 장치 및 컴퓨터프로그램을 저장하는 컴퓨터로 읽을 수 있는 기록 매체
CN111238535A (zh) 一种基于因子图的imu误差在线标定方法
CN115452003A (zh) 一种微机电陀螺非线性偏置在线估计方法
CN115200578A (zh) 基于多项式优化的惯性基导航信息融合方法及系统
JP2007232444A (ja) 慣性航法装置およびその誤差補正方法
Marco et al. EKF for simultaneous vehicle motion estimation and IMU bias calibration with observability-based adaptation
CN114526731A (zh) 一种基于助力车的惯性组合导航方向定位方法
Wang et al. Extended kalman filtering for robot joint angle estimation using mems inertial sensors
CN116644264A (zh) 一种非线性偏置-不变卡尔曼滤波方法
El Hadri et al. Attitude estimation with gyros-bias compensation using low-cost sensors
Madyastha et al. A novel INS/GPS fusion architecture for aircraft navigation
Emran et al. A cascaded approach for quadrotor's attitude estimation

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