一种基于卡尔曼滤波与显式互补滤波的姿态融合算法制造技术

本发明专利技术提供了一种基于卡尔曼滤波与显式互补滤波的姿态融合算法，属于传感器数据采集与融合集成技术领域。包括如下步骤：步骤1，选用多轴姿态传感器，将多轴姿态传感器中的加速计的加速度及陀螺仪的角速度进行卡尔曼滤波，输出a、ω；步骤2，将a、ω通过四元数法转换成四元数向量；以及步骤3，将四元数向量进行显示互补滤波融合计算输出俯仰角θ、横滚角φ以及偏航角ψ。因为在本发明专利技术中，姿态传感器实时采集加速度、角速度的六个参数，并通过卡尔曼滤波与显式互补滤波进行姿态融合，所以可以避免使用加速度计和陀螺仪时造成的误差，得到精确的俯仰角θ、横滚角φ以及偏航角ψ，在多旋翼无人机的姿态估计中具有广泛的应用前景。人机的姿态估计中具有广泛的应用前景。人机的姿态估计中具有广泛的应用前景。

【技术实现步骤摘要】
一种基于卡尔曼滤波与显式互补滤波的姿态融合算法


[0001]本专利技术涉及一种传感器数据采集与融合集成
，具体涉及一种基于卡尔曼滤波与显式互补滤波的姿态融合算法。

技术介绍

[0002]近几年，无人机企业在世界范围内的行业地位越来越高，从而也带动了一批优秀的科技创新型企业投入到了无人机的研发队伍当中。无人机作为一个空中的平台，通过其于携带的机载设备可具备多种功能。由于多旋翼飞行器具有可垂直升降、成本低、结构简单、性能稳定等诸多优点，越来越多的企业集中在多旋翼飞行器行业应用(如航拍、植保、物流、低空探测等)的研究。
[0003]但目前的无人机领域仍是蓝海行业，针对于无人机的测试也并不完善。一些商用无人机企业更注重用户的实际体验，因此往往会在设计完成后在合适地点进行试飞，但是无人机野外调试成本高、数据复杂、安全性差、受干扰因素多。此外，现今对无人机的功能和性能等方面要求越来越多，许多无人机需适应一些较恶劣或极端恶劣的飞行条件。综上，在无人机试航要求逐渐提高的当下，用于进行无人机飞行能力测试仿真平台已经变的越来越不可或缺。
[0004]目前市场上现有的无人机测试平台为二轴二自由度，只能测试无人机前后左右的移动。z轴的移动需要通过底层平台的手动上下运动模拟，且不能读取数据。此外，无人机实际飞行时会产生的俯仰角，偏航角以及旋转角并不能通过该测试平台进行实时监测。由于其功能单一、通用化程度低、测试传感器不全面等缺陷，使其不能满足综合、快速、准确、可视化的测试需要。

技术实现思路

