CN102980577A - 一种微型捷联航姿系统及其工作方法 - Google Patents

Abstract

本发明为一种微型捷联航姿系统及其工作方法。该系统包括数据采集模块、数据处理解算模块和外围通信接口模块，其中：数据采集模块采用三轴陀螺仪、三轴加速度计、三轴磁阻传感器和SPI接口集成一体化的微电子机械系统传感器；数据处理解算模块采用32位ARM微控制器；外围通信接口模块包括串口电平转换芯片、RS_232接口、CAN收发器和CAN接口。该系统工作步骤为：系统上电初始化；系统自检；磁场自标定；初始对准；组合测姿：根据初始三维姿态角和捷联航姿系统姿态解算算法，解算出各航姿信息，同时用补偿后的磁场信息和加速度信息解算出三维磁姿态角，两者经过信息融合算法，最终得到稳定的航姿信息；发送航姿信息到应用设备。

Description

一种微型捷联航姿系统及其工作方法

一、技术领域

本发明属于惯性技术领域，特别是一种微型捷联航姿系统及其工作方法。

二、背景技术

在航天器、导弹、车辆的导航以及卫星、机器人、平台的姿态控制等领域中都需要用航姿系统来测量载体的姿态信息。目前发展较为迅速的为捷联航姿系统，捷联航姿系统是指直接把惯性测量组件(陀螺仪和加速度计等)固联在运载体上用数学平台解算载体姿态信息的系统。捷联航姿系统依靠算法建立起导航坐标系，即平台坐标系以数学平台形式存在，这样省略了复杂的物理实体平台，因此应用越来越广泛。但现有的捷联航姿系统集成度不够高，体积偏大，使用的惯性测量组件成本太高，没有考虑到外界信号(如GPS等)失效的情况下如何利用地球周围固有的磁场来补偿航姿精度使其长时间稳定工作的问题，且隐蔽性不强，因此设计一种体积小，集成度高，性能好，成本低和适应能力强的捷联航姿系统成为航姿系统发展的趋势。

文献1：中国实用新型专利“航姿组合测量装置”(公开号CN 201402140Y，公开日2010年02月10日)公开了一种可靠性高、成本较低的航姿组合测量装置，由信号处理与解算模块、惯性传感器组合及三轴磁传感器组成，其中的传感器模块由惯性传感器组合和三轴磁传感器两个独立部分组成，集成度不高；且该装置外围通信接口只有RS422，效率较低，数据传输不够稳定。

文献2：中国发明专利“一种基于光纤陀螺的捷连航姿系统”(公开号CN 102135430A，公开日2011年07月27日)公开了一种集成度高的小型捷联航姿系统，这种系统的传感器模块由陀螺仪和数字双轴倾角仪组成，在解算航姿信息的过程中，解算算法为积分运算，各种误差也会经过积分运算，导致系统的数据解算误差随时间不断积累，并且不断震荡和发散，航姿信息精度越来越低；其惯性测量传感器的陀螺仪为价格昂贵的光纤陀螺仪，使得航姿系统成本较高；该装置外围通信接口只有CAN总线，不够丰富，应用受限。

三、发明内容

本发明的目的是提供一种体积小，集成度高，性能好，成本低和适应能力强的微型捷联航姿系统及其工作方法。

实现本发明目的的技术解决方案为：一种微型捷联航姿系统，包括数据采集模块、数据处理解算模块和外围通信接口模块，其中：

数据采集模块采用三轴陀螺仪、三轴加速度计、三轴磁阻传感器和第1SPI接口集成一体化的微电子机械系统传感器，其中三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别独立工作；数据处理解算模块采用32位ARM微控制器，该微控制器集成了数据处理解算单元、CAN控制器、UART接口和第2SPI接口；外围通信接口模块包括串口电平转换芯片、RS_232接口、CAN收发器和CAN接口；

三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别通过第1SPI接口与第2SPI接口连接；第2SPI接口与数据处理解算单元连接；数据处理解算单元通过UART接口与串口电平转换芯片连接，数据处理解算单元还通过CAN控制器与外围通信接口模块的CAN收发器连接；串口电平转换芯片与RS_232接口连接，外围通信接口模块的CAN收发器与CAN接口连接；

第1SPI接口与第2SPI接口之间通过SPI总线连接，数据采集模块的三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别通过SPI总线与数据处理解算模块通信；数据处理解算模块通过SPI总线读取数据采集模块的三轴角速度信息、三轴加速度信息和周围三轴磁场强度信息，并传输给数据处理解算单元，解算的航姿信息通过UART接口或CAN控制器发送给外围通信接口模块；外围通信接口模块的串口电平转换芯片把UART接口发来的TTL电平数据转换为RS_232电平数据，并通过RS_232接口发给外设；外围通信接口模块的CAN收发器将CAN控制器发来的TTL电平数据转换为物理总线的差分电平数据，然后通过CAN接口传输到CAN总线上的设备。

