专利名称：一种载体姿态测量方法及其系统的制作方法
技术领域：
本发明涉及姿态测量领域，更具体的说，本发明涉及一种用于飞行器等载体的姿态测量。
背景技术：
姿态测量是指利用各种传感器进行测量分析得到飞行器等载体相对于参考基准的姿态数据，这里所说的姿态数据通常指载体的姿态角，包括航向角ψ、俯仰角θ和滚转角γ。现有的姿态测量方法主要包括以下两种(1)传感器由三轴速率陀螺组成，速率陀螺测量得到的是载体运动的角速度，必须对其进行积分运算才能得到姿态角。但是微机电速率陀螺存在严重零点漂移问题，因此经过积分计算得到的姿态角将不再准确，这种测量系统受陀螺漂移影响严重。
(2)本申请人在申请号为01110135.0的中国专利“基于微机电技术的微型导航系统”中提供了一种由三轴磁强计和三轴加速度计组成的传感器测量载体姿态的方法，该专利在本申请中引入作为参考。在该方法中，用三轴加速度计测量重力加速度g在载体坐标系上三个正交轴的分量，用三轴磁强计测量地磁感应强度h在载体坐标系上三个正交轴的分量，根据重力加速度g和地磁感应强度h在地理坐标系中的表示，利用表述地理坐标系和载体坐标系之间相互转换关系的方向余弦矩阵建立方程组，最后求得载体的姿态角。当载体的以一定姿态平稳运动时，利用该方法测量得到的姿态角精度较高。但是当载体以加速度运动时或者在运动过程中受到扰动时，例如飞行器在飞行过程中受气流影响而具有某种加速度时，则三轴加速度计测量的加速度中既包括重力加速度还包括有载体的运动加速度，此时利用该方法计算得到的姿态角就具有相当大的误差。
因此就需要有一种方法能够在做加速运动时，消除载体的运动加速度对姿态测量的影响。

发明内容
本发明的在于克服现有技术的缺点和不足，提供一种载体姿态测量方法及其系统，以消除载体的运动加速度对姿态测量的影响。
为了实现上述目的，本发明提供一种载体姿态测量方法，包括测量重力加速度和地磁感应强度在载体坐标系三轴上的分量；还包括测量载体转动角速度在载体坐标系三轴上的分量[ωxωyωz]；利用测得的载体转动角速度分量对测得的重力加速度和地磁感应强度分量进行卡尔曼滤波，得到重力加速度和地磁感应强度分量真值的估计值；最后用所述估计值进行载体姿态解算；其中，所述卡尔曼滤波的状态方程为S·=ΩS+W;]]>所述卡尔曼滤波的量测方程为Z＝HS+V；S和Z分别为所述重力加速度和地磁感应强度在载体坐标系三轴上的分量[XgYgZgXmYmZm]的真值和量测值， 为所述真值对时间的导数，H为量测阵，V为量测噪声，W为系统激励噪声，Ω=-ωibbk00-ωibbk,ωibbk=0-ωzωyωz0-ωx-ωyωx0.]]>本发明还提供一种载体姿态测量系统，包括传感器和数据处理装置，所述数据处理装置根据所述传感器测得的信号进行载体姿态解算；所述传感器包括沿载体坐标系三轴布置的三轴加速度计，用于测量重力加速度在载体坐标系三轴上的分量；沿载体坐标系三轴布置的三轴磁强计，用于测量地磁感应强度在载体坐标系三轴上的分量；沿载体坐标系三轴布置的三轴速率陀螺，用于测量载体加速度在载体坐标系三轴上的分量。
所述加速度计为微机电加速度计，所述磁强计为微机电磁强计，所述速率陀螺为微机电速率陀螺。
所述载体姿态测量系统还包括第一滤波电路和第一放大电路，所述三轴加速度计的输出信号经第一滤波电路和第一放大电路滤波和放大后，再经A/D转换器进入所述数据处理装置。
所述载体姿态测量系统还包括第二滤波电路和第二放大电路，所述三轴磁强计的输出信号经第二滤波电路和第二放大电路滤波和放大后，再经A/D转换器进入所述数据处理装置。
所述载体姿态测量系统还包括第三滤波电路，所述三轴速率陀螺的输出信号经第三滤波电路滤波后经A/D转换器进入所述数据处理装置。
所述数据处理装置包括卡尔曼滤波模块和姿态解算模块；其中卡尔曼滤波模块，利用测得的载体转动角速度的分量[ωxωyωz]对测得的重力加速度和地磁感应强度的分量[XgYgZgXmYmZm]进行卡尔曼滤波，输出重力加速度和地磁感应强度分量真值的估计值；其中，所述卡尔曼滤波的状态方程为S·=ΩS+W;]]>所述卡尔曼滤波的量测方程为Z＝HS+V；S和Z分别为所述重力加速度和地磁感应强度在载体坐标系三轴上的分量[XgYgZgXmYmZm]的真值和量测值， 为所述真值对时间的导数，H为量测阵，V为量测噪声，W为系统激励噪声，Ω=-ωibbk00-ωibbk,ωibbk=0-ωzωyωz0-ωx-ωyωx0;]]>姿态解算模块，用于根据卡尔曼滤波模块输出的重力加速度和地磁感应强度在载体坐标系三轴上的分量真值的估计值进行载体姿态的解算。
本发明对现有的姿态测量系统进行了融合，得到了一个新型的包括九个传感器(三轴加速度计、三轴磁强计和三轴速率陀螺)的姿态测量系统。它采用包括卡尔曼滤波内在的算法进行数据处理，用稳定的陀螺信号来减小运动加速度对系统的影响，而且不存在陀螺的积分漂移问题，从而得到的载体姿态动态精度和稳定性优于现有的姿态测量系统。本发明可以实现载体的全姿态测量，即俯仰角测量范围为-90°～+90°，滚转角测量范围为-180°～+180°，航向角测量范围为0°～360°。进一步的，本发明中的传感器采用一体化MEMS芯片集成封装技术，使仪器更加小型化。


