CN110470294B - 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 - Google Patents

一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 Download PDF

Info

Publication number
CN110470294B
CN110470294B CN201910734138.7A CN201910734138A CN110470294B CN 110470294 B CN110470294 B CN 110470294B CN 201910734138 A CN201910734138 A CN 201910734138A CN 110470294 B CN110470294 B CN 110470294B
Authority
CN
China
Prior art keywords
matrix
representing
kalman filtering
stage
gyroscope
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
Application number
CN201910734138.7A
Other languages
English (en)
Other versions
CN110470294A (zh
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.)
Shandong Chuyun Communication Technology Co ltd
Original Assignee
Xidian 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 Xidian University filed Critical Xidian University
Priority to CN201910734138.7A priority Critical patent/CN110470294B/zh
Publication of CN110470294A publication Critical patent/CN110470294A/zh
Application granted granted Critical
Publication of CN110470294B publication Critical patent/CN110470294B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/04—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means
    • G01C21/08—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by terrestrial means involving use of the magnetic field of the earth
    • 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

一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法
技术领域
本发明属于姿态估计方法技术领域，具体涉及一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法。
背景技术
载体姿态估计是导航定位领域的一大核心技术，具有广泛的应用场景，其中载体包括但不限于无人机、机器人、智能小车、行人等。
传统的姿态估计方法直接以陀螺采样输出作为输入，如欧拉角法通过求解欧拉角微分方程直接计算航向角、俯仰角和横滚角，欧拉角微分方程关系简单明了，概念直观，容易理解，解算过程无需做正交化处理，但方程中包含有三角运算，这给实时计算带来一定困难。方向余弦法是求解姿态矩阵微分方程，姿态矩阵微分方程实际上包含了九个未知量的线性微分方程组，存在6个多余的参数，计算量较大，不利于实时计算。传统姿态估计方法是按照某种数学表达式直接计算姿态，然而实际上载体姿态估计需要考虑更多的因素，包括环境噪声、载体运动状态，因此出现了先进的姿态估计方法。
先进的姿态估计方法包括两种方法：第一种方法基于互补滤波的方法，其利用加速度计、磁力计和陀螺仪在频域上的互补误差特性，利用梯度下降法或者PID方法(比例、积分、微分)进行姿态估计；第二种方法是基于卡尔曼滤波的方法，它是具有高斯噪声的线性动态系统的最小方差状态估计，卡尔曼滤波的非线性版本是扩展卡尔曼滤波。
相比于姿态估计的精度问题，姿态估计的实时性问题越来越突出，尤其表现在以下三种情况：1)初始态度未知时姿态的快速收敛问题；2)载体的不同运动状态之间的突然切换；3)可能在短时间内导致较大姿态改变的其他场合。鉴于以上三种情况，有必要解决姿态估计的实时性问题。
通常，基于卡尔曼滤波的方法可通过调整系统噪声协方差(Q)和量测噪声协方差(R)来改善载体姿态估计的实时性，如附图1所示，P表示误差协方差。一方面，当检测到状态估计不可靠，可以增加Q值；另一方面，如果检测到量测不可靠，可以增加R值。虽然基于卡尔曼滤波的方法很有吸引力，但是相关参数需要根据载体的运动状态以及环境噪声特性的变化而变化，这就导致参数调整过程复杂，一般方法得到的调整参数往往不可靠，限制了载体多运动状态下姿态估计实时性的提高。
因此现有的卡尔曼滤波方法在改善姿态估计的实时性方面还有很大的空间，为了解决上述问题，本发明提出了一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法。
发明内容
本发明的目的在于提供一种实时性好的载体姿态估计方法，有效解决背景技术中存在的问题，对复杂运动条件下载体姿态估计的实时性问题和相关应用给出了指导。
为了实现上述目的，本发明采取的技术方案为：
一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，包括如下步骤：
虚拟测量阶段；
卡尔曼滤波的预测阶段；
卡尔曼滤波的更新阶段；
虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合；
姿态估计；
其中，虚拟测量阶段是检测载体的运动状态，并估计虚拟测量的个数用于虚拟测量与预测阶段和更新阶段的融合，虚拟测量阶段包括如下步骤：
(1)MARG传感器校准；
(2)量纲归一化；
(3)模值计算；
(4)传感器相邻输出的叉乘；
(5)运动状态矩阵的合成；
(6)运动状态矩阵欧式距离的梯度；
(7)虚拟测量的个数估计。
进一步地，步骤(1)所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示，
Figure BDA0002161587980000021
Figure BDA0002161587980000022
Figure BDA0002161587980000023
其中，
Figure BDA0002161587980000024
分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵
Figure BDA0002161587980000025
和
Figure BDA0002161587980000026
分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；bm、bg和ba分别表示磁力计、加速度计和陀螺仪的零偏误差；
步骤(2)所述的量纲归一化是将加速度除以9.8米/秒2、角速度除以1度/秒和磁场强度除以0.55Gs，这样消去MARG传感器数据的量纲，便于多源数据融合；
步骤(3)所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，
Figure BDA0002161587980000031
Figure BDA0002161587980000032
Figure BDA0002161587980000033
其中，
Figure BDA0002161587980000034
和
Figure BDA0002161587980000035
分别表示磁力计输出、陀螺仪输出和加速度计输出的模值；
Figure BDA0002161587980000036
Figure BDA0002161587980000037
分别表示磁力计X轴，Y轴和Z轴输出的平方；
Figure BDA0002161587980000038
分别表示陀螺仪X轴，Y轴和Z轴输出的平方；
Figure BDA0002161587980000039
分别表示加速度计X轴，Y轴和Z轴输出的平方；
步骤(4)所述的传感器相邻输出的叉乘是分别计算磁力计、陀螺仪和加速度计的当前时刻(k时刻)的输出与前一时刻(k-1时刻)输出的叉乘，计算公式如(7)、式(8)、式(9)所示，
Figure BDA00021615879800000310
Figure BDA00021615879800000311
Figure BDA00021615879800000312
相应的模值计算公式为：
Figure BDA00021615879800000313
Figure BDA00021615879800000314
Figure BDA00021615879800000315
其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘；
Figure BDA00021615879800000316
Figure BDA00021615879800000317
分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值；
Figure BDA00021615879800000318
Figure BDA00021615879800000319
分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值；
步骤(5)所述的运动状态矩阵的合成是指将步骤(3)中得到的三个模值和步骤(4)中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示，该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示，
Figure BDA00021615879800000320
或
Figure BDA00021615879800000321
Figure BDA00021615879800000322
其中，ED表示运动状态矩阵；‖ED‖2表示运动状态矩阵的模值；
步骤(6)所述的运动状态矩阵欧式距离的梯度是计算前后两个运动状态矩阵模值的比值，该比值为梯度，表征了载体运动状态的相对变化，计算公式如式(13)所示，
Figure BDA0002161587980000041
其中，Grad表示梯度；
Figure BDA0002161587980000042
表示向上取整；‖EDk‖2、‖EDk-1‖2分别表示载体k时刻的运动状态矩阵的模值和载体k-1时刻运动状态矩阵的模值；
步骤(7)所述的虚拟测量个数的估计是利用分段函数的方法将步骤(6)中的梯度信息转化为虚拟测量的个数，如式(14)所示，
Figure BDA0002161587980000043
进一步地，所述卡尔曼滤波的预测阶段和卡尔曼滤波的更新阶段是基于卡尔曼率波方法实现的；所述卡尔曼滤波的预测阶段是根据状态误差协方差、系统噪声和当前的状态量估计下一时刻的状态量，包括如下步骤：
状态预测：
Figure BDA0002161587980000044
误差协方差矩阵预测：
Figure BDA0002161587980000045
其中，
Figure BDA0002161587980000046
表示k时刻状态量预测值；A表示状态矩阵；
Figure BDA0002161587980000047
表示k-1时刻的状态量；B表示输入矩阵；μk表示系统噪声向量；
Figure BDA0002161587980000048
表示k时刻预测的误差协方差矩阵；
Figure BDA0002161587980000049
表示k-1时刻的误差协方差矩阵；AT表示状态矩阵的转置；Q表示系统噪声协方差矩阵；
所述卡尔曼滤波的更新阶段是利用量测信息和量测噪声矩阵更正预测阶段得到的状态量，包括如下步骤：
卡尔曼增益计算：
Figure BDA00021615879800000410
状态更新：
Figure BDA00021615879800000411
误差协方差更新：
Figure BDA00021615879800000412
其中，
Figure BDA00021615879800000413
表示k时刻的卡尔曼增益；HT表示输出矩阵的转置；H表示输出矩阵；R表示测量噪声协方差矩阵；
Figure BDA00021615879800000414
表示k时刻的状态量更新值；
Figure BDA00021615879800000415
表示观测向量；
Figure BDA00021615879800000416
表示更新后的误差协方差矩阵；I表示单位矩阵。
进一步地，所述虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合包括多次迭代方案和参数调整方案；多次迭代方案和参数调整方案这两种具体的融合方案保证了对载体姿态估计的实时性；所述多次迭代方案是利用得到的虚拟测量的个数来控制预测阶段和更正阶段的迭代次数；所述参数调整方案是先进行预测阶段，然后利用得到的虚拟测量的个数调整状态误差协方差、系统噪声和量测噪声，最后进行更新阶段，所述误差协方差、量测噪声、系统噪声的参数调整如下：
Figure BDA0002161587980000051
进一步地，所述的姿态估计是将状态量转化为姿态角，所述姿态角包括俯仰角、横滚角和航向角，包括如下步骤：
Figure BDA0002161587980000052
其中，θ为俯仰角，
Figure BDA0002161587980000053
为横滚角，Ψ为航向角，arcsin()为反正弦函数，arctan2()为四象限反正切函数，q0,q1,q2,q3为包含在状态量中的四元数，是工程上用来解算姿态角的数学工具。
本发明的有益效果：(1)本发明提出了虚拟测量阶段的具体操作步骤，给出了虚拟测量个数的估计方法；(2)首次提出了载体运动状态与卡尔曼滤波方法的融合方法，通过多次迭代方案或参数调整方案实现虚拟测量与卡尔曼滤波方法的融合；(3)相比于卡尔曼滤波方法，所提出的方法大大改善了载体在复杂运动状态，或者运动状态快速切换时姿态估计的实时性。
附图说明
图1为传统的基于卡尔曼滤波方法改善载体姿态估计实时性的示意图；
图2为本发明一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法流程图；
图3为虚拟测量与卡尔曼滤波融合的方案为多次迭代方案时载体姿态估计方法的算法步骤图；
图4为虚拟测量与卡尔曼滤波融合的方案为参数调整方案时载体姿态估计方法的算法步骤图；
图5为载体在静态、变加速，匀加速切换时的数据输出的变化情况图(最大向心加速度8米/秒2)；
图6为载体在变加速、匀加速、变减速、静态切换时的数据输出的变化情况图(最大向心加速度15米/秒2)；
图7为载体在变加速、匀加速、变减速切换时的数据输出的变化情况图(最大向心加速度24米/秒2)。
具体实施方式
为了更好地理解本发明的内容，下面结合具体实施方法对本发明内容作进一步说明，但本发明的保护内容不局限以下实施例。
本发明旨在提出一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，该方法包括虚拟测量阶段的具体操作步骤以及虚拟测量个数的估计；通过多次迭代方案或参数调整方案实现虚拟测量与卡尔曼滤波方法的融合，提高了载体姿态估计的实时性。所提出的方法鲁棒性强、通用性好，具有较大的理论研究价值和工程实践意义。
实施例1
如图2所示，一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法包括如下阶段：
虚拟测量阶段；
预测阶段；
更新阶段；
融合方案
姿态估计；
其中，融合方案包括多次迭代方案或参数调整方案。
实施例2
如图3所示，虚拟测量与卡尔曼滤波融合的方案为多次迭代方案。
虚拟测量阶段包括如下步骤：
MARG传感器校准：
所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示；
Figure BDA0002161587980000061
Figure BDA0002161587980000062
Figure BDA0002161587980000063
其中，
Figure BDA0002161587980000064
分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵
Figure BDA0002161587980000065
和
Figure BDA0002161587980000066
分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；bm、bg和ba分别表示磁力计、加速度计和陀螺仪的零偏误差；
量纲归一化：
所述的量纲归一化是将加速度除以9.8米/秒2、角速度除以1度/秒和磁场强度除以0.55Gs，这样消去MARG传感器数据的量纲，便于多源数据融合。
模值计算：
所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，
Figure BDA0002161587980000071
Figure BDA0002161587980000072
Figure BDA0002161587980000073
其中，
Figure BDA0002161587980000074
和
Figure BDA0002161587980000075
分别表示磁力计输出、陀螺仪输出和加速度计输出的模值；
Figure BDA0002161587980000076
Figure BDA0002161587980000077
分别表示磁力计X轴，Y轴和Z轴输出的平方；
Figure BDA0002161587980000078
分别表示陀螺仪X轴，Y轴和Z轴输出的平方；
Figure BDA0002161587980000079
分别表示加速度计X轴，Y轴和Z轴输出的平方；
传感器相邻输出的叉乘：
所述的传感器相邻输出的叉乘是指分别计算加磁力计、陀螺仪和速度计的当前时刻(k时刻)的输出与前一时刻(k-1时刻)输出的叉乘，分别如式(7)、式(8)和式(9)所示。
Figure BDA00021615879800000710
Figure BDA00021615879800000711
Figure BDA00021615879800000712
相应的模值计算公式：
Figure BDA00021615879800000713
Figure BDA00021615879800000714
Figure BDA00021615879800000715
其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘；
Figure BDA00021615879800000716
Figure BDA00021615879800000717
分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值；
Figure BDA00021615879800000718
Figure BDA00021615879800000719
分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值；
运动状态矩阵的合成：
所述的运动状态矩阵的合成是指将模值计算步骤中得到的三个模值和传感器相邻输出的叉乘步骤中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示。该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示。
Figure BDA00021615879800000720
或
Figure BDA00021615879800000721
Figure BDA00021615879800000722
其中，ED表示运动状态矩阵；‖ED‖2表示运动状态矩阵的模值；
运动状态矩阵欧式距离的梯度：
所述的运动状态矩阵欧式距离的梯度是计算前后两个运动状态矩阵模值的比值，该比值为梯度，表征了载体运动状态的相对变化，如式(13)所示。
Figure BDA0002161587980000081
其中，Grad表示梯度；
Figure BDA0002161587980000082
表示向上取整；‖EDk‖2、‖EDk-1‖2分别表示载体k时刻的运动状态矩阵的模值和载体k-1时刻运动状态矩阵的模值；
虚拟测量的个数估计：
所述的虚拟测量的个数估计是利用分段函数的方法将运动状态矩阵欧式距离的梯度信息转化为虚拟测量的个数，如式(14)所示。
Figure BDA0002161587980000083
虚拟测量个数来控制For循环的次数，从而决定For循环体中预测阶段和更新阶段的迭代次数。
预测阶段包括如下步骤：
状态预测：
Figure BDA0002161587980000084
误差协方差矩阵预测：
Figure BDA0002161587980000085
其中，
Figure BDA0002161587980000086
表示k时刻状态量预测值；A表示状态矩阵；
Figure BDA0002161587980000087
表示k-1时刻的状态量；B表示输入矩阵；μk表示系统噪声向量；
Figure BDA0002161587980000088
表示k时刻预测的误差协方差矩阵；
Figure BDA0002161587980000089
表示k-1时刻的误差协方差矩阵；AT表示状态矩阵的转置；Q表示系统噪声协方差矩阵；
更新阶段包括如下步骤：
卡尔曼增益计算：
Figure BDA00021615879800000810
状态更新：
Figure BDA00021615879800000811
误差协方差更新：
Figure BDA00021615879800000812
其中，
Figure BDA00021615879800000813
表示k时刻的卡尔曼增益；HT表示输出矩阵的转置；H表示输出矩阵；R表示测量噪声协方差矩阵；
Figure BDA00021615879800000814
表示k时刻的状态量更新值；
Figure BDA00021615879800000815
表示观测向量；
Figure BDA00021615879800000816
表示更新后的误差协方差矩阵；I表示单位矩阵。
姿态估计包括如下步骤：
Figure BDA00021615879800000817
其中，θ为俯仰角，
Figure BDA00021615879800000818
为横滚角，Ψ为航向角，arcsin()为反正弦函数，arctan2()为四象限反正切函数，q0,q1,q2,q3为包含在状态量中的四元数，是工程上用来解算姿态角的数学工具。
实施例3
如图4所示，虚拟测量与卡尔曼滤波融合的方案为参数调整方案。
预测阶段包括如下步骤：
状态预测：
Figure BDA0002161587980000091
误差协方差矩阵预测：
Figure BDA0002161587980000092
其中，
Figure BDA0002161587980000093
表示k时刻状态量预测值；A表示状态矩阵；
Figure BDA0002161587980000094
表示k-1时刻的状态量；B表示输入矩阵；μk表示系统噪声向量；
Figure BDA0002161587980000095
表示k时刻预测的误差协方差矩阵；
Figure BDA0002161587980000096
表示k-1时刻的误差协方差矩阵；AT表示状态矩阵的转置；Q表示系统噪声协方差矩阵；
虚拟测量阶段包括如下步骤：
MARG传感器校准：
所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示；
Figure BDA0002161587980000097
Figure BDA0002161587980000098
Figure BDA0002161587980000099
其中，
Figure BDA00021615879800000910
分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵
Figure BDA00021615879800000911
和
Figure BDA00021615879800000912
分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；bm、bg和ba分别表示磁力计、加速度计和陀螺仪的零偏误差；
量纲归一化：
所述的量纲归一化是将加速度除以9.8米/秒2、角速度除以1度/秒和磁场强度除以0.55Gs,这样消去MARG传感器数据的量纲，便于多源数据融合。
模值计算：
所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，
Figure BDA00021615879800000913
Figure BDA00021615879800000914
Figure BDA00021615879800000915
其中，
Figure BDA00021615879800000916
和
Figure BDA00021615879800000917
分别表示磁力计输出、陀螺仪输出和加速度计输出的模值；
Figure BDA00021615879800000918
Figure BDA0002161587980000101
分别表示磁力计X轴，Y轴和Z轴输出的平方；
Figure BDA0002161587980000102
分别表示陀螺仪X轴，Y轴和Z轴输出的平方；
Figure BDA0002161587980000103
分别表示加速度计X轴，Y轴和Z轴输出的平方；
传感器相邻输出的叉乘：
所述的传感器相邻输出的叉乘是指分别计算磁力计、陀螺仪和加速度计的当前时刻(k时刻)的输出与前一时刻(k-1时刻)输出的叉乘，分别如式(7)、式(8)和式(9)所示。
Figure BDA0002161587980000104
Figure BDA0002161587980000105
Figure BDA0002161587980000106
相应的模值计算公式：
Figure BDA0002161587980000107
Figure BDA0002161587980000108
Figure BDA0002161587980000109
其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘；
Figure BDA00021615879800001010
Figure BDA00021615879800001011
分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值；
Figure BDA00021615879800001012
Figure BDA00021615879800001013
分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值；
运动状态矩阵的合成：
所述的运动状态矩阵的合成是指将模值计算步骤中得到的三个模值和传感器相邻输出的叉乘步骤中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示。该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示。
Figure BDA00021615879800001014
或
Figure BDA00021615879800001015
Figure BDA00021615879800001016
其中，ED表示运动状态矩阵；‖ED‖2表示运动状态矩阵的模值；
运动状态矩阵欧式距离的梯度：
所述的运动状态矩阵欧式距离的梯度是计算前后两个运动状态矩阵模值的比值，该比值为梯度，表征了载体运动状态的相对变化，如式(13)所示。
Figure BDA00021615879800001017
其中，Grad表示梯度；
Figure BDA00021615879800001018
表示向上取整；‖EDk‖2、‖EDk-1‖2分别表示载体k时刻的运动状态矩阵的模值和载体k-1时刻运动状态矩阵的模值；
虚拟测量的个数估计：
所述的虚拟测量个数的估计是利用分段函数的方法将运动状态矩阵欧式距离的梯度信息转化为虚拟测量的个数，如式(14)所示。
Figure BDA0002161587980000111
误差协方差、量测噪声、系统噪声等参数调整如下：
Figure BDA0002161587980000112
更新阶段包括如下步骤：
卡尔曼增益计算：
Figure BDA0002161587980000113
状态更新：
Figure BDA0002161587980000114
误差协方差更新：
Figure BDA0002161587980000115
其中，
Figure BDA0002161587980000116
表示k时刻的卡尔曼增益；HT表示输出矩阵的转置；H表示输出矩阵；R表示测量噪声协方差矩阵；
Figure BDA0002161587980000117
表示k时刻的状态量更新值；
Figure BDA0002161587980000118
表示观测向量；
Figure BDA0002161587980000119
表示更新后的误差协方差矩阵；I表示单位矩阵。
姿态估计包括如下步骤：
Figure BDA00021615879800001110
其中，θ为俯仰角，
Figure BDA00021615879800001111
为横滚角，Ψ为航向角，arcsin()为反正弦函数，arctan2()为四象限反正切函数，q0,q1,q2,q3为包含在状态量中的四元数，是工程上用来解算姿态角的数学工具。
实施例4
如图5所示，其中载体安装在半径为1.663米左右的转台上，最大向心加速度为8米/秒2，最大角速度为124.4度/秒，载体随转台一起，运动状态由静态切换到变加速，再由变加速切换到匀加速时3轴加速度(ax，ay，az)，3轴角速度(wx，wy，wz)以及3轴磁场(mx，my，mz)的变化情况。
实施例5
如图6所示，其中载体安装在半径为1.663米左右的转台上，最大向心加速度为15米/秒2，最大角速度为170.3度/秒，载体随转台一起，载体运动状态由变加速切换到匀加速，再由匀加速切换到变减速；再由变减速切换到静态时3轴加速度(ax，ay，az)，3轴角速度(wx，wy，wz)以及3轴磁场(mx，my，mz)的变化情况；。
实施例6
如图7所示，其中载体安装在半径为1.663米左右的转台上，最大向心加速度为24米/秒2，最大角速度为215.5度/秒，载体随转台一起，载体运动状态由变加速切换到匀加速，再由匀加速切换到变减速时3轴加速度(ax，ay，az)，3轴角速度(wx，wy，wz)以及3轴磁场(mx，my，mz)的变化情况。
实施例7
如表1所示，卡尔曼滤波、扩展卡尔曼滤波、以及本发明提出的姿态方法的实时性测试结果对比(延时均方根误差越小，实时性越好)。其中在实施例4中所示的几种状态变化中，用卡尔曼滤波方法进行姿态估计的延时均方根误差为35.2毫秒，扩展卡尔曼滤波方法的延时均方根误差为40.6毫秒，而本发明中所提出的方法延时均方根误差仅为10.2毫秒，远远低于前两种方法，实时性最好。类似地在实施例5中所示的几种状态变化中，用卡尔曼滤波方法进行姿态估计的延时均方根误差为61.2毫秒，扩展卡尔曼滤波方法的延时均方根误差为73.2毫秒，而本发明中所提出的方法延时均方根误差仅为9.8毫秒，远远低于前两种方法，实时性最好；类似地在实施例6中所示的几种状态变化中，用卡尔曼滤波方法进行姿态估计的延时均方根误差为80.2毫秒，扩展卡尔曼滤波方法的延时均方根误差为89.8毫秒，而本发明中所提出的方法延时均方根误差仅为10.6毫秒，远远低于前两种方法，实时性最好；在实施例4,5,6中，扩展卡尔曼滤波的延时均方根误差均大于卡尔曼滤波，这是因为扩展卡尔曼对系统模型进行泰勒展开和一阶近似，导致计算量比卡尔曼滤波方法大；实施例4，5，6中载体的相对运动状态依次剧烈，因此同一种算法(卡尔曼滤波和扩展卡尔曼滤波)，在不同运动状态下的延时均方根误差不同(实时性不同)，且载体运动越剧烈，实时性越差；而本发明所提出的方法，通过融合虚拟测量量，大大改善了实时性，且实时性不随载体的运动状态的剧烈程度而变化。值得一提的是，本发明提出的方法估计姿态时仍有延时存在，其原因与环境噪声以及数据输出频率相关。
表1测试结果对比
Figure BDA0002161587980000121
以上所述仅为本发明的具体实施方式，不是全部的实施方式，本领域普通技术人员通过阅读本发明说明书而对本发明技术方案采取的任何等效的变换，均为本发明的权利要求所涵盖。