使用所述的微型捷联航姿系统测量航姿的方法如下：

1)航姿系统上电后完成系统初始化工作，并进行系统自检，通过采集数据、发送数据，检查航姿系统有无异常；

2)磁场自标定：在没有磁干扰的环境，将航姿系统在水平面缓慢均匀转动一周，数据处理解算单元通过SPI总线，从三轴磁阻传感器读取地球磁场强度在载体系三轴上的分量，通过椭圆补偿自标定法，标定出磁场补偿系数：载体系X轴的标定因数Xsf、Y轴的标定因数Ysf，以及磁场强度在载体系X轴上的漂移量Xoff、Y轴上的漂移量Yoff；

3)初始对准状态：用上一步标定出的磁场补偿系数得到载体真实的磁场信息，使用该磁场信息和三轴加速度计值求出航姿系统在静止状态下的三维姿态角，同时算出陀螺的常值漂移，包括以下步骤：

(31)数据处理解算模块内的数据处理解算单元通过SPI总线，从数据采集模块的三轴磁阻传感器实时读取地球磁场强度在载体系三轴上的分量，并对它们进行补偿：

$B_b^{\mathrm{hx}}=B_b^x*\mathrm{Xsf}+\mathrm{Xoff}$

$B_b^{\mathrm{hy}}=B_b^y*\mathrm{Ysf}+\mathrm{Yoff}$

$B_b^{\mathrm{hz}}=B_b^z$

为磁场强度在载体系三轴上的分量，

为磁场强度在载体系三轴上补偿后的分量；

(32)数据处理解算单元通过SPI总线，从三轴加速度计实时读取载体加速度值在载体系三轴上的分量

结合重力加速度g求出载体的俯仰角和滚转角：

${\mathrm{\θ}}_0=\arcsin (-\frac{f_b^x}{g}),$ ${\mathrm{\γ}}_0=\arctan \left(\frac{f_b^y}{f_b^z}\right)$

其中θ₀为载体的俯仰角，γ₀为载体的滚转角；

(33)根据载体的俯仰角θ₀、滚转角γ₀和补偿后的磁场强度可以求得磁场水平分量的表达式为：

$B_b^X=B_b^{\mathrm{hx}}\cos {\mathrm{\θ}}_0+B_b^{\mathrm{hz}}\sin {\mathrm{\θ}}_0$

$B_b^Y=B_b^{\mathrm{hx}}\sin {\mathrm{\γ}}_0\sin {\mathrm{\θ}}_0+B_b^{\mathrm{hy}}\cos {\mathrm{\γ}}_0-B_b^{\mathrm{hz}}\sin {\mathrm{\γ}}_0\cos {\mathrm{\θ}}_0$

其中

为磁场强度在水平面X轴的分量，

为磁场强度在水平面Y轴的分量；则磁航向角ψ_M为：

${\mathrm{\ψ}}_M=\arctan \left(\frac{B_b^Y}{B_b^X}\right)$

(34)地磁北向与地理北向之间的夹角就称之为磁偏角D，地磁北极在地理北极东侧时该值为正，地磁北极在地理北极西侧时该值为负，从而得到真航向角ψ₀，航向角ψ₀的表达式为：

ψ₀＝ψ_M+D

(35)计算出陀螺的漂移：在静止状态，数据处理解算单元通过SPI总线，从三轴陀螺仪实时读取载体角速度值在载体系三轴上的分量

读取100～200次求均值，得到三轴陀螺仪在载体系三轴上的漂移ε_x、ε_y、ε_z，三轴陀螺仪的漂移记为ε＝[ε_x ε_y ε_z]^T；

4)进入组合测姿状态，组合测姿包括以下步骤：

(41)根据初始对准状态的俯仰角θ₀、滚转角γ₀、航向角ψ₀，进行四元数转换得到初始四元数[q₀₀ q₀₁ q₀₂ q₀₃]^T；

(42)在组合测姿状态，对三轴陀螺仪输出的角速度值在载体系三轴上的分量

进行补偿： ${\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{hx}}={\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bx}}-{\mathrm{\ϵ}}_x,$ ${\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{hy}}={\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{by}}-{\mathrm{\ϵ}}_y,$ ${\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{hz}}={\mathrm{\ω}}_{\mathrm{ib}}^{\mathrm{bz}}-{\mathrm{\ϵ}}_z,$ 根据初始四元数[q₀₀ q₀₁ q₀₂ q₀₃]^T和

