[发明专利]一种基于优化的UKF算法的双通道组合定姿算法的实现方法

权利要求书

1.一种基于优化的UKF算法的双通道组合定姿算法的实现方法，其特征在于：包括步骤：

步骤1：用加速度计与磁力计分别与陀螺仪组成滤波系统；通过MEMS九轴传感器采集九轴数据，并传输至主机端；

步骤2：主机端通过UKF算法分别对陀螺仪/加速度计通道及陀螺仪/磁力计通道进行姿态解算，分别得到定姿系统的横滚角和俯仰角以及航向角；

具体流程为：

1)读取k时刻陀螺仪、加速度计及磁力计数据；并计算得到k-1时刻的最优状态量和误差协方差矩阵；

2)根据陀螺仪/加速度计当前时刻的数值计算一步转移矩阵，根据计算得到的一步转移矩阵、步骤1)中得到的k-1时刻的最优状态量计算k时刻的一步预测状态量；

3)由步骤2)得到的一步转移矩阵、步骤1)得到的k-1时刻的误差协方差矩阵和系统状态噪声矩阵计算k时刻的预测误差协方差矩阵；

4)对步骤3)得到的预测误差协方差矩阵进行正定化检测与处理；

5)对步骤2)得到的一步预测状态量进行UT分解得到预测状态量的Sigma点集和相应权重；

6)将步骤5)得到的预测状态量的Sigma点集代入观测方程解算出观测量预测点集；其中，观测方程根据具体的传感器采用相应方程；

7)对步骤6)得到的观测量预测点集按照权重求和得到观测量预测点集的均值作为观测量预测值；

8)由步骤6)得到的观测量预测点集、步骤7)得到的观测量预测值、由预测状态量的Sigma点集、一步预测状态量和系统观测噪声矩阵计算观测预测值协方差矩阵和状态预测值协方差矩阵；

9)由观测预测值协方差矩阵和状态预测值协方差矩阵计算卡尔曼增益矩阵；

10)由一步预测状态量、步骤9)得到的卡尔曼增益矩阵、观测量预测值和加速度计/磁力计表示的观测量测量值计算k时刻最优状态量；

11)根据系统预测误差协方差矩阵、卡尔曼增益矩阵和观测预测值协方差矩阵计算出k时刻的系统误差协方差矩阵；

12)将k时刻的最优状态量转化为欧拉角作为系统k时刻的输出；

步骤3：将步骤2得到的定姿系统的横滚角和俯仰角以及航向角进行数据融合得到最终姿态。

2.根据权利要求1所述的实现方法，其特征在于：所述正定化处理为：对误差协方差矩阵非负定性判定，如果负定，则改变UT变化中的参数κ。

3.根据权利要求1所述的实现方法，其特征在于：所述陀螺仪的姿态解算的状态方程计算如下：

四元数与旋转的关系表示为：

其中，Q是表示姿态的四元数，μ^R表示参考坐标系下的旋转轴，θ表示是旋转的角度，符号服从右手定则；

上式两边对时间求导，再展开成矩阵形式为：

其中是四元数对时间求导的结果，ω_x、ω_y、ω_z为载体在载体坐标系的三个轴上k时刻的角速度，是k时刻四元数符号；

采用龙哥库塔算法解此微分方程得：

其中，表示t时刻的姿态，表示t+h时刻的姿态，h是采样周期；

将上式作为UKF的状态方程。

4.根据权利要求1所述的实现方法，其特征在于：对于加速度计和磁力计，计算姿态的模型表示为：

其中u^b是载体坐标系下的测量值，u^R是参考坐标系下的测量值，由于本系统的参考坐标系是固定的地理坐标系，所以u^R是常量；是由四元数组成的旋转矩阵；

加速度姿态解算公式如下：

其中，a_x、a_y、a_z是加速度计在载体坐标系上的加速度归一化的结果，V(k)是观测噪声，满足E[v_k]＝0，Cov(v_kv_j)＝R_gδ_kj，该参数根据经验和实际情况，初始化为一极小常量；

磁力计姿态解算公式如下：

其中，m_x、m_y、m_z是磁力计在载体坐标系上的磁场强度归一化的结果；M_y、M_z是地理磁场在地理坐标系的y和z轴分量，由当地地理磁场分布决定。