系统与Xsens传感器均安装在跑车内。
[0143] 整个测试过程持续30分钟，如图7和图8所示，从上位机中记录的本发明提供的 卡尔曼滤波姿态估计方法和基准MTI-300传感器的俯仰、横滚姿态角对比图中可以定性的 看出，使用发明提供的方法解算出的姿态角与基准MTI-300传感器的姿态角吻合性较好， 且根据计算姿态差值的标准差在1度以内，故可知，本发明提供的卡尔曼滤波姿态估计系 统和方法具有很好的实践意义。
[0144] 应当说明的是，上述实施例均可根据需要自由组合。以上所述仅是本发明的优选 实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提 下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。
【主权项】
1. 一种基于加速度的卡尔曼滤波姿态估计方法，其特征在于，所述卡尔曼滤波姿态估 计方法具体包括以下步骤： S1建立MEMS陀螺输出yc,t模型： Yg,t=ωt+bt+vG,t 其中，ωt表示t时刻载体的真实转动角速率；bt表示t时刻的偏置项；vw表示t时刻 的第一高斯白噪声；G表示MEMS陀螺输出模型； S2建立MEMS加速度计输出yAit模块： Ya, t- at_g+VA, t 其中，bat表示载体坐标系下t时刻的运动加速度；bg表示载体坐标系下重力场向量；vAit表示t时刻的第二高斯白噪声；A表示MEMS加速度计输出模型； S3使用AR过程建立地理坐标系下的运动加速度nat模型： Π&?= C a * 1+VA, t 其中，ca表示衰减系数，且取值范围为0~1 ;nati表示t-1时刻的运动加速度；vAit表 不t时刻的第二尚斯白噪声； S4搭建卡尔曼滤波姿态估计系统进行数据融合，得到卡尔曼滤波姿态估计系统的状 态方程义(/):其中，系统误差状态向量尤(纟%表不系统误差 状态向量X(t)的导数，表示东向欧拉失准角，仍v表示北向欧拉失准角，興/_表示天向欧 拉失准角，bx表示X轴陀螺零偏值，by表示y轴陀螺零偏值，bz表示z轴陀螺零偏值；系统 状态转移矩阵，Cf表示载体坐标系到地理坐标系的变换坐标矩阵； 系统零均值白噪声W(t) = [wgxwgywgzΟΟ0]τ，且W~N(0，Q)，Q为系统噪声矩阵，wgx、wgy、wgz分别为陀螺中X、y、z三个轴上的噪声； S5测量卡尔曼滤波姿态估计系统得到量测方程z(t); Z(t) =H(t)X(t)+V(t)，δgE表示地理坐标系中东向重力场矢量与测量东向 重力场矢量的差值，SgN表示地理坐标系中北向重力场矢量与测量北向重力场矢量的差:测量零均值白噪声V(t)~N(0,R)，R为测量噪 声矩阵； S6离散化所述状态方程X(t)和所述量测方程Z(t); Xk=Fk/kiXk1+Wk 1 Zk=HkXk+Vk 其中，k-1、k分别表示t=k-1时刻和t=k时刻；Fk/kAt=k时刻到t=k-1时 刻状态一步转移矩阵;Wki表示t=k-1时刻零均值白噪声；HkSt=k时刻的测量矩阵； Vk表示t=k时刻的零均值白噪声； S7分别对状态一步转移矩阵Fk/k 系统噪声矩阵Q(tk)进行二阶近似：其中，Fk/k1表示t=k时刻到t=k-Ι时刻状态一步转移矩阵；I表示单位矩阵；滤波 周期T=tk-tk1;F(tkJ表示t=k-Ι时刻的系统状态转移矩阵；其中，Q(tk 〇表示t=k-1时刻的系统噪声矩阵，F(tkJ表示t=k-1时刻的系统状 态转移矩阵； S8循环迭代入卡尔曼滤波公式实现载体姿态的修正。2. 如权利要求1的卡尔曼滤波姿态估计方法，其特征在于，在步骤S1中，所述偏置项bt 使用一个受白噪声驱动的一阶马尔科夫过程表示： bt=bti+wtt 其中，bt表示t时刻的偏置项，bti表示t-1时刻的偏置项，wtit表示t时刻的第三高斯 白噪声。3. 如权利要求1所述的卡尔曼滤波姿态估计方法，其特征在于，在步骤S2中，还包括以 下步骤： 对MEMS加速度计输出yAit采用滑动平均的方式进行低通滤波。4. 如权利要求1或2或3的卡尔曼滤波姿态估计方法，其特征在于，在步骤S5中，得到 系统测量方程Z(t)具体包括以下步骤： 在地理坐标系下重力场矢量ng为：ng= [〇 〇 -ι]τ 由步骤S2得到，载体坐标系下重力场矢量bg为： b b g=at-yA_t 得到地理坐标系下测量重力场矢量ng为：其中，为陀螺积分姿态环节解算出的方向余弦矩阵，具体为：故，通过地理坐标系下重力场矢量ng与地理坐标系下测量重力场矢量ng之间的差值 得到系统的测量方程：其中，δgE表示地理坐标系中东向重力场矢量与测量东向重力场矢量的差值，δgN表 示地理坐标系中北向重力场矢量与测量北向重力场矢量的差值，δgu表示地理坐标系中天 向重力场矢量与测量天向重力场矢量的差值； 又，由于测量方程对于天向欧拉失准角灼;不具备可观性，因此将对应的测量值删除， 得到系统测量方程Z(t): Z(t) =H(t)X(t)+V(t)。5. -种基于加速度的卡尔曼滤波姿态估计系统，其特征在于，所述卡尔曼滤波姿态估 计系统应用于如权利要求1-4任意一项所述的卡尔曼滤波姿态估计方法，所述卡尔曼滤波 姿态估计系统中包括:MEMS陀螺、MEMS加速度计以及处理器，其中， 所述MEMS陀螺，用于测量载体坐标系下载体的真实转动角速度、陀螺高斯白噪声以及 陀螺零偏噪声； 所述MEMS加速度计，用于测量载体坐标系下载体的运动加速度、载体坐标系下重力场 向量以及加速度计高斯白噪声； 所述处理器，分别与所述MEMS陀螺和所述MEMS加速度计连接，所述处理器对所述MEMS陀螺和所述MEMS加速度计的测量数据进行处理，实现对载体的卡尔曼滤波姿态估计和姿 态修正。6. 如权利要求5所述的卡尔曼滤波姿态估计系统，其特征在于：所述MEMS陀螺为三轴 陀螺，所述MEMS加速度计为三轴加速度计。7. 如权利要求6所述的卡尔曼滤波姿态估计系统，其特征在于：所述三轴陀螺和所述 三轴加速度计集成在一惯性测量模块中。8. 如权利要求6或7所述的卡尔曼滤波姿态估计系统，其特征在于，所述处理器中包 括： 载体运动状态判定模块，用于判断载体是处于运动状态还是处于静止状态； 陀螺零偏计算模块，与所述载体运动状态判定模块连接，所述陀螺零偏计算模块用于 计算陀螺零偏噪声，包括MEMS陀螺三轴上的零偏噪声； 初始对准模块，与所述陀螺零偏计算模块连接，所述初始对准模块用于完成MEMS陀螺 和MEMS加速度计的初始对准； 卡尔曼滤波模块34,与所述初始对准模块连接，所述卡尔曼滤波模块34对所述MEMS陀 螺和所述MEMS加速度计的测量数据进行处理，实现对载体的卡尔曼滤波姿态估计； 姿态修正模块，与所述卡尔曼滤波模块34连接，所述姿态修正模块根据卡尔曼滤波模 块34对载体的卡尔曼滤波姿态估计实现对载体的姿态修正。
【专利摘要】本发明提供了一种基于加速度的卡尔曼滤波姿态估计方法及系统，其中，该卡尔曼滤波姿态估计方法中包括以下步骤：S1建立MEMS陀螺输出yG,t模型；S2建立MEMS加速度计输出yA,t模块；S3使用AR过程建立地理坐标系下的运动加速度nat模型；S4搭建卡尔曼滤波姿态估计系统进行数据融合，得到卡尔曼滤波姿态估计系统的状态方程S5测量卡尔曼滤波姿态估计系统得到量测方程Z(t)；S6离散化所述状态方程X(t)和所述量测方程Z(t)；S7分别对状态一步转移矩阵Fk/k-1和系统噪声矩阵Q(tk)进行二阶近似；统状态转移矩阵；S8循环迭代入卡尔曼滤波公式实现载体姿态的修正。其将载体运动加速度引入卡尔曼姿态估计和修正中，大大提高了本发明中卡尔曼姿态估计方法的精确度。
【IPC分类】G01C21/16, G01C21/20
【公开号】CN105300379
【申请号】CN201510670207
【发明人】吕印新, 张家奇, 陈养彬
【申请人】上海新纪元机器人有限公司
【公开日】2016年2月3日
【申请日】2015年10月13日