图1是地理坐标系与载体坐标系示意图，其中(a)为地理坐标系，(b)为载体坐标系；图2是本发明的载体姿态测量系统的结构示意图；图3是图2的载体姿态测量系统中的传感器的分布示意图；图4是图2的载体姿态测量系统中的数据处理装置进行信号处理的流程图。
具体实施例方式
下面结合附图和具体实施方式
对本发明作进一步详细描述。
在一个实例性的实施方式中，坐标系的建立如图1所示。图1(a)是以载体所在位置为原点建立三个正交轴组成的地理坐标系北-东-地(即N-E-D)，其中N轴是由载体指向正北方，E轴是由载体指向正东方，D轴是由载体指向地心。图1(b)是固连于载体100的三轴正交载体坐标系X-Y-Z，其中X轴处于载体对称平面内，由载体质心指向载体运动前向，Y轴垂直于载体对称平面并指向右方；Z轴在载体对称平面内且垂直于X轴指向下方，图1(b)中是以飞机作为载体100的一个实例。
本发明的姿态测量系统如图2所示，它布置在如图1中的载体100上。该姿态测量系统中的传感器包括三轴正交的加速度计10、三轴正交的磁强计20、三轴正交的速率陀螺30。传感器在载体100上的分布如图3所示，三轴正交的加速度计10包括第一加速度计11、第二加速度计12和第三加速度计13，这三个加速度计11、12和13分别沿载体坐标系的三个正交轴X-Y-Z平行配置，用于分别测量重力加速度在载体坐标系三个正交轴X-Y-Z上的分量。三轴正交的磁强计20包括第一磁强计21、第二磁强计22和第三磁强计23，这三个磁强计21、22和23分别与载体坐标系的三个正交轴X-Y-Z平行配置，用于分别测量地磁感应强度在载体坐标系三个正交轴X-Y-Z上的分量。三轴正交的速率陀螺30包括第一速率陀螺31、第二速率陀螺32和第三速率陀螺33，这三个速率陀螺31、32和33分别与载体坐标系的三个正交轴X-Y-Z平行配置，用于分别测量载体转动角速度在三个正交轴X-Y-Z上的分量。
为了让姿态测量系统200适用于微型飞行器，在本发明中，传感器采用了体积小、质量轻、功耗小的硅微结构传感器，这种传感器是在单晶硅、石英晶体、铌酸锂等电光材料芯片上应用光刻、腐蚀、沉积、离子注入、键合等微机械加工技术生产而成。具体地说，在本发明中，加速度计10为三轴正交的微机电加速度计，磁强计20为三轴正交的微机电磁强计，速率陀螺30为三轴正交的微机电速率陀螺，均为市售的产品。
返回图2，加速度计10、磁强计20和速率陀螺30的输出为模拟的电压信号，分别经过第一滤波电路14、第二滤波电路24和第三滤波电路34对信号进行滤波，用于滤去传感器输出信号中的高频噪声。从滤波电路14和24输出的信号经过分别经过第一放大电路15和第二放大电路25放大后进入A/D转换器40转化为数字信号。由于速率陀螺30的电压输出范围一般已满足A/D转换器40的输入要求，因此滤波电路34输出的信号直接进入A/D转换器40转化为数字信号。
A/D转换器40输出的数字信号传输至一个数据处理装置50进行载体姿态的计算。其中，数据处理装置50可以是一个具有内嵌程序的微形处理器，由该内嵌的程序进行载体姿态的计算，这适用于载体100为微型飞行器的情况；数据处理装置50也可以是一个PC机，由PC机内存储的计算机程序进行载体姿态的计算，这适用于对姿态测量系统200的体积和重量要求不严的载体，例如大型的飞行器。特别是，当数据处理装置50为PC机时，A/D转换器40可以通过一个数据传输模块(图中未示出)将数字信号传输至PC机，该数据传输模块可以为USB传输模块、串口传输模块等。
如前所述，姿态测量系统进行载体姿态计算的程序装载于数据处理装置50中，该程序包括数据获取模块、信号融合模块、卡尔曼滤波模块和姿态解算模块，并按图4所示的流程图进行。
如图4所示，在步骤401中，数据获取模块控制数据处理装置50接收来自于A/D转换器40的数字信号，该信号为加速度计10、磁强计20和速率陀螺30测得的电压信号经模数转换后得到的数字电压信号。
在步骤402中，与所引用的申请号为01110135.0专利的方法相同，采用一个信号融合模块对信号进行处理。该信号融合模块包括信号滤波单元和信号转换单元。其中信号滤波单元对数据获取模块得到的数字信号进行滤波，用于进一步滤除传感器信号中的高频噪声，减小噪声对姿态测量的影响。数字信号滤波采用有限冲击相应(FIR)数字滤波，即数字滤波的输出取决于有限个过去的输入和现在的输入x(n)、x(n-1)、…、x(n-N+1)。在本发明中采用了二次滤波，其滤波的输入和输出关系是y(n)＝h0x(n)+h1x(n-1)式中h0和h1为滤波系数。
信号转换单元将经信号滤波单元滤波后数字电压信号转化为对应的物理量，即将与加速度计10对应的数字电压信号转化为表示加速度的数字信号，将与磁强计20对应的数字电压信号转化为表示磁感应强度的数字信号，将与速率陀螺30对应的数字电压信号转化为表示角速度的数字信号。其处理算式为u(n)＝(v(n)-v0)/kv。式中v(n)为输入的电压信号，u(n)为对应的物理量，v0为零点电压，kv为变化幅值，各传感器的零点和幅值由预先标定确定。
在步骤403中，卡尔曼滤波模块用速率陀螺30测得的角速度对加速度计10和磁强计20测得的重力加速度和地磁感应强度的分量进行卡尔曼滤波。这一步骤是本发明的核心部分，为了清楚的说明该步骤，设定三轴磁强计在载体坐标系X、Y和Z轴上的测得的地磁感应强度的分量分别为Xm、Ym和Zm；三轴加速度计在载体坐标系X、Y和Z轴上的测得的重力加速度分量分别为Xg、Yg和Zg；三轴速率陀螺在载体坐标系X、Y和Z轴上的测得的载体转动角速度分量分别为ωx、ωy和ωz。
在本发明中，卡尔曼滤波算法的状态方程和量测方程是针对本发明的特点而设计的，能够计算得到符合系统性能要求的姿态角。状态方程和量测方程分别如下S·=ΩS+W]]>(状态方程)，Z＝HS+V(量测方程)。
其中S和Z分别为地磁感应强度和重力加速度在载体坐标系上的分量[XgYgZgXmYmZm]的真值和量测值， 为所述真值对时间的导数，H为量测阵(在本实施例中取作单位阵)，V为量测噪声，W为系统激励噪声(为了计算方便，在本实施例中设定W＝0)，Ω=-ωibbk00-ωibbk,ωibbk=0-ωzωyωz0-ωx-ωyωx0.]]>在本发明中，卡尔曼滤波算法的状态方程描述了地磁感应强度和重力加速度在载体坐标系上的分量的真值S随时间变化的规律，可见真值S要受由载体转动加速度[ωxωyωz]组成的矩阵Ω的影响。卡尔曼滤波算法的量测方程描述了地磁感应强度和重力加速度在载体坐标系上的分量的量测值Z与真值S的关系。在本发明中，卡尔曼滤波过程的输入值是当前时刻实测的量测值Z，并结合前一时刻对真值S的估计，最终输出当前时刻真值S的估计 。
根据本发明给出的卡尔曼滤波算法的状态方程和量测方程，本领域的技术人员即可对信号进行卡尔曼滤波。下面以一个卡尔曼滤波的具体实现为例详细描述卡尔曼滤波状态方程需要进行离散化处理，得到如下离散状态方程
Sk＝φk，k-1Sk-1其中φk,k-1=I+TΩ+ΩT22+···,]]>用下标k表示k时刻。
根据通用卡尔曼滤波过程进行卡尔曼滤波，Sk的估计 按下述方程求解状态一步预测S^k,k-1=φk,k-1S^k-1]]>根据当前量测值Zk和预测状态 来估计当前时刻估计值 S^k=S^k,k-1+Kk(Zk-HkS^k,k-1)]]>滤波增益Kk=Pk,k-1HkT(HkPk,k-1HkT+Rk)-1,]]>一步预测均方误差Pk,k-1=φk+1,kPk-1φk+1,kT,]]>估计均方误差Pk=(I-KkHk)Pk,k-1(I-KkHk)T+KkRkKkT,]]>Rk为量测噪声V的方差阵，具有关系Cov[Vk,Vj]=E[VkVjT]=Rkδkj.]]>通过上述的卡尔曼滤波过程，即可得到[XgYgZgXmYmZm]在k时刻的真值Sk的估计值 。本领域的一般技术人员可以理解，在给定卡尔曼滤波的状态方程和量测方程后即可根据卡尔曼滤波的思想进行信号滤波，但在具体实施时可对具体过程作各种变化，本发明并不局限于前述具体的实现。
在步骤404中，姿态计算模块根据卡尔曼滤波后得到的估计值 进行载体姿态解算，在这里姿态的解算与所引用的申请号为01110135.0专利的方法相同，区别之处在于在本发明中进行姿态解算为经过卡尔曼滤波后得到的物理量。在不引起混淆的情况下，在下面的描述中，仍然用[XgYgZgXmYmZm]表示经卡尔曼滤波后得到的k时刻各个物理量的估计值。具体描述如下载体在空间中的姿态可用载体坐标系相对于地理坐标系的运动来表示，运动角度称为载体的姿态角。导航学中常用航向角ψ、俯仰角θ和滚转角γ作为载体的姿态角，起始时两坐标系重合(N与X轴、E与Y轴、D与Z轴相对应)，随后载体绕D轴(Z)偏行ψ角，再绕水平Y’轴俯仰θ角，最后绕X”轴滚转γ角。载体坐标系与地理坐标系中的矢量可通过以下方向余弦矩阵进行相互转换
Cnb=cosψcosγ+sinψsinθsinγsinψcosθcosψsinγ-sinψsinθcosγcosψsinθsinγ-sinψcosγcosψcosθ-sinψsinγ-cosψsinθγcos-cosθsinγsinθcosθcosγ=[Tij]3×3]]>其上标b表示载体坐标系，下标n表示地理坐标系，为矩阵Tij中的第i行第j列元素。
地磁场h和重力加速度g在地理坐标系和姿态坐标系中的表示通过方向余弦矩阵Cnb进行转换，从而得到以下姿态角解算算式，其中h为地磁感应强度，β为地磁倾角。
XmYmZm=hCnbcosβ0sinβ⇒h×(T11cosβ+T13sinβ)=Xmh×(T21cosβ+T23sinβ)=Ymh×(T31cosβ+T33sinβ)=Zm]]>XgYgZg=Cnb00g⇒xg=T13gyg=T23gzg=T33g]]>虽然在本发明中采用了卡尔曼滤波来消除载体运动加速度对姿态测量的影响，但是通过本发明提供的带有九个传感器(三轴加速度计、三轴磁强计和三轴速率陀螺)的姿态测量系统，本领域的技术人员也可以设计出其它的姿态测量算法来进行高精度的姿态测量，因此本发明的姿态测量系统并不局限于使用本发明提供的姿态测量方法，其它的测量方法也可以适用于本发明的姿态测量系统。
权利要求
1.一种载体姿态测量方法，包括测量重力加速度和地磁感应强度在载体坐标系三轴上的分量；其特征在于，还包括测量载体转动角速度在载体坐标系三轴上的分量[ωxωyωz]；利用测得的载体转动角速度分量对测得的重力加速度和地磁感应强度分量进行卡尔曼滤波，得到重力加速度和地磁感应强度分量真值的估计值；最后用所述估计值进行载体姿态解算；其中，所述卡尔曼滤波的状态方程为S·=ΩS+W;]]>所述卡尔曼滤波的量测方程为Z＝HS+V；S和Z分别为所述重力加速度和地磁感应强度在载体坐标系三轴上的分量[XgYgZgXmYmZm]的真值和量测值， 为所述真值对时间的导数，H为量测阵，V为量测噪声，W为系统激励噪声，Ω=-ωibbk00-ωibbk,]]>ωibbk=0-ωzωyωz0-ωx-ωyωx0.]]>
2.一种载体姿态测量系统，包括传感器和数据处理装置，所述数据处理装置根据所述传感器测得的信号进行载体姿态解算；其特征在于，所述传感器包括沿载体坐标系三轴布置的三轴加速度计，用于测量重力加速度在载体坐标系三轴上的分量；沿载体坐标系三轴布置的三轴磁强计，用于测量地磁感应强度在载体坐标系三轴上的分量；沿载体坐标系三轴布置的三轴速率陀螺，用于测量载体加速度在载体坐标系三轴上的分量。
3.根据权利要求2所述的载体姿态测量系统，其特征在于，还包括第一滤波电路和第一放大电路，所述三轴加速度计的输出信号经第一滤波电路和第一放大电路滤波和放大后，再经A/D转换器进入所述数据处理装置。
4.根据权利要求2所述的载体姿态测量系统，其特征在于，还包括第二滤波电路和第二放大电路，所述三轴磁强计的输出信号经第二滤波电路和第二放大电路滤波和放大后，再经A/D转换器进入所述数据处理装置。
5.根据权利要求2所述的载体姿态测量系统，其特征在于，还包括第三滤波电路，所述三轴速率陀螺的输出信号经第三滤波电路滤波后经A/D转换器进入所述数据处理装置。
6.根据权利要求2所述的载体姿态测量系统，其特征在于，所述数据处理装置包括卡尔曼滤波模块和姿态解算模块；其中卡尔曼滤波模块，利用测得的载体转动角速度的分量[ωxωyωz]对测得的重力加速度和地磁感应强度的分量[XgYgZgXmYmZm]进行卡尔曼滤波，输出重力加速度和地磁感应强度分量真值的估计值；其中，所述卡尔曼滤波的状态方程为S·=ΩS+W;]]>所述卡尔曼滤波的量测方程为Z＝HS+V；S和Z分别为所述重力加速度和地磁感应强度在载体坐标系三轴上的分量[XgYgZgXmYmZm]的真值和量测值， 为所述真值对时间的导数，H为量测阵，V为量测噪声，W为系统激励噪声，Ω=-ωibbk00-ωibbk,]]>ωibbk=0-ωzωyωz0-ωx-ωyωx0;]]>姿态解算模块，用于根据卡尔曼滤波模块输出的重力加速度和地磁感应强度在载体坐标系三轴上的分量真值的估计值进行载体姿态的解算。
7.根据权利要求2所述的载体姿态测量系统，其特征在于，所述加速度计为微机电加速度计，所述磁强计为微机电磁强计，所述速率陀螺为微机电速率陀螺。
全文摘要
本发明公开了一种载体姿态测量方法和系统。该系统包括传感器和数据处理装置，传感器包括三轴加速度计、三轴磁强计和三轴速率陀螺，分别测量重力加速度、地磁感应强度和载体运动角速度在载体坐标系三轴上的分量。在进行信号处理时，利用测得的载体转动角速度分量对测得的重力加速度和地磁感应强度分量进行卡尔曼滤波，得到重力加速度和地磁感应强度分量真值的估计值，最后用该估计值进行载体姿态解算。本发明用稳定的陀螺信号来减小运动加速度对系统的影响，而且不存在陀螺的积分漂移问题，从而得到的载体姿态动态精度和稳定性优于现有的姿态测量系统。本发明中的传感器采用一体化MEMS芯片集成封装技术，使仪器更加小型化。
文档编号G01C21/10GK1664506SQ200410004660
公开日2005年9月7日 申请日期2004年3月5日 优先权日2004年3月5日
发明者周兆英, 朱荣, 王鼎渠, 王晓浩, 宋宇宁, 王劲东 申请人:清华大学