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

本发明专利技术公开了一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，包括：虚拟测量阶段、卡尔曼滤波的预测阶段、更新阶段、虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合和姿态估计；虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合包括多次迭代方案和参数调整方案。为了验证所提方法的性能，我们进行了多次的转台实验。测试结果表明，与传统的基于卡尔曼滤波的方法相比，所提出的方法显著提高了姿态估计的实时性，具有较大的理论研究价值和工程实践意义。

A carrier attitude estimation method based on the fusion of virtual measurement and Kalman filter

The invention discloses a carrier attitude estimation method of virtual measurement and Kalman filter fusion, which includes: virtual measurement stage, Kalman filter prediction stage, update stage, virtual measurement stage and Kalman filter prediction stage, update stage fusion and attitude estimation; the fusion of virtual measurement stage and Kalman filter prediction stage and update stage includes multiple times Iterative scheme and parameter adjustment scheme. In order to verify the performance of the proposed method, we have carried out many turntable experiments. The test results show that compared with the traditional method based on Kalman filter, the proposed method significantly improves the real-time performance of attitude estimation, and has great theoretical research value and engineering practical significance.

【技术实现步骤摘要】
一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法
本专利技术属于姿态估计方法
，具体涉及一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法。
技术介绍
载体姿态估计是导航定位领域的一大核心技术，具有广泛的应用场景，其中载体包括但不限于无人机、机器人、智能小车、行人等。传统的姿态估计方法直接以陀螺采样输出作为输入，如欧拉角法通过求解欧拉角微分方程直接计算航向角、俯仰角和横滚角，欧拉角微分方程关系简单明了，概念直观，容易理解，解算过程无需做正交化处理，但方程中包含有三角运算，这给实时计算带来一定困难。方向余弦法是求解姿态矩阵微分方程，姿态矩阵微分方程实际上包含了九个未知量的线性微分方程组，存在6个多余的参数，计算量较大，不利于实时计算。传统姿态估计方法是按照某种数学表达式直接计算姿态，然而实际上载体姿态估计需要考虑更多的因素，包括环境噪声、载体运动状态，因此出现了先进的姿态估计方法。先进的姿态估计方法包括两种方法：第一种方法基于互补滤波的方法，其利用加速度计、磁力计和陀螺仪在频域上的互补误差特性，利用梯度下降法或者PID方法(比例、本文档来自技高网...

【技术保护点】
1.一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，其特征在于，包括如下步骤：/n虚拟测量阶段；/n卡尔曼滤波的预测阶段；/n卡尔曼滤波的更新阶段；/n虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合；/n姿态估计；/n其中，所述的虚拟测量阶段包括如下步骤：/n(1)MARG传感器校准；/n(2)量纲归一化；/n(3)模值计算；/n(4)传感器相邻输出的叉乘；/n(5)运动状态矩阵的合成；/n(6)运动状态矩阵欧式距离的梯度；/n(7)虚拟测量的个数估计。/n

【技术特征摘要】
1.一种虚拟测量与卡尔曼滤波融合的载体姿态估计方法，其特征在于，包括如下步骤：
虚拟测量阶段；
卡尔曼滤波的预测阶段；
卡尔曼滤波的更新阶段；
虚拟测量阶段与卡尔曼滤波的预测阶段、更新阶段的融合；
姿态估计；
其中，所述的虚拟测量阶段包括如下步骤：
(1)MARG传感器校准；
(2)量纲归一化；
(3)模值计算；
(4)传感器相邻输出的叉乘；
(5)运动状态矩阵的合成；
(6)运动状态矩阵欧式距离的梯度；
(7)虚拟测量的个数估计。


2.根据权利要求1所述的虚拟测量与卡尔曼滤波融合的载体姿态估计方法，其特征在于，步骤(1)所述的MARG传感器的校准是校准加速度计、陀螺仪和磁力计的正交耦合误差、零偏误差和比例因子误差，校准公式如(1)、式(2)、式(3)所示，









其中，分别表示磁力计、陀螺仪和加速度计校准后的输出；m，ω和a分别表示磁力计三轴的原始输出，陀螺仪三轴的原始输出和加速度计三轴的原始输出；矩阵和分别表示磁力计、加速度计和陀螺仪的线性时不变误差，即比例因子误差和正交耦合误差；bm、bg和ba分别表示磁力计、加速度计和陀螺仪的零偏误差；
步骤(2)所述的量纲归一化是将加速度除以9.8米/秒2、角速度除以1度/秒和磁场强度除以0.55Gs；
步骤(3)所述的模值计算是分别计算三轴加速度的模值、三轴角速度的模值和三轴磁场的模值，计算公式如(4)、式(5)、式(6)所示，









其中，和分别表示磁力计输出、陀螺仪输出和加速度计输出的模值；分别表示磁力计X轴，Y轴和Z轴输出的平方；分别表示陀螺仪X轴，Y轴和Z轴输出的平方；分别表示加速度计X轴，Y轴和Z轴输出的平方；
步骤(4)所述的传感器相邻输出的叉乘是分别计算磁力计、陀螺仪和加速度计的当前时刻的输出与前一时刻输出的叉乘，计算公式如(7)、式(8)、式(9)所示，









相应的模值计算公式为：






其中，deltm、deltω、delta分别表示磁力计、陀螺仪和加速度计相邻输出的叉乘；分别表示当前时刻(k时刻)磁力计、陀螺仪和加速度计的输出值；分别表示前一时刻(k-1时刻)磁力计、陀螺仪和加速度计的输出值；
步骤(5)所述的运动状态矩阵的合成是指将步骤(3)中得到的三个模值和步骤(4)中得到的三个叉乘结果的模值合成一个6行1列或1行6列的运动状态矩阵，如式(10)或式(11)所示，该矩阵的模值表征了当前运动状态的复杂程度，如式(12)所示，...

【专利技术属性】
技术研发人员：史凌峰，刘公绪，赵磊，于淼鑫，侯志勇，辛东金，
申请(专利权)人：西安电子科技大学，
类型：发明
国别省市：陕西;61