基于卡尔曼滤波的云台自适应姿态优化算法

刘 义，杨 鹏

（南京理工大学 瞬态物理国家重点实验室，南京 210094）

云台开发的核心就是对姿态进行实时解析与传送。姿态检测技术利用集成惯性测量单元（inertial measurement unit，IMU）对目标的姿态进行精确解算[1]。在IMU 中，陀螺仪负责采集姿态变化的角速度，理论上只对陀螺仪捕捉的角速度进行积分就可以得到负载的姿态角。但由于噪声误差的存在，直接积分会影响测量精度。因此，开发者需采用合理的算法对IMU 的信息进行融合解算。

常见的姿态解算算法有互补滤波、卡尔曼滤波等。线性互补滤波原理简单、易实现，但精度不高[2]。非线性互补滤波采用误差对角速度进行修正，能够改善姿态更新的精度和稳定性。

常规扩展卡尔曼滤波利用四元数进行姿态解算，这种方法能调用四元数的优势改善姿态解算的精度，但常规EKF 的系统噪声和量测噪声固定，难以削弱有害加速度对精度的影响，同时EKF 处理非线性系统时仍存在弊端，限制了卡尔曼滤波器的性能。

本文搭建模型对卡尔曼滤波器的系统噪声和量测噪声进行自适应调整，有效提高姿态的解算精度。同时，本文利用非线性互补滤波并行更新姿态，利用二者解算结果的变化率对滤波结果进行自适应融合，提高算法所得姿态的稳定性，以便为伺服电机输出平稳的角度补偿信号。

1 旋转动作的表示方法

1.1 欧拉角和旋转矩阵

根据惯导理论[3]，本文定义模型的x，y，z 轴分别为横滚轴、俯仰轴、航向轴，n 系为地理坐标系，b 系为机体坐标系。任一时刻的机体坐标系都能转换为地理坐标系依次绕z，y，x 轴旋转一定角度得到，如图1所示。

图1 坐标系间的姿态转换Fig.1 Attitude conversion between coordinate systems

以绕横滚轴（x 轴）旋转为例，旋转动作投影到yOz 面的坐标等效转换关系，如图2所示。

图2 绕x 轴旋转的坐标转换Fig.2 Coordinate transformation of rotation around x axis

结合几何原理，空间中一点（xn，yn，zn）从地理坐标系到机体坐标系的转换关系式为

式中：α 为机体绕横滚轴（x 轴）旋转的欧拉角。同理可得旋转体发生俯仰和偏航动作的坐标转换矩阵分别为，，故地理坐标系到载体坐标系的坐标转换阵Cnb表示为

展开可得Cnb的三角函数表达矩阵如式（4）所示：

综上，用欧拉角表示旋转动作的方法原理简单、表象直观，但此方法存在“万向节死锁”的现象[4]，且旋转轴顺序的改变也会影响旋转的结果，存在一定的弊端。

1.2 四元数

四元数（Quaternions）相较于欧拉角表示旋转，能避免万向锁的情况出现，而且运用于计算机运算时，使用四元数表示旋转仅需存储四个浮点数，相比矩阵更为轻量。因此，本文选用四元数进行姿态解算。

物体在三维空间中的任一旋转动作可以抽象表示为“向量绕轴旋转一定角度”的模型，如图3所示。

图3 空间向量旋转模型Fig.3 Space vector rotation model

根据旋转变换模型，推导出向量转换关系如式（5）所示：

其中：

式中：ε 为旋转角度；D 为转换矩阵；N 为中心轴向量，N=（l，m，n）。令四元数（q0，q1，q2，q3）各项为

整理得坐标系转换矩阵为

式（4）和式（8）对比整理可得，俯仰角、横滚角、航向角的四元数法表示形式如式（9）所示：

2 四元数卡尔曼滤波器

卡尔曼滤波是一种利用系统状态方程和观测数据来估计线性系统状态信息的最优估计算法。四元数卡尔曼滤波算法流程如图4所示[5]。

图4 四元数EKF 算法流程Fig.4 Flow chart of quaternion EKF algorithm

四元数卡尔曼滤波算法的原理是利用四元数微分方程来更新四元数[6]，从而计算出姿态角信息。结合云台模型和四元数理论，建立云台姿态解算系统的状态方程为