用四阶-龙格库塔法求解四元数微分方程，得到更新后的四元数[q_b0 q_b1 q_b2 q_b3]^T，转换为俯仰角θ_b、滚转角γ_b、航向角ψ_b，即为组合测姿得到的航姿信息，将该航姿信息通过UART接口或CAN控制器发送给外围通信接口模块；

(43)数据处理解算单元通过SPI总线，从数据采集模块的三轴磁阻传感器实时读取地球磁场强度在载体系三轴上的分量，从三轴加速度计实时读取载体加速度值在载体系三轴上的分量，按照步骤(31)～步骤(34)的方法求得俯仰角θ_a、滚转角γ_a、航向角ψ_a，转换为四元数[q_a0 q_a1 q_a2 q_a3]^T；

(44)卡尔曼滤波及信息融合具体如下：

本系统采用大失准角下的加性四元数法误差模型，姿态误差方程如下：

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{Q}=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>\mathrm{\&delta;Q}+\frac{1}{2}U\left(\widehat{Q_b^n}\right){\mathrm{\&delta;\&omega;}}_{\mathrm{ib}}^b$

式中，δQ为四元数误差，

为陀螺误差，

$<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>=\left[\begin{array}{cccc}0& {-\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& 0& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& 0& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& 0\end{array}\right];$ $U\left(\widehat{Q_b^n}\right)=\left[\begin{array}{ccc}-q_{b1}& -q_{b2}& -q_{b3}\\ q_{b0}& {-q}_{b3}& q_{b2}\\ q_{b3}& q_{b0}& -q_{b1}\\ {-q}_{b2}& q_{b1}& q_{b0}\end{array}\right];$

参考卡尔曼滤波公式，状态方程为：

量测方程为：Z＝HX+V

其中，状态变量X为X＝[δq₀ δq₁ δq₂ δq₃ ε_x ε_y ε_z]^T＝[δQ ε]^T，即X包括δQ四元数变量部分和ε三轴陀螺仪漂移变量部分，W为系统激励白噪声；V为量测白噪声；

$H=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right];$

经卡尔曼滤波，将更新后的状态变量X的三轴陀螺仪漂移变量部分更新步骤(42)中三轴陀螺仪的漂移ε＝[ε_x ε_y ε_z]^T；

将更新后的状态变量X的四元数变量部分补偿到步骤(42)中三轴陀螺仪计算得到的四元数[q_b0 q_b1 q_b2 q_b3]^T，将补偿后的[q_b0 q_b1 q_b2 q_b3]^T作为步骤(42)中新的初始四元数；

重复步骤(42)～步骤(44)，实现航姿系统的信息融合。

本发明与现有技术相比，其显著优点是：

1、数据采集模块采用低成本的MEMS(微电子机械系统)传感器，该传感器利用MEMS技术和微电子技术从根本上改变了传统的惯性测量组合系统的设计思想和制造方法，实现了微型惯性测量组合系统中的微机械陀螺、微机械加速度计和磁强计获取测量信号、处理电路的一体化集成，不仅成本低、集成度高，而且对外输出接口为SPI口，与数据处理解算模块物理连接方便，数据传输高效、稳定。

2、数据处理解算模块采用32位ARM微控制器，片内资源丰富，在具体应用时可以存储程序和临时数据，也可以实现系统扩展，并且集成了两路CAN控制器，通过CAN收发器可实现CAN通信接口，把解算得到的丰富航姿信息传输到其他CAN总线上的设备，实现稳定，高效的数据传输。

3、外围通信接口模块由RS_232串口和CAN通信接口组成：RS_232串口不仅能把处理器解算的航姿信息发给外设(如电脑)进行监控，而且还可以扩展外接GPS等设备，实现更深度的信息融合；CAN接口用于把解算得到的丰富航姿信息传输到其他CAN总线上的设备，实现稳定，高效的数据传输。

4、不仅用了纯惯性组件解算航姿信息，而且还用三轴加速度计和磁阻传感器获取的信息解算出三维磁姿态角，两者经过信息融合算法，最终输出稳定的航姿信息，使航姿系统长时间稳定工作。各功能模块化、集成化，且对外无任何电磁辐射，隐蔽性好，不易暴露自身目标，环境适应能力强。

四附图说明

图1是本发明的微型捷联航姿系统硬件模块框图。

图2是本发明的微型捷联航姿系统的工作流程图。

图3是本发明的微型捷联航姿系统的卡尔曼滤波信息融合算法流程图。

五具体实施方式

以下结合附图，详细说明本发明的实施方式。

结合图1，本发明的微型捷联航姿系统，数据采集模块采用三轴陀螺仪、三轴加速度计、三轴磁阻传感器和第1SPI接口集成一体化的微电子机械系统传感器，其中三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别独立工作；数据处理解算模块采用32位ARM微控制器，该微控制器集成了数据处理解算单元、CAN控制器、UART接口和第2SPI接口；外围通信接口模块包括串口电平转换芯片、RS_232接口、CAN收发器和CAN接口；