Claims (3)

1.一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，其特征在于，包括如下步骤：
虚拟测量阶段；
卡尔曼滤波的预测阶段；
卡尔曼滤波的更新阶段；
虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合；
姿态估计；
其中，所述的虚拟测量阶段包括如下步骤：
(1)MARG传感器校准；
(2)量纲归一化；
(3)模值计算；
(4)传感器相邻输出的叉乘；
(5)运动状态矩阵的合成；
(6)运动状态矩阵欧式距离的梯度；
(7)虚拟测量的个数估计；
步骤(5)所述的运动状态矩阵的合成是指将步骤(3)中得到的三个模值和步骤(4)中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示，该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示，
Figure FDA0002768259900000011
或
Figure FDA0002768259900000012
Figure FDA0002768259900000013
其中，ED表示运动状态矩阵；‖ED‖2表示运动状态矩阵的模值；
步骤(6)所述的运动状态矩阵欧式距离的梯度是计算前后两个运动状态矩阵模值的比值，该比值为梯度，表征了载体运动状态的相对变化，计算公式如式(13)所示，
Figure FDA0002768259900000014
其中，Grad表示梯度；
Figure FDA0002768259900000015
表示向上取整；‖EDk‖2、‖EDk-1‖2分别表示载体k时刻的运动状态矩阵的模值和载体k-1时刻运动状态矩阵的模值；
步骤(7)所述的虚拟测量个数的估计是利用分段函数的方法将步骤(6)中的梯度信息转化为虚拟测量的个数，如式(14)所示，
Figure FDA0002768259900000021
所述卡尔曼滤波的预测阶段和卡尔曼滤波的更新阶段是基于卡尔曼率波方法实现的；所述卡尔曼滤波的预测阶段是根据状态误差协方差、系统噪声和当前的状态量估计下一时刻的状态量，包括如下步骤：
状态预测：
Figure FDA0002768259900000022
误差协方差矩阵预测：
Figure FDA0002768259900000023
其中，
Figure FDA0002768259900000024
表示k时刻状态量预测值；A表示状态矩阵；
Figure FDA0002768259900000025
表示k-1时刻的状态量；B表示输入矩阵；μk表示系统噪声向量；
Figure FDA0002768259900000026
表示k时刻预测的误差协方差矩阵；
Figure FDA0002768259900000027
表示k-1时刻的误差协方差矩阵；AT表示状态矩阵的转置；Q表示系统噪声协方差矩阵；
所述卡尔曼滤波的更新阶段是利用量测信息和量测噪声矩阵更正预测阶段得到的状态量，包括如下步骤：
卡尔曼增益计算：
Figure FDA0002768259900000028
状态更新：
Figure FDA0002768259900000029
误差协方差更新：
Figure FDA00027682599000000210
其中，
Figure FDA00027682599000000211
表示k时刻的卡尔曼增益；HT表示输出矩阵的转置；H表示输出矩阵；R表示测量噪声协方差矩阵；
Figure FDA00027682599000000212
表示k时刻的状态量更新值；
Figure FDA00027682599000000213
表示观测向量；
Figure FDA00027682599000000214
表示更新后的误差协方差矩阵；I表示单位矩阵；
所述虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合包括多次迭代方案和参数调整方案；所述多次迭代方案是利用得到的虚拟测量的个数来控制预测阶段和更正阶段的迭代次数；所述参数调整方案是先进行预测阶段，然后利用得到的虚拟测量的个数调整状态误差协方差、系统噪声和量测噪声，最后进行更新阶段，所述误差协方差、量测噪声、系统噪声的参数调整如下：
Figure FDA00027682599000000215
2.根据权利要求1所述的虚拟测量与卡尔曼滤波融合的载体姿态估计方法，其特征在于，步骤(1)所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示，
Figure FDA00027682599000000216
Figure FDA00027682599000000217
Figure FDA0002768259900000031
其中，
Figure FDA0002768259900000032
分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵
Figure FDA0002768259900000033
和
Figure FDA0002768259900000034
分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；bm、bg和ba分别表示磁力计、加速度计和陀螺仪的零偏误差；
步骤(2)所述的量纲归一化是将加速度除以9.8米/秒2、角速度除以1度/秒和磁场强度除以0.55Gs；
步骤(3)所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，
Figure FDA0002768259900000035
Figure FDA0002768259900000036
Figure FDA0002768259900000037
其中，
Figure FDA0002768259900000038
和
Figure FDA0002768259900000039
分别表示磁力计输出、陀螺仪输出和加速度计输出的模值；
Figure FDA00027682599000000310
Figure FDA00027682599000000311
分别表示磁力计X轴，Y轴和Z轴输出的平方；
Figure FDA00027682599000000312
分别表示陀螺仪X轴，Y轴和Z轴输出的平方；
Figure FDA00027682599000000313
分别表示加速度计X轴，Y轴和Z轴输出的平方；
步骤(4)所述的传感器相邻输出的叉乘是分别计算磁力计、陀螺仪和加速度计的当前时刻的输出与前一时刻输出的叉乘，计算公式如(7)、式(8)、式(9)所示，
Figure FDA00027682599000000314
Figure FDA00027682599000000315
Figure FDA00027682599000000316
相应的模值计算公式为：
Figure FDA00027682599000000317
Figure FDA00027682599000000318
Figure FDA00027682599000000319
其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘；
Figure FDA00027682599000000320
Figure FDA00027682599000000321
分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值；
Figure FDA00027682599000000322
Figure FDA00027682599000000323
分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值。
3.根据权利要求1所述的虚拟测量与卡尔曼滤波融合的载体姿态估计方法，其特征在于，所述的姿态估计是将状态量转化为姿态角，所述姿态角包括俯仰角、横滚角和航向角，包括如下步骤：
Figure FDA0002768259900000041
其中，θ为俯仰角，
Figure FDA0002768259900000042
为横滚角，Ψ为航向角，arcsin()为反正弦函数，arctan2()为四象限反正切函数，q0,q1,q2,q3为包含在状态量中的四元数，是工程上用来解算姿态角的数学工具。
CN201910734138.7A 2019-08-09 2019-08-09 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法 Active CN110470294B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910734138.7A CN110470294B (zh) 2019-08-09 2019-08-09 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910734138.7A CN110470294B (zh) 2019-08-09 2019-08-09 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法

