电子下载站wwwbbwwnet资料版权归合法所有者所有严禁

电子下载站www.bbww.net资料版权归合法所有者所有严禁用于商业用途微型惯性测量组合姿态测试系统邓海棠王凤梅摘要：本文介绍了微型惯性测量组合系统的构成，姿态的解算算法和参数识别的卡尔曼滤波方程。提出了用四元素四阶龙格-库塔法作为捷联惯导系统的姿态更新算法，并叙述了算法仿真程序。关键词：微机械系统；惯性测量组合；捷联姿态算法；卡尔曼滤波方程中图分类号：TP212.12文献标识码：A一、引言微型惯性测量组合(MIMU：Micro-inertialmeasurementunit)是国际上近年来发展起来的新技术，作为一种不依赖任何外部信息，也不向外部辐射能量的自主式系统，它具有数据更新率高、短期精度和稳定性好等特点，加之其具有体积小、重量轻等优点，这种技术在军事导航及民用领域正在起到越来越大的作用。因此，微惯性测量组合的研究成为近年来惯性技术的一个发展方向。在捷联惯性系统中，数学平台代替了常规的物理平台，计算机则完成了导航平台的功能。但由于这些计算的算法复杂，变量动态范围大且实时性要求高，计算机的任务变得十分繁重，而编制优良的软件可以补偿这些不足、提高系统的性能。所以，对捷联惯导系统，不仅要重视硬件方面的研究，对加强算法和软件的研究也不能忽视。二、基于微型惯性测量组合姿态测试的算法方程捷联惯导系统中，加速度传感器是沿机体坐标系Ox1y1z1安装的，它只能测量沿机体坐标系的比力分量fxb、fyb、fzb，但真正需测量的是平台坐标系中的分量，因此要将fxb、fyb、fzb转换为fxp、fyp、fzp。从机体坐标系到平台坐标系的转变要用到捷联矩阵T，由于根据其元素可以确定飞行器的姿态角，因此该矩阵又可叫做飞行器的姿态矩阵。转换公式如下：=TT=(1)~(2)式中，xp、yp、zp—平台坐标系坐标；xb、yb、zb—机体坐标系坐标。当矩阵T求得后，沿机体坐标系测量的就可以转换为平台坐标系上的，从而便可以进行导航计算。假设惯性坐标系Oxyz，载体坐标系Ox1y1z1，运动的飞行体从惯性坐标系首先绕Oz轴转动ϕ角，形成临时坐标系Ox′y′z′，再绕Oy′转动ψ角，形成临时坐标系Ox″y″z″，昀后绕Ox″轴转动γ角，昀后形成飞行体坐标系（载体坐标系）Ox1y1z1（图1所示），这时飞行体相对于惯性坐标系的姿态角ϕ、ψ、γ分别称位飞行器的格网航向角、俯仰角和倾斜角。欢迎光临中国昀大的电子工程师应用网站网址：www.bbww.net电子下载站www.bbww.net资料版权归合法所有者所有严禁用于商业用途1、四元数法运动学方程由欧拉定理可以证明，由四元素法表示的弹体坐标系相对于惯性坐标系的转动四元素与弹体坐标系转动角速度矢量ω1（t）之间的关系为：（3）式中，q(t)—t时刻的四元素矩阵；ω1(t)—t时刻的测量角速度矢量。此式称为四元素的微分方程，写成矩阵形式为：（4）将四元素建立联立微分方程组：（5）欢迎光临中国昀大的电子工程师应用网站网址：www.bbww.net2电子下载站www.bbww.net资料版权归合法所有者所有严禁用于商业用途对于任何参数值上式均无奇点，方程式不退化，可解，因此能用于全姿态的飞行计算，其约束方程为：姿态角表示为：（6）式中，；；；。2、四阶龙格—库塔法的描述龙格—库塔法是一种便于使用计算机求解微分方程的数值积分算法，表达为当已知方程式解的初值时，可在解存在的区间各点上，逐点求出方程式的近似解，算法的阶次越高，计算精度越高。由前述知，，若已知t时刻的q(t)，即q0(t)、q1(t)、q2(t)、q3(t)，t+T/2和t+T时刻的ω(t+T/2)、ω(t+T)，则的近似解为：(7)其中：；；；欢迎光临中国昀大的电子工程师应用网站网址：www.bbww.net3电子下载站www.bbww.net资料版权归合法所有者所有严禁用于商业用途。而实际测量值为ω1x1(t)，ω1y1(t)，ω1z1(t)，即载体坐标系下的角速度在该坐标系矢量轴上的分量。若已知t时刻的q(t)，即q0(t)、q1(t)、q2(t)、q3(t)，t时刻的ω1(t)，即ω1x1(t)、ω1y1(t)、ω1z1(t)，t+T/2时刻的ω1(t+T/2)，即ω1x1(t+T/2)、ω1y1(t+T/2)、ω1z1(t+T/2)，t+T时刻的ω1(t+T)，即ω1x1(t+T)、ω1y1(t+T)、ω1z1(t+T)，便可解t+T时刻的q(t+T)，即q0(t+T)、q1(t+T)、q2(t+T)、q3(t+T)。三、微型惯性测量组合姿态确定的卡尔曼滤波方程微型惯性测量组合姿态确定的状态估计是指在考虑姿态参数的动态特性和随机特性的情况下，对测量数据在统计的基础上进行处理，消除系统噪声和量测噪声，并根据信号和测量值的统计特性，从测量值中得出某种统计意义下具有昀小误差的状态估计。1、系统的状态方程选取姿态参数、位置参数和敏感器误差参数为状态变量，记为：（8）式中，ϕ，ψ，γ—姿态角参数；ax，ay，az—加速度参数三轴分量参数；ω′，ψ′，γ′—姿态角误差；ax′，ay′，az′—加速度误差。则可建立离散时间的状态方程为：X(k+1)=Φ(k+1,k)X(k)+W(k)（9）式中，Φ(k+1,k)—系统状态转移矩阵；W(k)—系统零均值随机噪声矢量。W(k)满足以下关系：E{W(k)}=0，E{W(k)WT(l)}=Q(k)δkl式中：；δkl—克罗内函数；(t)—系统噪声矩阵;Q(t)—系统的噪声方差强度阵2、系统状态观测方程欢迎光临中国昀大的电子工程师应用网站网址：www.bbww.net4电子下载站www.bbww.net资料版权归合法所有者所有严禁用于商业用途选取姿态参数、位置参数输出为状态观测变量，记为Z=[ϕ，ψ，γ，ax，ay，az]（10）式中，ϕ，ψ，γ—姿态角参数；ax，ay，az—加速度参数在三轴上的分量。则可建立状态观测方程为：Z(K+1)=H(K+1,k)X(k)+V(k)（11）式中，H(K+1k)—系统状态观测矩阵；V(k)—系统零均值随机观测噪声，满足E{V(k)}=0，E{V(k)VT(l)}=R(k)δkl。R(k)—测量噪声的方差矩阵3、估计误差方程设k时刻的状态向量为X(k)，估计值为，误差向量为，则估计误差方差矩阵为：（12）估计误差方差为：（13）卡尔曼滤波就是根据测量向量Z(k)，求出状态向量X(k)的昀佳估计值，使得估计误差的方差ξX(k)为昀小。4、预测误差方差矩阵根据估计方差昀小理论，k+1时刻的状态向量估计值等于状态向量X(k+1)在给定{Z(1)，Z(2)，…Z(k)}时的条件均值，即：将此式带入状态方程式(9)，其中：则得到状态向量X(k)在k+1时刻的昀优预测向量为：（14）欢迎光临中国昀大的电子工程师应用网站网址：www.bbww.net5电子下载站www.bbww.net资料版权归合法所有者所有严禁用于商业用途同理可以得到观测向量的昀优预测向量为：（15）联立求解，得到估值误差方差矩阵为：（16）5、滤波估值方程在对k时刻的状态向量X(k)进行预测后，根据实际得到k+1时刻的观测值Z(k+1)，对预测值进行修正，即可得到k+1时刻的状态向量X(k+1)的滤波估值为：（17）式中，X(k+1)——增益矩阵，（18）由式（17）、（18）、（9）和（11）联立得到滤波误差方差矩阵为：（19）6、卡尔曼滤波器状态估值计算程序基于以上计算公式，卡尔曼滤波器状态估值计算程序为：（1）算法初值（20）（2）预测方程（14）（15）（3）预测方差矩阵（16）欢迎光临中国昀大的电子工程师应用网站网址：www.bbww.net6电子下载站www.bbww.net资料版权归合法所有者所有严禁用于商业用途（4）增益矩阵（18）（5）滤波估值（17）（6）滤波方差矩阵（19）（7）令k=k+1，转回第（2）步。四、算法仿真与结果分析在这里，建立数学平台代替实际平台对四阶龙格-库塔姿态算法程序进行仿真。设三轴转动轨迹为：（21）把t=0代入式(21)得到三轴的初始角度为：ϕ=0.0872°；ψ=0.0523°；γ=0.0175°（22）对式(21)进行求导得出绕三个轴角速度为：（23）把式（21）的角速度和式（22）初始角度代入程序，并设置采样周期为10ms，得到俯仰角的仿真数据（见表1）。欢迎光临中国昀大的电子工程师应用网站网址：www.bbww.net7电子下载站www.bbww.net资料版权归合法所有者所有严禁用于商业用途欢迎光临中国昀大的电子工程师应用网站网址：www.bbww.net8五、结论本文从实际应用出发，验证了用四元素四阶龙格-库塔法计算四元素的即时修正算法是一种简洁、有效、实时的算法。捷联姿态算法性能的优劣直接影响到系统的导航精度，而四元素算法弥补了矩阵算法的不足，只需求解四个一阶微分方程，提高了解算速度。采用卡尔曼滤波算法可减小误差积累以及陀螺、加速度计的漂移影响，提高导航精度。四元素四阶龙格-库塔法和卡尔曼滤波为捷联惯导系统算法和编制捷联惯导系统程序提供了参考依据。参考文献：[1]单茂华，周百令.MIMU定位定向系统数据采集装置的设计[J].数据采集与处理.2002,17(4):390-394[2]陈哲.捷联惯性导航系统原理[M].北京:宇航出版社,1986:66-83.[3]郭圣权，贾建芳，吴强.捷联惯性导航中一种算法的推证[J].中国惯性技术学报.2002,10(2)：24-27.[4]费景高.四元数计算舍入误差的概率分析[J].导弹与航天运载技术.1995，2：12-19.[5]谢荣生，孙枫.误差估计模型的捷联姿态解算方法[J].哈尔滨工程大学学报.2000，21（3）：15-19.MicroInertialAttitudeTestunitsystemAbstract：TheMicrostrapdowninertialmeasurementunitSystemareintroduced,attitudealgorithmandKalmanfilteringequationofparameteridentifyingofMIMUarealsodescribed.Usingfour-orderquaternionRunge-Kuttamethodtoupdateattitudealgorithmispresented,thecorrespondingsimulationprogramsaredescribed.Keywords：micromechanicalsystem;inertialmeasurementunit;strapdownattitudealgorithm;Kalmanfilteringequation