三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别通过第1SPI接口与第2SPI接口连接；第2SPI接口与数据处理解算单元连接；数据处理解算单元通过UART接口与串口电平转换芯片连接，数据处理解算单元还通过CAN控制器与外围通信接口模块的CAN收发器连接；串口电平转换芯片与RS_232接口连接，外围通信接口模块的CAN收发器与CAN接口连接；

第1SPI接口与第2SPI接口之间通过SPI总线连接，数据采集模块的三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别通过SPI总线与数据处理解算模块通信；数据处理解算模块通过SPI总线读取数据采集模块的三轴角速度信息、三轴加速度信息和周围三轴磁场强度信息，并传输给数据处理解算单元，解算的航姿信息通过UART接口或CAN控制器发送给外围通信接口模块；外围通信接口模块的串口电平转换芯片把UART接口发来的TTL电平数据转换为RS_232电平数据，并通过RS_232接口发给外设；外围通信接口模块的CAN收发器将CAN控制器发来的TTL电平数据转换为物理总线的差分电平数据，然后通过CAN接口传输到CAN总线上的设备。

本发明的微型捷联航姿系统，所述的微电子机械系统传感器采用ADIS16405，所述的微控制器采用ARM7为核心的32位微控制器LPC2xxx系列，所述的RS_232串口还可以扩展外接GPS等设备。

ADIS16405惯性传感器产品是一套完整的惯性传感器系统，包括三轴陀螺仪、三轴加速度计和三轴磁阻传感器，并将工业领先的惯性MEMS技术与信号调理相结合，动态性能达到最优化。产品出厂前的校准表征了每个传感器的灵敏度、偏差、校准值、线性加速度以及陀螺漂移。因此，每个传感器都有其动态补偿校准方案，从而能在常温下提供精确的传感器测量值。磁阻传感器运用自校正功能使测量值与真实值的偏差降到最小。

该传感器体积仅为23mm*23mm*23mm，三个轴的角速率测量范围为±75°/s、±150°/s、±300°/(可设定)，分辨率为14位；三个轴的加速度测量范围为±10g，分辨率为14位。对外输出接口为SPI口，与LPC2119物理连接方便。使用时，只需给它供电，它就会以一定的频率向外输出数据。具有体积小、重量轻、寿命长、可靠性高、成本低和适应环境能力强等优点。

数据处理解算模块采用的是飞利浦公司的ARM7为核心的32位微控制器LPC2119。LPC2119片内资源比较丰富，片内ROM高达128K，RAM空间有16K，在具体应用时可以存储程序和临时数据，也可以实现系统扩展。并且，LPC2119的片内Flash支持认IAP功能，可以用来存储数据。LPC2119集成了两路CAN控制器、两路UART及两路SPI，满足接口要求。

微控制器LPC2119以一定频率实时通过SPI总线读取数据采集模块的加速度、角速度和磁场强度信息，存储在一定内存空间，然后进行三维姿态角解算。由于纯惯性组件解算数据的固有不足，一般通过RS_232接口外接GPS设备，以固定频率接收GPS信息，对航姿信息进行补偿，使航姿系统长时间稳定工作。但在GPS信息完全失效的情况下，航姿系统不能长时间稳定工作，为了解决这个问题，本微型捷联航姿系统不仅用了纯惯性组件解算航姿信息，而且还用三轴加速度计和磁阻传感器测量的地磁场信息解算出三维磁姿态角，两者经过信息融合算法，最终输出稳定的航姿信息，使航姿系统长时间稳定工作。

在实际使用环境中，由于周围铁磁材料的影响，地磁场不可避免地受到干扰磁场的影响。在这种情况下，地磁场的测量会带来一定的误差，计算得到的磁航向角也会有偏差。为了保证航姿系统输出精确的航向角，需对磁场干扰进行补偿。现行的磁航向误差补偿技术分为两类：一类是借助于外界因素的标定方法，如依靠高精度转台或标准六面体进行多点标定，这类方法虽然精度较高，但操作复杂，对大多数实际使用场合并不适用；另一类是不借助外界因素的自标定方法，这类方法操作方便，运算量小，补偿效果显著。本发明采用自标定方法，根据具体场合可使用椭圆补偿法、最小二乘曲线拟合法或矩形域的最小二乘曲面拟合法，经实践证明效果显著。