式中：qK为状态向量；ηK，γK分别表示系统噪声和量测噪声；zK为系统的量测值，这里指的是加速度计的输出；f（·），h（·）为非线性向量函数。根据捷联式惯性导航理论，四元数微分方程的表达形式如式（11）所示：

根据卡尔曼滤波器的原理，推导出四元数的先验估计值和噪声先验矩阵为式（14）和式（15）所示：

式中：QK-1为系统的噪声方差；PK-1为前一时刻的后验均方差矩阵；ΦK∣K-1为状态方程中的Jacobi 矩阵，根据式（12）得ΦK∣K-1的数学表达式如式（16）所示：

结合旋转矩阵和水平静置的加速度计数据特点，得出加速度预测矩阵如式（17）所示：

进而得出预测矩阵的Jacobi 矩阵为式（18）所示：

根据卡尔曼滤波器的基础公式中，滤波器增益公式如式（19）所示：

利用实际测量加速度值和预测加速度矩阵获得新息，并利用新息对式（14）四元数先验估计值进行补偿得到量测的最优估计后验值为

式中：ZK为加速度计的量测数据。下一步计算后验均方差矩阵PK，构成完整增益解算回路。增益计算公式如式（21）所示：

式（20）所得qK为经过滤波所得最优估计四元数，将qK代入三轴姿态角的四元数解算公式（9）可推算出当前采样时刻的姿态角最优估计值。

3 融合卡尔曼滤波的姿态解算算法

云台系统中影响系统进行IMU 数据姿态解算精度的因素主要有机体抖动、陀螺仪噪声等。卡尔曼滤波器能够滤除高频振动噪声，但在实际的云台系统中，非重力加速度对姿态解算结果的扰动作用必须予以重视，因为加速度计数据的统计学特性会发生非线性波动，同时云台的运动状态不能保持绝对的匀速，由此产生的非重力加速度会对姿态解算产生不利影响。假设式（15）、式（19）中噪声Q，R 恒定的做法不能准确地模拟现实中的噪声误差，导致算法精度降低。本文提出噪声调整模型来自适应改变量测值的可信度，改善算法的测量精度。

卡尔曼滤波器适用于线性系统，但现实中工作系统多为非线性时变系统，因此在第二章将模型进行了非线性近似化，如式（10）所示。但仅通过泰勒展开进行模型非线性化仍会带来额外误差[8]。本文采用非线性互补滤波的方法，对卡尔曼滤波回路的解算结果进行融合，一定程度上提高算法处理非线性系统的可靠性，并在云台姿态不变的情况下，降低对测量恒定角的均方差，提高系统的稳定性。

3.1 基于加速度的变噪声模型

针对有害加速度，常规过滤方法是通过设置加速度误差阈值来调整滤波器性能，这种办法虽然原理简单，但阈值的不确定性会对滤波效果造成影响。

本文构建模型来自适应地改变噪声矩阵Q，R的可信度因子，调整先验预测值和观测值对解算结果的影响权重。调整模型的数学原型是Sigmoid 函数。

Sigmoid 函数能够把函数的输入值变换为0 和1之间的输出。由Sigmoid 函数变形得到的模型函数不仅可为系统引入非线性因素，而且能使输出值在合理的范围内变化。Sigmoid 函数的数学特性如图5所示。

图5 Sigmoid 函数数学特性Fig.5 Mathematical characteristics of Sigmoid

利用常规EKF 算法进行解算时，系统的噪声方差Q 来源于陀螺仪误差Q0，量测噪声R 来源于加速度计误差R0。本文设计的系统噪声和量测噪声矩阵自适应计算公式为

式中：δQ，δR定义为自适应调整系数，作用特性为：当加速度数据的非重力部分比例减小时，需提高量测值的可信度，即增大δQ以增大Q、减小δR以减小R；反之，当非重力部分占比提高时，则需增大δR、减小δQ。联立Sigmoid 函数特点，设计δQ和δR的模型表达式如式（23）所示：

式中：kQ，kR分别代表加速度调整系数，控制自适应调整的速度，需根据具体工况进行调整；ab%表示加速度计数据中非重力加速度部分的占比，定义为

式中：a 为三轴加速度的合成量。根据上述模型可得，当kQ=kR=0 或者ab%=0 时，代表放弃关于噪声矩阵的自适应调整功能。

结合激活函数的数学特性，本文所提变噪声数学模型既可以自适应调节噪声以调整对估计值和量测值的信任度，又可以将对误差的调节力度限制在一定的区间内，避免因出现异常值而对预测结果产生不良的影响。