[0005]本专利技术是为了解决上述问题而进行的，目的在于提供一种基于卡尔曼滤波与显式互补滤波的姿态融合算法。
[0006]本专利技术提供了一种基于卡尔曼滤波与显式互补滤波的姿态融合算法，包括如下步骤：步骤1，选用多轴姿态传感器，将多轴姿态传感器中的加速计的加速度及陀螺仪的角速度进行卡尔曼滤波，输出a、ω；步骤2，将a、ω通过四元数法转换成四元数向量；以及步骤3，将四元数向量进行显示互补滤波融合计算输出俯仰角θ、横滚角φ以及偏航角ψ。
[0007]在本专利技术提供的基于卡尔曼滤波与显式互补滤波的姿态融合算法中，还具有这样的特征：其中，步骤1中，卡尔曼滤波的过程如下：
[0008][0009][0010][0011][0012][0013]式(1)
‑
(5)中，为预测量，A为状态转移矩阵，为预测的均方误差，Q为系统噪声的协方差矩阵，K
k
为卡尔曼增益，R为测量噪声协方差，y
k
为观测值，H为状态量到观测量的转移矩阵。
[0014]在本专利技术提供的基于卡尔曼滤波与显式互补滤波的姿态融合算法中，还具有这样的特征：其中，步骤2中，四元数向量与角速度的关系式如式(6)所示：
[0015][0016]式(6)中，q为四元数，为q的微分，q0、q1、q2、q3为四元数向量，ω
x
、ω
y
、ω
z
分别为ω在x方向、y方向、z方向的角速度，假设在一个时间周期T内，陀螺仪的角速度ω是一个固定值，则四元数的迭代方程如式(7)所示：
[0017][0018]在本专利技术提供的基于卡尔曼滤波与显式互补滤波的姿态融合算法中，还具有这样的特征：其中，步骤3中，显示互补滤波的过程如下：
[0019][0020]δ＝k
P
·
e+k
I
·
∫e
ꢀꢀꢀ
(9)
[0021][0022]式(8)
‑
(10)中e为误差向量，为加速度计的输出向量，由四元数的估计值经坐标变换得到的向量，δ由误差向量e通过PI运算得到的新息，k
P
为低通滤波器和高通滤波器的转接频率，k
I
为陀螺仪的矫正误差，为姿态估计的四元数，为四元数q的估计值，P为四元数只有旋转，实部为零，Ω为陀螺仪的角速度。
[0023]在本专利技术提供的基于卡尔曼滤波与显式互补滤波的姿态融合算法中，还具有这样的特征：其中，的计算公式如式(11)所示：
[0024][0025]a为加速计的加速度，||a||为a的绝对值。
[0026]在本专利技术提供的基于卡尔曼滤波与显式互补滤波的姿态融合算法中，还具有这样的特征：其中，多轴姿态传感器为六轴姿态传感器或九轴姿态传感器。
[0027]专利技术的作用与效果
[0028]根据本专利技术所涉及的基于卡尔曼滤波与显式互补滤波的姿态融合算法，包括如下步骤：步骤1，选用多轴姿态传感器，将多轴姿态传感器中的加速计的加速度及陀螺仪的角速度进行卡尔曼滤波，输出a、ω；步骤2，将a、ω通过四元数法转换成四元数向量；以及步骤
3，将四元数向量进行显示互补滤波融合计算输出俯仰角θ、横滚角φ以及偏航角ψ。因为在本专利技术中，姿态传感器实时采集加速度、角速度的六个参数，并先通过一级滤波算法卡尔曼滤波，然后再经过二级滤波算法显式互补滤波进行姿态融合，所以可以避免使用加速度计和陀螺仪时造成的误差，得到精确的俯仰角θ、横滚角φ以及偏航角ψ，在多旋翼无人机的姿态估计中具有广泛的应用前景。
附图说明
[0029]图1是本专利技术的实施例中基于卡尔曼滤波与显式互补滤波的姿态融合算法的流程图；以及
[0030]图2是本专利技术的实施例中基于卡尔曼滤波与显式互补滤波的姿态融合算法的参考流程图。
具体实施方式
[0031]为了使本专利技术实现的技术手段、创作特征、达成目的与功效易于明白了解，以下结合实施例及附图对本专利技术一种基于卡尔曼滤波与显式互补滤波的姿态融合算法作具体阐述。
[0032]<实施例>
[0033]在本实施例中，以六轴姿态传感器为研究对象，通过基于卡尔曼滤波与显式互补滤波的姿态融合算法对该六轴姿态传感器进行融合算法为例进行具体说明。
[0034]图1是本专利技术的实施例中基于卡尔曼滤波与显式互补滤波的姿态融合算法的流程图，图2是本专利技术的实施例中基于卡尔曼滤波与显式互补滤波的姿态融合算法的参考流程图。
[0035]如图1
‑
2所示，六轴姿态传感器的融合算法具体包括如下步骤：
[0036]步骤1，将六轴姿态传感器中的加速计的加速度及陀螺仪的角速度进行卡尔曼滤波，输出a、ω，进入步骤2。
[0037]在本实施例中，步骤1包括如下子步骤：
[0038]步骤1
‑
1，计算出六轴姿态传感器中外设磁力计的偏航角ψ；
[0039]步骤1
‑
2，再输入六轴姿态传感器中加速度计分别在x、y、z三个方向上的加速度值a
x
、a
y
、a
z
以及陀螺仪在x、y、z三个方向上的角速度值ω
x
、ω
y
、ω
z
，进行卡尔曼滤波；
[0040]步骤1
‑
3，将六本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于卡尔曼滤波与显式互补滤波的姿态融合算法，其特征在于，包括如下步骤：步骤1，选用多轴姿态传感器，将所述多轴姿态传感器中的加速计的加速度及陀螺仪的角速度进行卡尔曼滤波，输出a、ω；步骤2，将所述a、所述ω通过四元数法转换成四元数向量；以及步骤3，将所述四元数向量进行显示互补滤波融合计算输出俯仰角θ、横滚角φ以及偏航角ψ。2.根据权利要求1所述的基于卡尔曼滤波与显式互补滤波的姿态融合算法，其特征在于：其中，步骤1中，所述卡尔曼滤波的过程如下：卡尔曼滤波的过程如下：卡尔曼滤波的过程如下：卡尔曼滤波的过程如下：卡尔曼滤波的过程如下：式(1)
‑
(5)中，为预测量，A为状态转移矩阵，为预测的均方误差，Q为系统噪声的协方差矩阵，K
k
为卡尔曼增益，R为测量噪声协方差，y
k
为观测值，H为状态量到观测量的转移矩阵。3.根据权利要求1所述的基于卡尔曼滤波与显式互补滤波的姿态融合算法，其特征在于：其中，步骤2中，所述四元数向量与所述角速度的关系式如式(6)所示：式(6)中，q为四元数，为q的微分，q0、q1、q2、q3为四元数向量，ω
x
、ω
y
、ω
z
分别为所述ω在x...

【专利技术属性】
技术研发人员：刘乙人，夏鲲，李翔，樊鸿亮，李银，楼阳，
申请(专利权)人：上海理工大学，
类型：发明
国别省市：