外围通信接口模块由RS_232串口和CAN通信接口组成：LPC2119自带UART，通过串口电平转换芯片将TTL电平转换为RS_232电平，此接口不仅能把处理器解算的航姿信息发给外设(如PC机)显示导航数据，而且还可以功能扩展外接GPS信息，从而能得到载体的速度位置信息以及当前位置的磁偏角，进而实现更深度的信息融合；LPC2119集成了CAN控制器，通过CAN收发器将CAN控制器收发引脚的TTL电平转换为物理总线的差分电平，实现CAN通信接口，把解算得到的丰富航姿信息传输到其他CAN总线上的设备如图像控制板等，实现稳定，高效的数据传输。

结合图2：本发明的微型捷联航姿系统具体工作如下：

使用所述的微型捷联航姿系统测量航姿的方法如下：

1)航姿系统上电后完成系统初始化工作，并进行系统自检，通过采集数据、发送数据，检查航姿系统有无异常；

2)磁场自标定：在没有磁干扰的环境，将航姿系统在水平面缓慢均匀转动一周，数据处理解算单元通过SPI总线，从三轴磁阻传感器读取地球磁场强度在载体系三轴上的分量，通过椭圆补偿自标定法，标定出磁场补偿系数：载体系X轴的标定因数Xsf、Y轴的标定因数Ysf，以及磁场强度在载体系X轴上的漂移量Xoff、Y轴上的漂移量Yoff；

3)初始对准状态：用上一步标定出的磁场补偿系数得到载体真实的磁场信息，使用该磁场信息和三轴加速度计值求出航姿系统在静止状态下的三维姿态角，同时算出陀螺的漂移，包括以下步骤：

(31)数据处理解算模块内的数据处理解算单元通过SPI总线，从数据采集模块的三轴磁阻传感器实时读取地球磁场强度在载体系三轴上的分量，并对它们进行补偿：

$B_b^{\mathrm{hx}}=B_b^x*\mathrm{Xsf}+\mathrm{Xoff}$

$B_b^{\mathrm{hy}}=B_b^y*\mathrm{Ysf}+\mathrm{Yoff}$

$B_b^{\mathrm{hz}}=B_b^z$

为磁场强度在载体系三轴上的分量，

为磁场强度在载体系三轴上补偿后的分量；

(32)数据处理解算单元通过SPI总线，从三轴加速度计实时读取载体加速度值在载体系三轴上的分量结合重力加速度g求出载体的俯仰角和滚转角：

${\mathrm{\&theta;}}_0=\arcsin (-\frac{f_b^x}{g}),$ ${\mathrm{\&gamma;}}_0=\arctan \left(\frac{f_b^y}{f_b^z}\right)$

其中θ₀为载体的俯仰角，γ₀为载体的滚转角；

(33)根据载体的俯仰角θ₀、滚转角γ₀和补偿后的磁场强度可以求得磁场水平分量的表达式为：

$B_b^X=B_b^{\mathrm{hx}}\cos {\mathrm{\&theta;}}_0+B_b^{\mathrm{hz}}\sin {\mathrm{\&theta;}}_0$

$B_b^Y=B_b^{\mathrm{hx}}\sin {\mathrm{\&gamma;}}_0\sin {\mathrm{\&theta;}}_0+B_b^{\mathrm{hy}}\cos {\mathrm{\&gamma;}}_0-B_b^{\mathrm{hz}}\sin {\mathrm{\&gamma;}}_0\cos {\mathrm{\&theta;}}_0$

其中为磁场强度在水平面X轴的分量，

为磁场强度在水平面Y轴的分量；则磁航向角ψ_M为：

${\mathrm{\&psi;}}_M=\arctan \left(\frac{B_b^Y}{B_b^X}\right)$

(34)地磁北向与地理北向之间的夹角就称之为磁偏角D，地磁北极在地理北极东侧时该值为正，地磁北极在地理北极西侧时该值为负，从而得到真航向角ψ₀，航向角ψ₀的表达式为：

ψ₀＝ψ_M+D

(35)计算出陀螺的漂移：在静止状态，数据处理解算单元通过SPI总线，从三轴陀螺仪实时读取载体角速度值在载体系三轴上的分量

读取100～200次求均值，得到三轴陀螺仪在载体系三轴上的漂移ε_x、ε_y、ε_z，三轴陀螺仪的漂移记为ε＝[ε_x ε_y ε_z]^T；

4)进入组合测姿状态，组合测姿包括以下步骤：

(41)根据初始对准状态的俯仰角θ₀、滚转角γ₀、航向角ψ₀，进行四元数转换得到初始四元数[q₀₀ q₀₁ q₀₂ q₀₃]^T；

(42)在组合测姿状态，对三轴陀螺仪输出的角速度值在载体系三轴上的分量

进行补偿： ${\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}={\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{bx}}-{\mathrm{\&epsiv;}}_x,$ ${\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}={\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{by}}-{\mathrm{\&epsiv;}}_y,$ ${\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}={\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{bz}}-{\mathrm{\&epsiv;}}_z,$ 根据初始四元数[q₀₀ q₀₁ q₀₂ q₀₃]^T和