3.2 非线性互补滤波补偿模型

四元数解算算法的核心是四元数的更新。在EKF 算法中，式（14）调用惯性元件所测得陀螺仪数据和四元数微分方程计算出先验四元数，后续利用增益计算回路对结果进行补偿修正。

非线性互补滤波调用PI 控制器，利用加速度在水平方向上对重力进行补偿，修正陀螺仪的垂直误差。具体原理是将加速度计数据和四元数计算得到的重力分量叉乘得到误差，如式（25）所示，并利用PI 控制器对误差进行比例积分运算，如式（26）所示，对陀螺仪数据进行补偿[9]。

下一步根据式（12）完成四元数更新，继而由姿态角求解公式计算姿态角。如图6所示，非线性互补滤波算法流程参考图中Mathony 框选部分。

图6 改进型算法流程Fig.6 Improved algorithm flow chart

本文根据解算结果变化率对两个计算回路所计算的姿态进行自适应融合，数据融合模型的表达式如式（27）所示：

式中：δ 为融合权重因子，计算公式为

通过上述方式将非线性互补滤波与改进型变噪声EKF 算法进行融合，一定程度地降低系统处理非线性系统的误差，并有效提高姿态的稳定性，利于实际的工程应用。

4 仿真实验与结果分析

本次的验证实验主要以基于Matlab 软件平台的仿真实验为主。根据设定姿态产生6 轴IMU 传感器数据，包括陀螺仪数据、加速度计数据，没有对磁力计的数据进行模拟，且根据数学原理加速度计数据只能对俯仰角、横滚角进行修正，因此本次验证仅针对俯仰角和横滚角进行解算。

4.1 动态摇摆实验

摇摆姿态的初始条件如表1所示。

表1 仿真姿态初始条件Tab.1 Initial conditions of simulated attitude

根据摇摆的初始条件产生仿真用陀螺仪、加速度计数据。采用恒定噪声的扩展卡尔曼滤波和改进型融合滤波算法分别对惯导数据进行姿态角解算。摇摆数据真实值以及两种算法的滤波结果如图7所示。

图7 摇摆实验姿态对比图Fig.7 Comparison figure of swing test attitude

图中实线部分表示姿态角理论姿态，虚线表示扩展卡尔曼滤波算法解算姿态，点虚线表示本文改进型融合滤波算法解算的姿态。

如图7所示，EKF 算法解算的结果与真实姿态间存在较大的偏差，而改进型融合滤波算法在解算过程中利用平动加速度所占比例来动态调整系统对估计值和误差值的信任度因子，因此所得姿态与真实姿态几乎一致。算法具体的滤波效果量化分析如表2所示。

表2 摇摆实验误差分析Tab.2 Error analysis of rocking experiment

4.2 静态悬停实验

为了进一步验证融合非线性互补滤波对解算算法的影响，模拟了云台锁定跟踪目标的工况来验证在姿态角为恒定值时算法模型所得结果的精度。

设定俯仰角为30°、姿态角为20°时，EKF 算法和改进型融合算法对恒定姿态的检测结果，如图8所示。

图8中虚线表示模拟理论姿态；实线代表算法解算的实际姿态。滤波效果量化分析如表3所示。

表3 恒定角度实验误差分析Tab.3 Error analysis of constant angle experiment

图8 固定姿态角解算图Fig.8 Fixed attitude angle solution

综上，改进算法不仅可以有效地降低姿态角测量的误差，而且还能够将俯仰角和横滚角的测量结果标准差分别降低44%和47%，证明改进算法不仅可以提高云台姿态角恒定时的测量精度，还可以一定程度上改善系统的稳定性，以便为伺服电机输出平稳的角度补偿信号。

5 结语

本文提出了一种基于四元数扩展卡尔曼滤波器的云台姿态解算算法，利用Sigmoid 函数的数学特性搭建噪声自适应调整模型，并利用非线性互补滤波对解算结果进行进一步融合。实验证明，本文设计的改进型算法能有效提高姿态角的解算精度，并能在一定程度上改善悬停工况下姿态信号输出的稳定性，提高算法的工程实用性。

本文算法没有引入磁力计来进行数据融合，因此没能引入偏航角进行全姿态的测量研究。同时，由于非线性互补滤波的引入必将会增加算法的解算时间，实时性问题也应作为下一步研究的重点方向。