CN107830861A - 基于自适应增益互补滤波运动物体姿态测量方法及装置 - Google Patents
基于自适应增益互补滤波运动物体姿态测量方法及装置 Download PDFInfo
- Publication number
- CN107830861A CN107830861A CN201711288301.9A CN201711288301A CN107830861A CN 107830861 A CN107830861 A CN 107830861A CN 201711288301 A CN201711288301 A CN 201711288301A CN 107830861 A CN107830861 A CN 107830861A
- Authority
- CN
- China
- Prior art keywords
- msub
- mrow
- mtd
- mtr
- msubsup
- 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
Links
- 230000000295 complement effect Effects 0.000 title claims abstract description 44
- 230000003044 adaptive effect Effects 0.000 title claims abstract description 36
- 238000000691 measurement method Methods 0.000 title claims abstract description 18
- 230000033001 locomotion Effects 0.000 claims abstract description 32
- 230000001133 acceleration Effects 0.000 claims abstract description 26
- 239000011159 matrix material Substances 0.000 claims description 18
- 238000000034 method Methods 0.000 claims description 18
- 238000005259 measurement Methods 0.000 claims description 16
- 238000012937 correction Methods 0.000 claims description 6
- 238000004891 communication Methods 0.000 claims description 4
- 238000000205 computational method Methods 0.000 claims description 4
- 241001269238 Data Species 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000005611 electricity Effects 0.000 claims description 3
- 238000010606 normalization Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000010354 integration Effects 0.000 claims description 2
- 230000003068 static effect Effects 0.000 abstract description 10
- 230000008859 change Effects 0.000 abstract description 8
- 230000000694 effects Effects 0.000 abstract description 5
- 230000004044 response Effects 0.000 abstract description 3
- 238000009499 grossing Methods 0.000 abstract description 2
- 230000008901 benefit Effects 0.000 description 4
- 238000013461 design Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000001914 filtration Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000003111 delayed effect Effects 0.000 description 2
- 239000006260 foam Substances 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 241001465754 Metazoa Species 0.000 description 1
- 230000001154 acute effect Effects 0.000 description 1
- 238000007796 conventional method Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 239000000686 essence Substances 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000005358 geomagnetic field Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000007774 longterm Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000012360 testing method 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
- G01C21/18—Stabilised platforms, e.g. by gyroscope
Abstract
本发明属于惯性导航技术领域，公开了一种基于自适应增益互补滤波运动物体姿态测量方法及装置，读取三轴角速率，三轴加速度和三轴磁场强度的数据；通过互补滤波算法，给出运动物体相对于北东地坐标系的姿态，随着运动物体的角运动，实时提供运动物体的姿态信息，用于控制和分析该物体的运动。静态下，小增益的滤波效果更好，不会敏感加速度计的抖动，高增益则会对加速度计的抖动做出错误的响应。动态下，高增益的估算结果，更贴近实际角度，且曲线光滑无毛刺，低增益的估算结果，在角速度有较大变化处，则略有衰减；采用自适应增益互补滤波的剩余残差不仅更小，而且稳定性更好。
Description
技术领域
本发明属于惯性导航技术领域，尤其涉及一种基于自适应增益互补滤波运动物体姿态测量方法及装置。
背景技术
空间运动共六个自由度，分别是三个线自由度和三个角自由度。在实际生产和生活中，运动物体的三个角自由度运动，即空间转动，可以用欧拉角来直观的进行表示。三个欧拉角被称为姿态角，或者简称姿态。欧拉角的变化，即姿态的变化。运动物体可以是飞机，汽车或者人的手臂等可以发生姿态变化的物体。在实际生活中，运动物体的姿态测量是必不可少的。通过对姿态的测量，可以对运动物体进行控制，使其按照期望进行姿态运动；也可以通过姿态的测量，了解运动物体的姿态变化，进而得知其内在的运动规律。因此，姿态测量在生产生活中无处不在。姿态测量装置一般采用陀螺仪，加速度计和磁强计。传统的高精度IMU，使用的是高精度的传感器器件，价格昂贵，体积较大，重量较重，不适于精度要求较低的应用场合。因此，近年来，随着mems器件的发展，出现了以mems陀螺仪，mems加速度计和mems磁强计为传感器的各种小型化的AHRS系统。mems器件，成本低，质量轻，体积小。在精度满足使用需求的前提下，使用mems器件是不二选择。mems陀螺仪，利用科里奥利力测量运动物体的角速度，其短时精度高，长时间则会发生零点漂移，解算累积误差持续增大等缺点；mems加速度计用于测量物体的运动加速度和重力加速度，易受物体振动的影响，低频特性好，静态精度高；mems磁强计，测量物体所处当地的地磁场强度，易受外界电磁干扰，但是长期精度有界，不会发散。由于各个传感器本身固有的特点，导致使用单一种类传感器难以获得有效稳定的姿态信息。因此，采用适当的数据融合滤波方法，使三种传感器优势互补，提炼有效的姿态信息，是业界广泛研究的方向。现有的技术方案中，采用固定增益的互补滤波方法，融合传感器数据，得到了广泛的使用。三种传感器中，陀螺仪包含了高频信息，需要滤掉低频噪声；加速度计和磁强计包含了低频信息，需要滤掉高频噪声。互补滤波，主要是利用高通滤波器过滤包含低频噪声的姿态信息，利用低通滤波器过滤包含高频噪声的姿态信息，并使二者加权相加得到最终的有效的姿态信息。二者的权重在实际应用中用增益k进行表示。现有的大多数方案中增益k是固定不变的，使得高通和低通两个滤波器的使用受到了限制，不能随着运动状态的改变而调整，最终导致会过滤掉更多的有用的信息。因此使用固定增益k，仅仅适用于物体静止或者匀速运动情况下。在运动加速度较大和角速度变化较大的情况下，测量出的姿态信息会发生幅值衰减和相位滞后，并发生畸变。这不利于运动物体的姿态控制，也难以反映运动物体的内在运动规律。
综上所述，现有技术存在的技术问题：现有的方案中增益k是固定不变存在运动加速度较大和角速度变化较大的情况下，测量出的姿态信息会发生幅值衰减和相位滞后，并发生畸变；不利于运动物体的姿态控制，难以反映运动物体的内在运动规律。采用自适应增益的方法，可以使k随着运动状态的改变实时调整。使高通和低通两个滤波器的比重根据当前转动状态进行自动改变，极大地提高了姿态测量的准确性，使其能够在静态和动态下，均能保持较高的测量精度。
发明内容
针对现有技术存在的问题，本发明提供了一种基于自适应增益互补滤波运动物体姿态测量方法及装置。
本发明是这样实现的，一种基于自适应增益互补滤波运动物体姿态测量方法，所述基于自适应增益互补滤波运动物体姿态测量方法读取三轴角速率，三轴加速度和三轴磁场强度的数据；通过互补滤波算法，给出运动物体相对于北东地坐标系的姿态，随着运动物体的角运动，实时提供运动物体的姿态信息。该姿态信息可以准确反映物体的姿态变化，可以用于控制和分析该物体的运动。
进一步，所述基于自适应增益互补滤波运动物体姿态测量方法包括以下步骤：
步骤一，传感器集成陀螺仪，加速度计和磁强计，上电后，敏感运动物体本体系下的三轴角速度，三轴加速度和三轴磁场强度；
步骤二，主控芯片通过IIC总线，获取九个原始测量数据；
步骤三，通过传感器模型，校准获得校正后的传感器数据；
步骤四，在开机静止不动的时候对姿态进行初始化；
步骤五，根据加速度计和磁强计数据，计算姿态角误差，得到角速率的补偿值；
步骤六，陀螺仪测得角速度，与得到的角速率补偿值相加，得到融合后角速度；代入四元数更新公式，得到新的四元数；四元数归一化后，得到姿态矩阵，计算出姿态角；
步骤七，步骤二-步骤六在10ms内执行完成，下一个10ms重复，循环往复，得到运动物体的实时姿态；
步骤八，得到的数据，按照通信协议，通过串口发送给外部接收设备。
进一步，欧拉角用四元数表示，四元数的导数和角速度有固定的关系：
通过四元数和姿态矩阵的关系求出姿态矩阵，求出姿态角；
绕x轴转动产生的姿态角为滚转角φ，绕y轴转动产生的姿态角为俯仰角θ，绕z轴转动产生的姿态角为偏航角ψ；xyz轴满足右手坐标系，根据坐标变化原理可得姿态矩阵的表示为：
两个姿态矩阵表示方法不同，但是值相同，因此有：
先更新四元数，再求解姿态角。
进一步，加速度计校正误差计算方法为：
体系加速度计测量值[ax ay az]T，理论值[νx νy νz]T和参考系下加速度计理论值[0 0 1]T；
将加速度计归一化：
计算体系下，加速度计的理论值：
理论上，[ax ay az]＝[νx νy νz]，二者叉积应为0，叉积的数值代表了误差；
进一步，磁强计校正误差计算方法为：
体系磁强计测量值[hx hy hz]T，理论值[mx my mz]T，参考系下磁强计理论值[Bx ByBz]T，x轴指向地磁北极，则By＝0，参考系下理论值为[Bx 0 Bz]T；
By＝0，将测量值转换到参考系，Bx，By合成为Bx，By＝0；反算回本体系，得到m；再利用m和h的叉积计算误差；
将测量值转到参考系下：
理论上，By＝0；做如下调整：
再转换至本体系下：
同理，m和h应为同一矢量，利用其叉积作为误差：
加速度计和磁强计校正得到的误差相加得到总误差，利用P控制的方法，补偿到陀螺仪测得的角速度上；
Δ＝Kpe；
ω＝ωg+Δ；
ωg是陀螺仪测出的角速度；
利用补偿后的角速度，计算出新的q，q规范化后，更新姿态矩阵，进而计算得到欧拉角。
进一步，自适应增益互补滤波方法为：
kp＝k0f(α)f(ω)；
本发明的另一目的在于提供一种所述基于自适应增益互补滤波运动物体姿态测量方法的基于自适应增益互补滤波运动物体姿态测量装置，所述基于自适应增益互补滤波运动物体姿态测量装置包括：
主控芯片stm32f105，用于处理数据；
传感器，通过IIC与主控芯片连接，用于提供三轴角速率，三轴加速度和三轴磁场强度。
本发明的优点及积极效果为：本发明的主要策略是从三种传感器(陀螺仪，加速度计和磁强计)，获取运动物体在本体系下的角速率，加速度和磁场强度三种数据。这些数据包含了物体的姿态信息。在实际应用中，数据的采集必然会携带一定的噪声。噪声的频率有高频和低频之分。对于角速率，需要滤除低频噪声；对于加速度计和磁强计，需要滤除高频噪声。互补滤波方法，通过设计高通和低通滤波器，对三种传感器进行数据融合。通过增益k进行高通和低通滤波器的调整，以使两种滤波器的频带可以互补，将无用的低频和高频噪声滤掉，留下有用的姿态信息。
在本发明中，主要针对物体运动加速度和角速度剧烈变化时，固定增益带来的不便来进行改进。从给出的自适应律中可以看出，对于物体在运动加速度很大的情况下，选择完全采用陀螺仪的姿态信息；在运动加速度较小时，根据运动加速度的大小，适当的调整增益k的大小；对于角速率，在角速率为0时，不做调整，在角速率较大时，适当的降低增益k。
静态下，增益k在初始值k0附近小范围波动，对传感器数据携带的高频和低频噪声均具有较好的滤波效果，既不会对加速度计的高频尖峰干扰有强烈的响应，也不会呈现出低频波动的特征；动态下，增益自动调节，更加准确的跟随物体姿态的变化，没有明显的幅值衰减；采用自适应增益互补滤波的剩余残差不仅更小，而且稳定性更好。
附图说明
图1是本发明实施例提供的基于自适应增益互补滤波运动物体姿态测量方法流程图。
图2和图3是本发明实施例提供的基于自适应增益互补滤波运动物体姿态测量装置结构示意图。
图中：1、电路板；2、下盖；3、上泡沫；4、上盖；5、下泡沫板；6、塑料板。
图4是本发明实施例提供的静态俯仰角估值对比示意图。
图5是本发明实施例提供的动态俯仰角估值对比示意图。
图6是本发明实施例提供的是飞行状态下获取的俯仰角估算数据示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明在不同运动状态下，自动调整增益，使之在高动态下也能保证较高的测量精度。本发明在应用中，增益k可以根据运动状态自行调整。在静止和匀速运动状态下，适当降低增益k，使测量信息更快收敛；在大的运动加速度和角速度下，适当增大增益k，使测量信息可以快速跟随真实的姿态变化。试验结果显示，采用自适应增益互补滤波方法的姿态测量装置，可以适用于更复杂多样的运动状态。
下面结合附图对本发明的应用原理作详细的描述。
如图1所示，本发明实施例提供的基于自适应增益互补滤波运动物体姿态测量方法包括以下步骤：
S101：传感器上集成了陀螺仪，加速度计和磁强计，上电后，敏感运动物体本体系下的三轴角速度，三轴加速度和三轴磁场强度；
S102：主控芯片通过IIC总线，获取九个原始测量数据；
S103：通过一定的传感器模型，校准获得校正后的传感器数据；
S104：在开机静止不动的时候对姿态进行初始化；
S105：根据加速度计和磁强计数据的，计算姿态角误差，得到角速率的补偿值；
S106：陀螺仪测得角速度，与得到的角速率补偿值相加，得到融合后角速度。代入四元数更新公式，得到新的四元数；四元数归一化后，可以得到姿态矩阵，计算出姿态角；
S107：S102-S106在10ms内执行完成，下一个10ms重复上述过程，循环往复，便可的得到运动物体的实时姿态；
S108：将得到的数据，按照通信协议，通过串口发送给外部接收设备。
在本发明的优选实施例中：
(1)欧拉角可以用四元数表示，四元数的导数和角速度有固定的关系：
在已知角速度和四元数初始值的情况下，可通过对上式积分得到以后的四元数；通过四元数和姿态矩阵的关系求出姿态矩阵，求出姿态角。
绕x轴转动产生的姿态角为滚转角φ，绕y轴转动产生的姿态角为俯仰角θ，绕z轴转动产生的姿态角为偏航角ψ。xyz轴满足右手坐标系。根据坐标变化原理可得姿态矩阵的表示为：
两个姿态矩阵表示方法不同，但是值相同，因此有：
先更新四元数，再求解姿态角。
(2)加速度计校正误差计算：
设本体系加速度计测量值[ax ay az]T，理论值[νx νy νz]T和参考系下加速度计理论值[0 0 1]T。
将加速度计归一化：
计算本体系下，加速度计的理论值：
理论上，[ax ay az]＝[νx νy νz]。二者叉积应为0。因此，二者叉积的数值代表了误差。
(3)磁强计校正误差计算：
已知本体系磁强计测量值[hx hy hz]T，理论值[mx my mz]T，参考系下磁强计理论值[Bx By Bz]T，假设x轴指向地磁北极，则By＝0，即参考系下理论值为[Bx 0 Bz]T。
由于Bx，Bz是无法准确获知的，无法按照加速度计那样进行校正。但是By＝0。利用这一点，先将测量值转换到参考系，再强行将Bx，By合成为Bx，让By＝0；反算回本体系，得到m；再利用m和h的叉积计算误差。
将测量值转到参考系下：
理论上，By＝0。因此，做如下调整：
再转换至本体系下：
同理，m和h应为同一矢量，利用其叉积作为误差。
加速度计和磁强计校正得到的误差相加得到总误差。利用P控制的方法，补偿到陀螺仪测得的角速度上；
Δ＝Kpe；
ω＝ωg+Δ
ωg是陀螺仪测出的角速度。
利用补偿后的角速度，计算出新的q，q规范化后，更新姿态矩阵，进而计算得到欧拉角。
(4)自适应增益互补滤波方法：
传统方法的kp值是固定的，只能在一定范围内满足使用要求。由于传感器的频带不同，在静态和低动态时，kp值应该取得大一点，在高动态时，kp值应该取得小一点。因此，设计依赖于物体运动状态的可以自动调节的kp值，必然可以使测量精度和稳定性显著提高。
有两个因素影响kp值得选取。一个是运动加速度的大小，另一个是当前角速度的大小。运动加速度大则减小k值，运动加速度很大时，应该只相信陀螺仪；角速度很大时，由于加速度计动态性能不好，也应该适当削减k值。由此设计k的自适应调整方法为：
kp＝k0f(α)f(ω)；
如图2和图3所示，本发明实施例提供的基于自适应增益互补滤波运动物体姿态测量装置包括：电路板1、下盖2、上泡沫3、上盖4、下泡沫板5、塑料板6。
STM32F105为主控芯片，用来处理数据；
传感器采用mpu9250，提供三轴角速率，三轴加速度和三轴磁场强度；通信接口为IIC。
本发明实施例提供的基于自适应增益互补滤波运动物体姿态测量装置先固连在运动物体上，通过5V电源供电；开机后，STM32F105通过IIC接口访问mpu9250芯片，读取传感器数据，包括三轴角速率，三轴加速度和三轴磁场强度。之后通过互补滤波算法，给出运动物体相对于北东地坐标系的姿态。随着运动物体的角运动，该可以实时提供运动物体的姿态信息，用于控制和分析该物体的运动。
下面结合实验对本发明的应用效果作详细的描述。
如图4和图5所示，在静态和动态两种状态下，俯仰角估算效果图。黑色线代表由加速度计直接计算的俯仰角，红色线代表滤波增益K＝1.5时，互补滤波器计算的俯仰角；蓝色线代表滤波增益k＝20时，互补滤波器计算的俯仰角。横轴单位为s，纵轴单位为deg(度)。可以明显看出，静态下，小增益的滤波效果更好，不会敏感加速度计的抖动，高增益则会对加速度计的抖动做出错误的响应。动态下，高增益的估算结果，更贴近实际角度，且曲线光滑无毛刺，低增益的估算结果，在角速度有较大变化处，则略有衰减。
如图6所示，是飞行状态下获取的俯仰角估算数据，可以明显看出，采用自适应增益互补滤波的剩余残差不仅更小，而且稳定性更好。
以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (7)
1.一种基于自适应增益互补滤波运动物体姿态测量方法，其特征在于，所述基于自适应增益互补滤波运动物体姿态测量方法读取三轴角速率，三轴加速度和三轴磁场强度的数据；通过互补滤波算法，给出运动物体相对于北东地坐标系的姿态，随着运动物体的角运动，实时提供运动物体的姿态信息，用于控制和分析该物体的运动。
2.如权利要求1所述的基于自适应增益互补滤波运动物体姿态测量方法，其特征在于，所述基于自适应增益互补滤波运动物体姿态测量方法包括以下步骤：
步骤一，传感器集成陀螺仪，加速度计和磁强计，上电后，敏感运动物体本体系下的三轴角速度，三轴加速度和三轴磁场强度；
步骤二，主控芯片通过IIC总线，获取九个原始测量数据；
步骤三，通过传感器模型，校准获得校正后的传感器数据；
步骤四，在开机静止不动的时候对姿态进行初始化；
步骤五，根据加速度计和磁强计数据，计算姿态角误差，得到角速率的补偿值；
步骤六，陀螺仪测得角速度，与得到的角速率补偿值相加，得到融合后角速度；代入四元数更新公式，得到新的四元数；四元数归一化后，得到姿态矩阵，计算出姿态角；
步骤七，步骤二-步骤六在10ms内执行完成，下一个10ms重复，循环往复，得到运动物体的实时姿态；
步骤八，得到的数据，按照通信协议，通过串口发送给外部接收设备。
3.如权利要求2所述的基于自适应增益互补滤波运动物体姿态测量方法，其特征在于，欧拉角用四元数表示，四元数的导数和角速度有固定的关系：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mover>
<mi>q</mi>
<mo>&CenterDot;</mo>
</mover>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&omega;</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>&omega;</mi>
<mi>z</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
通过四元数和姿态矩阵的关系求出姿态矩阵，求出姿态角；
绕x轴转动产生的姿态角为滚转角φ，绕y轴转动产生的姿态角为俯仰角θ，绕z轴转动产生的姿态角为偏航角ψ；xyz轴满足右手坐标系，根据坐标变化原理可得姿态矩阵的表示为：
<mrow>
<msub>
<mi>C</mi>
<mrow>
<mi>R</mi>
<mi>B</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
<mrow>
<msub>
<mi>R</mi>
<mrow>
<mi>R</mi>
<mi>B</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&theta;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>cos</mi>
<mi>&phi;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
<mo>+</mo>
<mi>sin</mi>
<mi>&phi;</mi>
<mi>sin</mi>
<mi>&theta;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi>&phi;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
<mo>+</mo>
<mi>cos</mi>
<mi>&phi;</mi>
<mi>sin</mi>
<mi>&theta;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&theta;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&phi;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
<mo>+</mo>
<mi>sin</mi>
<mi>&phi;</mi>
<mi>sin</mi>
<mi>&theta;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi>&phi;</mi>
<mi>cos</mi>
<mi>&psi;</mi>
<mo>+</mo>
<mi>cos</mi>
<mi>&phi;</mi>
<mi>sin</mi>
<mi>&theta;</mi>
<mi>sin</mi>
<mi>&psi;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>sin</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>sin</mi>
<mi>&phi;</mi>
<mi>cos</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&phi;</mi>
<mi>cos</mi>
<mi>&theta;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
两个姿态矩阵表示方法不同，但是值相同，因此有：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>&theta;</mi>
<mo>=</mo>
<mi>arcsin</mi>
<mrow>
<mo>(</mo>
<mrow>
<mo>-</mo>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&phi;</mi>
<mo>=</mo>
<mi>arcsin</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>&psi;</mi>
<mo>=</mo>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<mrow>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
</mrow>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>;</mo>
</mrow>
先更新四元数，再求解姿态角。
4.如权利要求2所述的基于自适应增益互补滤波运动物体姿态测量方法，其特征在于，加速度计校正误差计算方法为：
体系加速度计测量值[ax ay az]T，理论值[vx vy vz]T和参考系下加速度计理论值[0 01]T；
将加速度计归一化：
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
<mo>=</mo>
<mfrac>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
<msqrt>
<mrow>
<msubsup>
<mi>a</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
<mo>=</mo>
<mfrac>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
<msqrt>
<mrow>
<msubsup>
<mi>a</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
<mo>=</mo>
<mfrac>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
<msqrt>
<mrow>
<msubsup>
<mi>a</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>;</mo>
</mrow>
计算体系下，加速度计的理论值：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>v</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msubsup>
<mi>C</mi>
<mrow>
<mi>R</mi>
<mi>B</mi>
</mrow>
<mi>T</mi>
</msubsup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>-</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>q</mi>
<mn>2</mn>
</msub>
<msub>
<mi>q</mi>
<mn>3</mn>
</msub>
<mo>+</mo>
<msub>
<mi>q</mi>
<mn>0</mn>
</msub>
<msub>
<mi>q</mi>
<mn>1</mn>
</msub>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>q</mi>
<mn>0</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
<mo>-</mo>
<msubsup>
<mi>q</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>q</mi>
<mn>3</mn>
<mn>2</mn>
</msubsup>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
理论上，[ax ay az]＝[vx vy vz]，二者叉积应为0，叉积的数值代表了误差；
<mrow>
<msub>
<mi>e</mi>
<mrow>
<mi>&theta;</mi>
<mi>&phi;</mi>
</mrow>
</msub>
<mo>=</mo>
<mi>a</mi>
<mo>&CircleTimes;</mo>
<mi>v</mi>
<mo>.</mo>
</mrow>
5.如权利要求2所述的基于自适应增益互补滤波运动物体姿态测量方法，其特征在于，磁强计校正误差计算方法为：
体系磁强计测量值[hx hy hz]T，理论值[mx my mz]T，参考系下磁强计理论值[Bx By Bz]T，x轴指向地磁北极，则By＝0，参考系下理论值为[Bx 0 Bz]T；
By＝0，将测量值转换到参考系，Bx，By合成为Bx，By＝0；反算回本体系，得到m；再利用m和h的叉积计算误差；
将测量值转到参考系下：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>B</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>B</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>B</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msub>
<mi>C</mi>
<mrow>
<mi>R</mi>
<mi>B</mi>
</mrow>
</msub>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>h</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
理论上，By＝0；做如下调整：
<mrow>
<msub>
<mi>B</mi>
<mi>x</mi>
</msub>
<mo>=</mo>
<msqrt>
<mrow>
<msubsup>
<mi>B</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>B</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
<mo>,</mo>
<msub>
<mi>B</mi>
<mi>y</mi>
</msub>
<mo>=</mo>
<mn>0</mn>
<mo>;</mo>
</mrow>
再转换至本体系下：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msubsup>
<mi>C</mi>
<mrow>
<mi>R</mi>
<mi>B</mi>
</mrow>
<mi>T</mi>
</msubsup>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>B</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>B</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
同理，m和h应为同一矢量，利用其叉积作为误差：
<mrow>
<msub>
<mi>e</mi>
<mi>&psi;</mi>
</msub>
<mo>=</mo>
<mi>h</mi>
<mo>&CircleTimes;</mo>
<mi>m</mi>
<mo>;</mo>
</mrow>
加速度计和磁强计校正得到的误差相加得到总误差，利用P控制的方法，补偿到陀螺仪测得的角速度上；
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>e</mi>
<mi>x</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>e</mi>
<mi>y</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>e</mi>
<mi>z</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
<msub>
<mi>v</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
<msub>
<mi>v</mi>
<mi>y</mi>
</msub>
<mo>+</mo>
<msub>
<mi>h</mi>
<mi>y</mi>
</msub>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
<mo>-</mo>
<msub>
<mi>h</mi>
<mi>z</mi>
</msub>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>z</mi>
</msub>
<msub>
<mi>v</mi>
<mi>x</mi>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
<msub>
<mi>v</mi>
<mi>z</mi>
</msub>
<mo>+</mo>
<msub>
<mi>h</mi>
<mi>z</mi>
</msub>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
<mo>-</mo>
<msub>
<mi>h</mi>
<mi>x</mi>
</msub>
<msub>
<mi>m</mi>
<mi>z</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>a</mi>
<mi>x</mi>
</msub>
<msub>
<mi>v</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<msub>
<mi>a</mi>
<mi>y</mi>
</msub>
<msub>
<mi>v</mi>
<mi>x</mi>
</msub>
<mo>+</mo>
<msub>
<mi>h</mi>
<mi>x</mi>
</msub>
<msub>
<mi>m</mi>
<mi>y</mi>
</msub>
<mo>-</mo>
<msub>
<mi>h</mi>
<mi>y</mi>
</msub>
<msub>
<mi>m</mi>
<mi>x</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
Δ＝Kpe；
ω＝ωg+Δ；
ωg是陀螺仪测出的角速度；
利用补偿后的角速度，计算出新的q，q规范化后，更新姿态矩阵，进而计算得到欧拉角。
6.如权利要求2所述的基于自适应增益互补滤波运动物体姿态测量方法，其特征在于，自适应增益互补滤波方法为：
<mrow>
<mi>&alpha;</mi>
<mo>=</mo>
<mo>|</mo>
<msqrt>
<mrow>
<msubsup>
<mi>a</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>y</mi>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>a</mi>
<mi>z</mi>
<mn>2</mn>
</msubsup>
</mrow>
</msqrt>
<mo>-</mo>
<mi>g</mi>
<mo>|</mo>
<mo>;</mo>
</mrow>
kp＝k0f(α)f(ω)；
<mrow>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>&alpha;</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>&alpha;</mi>
<mo>></mo>
<mn>0.2</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mfrac>
<mn>1</mn>
<msup>
<mi>e</mi>
<mrow>
<mn>10</mn>
<mi>&alpha;</mi>
</mrow>
</msup>
</mfrac>
</mtd>
<mtd>
<mrow>
<mi>&alpha;</mi>
<mo>&le;</mo>
<mn>0.2</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>&omega;</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<msup>
<mi>e</mi>
<mrow>
<mn>10</mn>
<mi>&omega;</mi>
</mrow>
</msup>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>.</mo>
</mrow>
7.一种如权利要求1所述基于自适应增益互补滤波运动物体姿态测量方法的基于自适应增益互补滤波运动物体姿态测量装置，其特征在于，所述基于自适应增益互补滤波运动物体姿态测量装置包括：
主控芯片，用于处理数据；
传感器，通过IIC与主控芯片连接，用于提供三轴角速率，三轴加速度和三轴磁场强度。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711288301.9A CN107830861A (zh) | 2017-12-07 | 2017-12-07 | 基于自适应增益互补滤波运动物体姿态测量方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711288301.9A CN107830861A (zh) | 2017-12-07 | 2017-12-07 | 基于自适应增益互补滤波运动物体姿态测量方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN107830861A true CN107830861A (zh) | 2018-03-23 |
Family
ID=61642146
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711288301.9A Pending CN107830861A (zh) | 2017-12-07 | 2017-12-07 | 基于自适应增益互补滤波运动物体姿态测量方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN107830861A (zh) |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108731676A (zh) * | 2018-05-04 | 2018-11-02 | 北京摩高科技有限公司 | 一种基于惯性导航技术的姿态融合增强测量方法及系统 |
| CN108939512A (zh) * | 2018-07-23 | 2018-12-07 | 大连理工大学 | 一种基于穿戴式传感器的游泳姿态测量方法 |
| CN109674480A (zh) * | 2019-02-02 | 2019-04-26 | 北京理工大学 | 一种基于改进互补滤波的人体运动姿态解算方法 |
| CN109883451A (zh) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | 一种用于行人方位估计的自适应增益互补滤波方法及系统 |
| CN111141283A (zh) * | 2020-01-19 | 2020-05-12 | 杭州十域科技有限公司 | 一种通过地磁数据判断行进方向的方法 |
| CN112462793A (zh) * | 2019-09-09 | 2021-03-09 | 顺丰科技有限公司 | 基于传感器的设备控制方法、装置、计算机设备 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7216055B1 (en) * | 1998-06-05 | 2007-05-08 | Crossbow Technology, Inc. | Dynamic attitude measurement method and apparatus |
| CN102880049A (zh) * | 2012-10-15 | 2013-01-16 | 北京控制工程研究所 | 一种基于帆板挠性形变测量的自适应振动控制方法 |
| CN103153790A (zh) * | 2010-10-01 | 2013-06-12 | 希尔克瑞斯特实验室公司 | 使用运动传感器和附接至装置的磁力计的测量数据估计该装置在重力参照系中的偏航角的设备和方法 |
| US20130238268A1 (en) * | 2010-11-17 | 2013-09-12 | Hillcrest Laboratories ,Inc. | Apparatuses and methods for calibrating magnetometer attitude-independent parameters |
| US20130245984A1 (en) * | 2010-11-17 | 2013-09-19 | Hillcrest Laboratories, Inc. | Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic field |
| US20150177020A1 (en) * | 2012-08-02 | 2015-06-25 | Memsic, Inc. | Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer |
| CN106918352A (zh) * | 2017-05-02 | 2017-07-04 | 桂林电子科技大学 | 一种手持式mems磁力计的航向校正方法 |
-
2017
- 2017-12-07 CN CN201711288301.9A patent/CN107830861A/zh active Pending
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US7216055B1 (en) * | 1998-06-05 | 2007-05-08 | Crossbow Technology, Inc. | Dynamic attitude measurement method and apparatus |
| CN103153790A (zh) * | 2010-10-01 | 2013-06-12 | 希尔克瑞斯特实验室公司 | 使用运动传感器和附接至装置的磁力计的测量数据估计该装置在重力参照系中的偏航角的设备和方法 |
| US20130238268A1 (en) * | 2010-11-17 | 2013-09-12 | Hillcrest Laboratories ,Inc. | Apparatuses and methods for calibrating magnetometer attitude-independent parameters |
| US20130245984A1 (en) * | 2010-11-17 | 2013-09-19 | Hillcrest Laboratories, Inc. | Apparatuses and methods for magnetometer alignment calibration without prior knowledge of the local magnetic field |
| US20150177020A1 (en) * | 2012-08-02 | 2015-06-25 | Memsic, Inc. | Method and apparatus for data fusion of a three-axis magnetometer and three axis accelerometer |
| CN102880049A (zh) * | 2012-10-15 | 2013-01-16 | 北京控制工程研究所 | 一种基于帆板挠性形变测量的自适应振动控制方法 |
| CN106918352A (zh) * | 2017-05-02 | 2017-07-04 | 桂林电子科技大学 | 一种手持式mems磁力计的航向校正方法 |
Non-Patent Citations (1)
| Title |
|---|
| 徐涛;温东;孙晓磊;: "基于加速度计和磁强计的方位测量与校正技术研究" * |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108731676A (zh) * | 2018-05-04 | 2018-11-02 | 北京摩高科技有限公司 | 一种基于惯性导航技术的姿态融合增强测量方法及系统 |
| CN108731676B (zh) * | 2018-05-04 | 2021-06-15 | 北京摩高科技有限公司 | 一种基于惯性导航技术的姿态融合增强测量方法及系统 |
| CN108939512A (zh) * | 2018-07-23 | 2018-12-07 | 大连理工大学 | 一种基于穿戴式传感器的游泳姿态测量方法 |
| CN109674480A (zh) * | 2019-02-02 | 2019-04-26 | 北京理工大学 | 一种基于改进互补滤波的人体运动姿态解算方法 |
| CN109883451A (zh) * | 2019-04-15 | 2019-06-14 | 山东建筑大学 | 一种用于行人方位估计的自适应增益互补滤波方法及系统 |
| CN112462793A (zh) * | 2019-09-09 | 2021-03-09 | 顺丰科技有限公司 | 基于传感器的设备控制方法、装置、计算机设备 |
| CN111141283A (zh) * | 2020-01-19 | 2020-05-12 | 杭州十域科技有限公司 | 一种通过地磁数据判断行进方向的方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN107830861A (zh) | 基于自适应增益互补滤波运动物体姿态测量方法及装置 | |
| CN104898681B (zh) | 一种采用三阶近似毕卡四元数的四旋翼飞行器姿态获取方法 | |
| CN107478223A (zh) | 一种基于四元数和卡尔曼滤波的人体姿态解算方法 | |
| CN102980577B (zh) | 一种微型捷联航姿系统及其工作方法 | |
| CN106767804B (zh) | 一种运动物体的多维数据测量装置及方法 | |
| CN104698485B (zh) | 基于bd、gps及mems的组合导航系统及导航方法 | |
| CN106482734A (zh) | 一种用于imu多传感器数据融合的滤波方法 | |
| CN108225308A (zh) | 一种基于四元数的扩展卡尔曼滤波算法的姿态解算方法 | |
| CN106979780B (zh) | 一种无人车实时姿态测量方法 | |
| CN108318038A (zh) | 一种四元数高斯粒子滤波移动机器人姿态解算方法 | |
| CN107785663A (zh) | 天线波束姿态控制方法和系统 | |
| CN104956648B (zh) | 用于在固定参照系中感测物体在空间中的取向的方法和设备 | |
| CN103557866B (zh) | 一种基于地磁技术的虚拟陀螺仪及算法的方法 | |
| CN110377056B (zh) | 无人机航向角初值选取方法及无人机 | |
| CN103017763A (zh) | 状态估计设备和偏移更新方法 | |
| CN108225370A (zh) | 一种运动姿态传感器的数据融合与解算方法 | |
| CN104215229B (zh) | Rtk设备调节方法、系统及rtk测量方法 | |
| CN106885566A (zh) | 一种可穿戴式运动传感器及其抗磁场干扰的方法 | |
| CN106643802A (zh) | 一种陀螺仪漂移校正方法及装置 | |
| CN103175502A (zh) | 一种基于数据手套低速运动的姿态角检测方法 | |
| CN105841698A (zh) | 一种无需调零的auv舵角精确实时测量系统 | |
| CN110007354A (zh) | 无人机半航空瞬变电磁接收线圈飞行参数测量装置及方法 | |
| CN107402007A (zh) | 一种提高微型ahrs模块精度的方法和微型ahrs模块 | |
| CN106017452A (zh) | 双陀螺抗扰动寻北方法 | |
| CN109550219A (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 | ||
| AD01 | Patent right deemed abandoned | ||
| AD01 | Patent right deemed abandoned |
Effective date of abandoning: 20231229 |