用四阶-龙格库塔法求解四元数微分方程，得到更新后的四元数[q_b0 q_b1 q_b2 q_b3]^T，转换为俯仰角θ_b、滚转角γ_b、航向角ψ_b，即为组合测姿得到的航姿信息，将该航姿信息通过UART接口或CAN控制器发送给外围通信接口模块；

(43)数据处理解算单元通过SPI总线，从数据采集模块的三轴磁阻传感器实时读取地球磁场强度在载体系三轴上的分量，从三轴加速度计实时读取载体加速度值在载体系三轴上的分量，按照步骤(31)～步骤(34)的方法求得俯仰角θ_a、滚转角γ_a、航向角ψ_a，转换为四元数[q_a0 q_a1 q_a2 q_a3]^T；

(44)卡尔曼滤波及信息融合具体如下：

本系统采用大失准角下的加性四元数法误差模型，姿态误差方程如下：

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{Q}=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>\mathrm{\&delta;Q}+\frac{1}{2}U\left(\widehat{Q_b^n}\right){\mathrm{\&delta;\&omega;}}_{\mathrm{ib}}^b$

式中，δQ为四元数误差，

为陀螺误差，

$<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>=\left[\begin{array}{cccc}0& {-\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& 0& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& 0& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& 0\end{array}\right],$ $U\left(\widehat{Q_b^n}\right)=\left[\begin{array}{ccc}-q_{b1}& -q_{b2}& -q_{b3}\\ q_{b0}& {-q}_{b3}& q_{b2}\\ q_{b3}& q_{b0}& -q_{b1}\\ {-q}_{b2}& q_{b1}& q_{b0}\end{array}\right];$

参考卡尔曼滤波公式，状态方程为：

量测方程为：Z＝HX+V

其中，状态变量X为X＝[δq₀ δq₁ δq₂ δq₃ ε_x ε_y ε_z]^T＝[δQ ε]^T，即X包括δQ四元数变量部分和ε三轴陀螺仪漂移变量部分，W为系统激励白噪声；V为量测白噪声；

$H=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right];$

将系统的状态方程和量测方程离散化表示为：

X_k＝Φ_k，k-1X_k-1+Γ_k-1W_k-1

Z_k＝H_kX_k+V_k

其中，X_k表示被估计状态，Φ_k，k-1为t_k-1时刻的一步转移矩阵，Γ_k-1为系统噪声驱动阵，W_k为系统激励噪声序列，Z_k为量测矩阵，H_k为量测阵，V_k为量测噪声序列。

根据适用于离散系统的卡尔曼滤波器基本方程的卡尔曼算法具体如下：

①状态一步预测方程：

X_k/k-1＝φ_k，k-1X_k-1

②一步预测均方误差方程：

$P_{k/k-1}={\mathrm{\&phi;}}_{k,k-1}{P}_{k-1}{\mathrm{\&phi;}}_{k,k-1}^T+{\mathrm{\&Gamma;}}_{k-1}{Q}_{k-1}{\mathrm{\&Gamma;}}_{k-1}^T$

③最优滤波增益方程：

$K_k=P_{k|k-1}{H}_k^T{[H_k{P}_{k|k-1}{H}_k^T+R_k]}^{-1}$

④状态估计方程：

X_k＝X_k/k-1+K_k(Z_k-H_kX_k/k-1)

⑤估计均方误差方程：

$P_k=\left({I-K}_k{H}_k\right)P_{k/k-1}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_{k-1}^T$

其中，Q_k是系统过程噪声方差阵，K_k为滤波增益系数，R_k是系统观测噪声方差阵，P_k为均方误差阵。

经卡尔曼滤波，将更新后的状态变量X的三轴陀螺仪漂移变量部分更新步骤(42)中三轴陀螺仪的漂移ε＝[ε_x ε_y ε_z]^T；

将更新后的状态变量X的四元数变量部分补偿到步骤(42)中三轴陀螺仪计算得到的四元数[q_b0 q_b1 q_b2 q_b3]^T，将补偿后的[q_b0 q_b1 q_b2 q_b3]^T作为步骤(42)中新的初始四元数；

重复步骤(42)～步骤(44)，实现航姿系统的信息融合。

Claims (6)

1.一种微型捷联航姿系统，其特征在于，包括数据采集模块、数据处理解算模块和外围通信接口模块，其中：

数据采集模块采用三轴陀螺仪、三轴加速度计、三轴磁阻传感器和第1SPI接口集成一体化的微电子机械系统传感器，其中三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别独立工作；数据处理解算模块采用32位ARM微控制器，该微控制器集成了数据处理解算单元、CAN控制器、UART接口和第2SPI接口；外围通信接口模块包括串口电平转换芯片、RS_232接口、CAN收发器和CAN接口；

