【技术实现步骤摘要】
本专利技术涉及通信
，特别涉及一种地磁场模型辅助的载体姿态和航向计算方法和系统。
技术介绍
姿态和航向参考系统(AttitudeandHeadingReferenceSystem，简称AHRS)是一种可以精确测量载体在空间坐标系下三轴姿态的测量装置，由三轴磁强计、三轴陀螺仪和三轴加速度计等硬件和相应的软件构成，为载体提供实时的姿态和航向信息，在航空航天、机器人、汽车工业以及行人导航定位领域得到广泛的应用。随着微机电技术的发展，基于微机电系统传感器的姿态和航向参考系统应用更广泛。但由于陀螺仪存在漂移，导致在计算载体姿态和航向信息，其误差不断累积。为解决此问题，在姿态和航向参考系统中引入加速度计和磁传感器的信息，并以重力场和地磁场作为参考矢量，利用加速计和磁强计输出值与陀螺仪的输出值实时计算载体的姿态和航向信息。虽然姿态和航向参考系统可为载体实时提供精确的姿态和航向信息，但在近地运动的载体经常受到外部环境的干扰，特别是磁传感器受环境磁干扰明显。当受到外部磁干扰时，利用磁传感器输出值作为量测值估计载体的姿态和航向信息存在误差，甚至不能正确计算载体的姿态和航向信息，从而给载体的自主控制带来很大的风险。
技术实现思路
本专利技术提供了一种地磁场模型辅助的载体姿态和航向计算方法和系统，以解决存在环境磁干扰时载体姿态和航向实时计算的技术问题。本专利技术提供了一种地磁场模型辅助的载体姿态和航向计算方法，包括以下步骤：<...
【技术保护点】
一种地磁场模型辅助的载体姿态和航向计算方法，其特征在于，包括以下步骤：根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的输出值和磁传感器的输出值，计算估计磁倾角；根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰；当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建所述磁传感器量测值的自适应权重，并根据所述磁传感器量测值的自适应权重，构建所述磁传感器量测值的等价权矩阵；根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，计算载体姿态信息和航向信息。
【技术特征摘要】
1.一种地磁场模型辅助的载体姿态和航向计算方法，其特征在于，包括以下步骤：
根据载体的位置信息和地磁场模型，计算理想磁倾角；根据加速度计的输出值和磁传
感器的输出值，计算估计磁倾角；
根据所述理想磁倾角和所述估计磁倾角，判断是否存在环境磁干扰；
当存在环境磁干扰时，根据磁传感器量测值的归一化残差和量测值协方差矩阵，构建
所述磁传感器量测值的自适应权重，并根据所述磁传感器量测值的自适应权重，构建所述
磁传感器量测值的等价权矩阵；
根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权矩
阵，计算载体姿态信息和航向信息。
2.如权利要求1所述的方法，其特征在于，根据所述加速度计量测值、所述磁传感器量
测值和所述磁传感器量测值的等价权矩阵，计算所述载体姿态信息和航向信息，具体包括：
根据所述加速度计量测值更新状态预测值，得到状态更新值，并使用所述状态更新值
更新姿态四元数的估计值；
根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵，更新所述状态更新
值，得到状态估计值，并使用所述状态估计值更新所述姿态四元数的估计值；
根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵，计算所述
载体姿态信息和航向信息。
3.如权利要求2所述的方法，其特征在于，根据所述加速度计量测值更新状态预测值，
得到状态更新值，具体包括：
根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；
根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值，构建
第一量测方程；
根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更新状
态预测值，得到状态更新值。
4.如权利要求2所述的方法，其特征在于，根据所述磁传感器量测值和所述磁传感器量
测值的等价权矩阵，更新所述状态更新值，得到状态估计值，具体包括：
根据惯性传感器误差模型和姿态误差四元数更新模型，构建Kalman滤波的状态方程；
根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值，构建
第二量测方程；
根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及所述
磁传感器量测值的等价权矩阵，更新状态更新值，得到状态估计值。
5.如权利要求3或4所述的方法，其特征在于，所述Kalman滤波的状态方程，具体为：
X . ( t ) = F ( t ) X ( t ) + W ( t ) ]]> F ( t ) = - [ ω ^ b × ] - 0.5 I 0 0 0 0 0 0 0 ]]> W ( t ) = - 0.5 v ω v b ω v b a ]]>其中，X(t)＝[qebωba]T∈R9×1为状态量，T为转置，F(t)为状态转移矩阵，W(t)为过程
噪声，为陀螺仪输出值的反对矩阵，vω为陀螺仪的随机漂移噪声，为陀螺仪随机
常值模型噪声，为加速度计随机常值模型噪声，qe为误差四元数的矢量部分，bω为陀螺仪
随机常值，ba为加速度计偏差。
6.如权利要求3所述的方法，其特征在于，所述第一量测方程，具体为：
L a = a ^ b - C n b ( q ^ ) g n = 2 [ C n b ( q ^ ) g n × ] q e + b a + v a ]]>其中，La为所述加速度计的输出值与根据重力场参考矢量估计的所述加速度计的输出
值之间的差值，为加速度计的输出值，为从导航坐标系到载体坐标系的转化矩阵，为
姿态四元数的估计值，gn为重力加速度在导航坐标系下的投影，va为加速度计的随机误差。
7.如权利要求6所述的方法，其特征在于，根据所述Kalman滤波的状态方程、所述第一
量测方程式以及所述加速度计量测值更新状态预测值，得到状态更新值，具体为：
采用以下公式更新状态预测值，得到状态更新值：
X ^ a , k = X ‾ k + K a , k ( L a , k - A a , k X ‾ k ) ]]> K a , k = ∑ X ‾ k A a , k T ( A a , k T ∑ X ‾ k A a , k + R a ) - 1 ]]> ∑ X ^ a , k = ( I - K a , k A a , k ) ∑ X ‾ k ( I - K a , k A a , k ) T + K a , k R a K a , k T ]]>其中，为k时刻的状态更新值，为k时刻的状态预测值，La，k为k时刻的所述加速度
计量测值，Aa，k为所述第一量测方程在k时刻的量测矩阵，l为单位阵，Ka，k为增益矩阵，为
的协方差矩阵，Ra为加速度计量测噪声的协方差矩阵；为k时刻的协方差矩阵。
8.如权利要求2所述的方法，其特征在于，使用所述状态更新值更新姿态四元数的估计
值，具体为：
采用以下公式，更新姿态四元数的估计值：
q ^ ⇐ q ^ ⊗ q e ]]> q ^ ⇐ q ^ / | | q ^ | | ]]> q e = X ^ a , k ( 1 : 3 ) ]]>其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，
qe为所述状态更新值的分量，为所述状态更新值。
9.如权利要求4所述的方法，其特征在于，所述第二量测方程，具体为：
L m = m ^ b - C n b ( q ^ ) m n = 2 [ C n b ( q ^ ) g n × ] q e + v m ]]>其中，Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出
值之间的差值，为磁传感器的输出值，为从导航坐标系到载体坐标系的转化矩阵，为
姿态四元数的估计值，mn为理想地磁场输出值的归一化结果，vm磁传感器的随机误差。
10.如权利要求9所述的方法，其特征在于，根据所述Kalman滤波的状态方程、所述第二
量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵，更新状态更新值，
得到状态估计值，具体为：
采用以下公式更新状态更新值，得到状态估计值：
X ^ k = X ^ a , k + K m , k ( L m , k - A m , k X ^ a , k ) ]]> ∑ X ^ k = ( I - K m , k A m , k ) ∑ X ^ a , k ( I - K m , k A m , k ) T + K m , k P ‾ m , k - 1 K m , k T ]]> K m , k = r 3 r 3 T O 3 × 6 O 6 × 3 O 6 × 6 ∑ X ‾ m , k A m , k T ( A m , k T ∑ X ‾ m , k A m , k + P ‾ m , k - 1 ) - 1 ]]> ∑ X ‾ m , k = ∑ X ^ a , k ( 1 : 3,1 : 3 ) O 3 × 6 O 6 × 3 O 6 × 6 ]]> r 3 = C n b ( q ^ ) 0 0 1 T ]]>其中，为k时刻的状态估计值，为k时刻的状态更新值，Lm，k为k时刻的所述磁传感
器量测值，为的协方差矩阵，Km，k为k时刻的增益矩阵，Am，k为k时刻的量测矩阵，为k
时刻的所述磁传感器量测值的等价权矩阵。
11.如权利要求2所述的方法，其特征在于，使用所述状态估计值更新所述姿态四元数
的估计值，具体为：
采用以下公式，更新姿态四元数的估计值：
q ^ ⇐ q ^ ⊗ q e ]]> q ^ ⇐ q ^ / | | q ^ | | ]]> q e = X ^ a ( 1 : 3 ) ]]>其中，为所述姿态四元数的估计值，为四元数乘法，为的归一化，为赋值符号，
qe为所述状态估计值的分量，为所述状态估计值。
12.如权利要求1所述的方法，其特征在于，根据加速度计的输出值和磁传感器的输出
值，计算估计磁倾角，具体为：
采用以下公式计算估计磁倾角：
Dcacul＝arccos(na·nm)
其中，Dcacul为所述估计磁倾角，na为所述加速度计在载体系输出值的方向矢量，nm为所
述磁传感器在载体系输出值的方向矢量。
13.如权利要求1所述的方法，其特征在于，根据所述理想磁倾角和所述估计磁倾角，判
断是否存在环境磁干扰，具体为：
判断所述理想磁倾角和所述估计磁倾角是否满足以下条件：
|Dcacul-Driference|＞λD其中，Driference为所述理想磁倾角，Dcacul为所述估计磁倾角，λD为预设阈值；
如果满足上述条件，则确定存在环境磁干扰；否则，确定不存在环境磁干扰。
14.如权利要求1所述的方法，其特征在于，根据磁传感器量测值的归一化残差和量测
值协方差矩阵，构建所述磁传感器量测值的自适应权重，具体为：
采用以下公式构建所述磁传感器量测值的自适应权重：
不存在环境磁干扰时；
存在环境磁干扰时；
其中，为k时刻所述磁传感器第i个量测值的自适应权重，pki为量测值协方差矩阵的
第i个主对角线元素，c为常数，Vi′为磁传感器量测值的归一化残差。
15.如权利要求9所述的方法，其特征在于，根据磁传感器量测值的归一化残差和量测
值协方差矩阵，构建所述磁传感器量测值的自适应权重之前，还包括：
采用以下公式计算所述磁传感器量测值的归一化残差：
V i , k ′ = ( L mi , k - A mi , k X ‾ k ) / | σ i | , i = 1,2 , 3 ]]>其中，Lmi为所述第二量测方程的第i个测量方程，Ami为量测矩阵的第i行；|σi|为磁传感器量测噪声...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。