一种考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，用于集成三轴微陀螺仪、三轴微加速度计和三轴磁阻传感器的惯性测量装置，该方法用转置检测到的旋转角速度矢量、加速度矢量和磁场传感器矢量，借助滤波技术实现装置载体的位姿跟踪估计。该方法包括：１）将加速度矢量视为装置载体本身加速度矢量和重力加速度矢量的复合，并对其幅值和归一化的方向矢量分别构建观测方程；２）利用位姿描述四元数、陀螺仪累积误差矢量、装置载体本身加速度矢量构建系统状态向量；３）因观测方程非线性，用无迹卡尔曼滤波技术实现系统的滤波估计过程。同传统忽略载体本身加速度的方法相比，本发明专利技术不但能够给出更为准确的估计结果，且拓宽了系统应用范围。
【技术实现步骤摘要】
本专利技术涉及惯性装置的姿态感知
，是一种适用于集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的惯性装置的姿态感知和估计方法，具体是一种考虑加速度补偿和基于无迹卡尔曼滤波(UKF，UnscentedKalman Filter)的惯性位姿跟踪方法。
技术介绍
利用机电惯性测量组合技术进行运动载体位姿的跟踪具有非常广阔的前景。惯性跟踪系统的基本原理是在目标初始位置和姿态已知的基础上，依据惯性原理，利用陀螺仪和加速度计等惯性敏感元件测量物体运动的角速度和直线加速度，然后通过积分获得物体的位置和姿态。由于惯性积分存在累积误差效应，通常需要附加磁场传感器等其它感知元件，以便能够在较长运行时间内保证系统的位姿感知精度。 对于集成三轴陀螺仪、三轴加速度计以及三轴磁场传感器的检测装置而言，其位姿跟踪算法通常利用四元数描述装置相应于初始时刻的姿态信息，然后借助于最陡下降法或者卡尔曼(Kalman)滤波技术实现装置姿态的实时跟踪。上述方法在对加速度信息进行处理时，通常假设装置本身的运动加速度幅值远远小于重力加速度，以便能够利用线性技术实现位姿跟踪过程。上述假设所带来的缺陷是姿态感知精度的降低，尤其在装置本身存在较大幅度的运动情况下。 Choukroun D.提出了一种基于四元数的Kalman滤波方法(见Choukroun D.，Bar-Itzhack I.，Oshman Y.，Novel quaternion Kalmanfilter，IEEE Transactions on Aerospace and Electronic Systems，2006，Vol.42，No.1174-190)。该方法较为详尽地描述了利用Kalman技术实现四元数姿态跟踪的系统数学模型和噪声模型，其突出贡献是系统模型的伪线性化和状态相关噪声的协方差矩阵的更新方法。然而，遗憾的是，该方法仍然没有考虑装置自身加速度对于系统精度的影响，从而导致其精度在某些情况下不能得到保证。 本专利技术在Choukroun D.所提方法的基础上，充分考虑到装置自身加速度对系统模型的影响，对其带来的非线性问题利用UKF技术对其进行解决，从而提出了一种考虑加速度补偿和基于UKF的惯性位姿跟踪方法。 本专利技术申请人在申请号为“200810114391.4”的中国专利“基于ZigBee无线单片机的微惯性测量装置”中提供了一种可用于运动载体姿态测量的装置。在该申请中，采用六轴微惯性传感器和三轴磁阻传感器来测量运动载体的姿态，通过基于ZigBee无线单片机对所测得的信号进行姿态解算，并将解算得到的姿态信息以无线方式传送给其他系统或者上位机，该申请在本申请中引入作为参考。
技术实现思路
本专利技术的目的是提供一种，适用于集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的惯性装置的惯性位姿跟踪算法，该算法不但能够克服系统误差，而且能够在装置本身存在较大幅度的运动情况下也能保持较高精度。 为了达到上述目的，本专利技术的技术解决方案是 一种考虑加速度补偿和基于无迹卡尔曼滤波(UKF，Unscented KalmanFilter)的惯性位姿跟踪方法，适用于以正交方式集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的姿态感知装置；其系统状态向量包含了装置本身的运动加速度，并对其进行滤波估计；包括以下步骤 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为相应阶次的单位矩阵； 4构建系统观测方程 式中，aob(k)＝ao+ab(k)，对于任意三维矢量r＝[rx，ry，rz]T， 为其归一化单位方向矢量，||r||为其幅值，矩阵 其中，[□]×表示由相应向量定义的反对称矩阵；nGz为观测噪声矢量，其协方差矩阵为 5系统状态Sigma点采样根据k-1时刻的系统状态X(k-1/k-1)和协方差矩阵P(k-1/k-1)进行Sigma点采样，得到21个点样本为Xsi，i＝1，…，20； 6UKF预测根据方程(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为相应点样本的权值； 7UKF更新对于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)中的四元数向量元素进行归一化处理，并利用四元数表示同欧拉角表示之间的关系将其转换为具有较为直观意义的俯仰角、横滚角和航向角。 所述的，其所述系统状态向量包含了装置本身的运动加速度，从而导致了系统的观测方程非线性，因此采用UKF技术实现对于系统状态的滤波估计。 所述的，其对于加速度矢量和磁场矢量分别依据其单位方向矢量构建观测方程Za1(k)和Zm(k)；为了能够有效地估计出装置本身的运动加速度，观测方程中的Za2(k)对加速度矢量进行了幅值限制，从而在单位方向矢量一定的情况下，能够唯一估计出加速度矢量ab(k)。 所述的，其系统状态方程或系统观测方程，其中的噪声均与系统状态向量相关，对于协方差矩阵的预测和更新均对此进行了处理。 所述的，其由于系统状态包含了装置本身的运动加速度，同以往忽略该项因素相比，能够获取更为精确本文档来自技高网...
【技术保护点】
一种考虑加速度补偿和基于无迹卡尔曼滤波的惯性位姿跟踪方法，适用于以正交方式集成三轴陀螺仪、三轴加速度计和三轴磁场传感器的姿态感知装置；其特征在于，系统状态向量包含了装置本身的运动加速度，并对其进行滤波估计；包括以下步骤：１）保持装置固定不动，当前姿态称为初始姿态；采集三轴加速度传感器和三轴磁阻传感器数据，得到初始姿态下的加速度矢量ａ↓［ｏ］＝［ａ↓［ｏｘ］，ａ↓［ｏｙ］，ａ↓［ｏｚ］］↑［Ｔ］和磁场矢量ｍ↓［ｏ］＝［ｍ↓［ｏｘ］，ｍ↓［ｏｙ］，ｍ↓［ｏｚ］］↑［Ｔ］；２）在ｋ…，２１（４）利用上述采样预测值确定系统状态向量和协方差矩阵的最终预测值为：Ｘ（ｋ／ｋ－１）＝＊ｗ↓［ｉ］Ｘ↓［ｓｐｉ］（５）Ｐ（ｋ／ｋ－１）＝＊ｗ↓［ｉ］［Ｘ↓［ｓｐｉ］－Ｘ（ｋ／ｋ－１）］［Ｘ↓［ｓｐｉ］－Ｘ（ｋ／ｋ－１）］↑［Ｔ］＋Ｑ↓［ｋ－１］（６）＊＊＊（７）Ｍ↓［ｋ－１］＝ｑ（ｋ－１／ｋ－１）ｑ↑［Ｔ］（ｋ－１／ｋ－１）＋Ｐ↓［ｑ］（ｋ－１／ｋ－１）（８）其中Ｐ↓［ｑ］（ｋ－１／ｋ－１）为矩阵Ｐ（ｋ－１／ｋ－１）中相应于四元数向量的协方差子阵；ｗ↓［ｉ］为相应点样本的权值；７）ＵＫＦ更新：对于Ｓｉｇｍａ点预测Ｘ↓［ｓｐｉ］，令ｑ↓［ｉ］（ｋ／ｋ－１）为由向量Ｘ↓［ｓｐｉ］前四个元素得到的归一化四元数，根据观测方程其观测值计算为：Ｚ↓［ｉ］（ｋ）＝Ｇ（Ｘ↓［ｓｐｉ］（ｋ／ｋ－１））ｉ＝０，…，２１（９）而最终观测值计算为：Ｚ（ｋ）＝＊ｗ↓［ｉ］Ｚ↓［ｉ］（ｋ）（１０）系统状态向量和协方差矩阵的最终更新为：Ｘ（ｋ／ｋ）＝Ｘ（ｋ／ｋ－１）－ＫＺ（ｋ）（１１）Ｐ（ｋ／ｋ）＝Ｐ（ｋ／ｋ－１）－ＫＰ↓［ＺＺ］Ｋ↑［Ｔ］（１２）其中：Ｋ＝Ｐ↓［ＸＺ］Ｐ↓［ＺＺ］↑［－１］（１３）Ｐ↓［ＺＺ］＝＊ｗ↓［ｉ］［Ｚ↓［ｉ］（ｋ）－Ｚ（ｋ）］［Ｚ↓［ｉ］（ｋ）－Ｚ（ｋ）］↑［Ｔ］＋＊ｗ↓［ｉ］Ｒ↓［ｉ］（１４）Ｐ↓［ＸＺ］＝＊ｗ↓［ｉ］［Ｘ↓［ｓｐｉ］－Ｘ（ｋ／ｋ－１）］［Ｚ↓［ｉ］（ｋ）－Ｚ（ｋ）］↑［Ｔ］（１５）＊＊＊（１６）Ｄ↓［＊ｉ］＝１／４σ↓［＊］↑［２］［ｔｒ（Ｍ↓［ｋｉ］）Ｉ↓［４×４］－Ｍ↓［ｋｉ］－Ｂ（＊↓［ｔ］（ｋ））Ｍ↓［ｋｉ］Ｂ↑［Ｔ］（＊↓［ｔ］（ｋ））］（１７）Ｄ↓［＊ｉ］＝１／４σ↓［＊］↑［２］［ｔｒ（Ｍ↓［ｋｉ］）Ｉ↓［４×４］－Ｍ↓［ｋｉ］－Ｂ（＊↓［ｔ］（ｋ））Ｍ↓［ｋｉ］Ｂ↑［Ｔ］（＊↓［ｔ］（ｋ））］（１８...
【技术特征摘要】
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)利用上述采样预测值确...
【专利技术属性】
技术研发人员：杜清秀，邹伟，原魁，
申请(专利权)人：中国科学院自动化研究所，
类型：发明
国别省市：11[中国|北京]
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。