Publications (2)

Publication Number Publication Date
CN110470294A CN110470294A (zh) 2019-11-19
CN110470294B true CN110470294B (zh) 2020-12-18

Family

ID=68511422

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910734138.7A Active CN110470294B (zh) 2019-08-09 2019-08-09 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法

Country Status (1)

Country Link
CN (1) CN110470294B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112683267B (zh) * 2020-11-30 2022-06-03 电子科技大学 一种附有gnss速度矢量辅助的车载姿态估计方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104764451A (zh) * 2015-04-23 2015-07-08 北京理工大学 一种基于惯性和地磁传感器的目标姿态跟踪方法
CN105652306A (zh) * 2016-01-08 2016-06-08 重庆邮电大学 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法
CN106052584A (zh) * 2016-05-24 2016-10-26 上海工程技术大学 一种基于视觉及惯性信息融合的轨道空间线形测量方法
CN107702712A (zh) * 2017-09-18 2018-02-16 哈尔滨工程大学 基于惯性测量双层wlan指纹库的室内行人组合定位方法
CN109388662A (zh) * 2017-08-02 2019-02-26 阿里巴巴集团控股有限公司 一种基于共享数据的模型训练方法及装置

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104567871B (zh) * 2015-01-12 2018-07-24 哈尔滨工程大学 一种基于地磁梯度张量的四元数卡尔曼滤波姿态估计方法
CN106813679B (zh) * 2015-12-01 2021-04-13 佳能株式会社 运动物体的姿态估计的方法及装置
CN109029435B (zh) * 2018-06-22 2021-11-02 常州大学 提高惯性-地磁组合动态定姿精度的方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104764451A (zh) * 2015-04-23 2015-07-08 北京理工大学 一种基于惯性和地磁传感器的目标姿态跟踪方法
CN105652306A (zh) * 2016-01-08 2016-06-08 重庆邮电大学 基于航迹推算的低成本北斗与mems紧耦合定位系统及方法
CN106052584A (zh) * 2016-05-24 2016-10-26 上海工程技术大学 一种基于视觉及惯性信息融合的轨道空间线形测量方法
CN109388662A (zh) * 2017-08-02 2019-02-26 阿里巴巴集团控股有限公司 一种基于共享数据的模型训练方法及装置
CN107702712A (zh) * 2017-09-18 2018-02-16 哈尔滨工程大学 基于惯性测量双层wlan指纹库的室内行人组合定位方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
An orientation estimation algorithm based on multi-source information fusion;Gong-Xu Liu,et al;《Meas. Sci. Technol》;20180920;第1-11页 *
基于传感器校正与融合的农用小无人机姿态估计算法;彭孝东等;《自 动 化 学 报》;20150430;第41卷(第4期);第854-860页 *
基于多传感器的姿态检测系统设计及数据融合算法研究;王翔;《中国优秀硕士学位论文全文数据库 信息科技辑》;20190115;第1-9页 *
多传感器数据的校准与融合的研究与实现;方根在;《中国优秀硕士学位论文全文数据库 信息科技辑》;20180715;第24-44页 *
室内定位技术的测试与评估标准综述;刘公绪，史凌峰;《导航定位学报》;20190630;第7卷(第2期);第45-51页 *