三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别通过第1 SPI接口与第2 SPI接口连接；第2 SPI接口与数据处理解算单元连接；数据处理解算单元通过UART接口与串口电平转换芯片连接，数据处理解算单元还通过CAN控制器与外围通信接口模块的CAN收发器连接；串口电平转换芯片与RS_232接口连接，外围通信接口模块的CAN收发器与CAN接口连接；

第1 SPI接口与第2 SPI接口之间通过SPI总线连接，数据采集模块的三轴陀螺仪、三轴加速度计、三轴磁阻传感器分别通过SPI总线与数据处理解算模块通信；数据处理解算模块通过SPI总线读取数据采集模块的三轴角速度信息、三轴加速度信息和周围三轴磁场强度信息，并传输给数据处理解算单元，解算的航姿信息通过UART接口或CAN控制器发送给外围通信接口模块；外围通信接口模块的串口电平转换芯片把UART接口发来的TTL电平数据转换为RS_232电平数据，并通过RS_232接口发给外设；外围通信接口模块的CAN收发器将CAN控制器发来的TTL电平数据转换为物理总线的差分电平数据，然后通过CAN接口传输到CAN总线上的设备。

2.根据权利要求1所述的微型捷联航姿系统，其特征在于所述的微电子机械系统传感器采用ADIS16405。

3.根据权利要求1所述的微型捷联航姿系统，其特征在于所述的微控制器采用ARM7为核心的32位微控制器LPC2xxx系列。

4.根据权利要求1所述的微型捷联航姿系统，其特征在于所述的RS_232串口还可以扩展外接GPS等设备。

5.如权利要求1所述的微型捷联航姿系统的测姿方法，其特征在于，工作方法如下：

1)航姿系统上电后完成系统初始化工作，并进行系统自检，通过采集数据、发送数据，检查航姿系统有无异常；

2)磁场自标定：在没有磁干扰的环境，将航姿系统在水平面缓慢均匀转动一周，数据处理解算单元通过SPI总线，从三轴磁阻传感器读取地球磁场强度在载体系三轴上的分量，通过椭圆补偿自标定法，标定出磁场补偿系数：载体系X轴的标定因数Xsf、Y轴的标定因数Ysf，以及磁场强度在载体系X轴上的漂移量Xoff、Y轴上的漂移量Yoff；

3)初始对准状态：用上一步标定出的磁场补偿系数得到载体真实的磁场信息，使用该磁场信息和三轴加速度计值求出航姿系统在静止状态下的三维姿态角，同时算出陀螺的漂移，包括以下步骤：

(31)数据处理解算模块内的数据处理解算单元通过SPI总线，从数据采集模块的三轴磁阻传感器实时读取地球磁场强度在载体系三轴上的分量，并对它们进行补偿：

$B_b^{\mathrm{hx}}=B_b^x*\mathrm{Xsf}+\mathrm{Xoff}$

$B_b^{\mathrm{hy}}=B_b^y*\mathrm{Ysf}+\mathrm{Yoff}$

$B_b^{\mathrm{hz}}=B_b^z$

为磁场强度在载体系三轴上的分量，

为磁场强度在载体系三轴上补偿后的分量；

(32)数据处理解算单元通过SPI总线，从三轴加速度计实时读取载体加速度值在载体系三轴上的分量

结合重力加速度g求出载体的俯仰角和滚转角：

${\mathrm{\&theta;}}_0=\arcsin (-\frac{f_b^x}{g}),$ ${\mathrm{\&gamma;}}_0=\arctan \left(\frac{f_b^y}{f_b^z}\right)$

其中θ₀为载体的俯仰角，γ₀为载体的滚转角；

(33)根据载体的俯仰角θ₀、滚转角γ₀和补偿后的磁场强度可以求得磁场水平分量的表达式为：

$B_b^X=B_b^{\mathrm{hx}}\cos {\mathrm{\&theta;}}_0+B_b^{\mathrm{hz}}\sin {\mathrm{\&theta;}}_0$

$B_b^Y=B_b^{\mathrm{hx}}\sin {\mathrm{\&gamma;}}_0\sin {\mathrm{\&theta;}}_0+B_b^{\mathrm{hy}}\cos {\mathrm{\&gamma;}}_0-B_b^{\mathrm{hz}}\sin {\mathrm{\&gamma;}}_0\cos {\mathrm{\&theta;}}_0$

其中为磁场强度在水平面X轴的分量，为磁场强度在水平面Y轴的分量；则磁航向角ψ_M为：

${\mathrm{\&psi;}}_M=\arctan \left(\frac{B_b^Y}{B_b^X}\right)$

