【摘要】 一种考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，用于集成三轴微陀螺仪、三轴微加速度计和三轴磁阻传感器的惯性测量装置，该方法用转置检测到的旋转角速度矢量、加速度矢量和磁场传感器矢量，借助滤波技术实现装置载体的位姿跟踪估计。该方法包括：1)将加速度矢量视为装置载体本身加速度矢量和重力加速度矢量的复合，并对其幅值和归一化的方向矢量分别构建观测方程；2)利用位姿描述四元数、陀螺仪累积误差矢量、装置载体本身加速度矢量构建系统状态向量；3)因观测方程非线性，用无迹卡尔曼滤波技术实现系统的滤波估计过程。同传统忽略载体本身加速度的方法相比，本发明不但能够给出更为准确的估计结果，且拓宽了系统应用范围。 【专利类型】发明申请 【申请人】中国科学院自动化研究所 【申请人类型】科研单位 【申请人地址】100080 北京市海淀区中关村东路95号 【申请人地区】中国 【申请人城市】北京市 【申请人区县】海淀区 【申请号】CN200810224898.5 【申请日】2008-10-24 【申请年份】2008 【公开公告号】CN101726295A 【公开公告日】2010-06-09 【公开公告年份】2010 【授权公告号】CN101726295B 【授权公告日】2011-09-07 【授权公告年份】2011.0 【IPC分类号】G01C21/16; G01C21/18; G01C21/10 【发明人】杜清秀; 邹伟; 原魁 【主权项内容】1.一种考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，适用于以正交方式集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的姿态感知装置；其特征在于，系统状态向量包含了装置本身的运动加速度，并对其进行滤波估计； 包括以下步骤： 1)保持装置固定不动，当前姿态称为初始姿态；采集三轴加速度传感器和三轴磁阻传感器数据，得到初始姿态下的加速度矢量ao＝[aox，aoy，aoz]T和磁场矢量mo＝[mox，moy，moz]T； 2)在k时刻采集三轴陀螺仪、三轴加速度传感器和三轴磁阻传感器数据，得到装置当前姿态下的旋转角速度矢量ωt(k)＝[ωtx(k)，ωty(k)，ωtz(k)]T、加速度矢量at(k)＝[atx(k)，aty(k)，atz(k)]T和磁场矢量mt(k)＝[mtx(k)，mty(k)，mtz(k)]T； 3)构建系统状态方程： 定义系统状态向量为： X(k)＝[qT(k)，μT(k)，abT(k)]T????(1) 其中：q(k)＝[q0(k)，q1(k)，q2(k)，q3(k)]T为描述当前姿态同初始姿态之间相对关系的旋转四元数矢量，μ(k)＝[μx(k)，μy(k)，μz(k)]T为三轴陀螺仪的累积误差矢量，ab(k)＝[abx(k)，aby(k)，abz(k)]T为装置自身的运动加速度矢量； 依据上述状态向量的系统状态方程为： 式中Δt为采样周期， nGx为噪声矢量，其协方差矩阵为 矩阵 I为相应阶次的单位矩阵，σω1、σω2、σω3和 为相关噪声变量的标准方差常数； 4)构建系统观测方程： 式中，aob(k)＝ao+ab(k)，对于任意三维矢量r＝[rx，ry，rz]T， 为其归一化单位方向矢量，||r||为其幅值，矩阵： 其中， 表示由相应向量定义的反对称矩阵；nGz为观测噪声矢量，其协方差矩阵为 σ|a|和 为相关噪声变量的标准方差常数； 5)系统状态Sigma点采样：根据k-1时刻的系统状态X(k-1/k-1)和协方差矩阵P(k-1/k-1)进行Sigma点采样，得到21个点样本为Xsi，i＝1，…，20； 6)UKF预测：根据方程(2)，对21个Sigma点进行状态预测： Xspi＝F(Xsi，ωt(k))??i＝0，…，21?????????(4) 利用上述采样预测值确定系统状态向量和协方差矩阵的最终预测值为： Mk-1＝q(k-1/k-1)qT(k-1/k-1)+Pq(k-1/k-1)????(8) 其中Pq(k-1/k-1)为矩阵P(k-1/k-1)中相应于四元数向量的协方差子阵；wi为相应点样本的权值； 7)UKF更新：对于Sigma点预测Xspi，令qi(k/k-1)为由向量Xspi前四个元素得到的归一化四元数，根据观测方程其观测值计算为： Zi(k)＝G(Xspi(k/k-1))??i＝0，…，21????????(9) 而最终观测值计算为： 系统状态向量和协方差矩阵的最终更新为： X(k/k)＝X(k/k-1)-KZ(k)?????(11) P(k/k)＝P(k/k-1)-KPZZKT????(12) 其中： 其中Pq(k/k-1)为矩阵P(k/k-1)中相应于四元数向量的协方差子阵； 8)将X(k/k)中的四元数向量元素进行归一化处理，并利用四元数表示同欧拉角表示之间的关系将其转换为具有较为直观意义的俯仰角、横滚角和航向角。 【当前权利人】中国科学院自动化研究所 【当前专利权人地址】北京市海淀区中关村东路95号 【统一社会信用代码】12100000400010945B 【引证次数】2.0 【被引证次数】92 【自引次数】2.0 【被自引次数】3.0 【被他引次数】89.0 【家族引证次数】4.0 【家族被引证次数】93
