• 四川郎酒股份有限公司获第十二届人民企业社会责任奖年度环保奖 2019-05-13
  • 银保监会新规剑指大企业多头融资和过度融资 2019-05-12
  • 韩国再提4国联合申办世界杯 中国网友无视:我们自己来 2019-05-11
  • 中国人为什么一定要买房? 2019-05-11
  • 十九大精神进校园:风正扬帆当有为 勇做时代弄潮儿 2019-05-10
  • 粽叶飘香幸福邻里——廊坊市举办“我们的节日·端午”主题活动 2019-05-09
  • 太原设禁鸣路段 设备在测试中 2019-05-09
  • 拜耳医药保健有限公司获第十二届人民企业社会责任奖年度企业奖 2019-05-08
  • “港独”没出路!“梁天琦们”该醒醒了 2019-05-07
  • 陈卫平:中国文化内涵包含三方面 文化复兴表现在其中 2019-05-06
  • 人民日报客户端辟谣:“合成军装照”产品请放心使用 2019-05-05
  • 【十九大·理论新视野】为什么要“建设现代化经济体系”?   2019-05-04
  • 聚焦2017年乌鲁木齐市老城区改造提升工程 2019-05-04
  • 【专家谈】上合组织——构建区域命运共同体的有力实践者 2019-05-03
  • 【华商侃车NO.192】 亲!楼市火爆,别忘了买车位啊! 2019-05-03
    • / 29
    • 下载费用:30 金币  

    玩重庆时时彩怎么稳赚: 一种载体运动状态信息辅助的姿态和航向计算方法和系统.pdf

    摘要
    申请专利号:

    重庆时时彩单双窍门 www.4mum.com.cn CN201610059857.X

    申请日:

    2016.01.28

    公开号:

    CN105606096A

    公开日:

    2016.05.25

    当前法律状态:

    授权

    有效性:

    有权

    法律详情: 授权|||实质审查的生效IPC(主分类):G01C 21/18申请日:20160128|||公开
    IPC分类号: G01C21/18; G01C21/20 主分类号: G01C21/18
    申请人: 北京航空航天大学
    发明人: 高楠; 赵龙
    地址: 100191 北京市海淀区学院路37号
    优先权:
    专利代理机构: 北京慕达星云知识产权代理事务所(特殊普通合伙) 11465 代理人: 苗青盛
    PDF完整版下载: PDF下载
    法律状态
    申请(专利)号:

    CN201610059857.X

    授权公告号:

    ||||||

    法律状态公告日:

    2018.03.30|||2016.06.22|||2016.05.25

    法律状态类型:

    授权|||实质审查的生效|||公开

    摘要

    本发明公开一种载体运动状态信息辅助的姿态和航向计算方法和系统,该方法包括以下步骤:根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状态信息;根据载体的运动状态信息,判断是否存在环境磁干扰;当存在环境磁干扰时,根据磁传感器量测值的自适应权重,构建磁传感器量测值的等价权矩阵;根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩阵,计算载体姿态信息和航向信息。本发明根据载体的运动状态信息检测环境磁干扰,并在检测到环境磁干扰时,构建磁传感器量测值的等价权矩阵,并实现磁传感器量测值对状态估计值贡献的自适应调整,使计算得到的姿态信息和航向信息具有抗差性。

    权利要求书

    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时刻的量测矩阵,I为单位阵,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 0 3 × 6 0 6 × 3 0 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 ) 0 3 × 6 0 6 × 3 0 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所述的方法,其特征在于,根据加速度计的输出值和陀螺仪的输出值,
    获取所述载体的运动状态信息,具体为:
    判断所述加速度计的输出值是否满足第一条件:
    | Max ( STD ( Accel . X b ) ) - Min ( STD ( Accel . X b ) ) | λ 2 | Max ( STD ( Accel . Y b ) ) - Min ( STD ( Accel . Y b ) ) | λ 2 ]]>
    其中,λ2为第一阈值,Accel.Xb为时间窗内x轴加速度计的输出值,Accel.Yb为时间窗内
    y轴加速度计的输出值,Max为最大值,Min为最小值,STD为标准差,上角标b为载体坐标系;
    如果所述加速度计的输出值满足第一条件,则确定所述载体处于静止状态;否则,判断
    所述陀螺仪的输出值是否满足第二条件:
    ω z b θ ω ]]>
    其中,为陀螺仪的输出值,θω为第二阈值;
    如果所述陀螺仪的输出值满足第二条件,则确定所述载体处于直行状态;否则,确定所
    述载体处于转弯状态。
    13.如权利要求1所述的方法,其特征在于,根据所述载体的运动状态信息,判断是否存
    在环境磁干扰,具体为:
    判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值,如果是,
    则确定存在环境磁干扰;否则,确定不存在环境磁干扰。
    14.如权利要求13所述的方法,其特征在于,判断卡方假设检验统计量是否大于与所述
    载体的运动状态信息对应的阈值之前,还包括:
    采用以下公式构建所述卡方假设检验统计量:
    λ k = V k T Σ V k V k , λ k ~ χ 2 ( m ) ]]>
    其中,和分别为Kalman滤波的预测残差和预测残差协方差矩阵,m为被检测观测维
    数。
    15.如权利要求1所述的方法,其特征在于,根据磁传感器量测值的归一化残差和量测
    值协方差矩阵,构建所述磁传感器量测值的自适应权重,具体为:
    采用以下公式构建所述磁传感器量测值的自适应权重:
    不存在环境磁干扰时;
    存在环境磁干扰时;
    其中,为k时刻所述磁传感器第i个量测值的自适应权重,pki为量测值协方差矩阵的第
    i个主对角线元素,c为常数,Vi′为磁传感器量测值的归一化残差。
    16.如权利要求9所述的方法,其特征在于,根据磁传感器量测值的归一化残差和量测
    值协方差矩阵,构建所述磁传感器量测值的自适应权重之前,还包括:
    采用以下公式计算所述磁传感器量测值的归一化残差:
    V i , k = ( L mi , k - A mi , k X k ) / | σ i | , i = 1,2,3 ]]>
    其中,Lmi为所述第二量测方程的第i个测量方程,Ami为量测矩阵的第i
    行;|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
    17.如权利要求1所述的方法,其特征在于,根据所述载体的运动状态信息,判断是否存
    在环境磁干扰之后,还包括:
    当存在环境磁干扰时,根据所述载体的运动状态信息,使用运动状态中的健康残差自
    适应取代受干扰后的残差。
    18.如权利要求17所述的方法,其特征在于,使用运动状态中的健康残差自适应取代受
    干扰后的残差之前,还包括:
    采用以下公式计算运动状态中的健康残差:
    r = r 1 + r 2 + · · · r n n ]]>
    其中,为运动状态中的健康残差,[r1,r2,…rn]为所述载体处于静止状态和直行状态下
    利用滑动时间窗保存的健康残差。
    19.一种姿态和航向参考系统,其特征在于,包括:
    获取???,用于根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状态信息;
    判断???,用于根据所述载体的运动状态信息,判断是否存在环境磁干扰;
    第一构建???,用于在所述判断??榕卸铣龃嬖诨肪炒鸥扇攀?,根据磁传感器量测值
    的归一化残差和量测值协方差矩阵,构建所述磁传感器量测值的自适应权重;
    第二构建???,用于根据所述磁传感器量测值的自适应权重,构建所述磁传感器量测
    值的等价权矩阵;
    第一计算???,用于根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器
    量测值的等价权矩阵,计算载体姿态信息和航向信息。
    20.如权利要求19所述的系统,其特征在于,所述第一计算???,包括:
    第一更新子???,用于根据所述加速度计量测值更新状态预测值,得到状态更新值;
    第二更新子???,用于使用所述状态更新值更新姿态四元数的估计值;
    第三更新子???,用于根据所述磁传感器量测值和所述磁传感器量测值的等价权矩
    阵,更新所述状态更新值,得到状态估计值;
    第四更新子???,用于使用所述状态估计值更新所述姿态四元数的估计值;
    计算子???,用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转
    化矩阵,计算所述载体姿态信息和航向信息。
    21.如权利要求20所述的系统,其特征在于,
    所述第一更新子???,具体用于根据惯性传感器误差模型和姿态误差四元数更新模
    型,构建Kalman滤波的状态方程;根据所述加速度计的输出值以及根据重力场参考矢量估
    计的所述加速度计的输出值,构建第一量测方程;根据所述Kalman滤波的状态方程、所述第
    一量测方程以及所述加速度计量测值更新状态预测值,得到状态更新值。
    22.如权利要求20所述的系统,其特征在于,
    所述第三更新子???,具体用于根据惯性传感器误差模型和姿态误差四元数更新模
    型,构建Kalman滤波的状态方程;根据所述磁传感器的输出值以及根据世界地磁场模型估
    计的所述磁传感器的输出值,构建第二量测方程;根据所述Kalman滤波的状态方程、所述第
    二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵,更新状态更新
    值,得到状态估计值。
    23.如权利要求21或22所述的系统,其特征在于,所述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为加速度计偏差。
    24.如权利要求21所述的系统,其特征在于,所述第一量测方程,具体为:
    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为加速度计的随机误差。
    25.如权利要求24所述的系统,其特征在于,
    所述第一更新子???,具体用于根据惯性传感器误差模型和姿态误差四元数更新模
    型,构建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时刻的量测矩阵,I为单位阵,Ka,k为增益矩阵,为
    的协方差矩阵,Ra为加速度计量测噪声的协方差矩阵;为k时刻的协方差矩阵。
    26.如权利要求20所述的系统,其特征在于,
    所述第二更新子???,具体用于采用以下公式,更新姿态四元数的估计值:
    q ^ q ^ q e ]]>
    q ^ q ^ / | | q ^ | | ]]>
    q e = X ^ a , k ( 1 : 3 ) ]]>
    其中,为所述姿态四元数的估计值,为四元数乘法,为的归一化,为赋值符号,
    qe为所述状态更新值的分量,为所述状态更新值。
    27.如权利要求22所述的系统,其特征在于,所述第二量测方程,具体为:
    L m = m ^ b - C n b ( q ^ ) m n = 2 [ C n b ( q ^ ) g n × ] q e + v m ]]>
    其中,Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的输出
    值之间的差值,为磁传感器的输出值,为从导航坐标系到载体坐标系的转化矩阵,为
    姿态四元数的估计值,mn为理想地磁场输出值的归一化结果,vm磁传感器的随机误差。
    28.如权利要求27所述的系统,其特征在于,
    所述第三更新子???,具体用于根据惯性传感器误差模型和姿态误差四元数更新模
    型,构建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 0 3 × 6 0 6 × 3 0 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 ) 0 3 × 6 0 6 × 3 0 6 × 6 ]]>
    r 3 = C n b ( q ^ ) 0 0 1 T ]]>
    其中,为k时刻的状态估计值,为k时刻的状态更新值,Lm,k为k时刻的所述磁传感
    器量测值,为的协方差矩阵,Km,k为k时刻的增益矩阵,Am,k为k时刻的量测矩阵,为k
    时刻的所述磁传感器量测值的等价权矩阵。
    29.如权利要求20所述的系统,其特征在于,
    所述第四更新子???,具体用于采用以下公式,更新姿态四元数的估计值:
    q ^ q ^ q e ]]>
    q ^ q ^ / | | q ^ | | ]]>
    q e = X ^ a ( 1 : 3 ) ]]>
    其中,为所述姿态四元数的估计值,为四元数乘法,为的归一化,为赋值符号,
    qe为所述状态估计值的分量,为所述状态估计值。
    30.如权利要求19所述的系统,其特征在于,
    所述获取???,具体用于判断所述加速度计的输出值是否满足第一条件:
    | Max ( STD ( Accel . X b ) ) - Min ( STD ( Accel . X b ) ) | λ 2 | Max ( STD ( Accel . Y b ) ) - Min ( STD ( Accel . Y b ) ) | λ 2 ]]>
    其中,λ2为第一阈值,Accel.Xb为时间窗内x轴加速度计的输出值,Accel.Yb为时间窗内
    y轴加速度计的输出值,Max为最大值,Min为最小值,STD为标准差,上角标b为载体坐标系;
    如果所述加速度计的输出值满足第一条件,则确定所述载体处于静止状态;否则,判断
    所述陀螺仪的输出值是否满足第二条件:
    ω z b θ ω ]]>
    其中,为陀螺仪的输出值,θω为第二阈值;
    如果所述陀螺仪的输出值满足第二条件,则确定所述载体处于直行状态;否则,确定所
    述载体处于转弯状态。
    31.如权利要求19所述的系统,其特征在于,
    所述判断???,具体用于判断卡方假设检验统计量是否大于与所述载体的运动状态信
    息对应的阈值,如果是,则确定存在环境磁干扰;否则,确定不存在环境磁干扰。
    32.如权利要求31所述的系统,其特征在于,还包括:
    第三构建???,用于采用以下公式构建所述卡方假设检验统计量:
    λ k = V k T Σ V k V k , λ k ~ χ 2 ( m ) ]]>
    其中,和分别为Kalman滤波的预测残差和预测残差协方差矩阵,m为被检测观测维
    数。
    33.如权利要求19所述的系统,其特征在于,
    所述第一构建???,具体用于在所述判断??榕卸铣龃嬖诨肪炒鸥扇攀?,采用以下公
    式构建所述磁传感器量测值的自适应权重:
    不存在环境磁干扰时;
    存在环境磁干扰时;
    其中,为k时刻所述磁传感器第i个量测值的自适应权重,pki为量测值协方差矩阵的
    第i个主对角线元素,c为常数,Vi′为磁传感器量测值的归一化残差。
    34.如权利要求27所述的系统,其特征在于,还包括:
    第二计算???,用于采用以下公式计算所述磁传感器量测值的归一化残差:
    V i , k = ( L mi , k - A mi , k X k ) / | σ i | , i = 1,2,3 ]]>
    其中,Lmi为所述第二量测方程的第i个测量方程,Ami为量测矩阵的第i
    行;|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线元素的平方根。
    35.如权利要求19所述的系统,其特征在于,还包括:
    取代???,用于当存在环境磁干扰时,根据所述载体的运动状态信息,使用运动状态中
    的健康残差自适应取代受干扰后的残差。
    36.如权利要求35所述的系统,其特征在于,还包括:
    第三计算???,用于采用以下公式计算运动状态中的健康残差:
    r = r 1 + r 2 + · · · r n n ]]>
    其中,为运动状态中的健康残差,[r1,r2,…rn]为所述载体处于静止状态和直行状态下
    利用滑动时间窗保存的健康残差。

    说明书

    一种载体运动状态信息辅助的姿态和航向计算方法和系统

    技术领域

    本发明涉及通信技术领域,特别涉及一种载体运动状态信息辅助的姿态和航向计
    算方法和系统。

    背景技术

    姿态和航向参考系统(AttitudeandHeadingReferenceSystem,简称AHRS)是
    一种可以精确测量载体在空间坐标系下三轴姿态的测量装置,由三轴磁强计、三轴陀螺仪
    和三轴加速度计等硬件和相应的软件构成,为载体提供实时的姿态和航向信息,在航空航
    天、机器人、汽车工业以及行人导航定位领域得到广泛的应用。随着微机电技术的发展,基
    于微机电系统传感器的姿态和航向参考系统应用更广泛。但由于陀螺仪存在漂移,导致在
    计算载体姿态和航向信息,其误差不断累积。为解决此问题,在姿态和航向参考系统中引入
    加速度计和磁传感器的信息,并以重力场和地磁场作为参考矢量,利用加速计和磁强计输
    出值与陀螺仪的输出值实时计算载体的姿态和航向信息。

    虽然姿态和航向参考系统可为载体实时提供精确的姿态和航向信息,但在近地运
    动的载体经常受到外部环境的干扰,特别是磁传感器受环境磁干扰明显。当受到外部磁干
    扰时,利用磁传感器输出值作为量测值估计载体的姿态和航向信息存在误差,甚至不能正
    确计算载体的姿态和航向信息,从而给载体的自主控制带来很大的风险。

    发明内容

    本发明提供了一种载体运动状态信息辅助的姿态和航向计算方法和系统,以解决
    存在环境磁干扰时载体姿态和航向实时计算的技术问题。

    本发明提供了一种载体运动状态信息辅助的姿态和航向计算方法,包括以下步
    骤:

    根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状态信息;

    根据所述载体的运动状态信息,判断是否存在环境磁干扰;

    当存在环境磁干扰时,根据磁传感器量测值的归一化残差和量测值协方差矩阵,
    构建所述磁传感器量测值的自适应权重,并根据所述磁传感器量测值的自适应权重,构建
    所述磁传感器量测值的等价权矩阵;

    根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值的等价权
    矩阵,计算所述载体姿态信息和航向信息。

    可选地,根据所述加速度计量测值、所述磁传感器量测值和所述磁传感器量测值
    的等价权矩阵,计算所述载体姿态信息和航向信息,具体包括:

    根据所述加速度计量测值更新状态预测值,得到状态更新值,并使用所述状态更
    新值更新姿态四元数的估计值;

    根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵,更新所述状态更
    新值,得到状态估计值,并使用所述状态估计值更新所述姿态四元数的估计值;

    根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系的转化矩阵,计算
    所述载体姿态信息和航向信息。

    可选地,根据所述加速度计量测值更新状态预测值,得到状态更新值,具体包括:

    根据惯性传感器误差模型和姿态误差四元数更新模型,构建Kalman滤波的状态方
    程;根据所述加速度计的输出值以及根据重力场参考矢量估计的所述加速度计的输出值,
    构建第一量测方程;

    根据所述Kalman滤波的状态方程、所述第一量测方程以及所述加速度计量测值更
    新状态预测值,得到状态更新值。

    可选地,根据所述磁传感器量测值和所述磁传感器量测值的等价权矩阵,更新所
    述状态更新值,得到状态估计值,具体包括:

    根据惯性传感器误差模型和姿态误差四元数更新模型,构建Kalman滤波的状态方
    程;根据所述磁传感器的输出值以及根据世界地磁场模型估计的所述磁传感器的输出值,
    构建第二量测方程;

    根据所述Kalman滤波的状态方程、所述第二量测方程、所述磁传感器量测值以及
    所述磁传感器量测值的等价权矩阵,更新状态更新值,得到状态估计值。

    可选地,所述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为加速度计偏差。

    可选地,所述第一量测方程,具体为:

    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为加速度计的随机误
    差。

    可选地,根据所述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时刻的量测矩阵,I为单位阵,Ka,k为增益矩阵,
    为的协方差矩阵,Ra为加速度计量测噪声的协方差矩阵;为k时刻的协方差矩
    阵。

    可选地,使用所述状态更新值更新姿态四元数的估计值,具体为:

    采用以下公式,更新姿态四元数的估计值:

    q ^ q ^ q e ]]>

    q ^ q ^ / | | q ^ | | ]]>

    q e = X ^ a , k ( 1 : 3 ) ]]>

    其中,为所述姿态四元数的估计值,为四元数乘法,为的归一化,为赋值
    符号,qe为所述状态更新值的分量,为所述状态更新值。

    可选地,所述第二量测方程,具体为:

    L m = m ^ b - C n b ( q ^ ) m n = 2 [ C n b ( q ^ ) g n × ] q e + v m ]]>

    其中,Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的
    输出值之间的差值,为磁传感器的输出值,为从导航坐标系到载体坐标系的转化矩阵,
    为姿态四元数的估计值,mn为理想地磁场输出值的归一化结果,vm磁传感器的随机误差。

    可选地,根据所述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 P m , k - 1 K m , k T ]]>

    K m , k = r 3 r 3 T 0 3 × 6 0 6 × 3 0 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 ) 0 3 × 6 0 6 × 3 0 6 × 6 ]]>

    r 3 = C n b ( q ^ ) 0 0 1 T ]]>

    其中,为k时刻的状态估计值,为k时刻的状态更新值,Lm,k为k时刻的所述磁
    传感器量测值,为的协方差矩阵,Km,k为k时刻的增益矩阵,Am,k为k时刻的量测矩阵,
    为k时刻的所述磁传感器量测值的等价权矩阵。

    可选地,使用所述状态估计值更新所述姿态四元数的估计值,具体为:

    采用以下公式,更新姿态四元数的估计值:

    q ^ q ^ q e ]]>

    q ^ q ^ / | | q ^ | | ]]>

    q e = X ^ a , k ( 1 : 3 ) ]]>

    其中,为所述姿态四元数的估计值,为四元数乘法,为的归一化,为赋值
    符号,qe为所述状态估计值的分量,为所述状态估计值。

    可选地,根据加速度计的输出值和陀螺仪的输出值,获取所述载体的运动状态信
    息,具体为:

    判断所述加速度计的输出值是否满足第一条件:

    | Max ( STD ( Accel . X b ) ) - Min ( STD ( Accel . X b ) ) | λ 2 | Max ( STD ( Accel . Y b ) ) - Min ( STD ( Accel . Y b ) ) | λ 2 ]]>

    其中,λ2为第一阈值,Accel.Xb为时间窗内x轴加速度计的输出值,Accel.Yb为时间
    窗内y轴加速度计的输出值,Max为最大值,Min为最小值,STD为标准差,上角标b为载体坐标
    系;

    如果所述加速度计的输出值满足第一条件,则确定所述载体处于静止状态;否则,
    判断所述陀螺仪的输出值是否满足第二条件:

    ω z b θ ω ]]>

    其中,为陀螺仪的输出值,θω为第二阈值;

    如果所述陀螺仪的输出值满足第二条件,则确定所述载体处于直行状态;否则,确
    定所述载体处于转弯状态。

    可选地,根据所述载体的运动状态信息,判断是否存在环境磁干扰,具体为:

    判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈值,如果
    是,则确定存在环境磁干扰;否则,确定不存在环境磁干扰。

    可选地,判断卡方假设检验统计量是否大于与所述载体的运动状态信息对应的阈
    值之前,还包括:

    采用以下公式构建所述卡方假设检验统计量:

    λ k = V k T Σ V k V k , λ k ~ χ 2 ( m ) ]]>

    其中,和分别为Kalman滤波的预测残差和预测残差协方差矩阵,m为被检测观
    测维数。

    可选地,根据磁传感器量测值的归一化残差和量测值协方差矩阵,构建所述磁传
    感器量测值的自适应权重,具体为:

    采用以下公式构建所述磁传感器量测值的自适应权重:

    不存在环境磁干扰时;

    存在环境磁干扰时;

    其中,为k时刻所述磁传感器第i个量测值的自适应权重,pki为量测值协方差矩
    阵的第i个主对角线元素,c为常数,Vi′为磁传感器量测值的归一化残差。

    可选地,根据磁传感器量测值的归一化残差和量测值协方差矩阵,构建所述磁传
    感器量测值的自适应权重之前,还包括:

    采用以下公式计算所述磁传感器量测值的归一化残差:

    V i , k = ( L mi , k - A mi , k X k ) / | σ i | , i = 1,2,3 ]]>

    其中,Lmi为所述第二量测方程的第i个测量方程,Ami为量测矩阵
    的第i行;|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线
    元素的平方根。

    可选地,根据所述载体的运动状态信息,判断是否存在环境磁干扰之后,还包括:

    当存在环境磁干扰时,根据所述载体的运动状态信息,使用运动状态中的健康残
    差自适应取代受干扰后的残差。

    可选地,使用运动状态中的健康残差自适应取代受干扰后的残差之前,还包括:

    采用以下公式计算运动状态中的健康残差:

    r = r 1 + r 2 + . . . r n n ]]>

    其中,为运动状态中的健康残差,[r1,r2,…rn]为所述载体处于静止状态和直行
    状态下利用滑动时间窗保存的健康残差。

    本发明还提供了一种姿态和航向参考系统,包括:

    获取???,用于根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状态
    信息;

    判断???,用于根据所述载体的运动状态信息,判断是否存在环境磁干扰;

    第一构建???,用于在所述判断??榕卸铣龃嬖诨肪炒鸥扇攀?,根据磁传感器量
    测值的归一化残差和量测值协方差矩阵,构建所述磁传感器量测值的自适应权重;

    第二构建???,用于根据所述磁传感器量测值的自适应权重,构建所述磁传感器
    量测值的等价权矩阵;

    第一计算???,用于根据所述加速度计量测值、所述磁传感器量测值和所述磁传
    感器量测值的等价权矩阵,计算载体姿态信息和航向信息。

    可选地,所述第一计算???,包括:

    第一更新子???,用于根据所述加速度计量测值更新状态预测值,得到状态更新
    值;

    第二更新子???,用于使用所述状态更新值更新姿态四元数的估计值;

    第三更新子???,用于根据所述磁传感器量测值和所述磁传感器量测值的等价权
    矩阵,更新所述状态更新值,得到状态估计值;

    第四更新子???,用于使用所述状态估计值更新所述姿态四元数的估计值;

    计算子???,用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系
    的转化矩阵,计算所述载体姿态信息和航向信息。

    可选地,所述第一更新子???,具体用于根据惯性传感器误差模型和姿态误差四
    元数更新模型,构建Kalman滤波的状态方程;根据所述加速度计的输出值以及根据重力场
    参考矢量估计的所述加速度计的输出值,构建第一量测方程;根据所述Kalman滤波的状态
    方程、所述第一量测方程以及所述加速度计量测值更新状态预测值,得到状态更新值。

    可选地,所述第三更新子???,具体用于根据惯性传感器误差模型和姿态误差四
    元数更新模型,构建Kalman滤波的状态方程;根据所述磁传感器的输出值以及根据世界地
    磁场模型估计的所述磁传感器的输出值,构建第二量测方程;根据所述Kalman滤波的状态
    方程、所述第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵,更
    新状态更新值,得到状态估计值。

    可选地,所述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为加速度计偏差。

    可选地,所述第一量测方程,具体为:

    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为加速度计的随机误
    差。

    可选地,所述第一更新子???,具体用于根据惯性传感器误差模型和姿态误差四
    元数更新模型,构建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时刻的量测矩阵,I为单位阵,Ka,k为增益矩阵,
    为的协方差矩阵,Ra为加速度计量测噪声的协方差矩阵;为k时刻的协方差矩
    阵。

    可选地,所述第二更新子???,具体用于采用以下公式,更新姿态四元数的估计
    值:

    q ^ q ^ q e ]]>

    q ^ q ^ / | | q ^ | | ]]>

    q e = X ^ a , k ( 1 : 3 ) ]]>

    其中,为所述姿态四元数的估计值,为四元数乘法,为的归一化,为赋值
    符号,qe为所述状态更新值的分量,为所述状态更新值。

    可选地,所述第二量测方程,具体为:

    L m = m ^ b - C n b ( q ^ ) m n = 2 [ C n b ( q ^ ) g n × ] q e + v m ]]>

    其中,Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的
    输出值之间的差值,为磁传感器的输出值,为从导航坐标系到载体坐标系的转化矩阵,
    为姿态四元数的估计值,mn为理想地磁场输出值的归一化结果,vm磁传感器的随机误差。

    可选地,所述第三更新子???,具体用于根据惯性传感器误差模型和姿态误差四
    元数更新模型,构建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 P m , k - 1 K m , k T ]]>

    K m , k = r 3 r 3 T 0 3 × 6 0 6 × 3 0 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 ) 0 3 × 6 0 6 × 3 0 6 × 6 ]]>

    r 3 = C n b ( q ^ ) 0 0 1 T ]]>

    其中,为k时刻的状态估计值,为k时刻的状态更新值,Lm,k为k时刻的所述磁
    传感器量测值,为的协方差矩阵,Km,k为k时刻的增益矩阵,Am,k为k时刻的量测矩阵,
    为k时刻的所述磁传感器量测值的等价权矩阵。

    可选地,所述第四更新子???,具体用于采用以下公式,更新姿态四元数的估计
    值:

    q ^ q ^ q e ]]>

    q ^ q ^ / | | q ^ | | ]]>

    q e = X ^ a , k ( 1 : 3 ) ]]>

    其中,为所述姿态四元数的估计值,为四元数乘法,为的归一化,为赋值
    符号,qe为所述状态估计值的分量,为所述状态估计值。

    可选地,所述获取???,具体用于判断所述加速度计的输出值是否满足第一条件:

    | Max ( STD ( Accel . X b ) ) - Min ( STD ( Accel . X b ) ) | λ 2 | Max ( STD ( Accel . Y b ) ) - Min ( STD ( Accel . Y b ) ) | λ 2 ]]>

    其中,λ2为第一阈值,Accel.Xb为时间窗内x轴加速度计的输出值,Accel.Yb为时间
    窗内y轴加速度计的输出值,Max为最大值,Min为最小值,STD为标准差,上角标b为载体坐标
    系;

    如果所述加速度计的输出值满足第一条件,则确定所述载体处于静止状态;否则,
    判断所述陀螺仪的输出值是否满足第二条件:

    ω z b θ ω ]]>

    其中,为陀螺仪的输出值,θω为第二阈值;

    如果所述陀螺仪的输出值满足第二条件,则确定所述载体处于直行状态;否则,确
    定所述载体处于转弯状态。

    可选地,所述判断???,具体用于判断卡方假设检验统计量是否大于与所述载体
    的运动状态信息对应的阈值,如果是,则确定存在环境磁干扰;否则,确定不存在环境磁干
    扰。

    可选地,所述的系统,还包括:

    第三构建???,用于采用以下公式构建所述卡方假设检验统计量:

    λ k = V k T Σ V k V k , λ k ~ χ 2 ( m ) ]]>

    其中,和分别为Kalman滤波的预测残差和预测残差协方差矩阵,m为被检测观
    测维数。

    可选地,所述第一构建???,具体用于在所述判断??榕卸铣龃嬖诨肪炒鸥扇攀?,
    采用以下公式构建所述磁传感器量测值的自适应权重:

    不存在环境磁干扰时;

    存在环境磁干扰时;

    其中,为k时刻所述磁传感器第i个量测值的自适应权重,pki为量测值协方差矩
    阵的第i个主对角线元素,c为常数,Vi′为磁传感器量测值的归一化残差。

    可选地,所述的系统,还包括:

    第二计算???,用于采用以下公式计算所述磁传感器量测值的归一化残差:

    V i , k = ( L mi , k - A mi , k X k ) / | σ i | , i = 1,2,3 ]]>

    其中,Lmi为所述第二量测方程的第i个测量方程,Ami为量测矩阵
    的第i行;|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线
    元素的平方根。

    可选地,所述的系统,还包括:

    取代???,用于当存在环境磁干扰时,根据所述载体的运动状态信息,使用运动状
    态中的健康残差自适应取代受干扰后的残差。

    可选地,所述的系统,还包括:

    第三计算???,用于采用以下公式计算运动状态中的健康残差:

    r = r 1 + r 2 + . . . r n n ]]>

    其中,为运动状态中的健康残差,[r1,r2,…rn]为所述载体处于静止状态和直行
    状态下利用滑动时间窗保存的健康残差。

    本发明根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状态信息,进
    而根据载体的运动状态信息检测环境磁干扰,并在检测到环境磁干扰时,构建磁传感器量
    测值的等价权矩阵,并实现磁传感器量测值对状态估计值贡献的自适应调整,使计算得到
    的姿态信息和航向信息具有抗差性,能够适应地磁场畸变环境,并解决存在环境磁干扰时
    载体姿态和航向实时计算的技术问题。

    附图说明

    图1为本发明实施例中的一种载体运动状态信息辅助的姿态和航向计算方法流程
    图;

    图2为本发明实施例中的另一种载体运动状态信息辅助的姿态和航向计算方法流
    程图;

    图3为本发明实施例中的一种姿态和航行参考系统的结构示意图。

    具体实施方式

    下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完
    整地描述,显然,所描述的实施例仅仅是本发明一部分实施例,而不是全部的实施例?;?br />本发明中的实施例,本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他
    实施例,都属于本发明?;さ姆段?。

    本发明实施例提供了一种载体运动状态信息辅助的姿态和航向计算方法,如图1
    所示,包括以下步骤:

    步骤101,根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状态信息。

    步骤102,根据载体的运动状态信息,判断是否存在环境磁干扰。

    步骤103,当存在环境磁干扰时,根据磁传感器量测值的归一化残差和量测值协方
    差矩阵,构建磁传感器量测值的自适应权重,并根据磁传感器量测值的自适应权重,构建磁
    传感器量测值的等价权矩阵。

    步骤104,根据加速度计量测值、磁传感器量测值和磁传感器量测值的等价权矩
    阵,计算载体姿态信息和航向信息。

    本发明实施例根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状态信
    息,进而根据载体的运动状态信息检测环境磁干扰,并在检测到环境磁干扰时,构建磁传感
    器量测值的等价权矩阵,并实现磁传感器量测值对状态估计值贡献的自适应调整,使计算
    得到的姿态信息和航向信息具有抗差性,能够适应地磁场畸变环境,并解决存在环境磁干
    扰时载体姿态和航向实时计算的技术问题。

    本发明实施例还提供了另一种载体运动状态信息辅助的姿态和航向计算方法,如
    图2所示,包括以下步骤:

    步骤201,根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状态信息。

    其中,载体的运动状态信息可以是静止状态,也可以是直行状态,还可以是转弯状
    态。

    具体地,可以根据加速度计的输出值,采用公式(1)检测载体是否处于静止状态:

    | Max ( STD ( Accel . X b ) ) - Min ( STD ( Accel . X b ) ) | λ 2 | Max ( STD ( Accel . Y b ) ) - Min ( STD ( Accel . Y b ) ) | λ 2 - - - ( 1 ) ]]>

    式中,λ2为第一阈值,Accel.Xb为时间窗内x轴加速度计的输出值,Accel.Yb为时间
    窗内y轴加速度计的输出值,Max为最大值,Min为最小值,STD为标准差,上角标b为载体坐标
    系;

    本实施例中,取时间窗为1.5s,取λ2=0.02m/s2。

    进一步地,根据陀螺仪的输出值,采用公式(2)检测载体处于直行状态或转弯状
    态:

    ω z b θ ω - - - ( 2 ) ]]>

    式中,为陀螺仪的输出值,θω为第二阈值;

    本实施例中,取θω=0.01rad/s。

    当式(1)成立时,载体处于静止状态;反之,式(1)不成立时,载体处于非静止状态;
    当式(2)成立时,载体处于直行状态;反之,式(2)不成立时,载体处于转弯状态。

    步骤202,根据惯性传感器误差模型和姿态误差四元数更新模型,构建Kalman滤波
    的状态方程。

    具体地,可以取误差四元数的矢量部分qe、陀螺仪随机常值bω和加速度计偏差ba作
    为状态量,构建Kalman滤波的状态方程:

    X · ( t ) = F ( t ) X ( t ) + W ( t ) - - - ( 3 ) ]]>

    F ( t ) = - [ ω ^ b × ] - 0.5 I 0 0 0 0 0 0 0 - - - ( 4 ) ]]>

    W ( t ) = - 0.5 v ω v b ω v b a - - - ( 5 ) ]]>

    其中,X(t)=[qebωba]T∈R9×1为状态量;T为转置;F(t)为状态转移矩阵;W(t)为
    过程噪声;为陀螺仪输出值的反对矩阵;vω为陀螺仪的随机漂移噪声;为陀螺仪
    随机常值模型噪声;为加速度计随机常值模型噪声;

    步骤203,根据地球重力场参考向量和地磁场参考向量在载体坐标系的投影值分
    别与加速度计和磁传感器实测值之差建立量测方程。

    具体地,利用加速度计的输出值与加速度计输出值的估计值的差La以及磁传感器
    输出值和磁传感器输出值的估计值的差Lm分别构建第一量测方程和第二量测方程:

    L a = a ^ b - C n b ( q ^ ) g n = 2 [ C n b ( q ^ ) g n × ] q e + b a + v a - - - ( 6 ) ]]>

    L m = m ^ b - C n b ( q ^ ) m n = 2 [ C n b ( q ^ ) g n × ] q e + v m - - - ( 7 ) ]]>

    其中,为加速度计的输出值;为磁传感器的输出值;为从导航坐标系n到载
    体坐标系b的转化矩阵;为四元数的估计值;gn=[00-g]T为重力加速度g在导航坐标系n
    下的投影;mn=[0cosαsinα]T为理想地磁场输出值的归一化结果;α为磁倾角;va和vm分别
    为加速度计和磁力计的随机误差。

    本实施例中,gn=9.8m/s2。

    步骤204,采用两步观测更新的Kalman滤波算法估计四元数误差和惯性传感器误
    差,通过姿态四元数更新方程实现载体的姿态和航向估计。

    具体地,根据式(6),利用在k时刻的观测值La,k更新k时刻的状态预测值

    X ^ a , k = X k + K a , k ( L a , k - A a , k X k ) - - - ( 8 ) ]]>

    K a , k = Σ X k A a , k T ( A a , k T Σ X k A a , k + R a ) - 1 - - - ( 9 ) ]]>

    Σ 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 - - - ( 10 ) ]]>

    其中,为量测方程式(6)在k时刻的量测矩阵;I为单位阵;
    为在k时刻状态预测值的更新值;Ka,k为增益矩阵;为在k时刻状态预测值的协
    方差矩阵;Ra为加速度计量测噪声的协方差矩阵;为在k时刻状态的协方差矩阵;

    本实施例中,Ra=(500μg)2I。

    利用状态量中的分量更新

    q ^ q ^ q e - - - ( 11 ) ]]>

    q ^ q ^ / | | q ^ | | - - - ( 12 ) ]]>

    其中,符号为四元数乘法;为四元数的归一化;为赋值符号,将等式右侧的
    计算值赋给等式左侧的变量;

    根据式(7),利用在k时刻的量测值Lm,k更新k时刻的获得k时刻的状态估计值
    和状态估计值的协方差矩阵

    X ^ k = X ^ a , k + K m , k ( L m , k - A m , k X ^ a , k ) - - - ( 13 ) ]]>

    Σ 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 P m , k - 1 K m , k T - - - ( 14 ) ]]>

    K m , k = r 3 r 3 T 0 3 × 6 0 6 × 3 0 6 × 6 Σ X m , k A m , k T ( A m , k T Σ X m , k A m , k + P m , k - 1 ) - 1 - - - ( 15 ) ]]>

    Σ X m , k = Σ X ^ a , k ( 1 : 3,1 : 3 ) 0 3 × 6 0 6 × 3 0 6 × 6 - - - ( 16 ) ]]>

    其中,Km,k为k时刻的增益矩阵;Lm,k为k时刻的由磁传感器计算的量测值;Am,k为k时
    刻的量测矩阵;为k时刻磁传感器量测值的等价权矩阵;

    根据式(11)和式(12)利用状态量中的分量来更新并利用来
    计算载体的姿态和航向信息。

    步骤205,根据Kalman滤波的预测残差构建卡方假设检验统计量,利用载体的运动
    状态信息生成不同运动状态下的阈值,对环境磁干扰进行检测。

    具体地,可以根据Kalman滤波的预测残差,采用公式(17)构建卡方假设检验统计
    量:

    λ k = V k T Σ V k V k , λ k ~ χ 2 ( m ) - - - ( 17 ) ]]>

    式中,和分别为Kalman滤波的预测残差和预测残差协方差矩阵,mm为被检测
    观测维数;

    进一步地,根据卡方假设检验统计量,采用公式(17)构建环境磁干扰检测准则:

    λk>TD(18)

    式中,TD为与载体的运动状态信息对应的阈值,即,载体处于静止状态、直行状态
    或转弯状态时,可以分别对应不同的TD。

    本实施例中,载体处于静止状态和直行状态时,TD的取值为7.88;载体处于转弯状
    态时,TD的取值为15。

    当式(18)成立时,存在环境磁干扰;当式(18)不成立时,不存在环境磁干扰。

    步骤206,根据环境磁干扰的检测结果,利用等价权矩阵在线调整磁传感器量测值
    的权重,使计算出的载体姿态和航向信息具有抗差性。

    根据量测方程式(7)计算磁传感器观测值的归一化残差为

    V i , k = ( L mi , k - A mi , k X k ) / | σ i | , i = 1,2,3 - - - ( 19 ) ]]>

    其中,Lmi为量测方程式(7)的第i个测量方程;Ami为量测矩阵
    的第i行;|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线
    元素的平方根;

    本实施例中,Rm=0.001I;

    根据环境磁干扰检测准则和磁传感器量测值的归一化残差构建量测值权重因子


    式中,为在k时刻磁传感器第i个量测值的自适应权重;pki为给定量测值协方差
    矩阵的第i个主对角线元素;c为常数;

    本实施例中,c=1.3-2.0。

    此外,根据载体的运动状态信息,采用公式(21)使用运动状态中的健康残差自适
    应取代受干扰后的残差:

    r = r 1 + r 2 + . . . r n n - - - ( 21 ) ]]>

    式中,为运动状态中的健康残差,[r1,r2,…rn]为载体处于静止状态和直行状态
    下利用滑动时间窗保存的健康残差。

    本发明实施例利用k时刻磁传感器量测值的自适应权重i=1,2,3,构建磁传感
    器量测值的等价权矩阵并实现磁传感器量测值对状态估计值贡献的自适应调整,使姿
    态和航向信息计算具有抗差性。

    本发明的优点在于:将载体的运动状态信息引入到载体的姿态和航向信息计算
    中,根据自适应两步Kalman滤波器预测残差构建卡方假设检验统计量,利用所述车辆运动
    状态感知信息生成不同运动状态下的阈值,对滤波算法中的地磁场量测是否存在干扰进行
    检测;根据磁干扰检测结果,利用等价权矩阵降低磁干扰量测值的权重,当载体处于环境磁
    干扰状态时,利用相同运动状态的健康残差自适应调整干扰的残差,使姿态与航向信息对
    于磁场畸变具有抗差性,为载体提供具有鲁棒性的姿态和航向信息,为实现载体自主运动
    的导航和控制奠定了基础。

    基于上述载体运动状态信息辅助的姿态和航向计算方法,本发明实施例还提供了
    一种姿态和航行参考系统,如图3所示,包括:

    获取???10,用于根据加速度计的输出值和陀螺仪的输出值,获取载体的运动状
    态信息。

    判断???20,用于根据载体的运动状态信息,判断是否存在环境磁干扰;

    第一构建???30,用于在判断???20判断出存在环境磁干扰时,根据磁传感器
    量测值的归一化残差和量测值协方差矩阵,构建磁传感器量测值的自适应权重;

    第二构建???40,用于根据磁传感器量测值的自适应权重,构建磁传感器量测值
    的等价权矩阵;

    第一计算???50,用于根据加速度计量测值、磁传感器量测值和磁传感器量测值
    的等价权矩阵,计算载体姿态信息和航向信息。

    其中,第一计算???50,包括:

    第一更新子???,用于根据所述加速度计量测值更新状态预测值,得到状态更新
    值;

    第二更新子???,用于使用所述状态更新值更新姿态四元数的估计值;

    第三更新子???,用于根据所述磁传感器量测值和所述磁传感器量测值的等价权
    矩阵,更新所述状态更新值,得到状态估计值;

    第四更新子???,用于使用所述状态估计值更新所述姿态四元数的估计值;

    计算子???,用于根据所述姿态四元数的估计值以及从导航坐标系到载体坐标系
    的转化矩阵,计算所述载体姿态信息和航向信息。

    具体地,上述第一更新子???,具体用于根据惯性传感器误差模型和姿态误差四
    元数更新模型,构建Kalman滤波的状态方程;根据所述加速度计的输出值以及根据重力场
    参考矢量估计的所述加速度计的输出值,构建第一量测方程;根据所述Kalman滤波的状态
    方程、所述第一量测方程以及所述加速度计量测值更新状态预测值,得到状态更新值。

    上述第三更新子???,具体用于根据惯性传感器误差模型和姿态误差四元数更新
    模型,构建Kalman滤波的状态方程;根据所述磁传感器的输出值以及根据世界地磁场模型
    估计的所述磁传感器的输出值,构建第二量测方程;根据所述Kalman滤波的状态方程、所述
    第二量测方程、所述磁传感器量测值以及所述磁传感器量测值的等价权矩阵,更新状态更
    新值,得到状态估计值。

    其中,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为加速度计偏差。

    其中,第一量测方程,具体为:

    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为加速度计的随机误
    差。

    相应地,上述第一更新子???,具体用于根据惯性传感器误差模型和姿态误差四
    元数更新模型,构建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时刻的量测矩阵,I为单位阵,Ka,k为增益矩阵,
    为的协方差矩阵,Ra为加速度计量测噪声的协方差矩阵;为k时刻的协方差矩
    阵。

    上述第二更新子???,具体用于采用以下公式,更新姿态四元数的估计值:

    q ^ q ^ q e ]]>

    q ^ q ^ / | | q ^ | | ]]>

    q e = X ^ a , k ( 1 : 3 ) ]]>

    其中,为所述姿态四元数的估计值,为四元数乘法,为的归一化,为赋值
    符号,qe为所述状态更新值的分量,为所述状态更新值。

    其中,上述第二量测方程,具体为:

    L m = m ^ b - C n b ( q ^ ) m n = 2 [ C n b ( q ^ ) g n × ] q e + v m ]]>

    其中,Lm为所述磁传感器的输出值与根据世界地磁场模型估计的所述磁传感器的
    输出值之间的差值,为磁传感器的输出值,为从导航坐标系到载体坐标系的转化矩阵,
    为姿态四元数的估计值,mn为理想地磁场输出值的归一化结果,vm磁传感器的随机误差。

    相应地,上述第三更新子???,具体用于根据惯性传感器误差模型和姿态误差四
    元数更新模型,构建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 P m , k - 1 K m , k T ]]>

    K m , k = r 3 r 3 T 0 3 × 6 0 6 × 3 0 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 ) 0 3 × 6 0 6 × 3 0 6 × 6 ]]>

    r 3 = C n b ( q ^ ) 0 0 1 T ]]>

    其中,为k时刻的状态估计值,为k时刻的状态更新值,Lm,k为k时刻的所述磁
    传感器量测值,为的协方差矩阵,Km,k为k时刻的增益矩阵,Am,k为k时刻的量测矩阵,
    为k时刻的所述磁传感器量测值的等价权矩阵。

    上述第四更新子???,具体用于采用以下公式,更新姿态四元数的估计值:

    q ^ q ^ q e ]]>

    q ^ q ^ / | | q ^ | | ]]>

    q e = X ^ a , k ( 1 : 3 ) ]]>

    其中,为所述姿态四元数的估计值,为四元数乘法,为的归一化,为赋值
    符号,qe为所述状态估计值的分量,为所述状态估计值。

    上述获取???,具体用于判断加速度计的输出值是否满足第一条件:

    | Max ( STD ( Accel . X b ) ) - Min ( STD ( Accel . X b ) ) | λ 2 | Max ( STD ( Accel . Y b ) ) - Min ( STD ( Accel . Y b ) ) | λ 2 ]]>

    其中,λ2为第一阈值,Accel.Xb为时间窗内x轴加速度计的输出值,Accel.Yb为时间
    窗内y轴加速度计的输出值,Max为最大值,Min为最小值,STD为标准差,上角标b为载体坐标
    系;

    如果加速度计的输出值满足第一条件,则确定载体处于静止状态;否则,判断陀螺
    仪的输出值是否满足第二条件:

    ω z b θ ω ]]>

    其中,为陀螺仪的输出值,θω为第二阈值;

    如果陀螺仪的输出值满足第二条件,则确定载体处于直行状态;否则,确定载体处
    于转弯状态。

    上述判断???20,具体用于判断卡方假设检验统计量是否大于与所述载体的运
    动状态信息对应的阈值,如果是,则确定存在环境磁干扰;否则,确定不存在环境磁干扰。

    相应地,上述系统,还包括:

    第三构建???,用于采用以下公式构建卡方假设检验统计量:

    λ k = V k T Σ V k V k , λ k ~ χ 2 ( m ) ]]>

    其中,和分别为Kalman滤波的预测残差和预测残差协方差矩阵,m为被检测观
    测维数。

    上述第一构建???30,具体用于在判断???20判断出存在环境磁干扰时,采用
    以下公式构建所述磁传感器量测值的自适应权重:

    不存在环境磁干扰时;

    存在环境磁干扰时;

    其中,为k时刻所述磁传感器第i个量测值的自适应权重,pki为量测值协方差矩
    阵的第i个主对角线元素,c为常数,Vi′为磁传感器量测值的归一化残差。

    进一步地,上述系统,还包括:

    第二计算???,用于采用以下公式计算所述磁传感器量测值的归一化残差:

    V i , k = ( L mi , k - A mi , k X k ) / | σ i | , i = 1,2,3 ]]>

    其中,Kmi为所述第二量测方程的第i个测量方程,Ami为量测矩阵
    的第i行;|σi|为磁传感器量测噪声协方差矩阵Rm第i个主对角线
    元素的平方根。

    进一步地,上述系统,还包括:

    取代???,用于当存在环境磁干扰时,根据载体的运动状态信息,使用运动状态中
    的健康残差自适应取代受干扰后的残差。

    相应地,上述系统,还包括:

    第三计算???,用于采用以下公式计算运动状态中的健康残差:

    r = r 1 + r 2 + . . . r n n ]]>

    其中,为运动状态中的健康残差,[r1,r2,…rn]为所述载体处于静止状态和直行
    状态下利用滑动时间窗保存的健康残差。

    本发明的优点在于:将载体的运动状态信息引入到载体的姿态和航向信息计算
    中,根据自适应两步Kalman滤波器预测残差构建卡方假设检验统计量,利用所述车辆运动
    状态感知信息生成不同运动状态下的阈值,对滤波算法中的地磁场量测是否存在干扰进行
    检测;根据磁干扰检测结果,利用等价权矩阵降低磁干扰量测值的权重,当载体处于环境磁
    干扰状态时,利用相同运动状态的健康残差自适应调整干扰的残差,使姿态与航向信息对
    于磁场畸变具有抗差性,为载体提供具有鲁棒性的姿态和航向信息,为实现载体自主运动
    的导航和控制奠定了基础。

    在本申请所提供的几个实施例中,应该理解到,所揭露的方法和系统,可以通过其
    它的方式实现。以上所描述的设备实施例仅仅是示意性的,例如,所述单元的划分,仅仅为
    一种逻辑功能划分,实际实现时可以有另外的划分方式,例如:多个单元或组件可以结合,
    或可以集成到另一个系统,或一些特征可以忽略,或不执行。此外,所显示或讨论的各组成
    部分相互之间的耦合、或直接耦合、或通信连接可以是通过一些接口,设备或单元的间接耦
    合或通信连接,可以是电性的、机械的或其它形式的。

    上述作为分离部件说明的单元可以是、或也可以不是物理上分开的,作为单元显
    示的部件可以是、或也可以不是物理单元,即可以位于一个地方,也可以分布到多个网络单
    元上;可以根据实际的需要选择其中的部分或全部单元来实现本实施例方案的目的。

    此外,在本发明各实施例中的各功能单元可以全部集成在一个处理??橹?,也可
    以是各单元分别单独作为一个单元,也可以两个或两个以上单元集成在一个单元中;上述
    集成的单元既可以采用硬件的形式实现,也可以采用硬件加软件功能单元的形式实现。

    本领域普通技术人员可以理解:实现上述方法实施例的全部或部分步骤可以通过
    程序指令相关的硬件来完成,前述的程序可以存储于一计算机可读取存储介质中,该程序
    在执行时,执行包括上述方法实施例的步骤;而前述的存储介质包括:移动存储设备、只读
    存储器(Read-OnlyMemory,ROM)、随机存取存储器(RandomAccessMemory,RAM)、磁碟或
    者光盘等各种可以存储程序代码的介质。

    本发明适用于载体姿态和航向参考系统信息计算。以上所述,仅为本发明的具体
    实施方式,但本发明的?;し段Р⒉痪窒抻诖?,任何熟悉本技术领域的技术人员在本发明
    揭露的技术范围内,可轻易想到变化或替换,都应涵盖在本发明的?;し段е?。因此,本
    发明的?;し段вσ运鋈ɡ蟮谋;し段?。

    以上所述,仅为本发明的具体实施方式,但本发明的?;し段Р⒉痪窒抻诖?,任何
    熟悉本技术领域的技术人员在本发明揭露的技术范围内,可轻易想到变化或替换,都应涵
    盖在本发明的?;し段е?。因此,本发明的?;し段вλ鲆匀ɡ蟮谋;し段?。

    关 键 词:
    一种 载体 运动 状态 信息 辅助 姿态 航向 计算方法 系统
      专利查询网所有资源均是用户自行上传分享,仅供网友学习交流,未经上传用户书面授权,请勿作他用。
    关于本文
    本文标题:一种载体运动状态信息辅助的姿态和航向计算方法和系统.pdf
    链接地址://www.4mum.com.cn/p-5886392.html
    关于我们 - 网站声明 - 网站地图 - 资源地图 - 友情链接 - 网站客服客服 - 联系我们

    [email protected] 2017-2018 www.4mum.com.cn网站版权所有
    经营许可证编号:粤ICP备17046363号-1 
     


    收起
    展开
  • 四川郎酒股份有限公司获第十二届人民企业社会责任奖年度环保奖 2019-05-13
  • 银保监会新规剑指大企业多头融资和过度融资 2019-05-12
  • 韩国再提4国联合申办世界杯 中国网友无视:我们自己来 2019-05-11
  • 中国人为什么一定要买房? 2019-05-11
  • 十九大精神进校园:风正扬帆当有为 勇做时代弄潮儿 2019-05-10
  • 粽叶飘香幸福邻里——廊坊市举办“我们的节日·端午”主题活动 2019-05-09
  • 太原设禁鸣路段 设备在测试中 2019-05-09
  • 拜耳医药保健有限公司获第十二届人民企业社会责任奖年度企业奖 2019-05-08
  • “港独”没出路!“梁天琦们”该醒醒了 2019-05-07
  • 陈卫平:中国文化内涵包含三方面 文化复兴表现在其中 2019-05-06
  • 人民日报客户端辟谣:“合成军装照”产品请放心使用 2019-05-05
  • 【十九大·理论新视野】为什么要“建设现代化经济体系”?   2019-05-04
  • 聚焦2017年乌鲁木齐市老城区改造提升工程 2019-05-04
  • 【专家谈】上合组织——构建区域命运共同体的有力实践者 2019-05-03
  • 【华商侃车NO.192】 亲!楼市火爆,别忘了买车位啊! 2019-05-03