(34)地磁北向与地理北向之间的夹角就称之为磁偏角D，地磁北极在地理北极东侧时该值为正，地磁北极在地理北极西侧时该值为负，从而得到真航向角ψ₀，航向角ψ₀的表达式为：

ψ₀＝ψ_M+D

(35)计算出陀螺的漂移：在静止状态，数据处理解算单元通过SPI总线，从三轴陀螺仪实时读取载体角速度值在载体系三轴上的分量

读取100～200次求均值，得到三轴陀螺仪在载体系三轴上的漂移ε_x、ε_y、ε_z，三轴陀螺仪的漂移记为ε＝[ε_x ε_y ε_z]^T；

4)进入组合测姿状态，组合测姿包括以下步骤：

(41)根据初始对准状态的俯仰角θ₀、滚转角γ₀、航向角ψ₀，进行四元数转换得到初始四元数[q₀₀ q₀₁ q₀₂ q₀₃]^T；

(42)在组合测姿状态，对三轴陀螺仪输出的角速度值在载体系三轴上的分量

进行补偿： ${\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}={\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{bx}}-{\mathrm{\&epsiv;}}_x,$ ${\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}={\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{by}}-{\mathrm{\&epsiv;}}_y,$ ${\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}={\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{bz}}-{\mathrm{\&epsiv;}}_z,$ 根据初始四元数[q₀₀ q₀₁ q₀₂ q₀₃]^T和用四阶-龙格库塔法求解四元数微分方程，得到更新后的四元数[q_b0 q_b1 q_b2 q_b3]^T，转换为俯仰角θ_b、滚转角γ_b、航向角ψ_b，即为组合测姿得到的航姿信息，将该航姿信息通过UART接口或CAN控制器发送给外围通信接口模块；

(43)数据处理解算单元通过SPI总线，从数据采集模块的三轴磁阻传感器实时读取地球磁场强度在载体系三轴上的分量，从三轴加速度计实时读取载体加速度值在载体系三轴上的分量，按照步骤(31)～步骤(34)的方法求得俯仰角θ_a、滚转角γ_a、航向角ψ_a，转换为四元数[q_a0 q_a1 q_a2 q_a3]^T；

(44)卡尔曼滤波信息融合：

本系统采用大失准角下的加性四元数法误差模型，姿态误差方程如下：

$\mathrm{\&delta;}\stackrel{\&CenterDot;}{Q}=\frac{1}{2}<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>\mathrm{\&delta;Q}+\frac{1}{2}U\left(\widehat{Q_b^n}\right){\mathrm{\&delta;\&omega;}}_{\mathrm{ib}}^b$

式中，δQ为四元数误差，

为陀螺误差，

$<{\mathrm{\&omega;}}_{\mathrm{ib}}^b>=\left[\begin{array}{cccc}0& {-\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& 0& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& 0& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}\\ {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hz}}& {\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hy}}& -{\mathrm{\&omega;}}_{\mathrm{ib}}^{\mathrm{hx}}& 0\end{array}\right],$ $U\left(\widehat{Q_b^n}\right)=\left[\begin{array}{ccc}-q_{b1}& -q_{b2}& -q_{b3}\\ q_{b0}& {-q}_{b3}& q_{b2}\\ q_{b3}& q_{b0}& -q_{b1}\\ {-q}_{b2}& q_{b1}& q_{b0}\end{array}\right];$

参考卡尔曼滤波公式，状态方程为：

量测方程为：Z＝HX+V

其中，状态变量X为X＝[δq₀ δq₁ δq₂ δq₃ ε_x ε_y ε_z]^T＝[δQ ε]^T，即X包括δQ四元数变量部分和ε三轴陀螺仪漂移变量部分，W为系统激励白噪声；V为量测白噪声；

$H=\left[\begin{array}{ccccccc}1& 0& 0& 0& 0& 0& 0\\ 0& 1& 0& 0& 0& 0& 0\\ 0& 0& 1& 0& 0& 0& 0\\ 0& 0& 0& 1& 0& 0& 0\end{array}\right];$

经卡尔曼滤波，将更新后的状态变量X的三轴陀螺仪漂移变量部分更新步骤(42)中三轴陀螺仪的漂移ε＝[ε_x ε_y ε_z]^T；

将更新后的状态变量X的四元数变量部分补偿到步骤(42)中三轴陀螺仪计算得到的四元数[q_b0 q_b1 q_b2 q_b3]^T，将补偿后的[q_b0 q_b1 q_b2 q_b3]^T作为步骤(42)中新的初始四元数；

重复步骤(42)～步骤(44)，实现航姿系统的信息融合。

6.如权利要求5所述的微型捷联航姿系统的测姿方法，其特征在于，步骤(44)中所述的卡尔曼滤波的滤波周期为1s。

Images