曼滤波算法程序，对互补卡尔曼滤波算法进行验证，最终验证得到本发明的基于互 补卡尔曼滤波算法计算的融合姿态角度的测量精度大大提高。
[0123] 以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定 本发明的具体实施只局限于这些说明。对于本发明所属技术领域的技术人员来说，在不脱 离本发明构思的前提下，还可以做出若干等同替代或明显变型，而且性能或用途相同，都应 当视为属于本发明的保护范围。
【主权项】
1. 一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，其特征在于，包括W下步 骤： SI:搭建姿态角度测量系统，根据巧螺仪测量数据计算得到第一姿态角度，根据加速度 计测量数据计算得到第二姿态角度； S2:根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第= 姿态角度； S3:根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度W及所述第=姿态角 度进行融合计算得到融合姿态角度。2. 根据权利要求1所述的方法，其特征在于，步骤S2中根据卡尔曼滤波算法将所述第一 姿态角度和所述第二姿态角度进行融合得到第=姿态角度还包括:采用姿态角度方差的方 法对卡尔曼滤波算法中的过程噪声协方差和测量噪声协方差进行计算。3. 根据权利要求1或2所述的方法，其特征在于，步骤S3中，计算所述融合姿态角度时， 根据所述第一姿态角度、所述第二姿态角度W及所述第=姿态角度的方差计算相应的权 重，其中方差越大，相应的权重越小。4. 根据权利要求1所述的方法，其特征在于，步骤Sl具体包括： W巧螺仪的测量数据和加速度计的测量数据，建立姿态角度测量系统的状态方程和测 量方程分别如下式（1)和式(2):其中： 在式（1)中，angle_gy;ro化）、angle_gy;ro化-1)分别为k时刻和k-1时刻巧螺仪测量数据 计算的第一姿态角度，w_gyro化-1)为k-1时刻巧螺仪测量的角速度，dt为采样间隔，gyro_ bias为巧螺仪的偏差，其值为巧螺仪测量数据的平均值，P化)为k时刻巧螺仪过程噪声； 在式（2)中，angle_acc化)为加速度计测量数据计算出的第二姿态角度，M化)为k时刻 加速度计测量噪声。5. 根据权利要求4所述的方法，其特征在于，步骤S2具体包括： S21:根据卡尔曼滤波的离散状态方程和测量方程W及建立的姿态角度测量系统的状 态方程和测量方程进行比较，计算得到卡尔曼滤波算法中的参数； 卡尔曼滤波的离散状态方程和测量方程如下： X(k) = S ? X化-1)+C ? lKk-l)+P(k) (3) Y化）=H ? X化)+M化） （4) 其中：X化)和Y化)分别是k时刻的状态量和测量量;IKk-I)为k-1时刻的系统输入量;S 为系统的状态矩阵，C为系统的控制矩阵;H为测量矩阵;P和M分别表示系统的过程噪声和测 量噪声，P和M的协方差分别是Q和N; 对比式（I)和式(3)可得，系统状态量-，状态矩阵控制矩阵对比式(2)和式(4)可得，系统测量量Y化）=[angle_acc化）]，测量矩阵H=[l 0]; S22:根据巧螺仪的测量数据，由上一时刻巧螺仪测量数据计算的姿态角度预测当前时 刻的姿态角度，计算公式如下： 曰ngle_gyro(k)=曰ngle_gyro(k-l) + [w_gyro(k-l)-gyro_bi曰8] ? dt (5) S23:根据加速度计测量数据计算得到的姿态角度与步骤S22中巧螺仪测量数据计算的 姿态角度，计算姿态角度误差，计算公式如下： angle_err(k)=angle_acc(k)-angle_gyro(k) (6) 在式（6)中，angle_acc(k)为k时刻加速度计测量数据计算的第二姿态角度，angle_ gyro化)为步骤S22中计算的第一姿态角度，angle_erKk)为k时刻的上述两者的误差值； S25:计算卡尔曼滤波的状态量r化)对应的协方差R化），计算公式如下： R化)=S ? R化-1) ? sT+Q (9)式(9)中S为控制矩阵，sT为控制矩阵S的转置， Q为过程噪声P的协方差； S26:计算卡尔曼滤波增益Kg,计算公式如下：'10) 式(10)中H为测量矩阵，HT为测量矩阵H的转置，N为测量噪声M的协方差； S27:计算修正后的k时刻的卡尔曼滤波的状态量r化），计算方式如下： X' (k)=X'化-1)+Kg(k) ? angle_e;r;r(k) (11) S28:将k时刻的卡尔曼滤波的状态量r化)中的第一行第一列的值提取出来，即为根据 卡尔曼滤波算法融合所述第一姿态角度和所述第二姿态角度而计算的所述第=姿态角度， Wangle_Kalman化)表示，计算公式如下： angle_Kalman(k)=X'化）（1,1) (12)6. 根据权利要求5所述的方法，其特征在于，步骤S2具体还包括： S24:采用姿态角度方差的方法计算系统的过程噪声P的协方差Q和测量噪声M的协方差 N，计算公式如下：其中：Q_acc为加速度计的过程噪声协方差，Q_gyro为巧螺仪的过程噪声协方差，Var (ang 1 e_acc)为加速度计测量数据计算的姿态角度的方差，Var (ang 1 e_gy;ro)为巧螺仪测量 数据计算的姿态角度的方差。7. 根据权利要求5所述的方法，其特征在于，步骤S2具体还包括： 在进行卡尔曼滤波算法的过程中，对下一时刻的卡尔曼滤波的状态量r(k+l)对应的 协方差R化+1)进行更新，计算公式如下： R(k+l)=R(k)-Kg(k) ? H ? R(k) (13)8. 根据权利要求5至7任一项所述的方法，其特征在于，步骤S2具体还包括:在进行卡尔 曼滤波算法之前，将卡尔曼滤波的状态量r化)的初始值r (O)设定为任意值，将卡尔曼滤 波的状态量公化)对应的协方差R化)的初始值R(O)设定为非零任意值。9. 根据权利要求5至7任一项所述的方法，其特征在于，步骤S3具体包括： S31:根据巧螺仪测量数据计算的所述第一姿态角度和加速度计测量数据计算的所述 第二姿态角度，求出所述第一姿态角度和所述第二姿态角度的权重，计算公式如下：其中：B_gyro为巧螺仪测量数据计算出的所述第一姿态角度在两种传感器姿态角度中 的权重，化：1"(曰]1旨16_曰〇(3)为加速度计测量数据计算的姿态角度的方差，'Va;r(angle_gy;ro)为 巧螺仪测量数据计算的姿态角度的方差； S32:计算根据卡尔曼滤波算法计算得到的所述第=姿态角度在互补卡尔曼滤波算法 中的权重，计算公式如下：其中:A_Kalman为根据卡尔曼滤波算法计算得到的所述第S姿态角度在互补卡尔曼滤 波算法中的权重，化r(angle_Kalman)为根据卡尔曼滤波算法计算的姿态角度的方差； S33:计算互补卡尔曼滤波算法融合=个姿态角度而计算的融合姿态角度，计算公式如 下：其中:angle_ComKalman化)为根据互补卡尔曼滤波算法计算的融合姿态角度。
【专利摘要】本发明公开了一种基于互补卡尔曼滤波算法计算融合姿态角度的方法，包括以下步骤：搭建姿态角度测量系统，根据陀螺仪测量数据计算得到第一姿态角度，根据加速度计测量数据计算得到第二姿态角度；根据卡尔曼滤波算法将所述第一姿态角度和所述第二姿态角度进行融合得到第三姿态角度；根据互补滤波原理，将所述第一姿态角度、所述第二姿态角度以及所述第三姿态角度进行融合计算得到融合姿态角度。本发明提出的基于互补卡尔曼滤波算法计算融合姿态角度的方法，大大提高了姿态角度的测量精度。
【IPC分类】G01C1/00
【公开号】CN105651242
【申请号】
【发明人】马建设, 李国朋, 苏萍, 刘彤, 董瑛
【申请人】清华大学深圳研究生院
【公开日】2016年6月8日
【申请日】2016年4月5日