[发明专利]一种微型捷联航姿系统及其工作方法

权利要求书

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总线，从数据采集模块的三轴磁阻传感器实时读取地球磁场强度在载体系三轴上的分量，并对它们进行补偿：

Bbhx=Bbx*Xsf+Xoff]]>

Bbhy=Bby*Ysf+Yoff]]>

Bbhz=Bbz]]>

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

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

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

θ0=arcsin(-fbxg),]]>γ0=arctan(fbyfbz)]]>

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

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

BbX=Bbhxcosθ0+Bbhzsinθ0]]>

BbY=Bbhxsinγ0sinθ0+Bbhycosγ0-Bbhzsinγ0cosθ0]]>

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

ψM=arctan(BbYBbX)]]>

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

ψ₀＝ψ_M+D

(35)计算出陀螺的漂移：在静止状态，数据处理解算单元通过SPI总线，从三轴陀螺仪实时读取载体角速度值在载体系三轴上的分量读取100～200次求均值，得到三轴陀螺仪在载体系三轴上的漂移ε_x、ε_y、ε_z，三轴陀螺仪的漂移记为ε＝[ε_x ε_y ε_z]^T；

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

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

(42)在组合测姿状态，对三轴陀螺仪输出的角速度值在载体系三轴上的分量进行补偿：ωibhx=ωibbx-ϵx,]]>ωibhy=ωibby-ϵy,]]>ωibhz=ωibbz-ϵ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)卡尔曼滤波信息融合：

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

δQ·=12<ωibb>δQ+12U(Qbn^)δωibb]]>

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

<ωibb>=0-ωibhx-ωibhy-ωibhzωibhx0ωibhz-ωibhyωibhy-ωibhz0ωibhxωibhzωibhy-ωibhx0,]]>U(Qbn^)=-qb1-qb2-qb3qb0-qb3qb2qb3qb0-qb1-qb2qb1qb0;]]>

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

量测方程为：Z＝HX+V

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

H=1000000010000000100000001000;]]>

经卡尔曼滤波，将更新后的状态变量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。