Also Published As

Publication number Publication date
CN110470294A (zh) 2019-11-19

Similar Documents

Publication Publication Date Title
CN107525503B (zh) 基于双天线gps和mimu组合的自适应级联卡尔曼滤波方法
CN111024064B (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN108318038A (zh) 一种四元数高斯粒子滤波移动机器人姿态解算方法
CN104698485B (zh) 基于bd、gps及mems的组合导航系统及导航方法
CN102981151B (zh) 相控阵雷达电控波束稳定方法
CN105806363B (zh) 基于srqkf的sins/dvl水下大失准角对准方法
CN101246012B (zh) 一种基于鲁棒耗散滤波的组合导航方法
CN105136145A (zh) 一种基于卡尔曼滤波的四旋翼无人机姿态数据融合的方法
CN113063429B (zh) 一种自适应车载组合导航定位方法
CN109506660B (zh) 一种用于仿生导航的姿态最优化解算方法
CN109827571A (zh) 一种无转台条件下的双加速度计标定方法
CN108645404B (zh) 一种小型多旋翼无人机姿态解算方法
CN108731676A (zh) 一种基于惯性导航技术的姿态融合增强测量方法及系统
CN111189474A (zh) 基于mems的marg传感器的自主校准方法
CN107860382B (zh) 一种在地磁异常情况下应用ahrs测量姿态的方法
CN116817896B (zh) 一种基于扩展卡尔曼滤波的姿态解算方法
CN111750865A (zh) 一种用于双功能深海无人潜器导航系统的自适应滤波导航方法
CN110470294B (zh) 一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法
CN106595669B (zh) 一种旋转体姿态解算方法
CN110595434A (zh) 基于mems传感器的四元数融合姿态估计方法
CN108871319B (zh) 一种基于地球重力场与地磁场序贯修正的姿态解算方法
CN111190207A (zh) 基于pstcsdref算法的无人机ins bds组合导航方法
CN114459466A (zh) 一种基于模糊控制的mems多传感器数据融合处理方法
CN107389092B (zh) 一种基于磁传感器辅助的陀螺标定方法
CN112729332B (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
TR01 Transfer of patent right

Effective date of registration: 20220713

Address after: 272073 room 316, 3 / F, block B, building A1, industry university research base, No. 9 Haichuan Road, high tech Zone, Jining City, Shandong Province

Patentee after: Shandong chuyun Communication Technology Co.,Ltd.

Address before: 710071 Taibai South Road, Yanta District, Xi'an, Shaanxi Province, No. 2

Patentee before: XIDIAN University

TR01 Transfer of patent right