一种捷联惯性导航的误差校正方法及装置制造方法及图纸

本发明专利技术涉及导航定位技术领域，公开了一种捷联惯性导航的误差校正方法及装置，通过对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据，根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率，将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据，根据上一次的第二姿态数据与当前的第二姿态数据，计算第二姿态变化率，跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，并通过所述第三姿态数据初始化陀螺仪姿态，以校正陀螺仪积分误差。本发明专利技术解算的终端姿态能避免陀螺仪因长时间运动模式积分误差带来的终端姿态失效问题。

【技术实现步骤摘要】
一种捷联惯性导航的误差校正方法及装置
本专利技术涉及导航定位
，尤其涉及一种捷联惯性导航的误差校正方法及装置。
技术介绍
捷联惯性导航系统(StrapdownInertialNavigationSystem简称SINS)是根据牛顿提出的相对惯性空间的力学定律获得载体导航信息的装置。它利用磁力计、陀螺仪、加速度计等惯性元件感受运行体运动信息，然后通过计算机进行积分运算得到运动体的姿态、速度和位置等导航参数。捷联惯性导航可实现无外部全球定位信号如GPS、北斗，或其他定位技术(如wifi，RFID)时，仅仅通过载体对相关传感器采集数据的计算进行自主导航。SINS信息丰富，信号频率高，工作方式隐蔽，但SINS中磁力计容易受到外部磁场干扰，且响应水平方向旋转迟缓，无法实时准确测定终端水平方向；受陀螺仪漂移误差累积的影响，系统精度会随着时间的推移而降低；加速度计运动状态下，因外部作用力的影响，难以确定姿态；现有技术中，采用融合姿态解算方法提高精确度，例如：1、通过加速度计与磁力计，计算方向角，与陀螺仪计算方向角进行卡尔曼滤波获得滤波后姿态。2、通过加速度计与磁力计，计算方向角，与陀螺仪计算方向角进行一阶、二阶互补滤波后姿态。3、采用低通滤波和高通滤波的结合方式，以实现加速度计、磁力计、与陀螺仪的参数融合解算。以上几种方案能获得矫正后姿态，最终姿态收敛，但中间姿态可能出现较大误差，如外部磁场干扰，或移动终端持续单向加速将对中间姿态产生较大误差。以上几种滤波方式，主要考虑对白噪声，以及频率特性进行滤波，未考虑移动终端的运动特性引入的随机干扰。因此，基于以上算法获得的姿态，进行运动加速度积分得到的运动轨迹存在较大误差。
技术实现思路
本专利技术提供一种捷联惯性导航的误差校正方法及装置，解决现有技术中运动特性引入的随机干扰导致运动加速度积分得到的运动轨迹存在较大误差的技术问题。本专利技术的目的是通过以下技术方案实现的：一种捷联惯性导航的误差校正方法，包括：对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据；根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率；将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据；根据上一次的第二姿态数据与当前的第二姿态数据，计算第二姿态变化率；跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，并通过所述第三姿态数据初始化陀螺仪姿态，以校正陀螺仪积分误差，其中，所述姿态可信度由所述偏差率及修正参数计算获得。一种捷联惯性导航的误差校正装置，包括：第一姿态计算模块，用于对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据；第一姿态变化率计算模块，用于根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率；第二姿态计算模块，用于将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据；第二姿态变化率计算模块，用于根据上一次的第二姿态数据与当前的第二姿态数据，计算第二姿态变化率；校正模块，用于跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，并通过所述第三姿态数据初始化陀螺仪姿态，以校正陀螺仪积分误差，其中，所述姿态可信度由所述偏差率及修正参数计算获得。本专利技术提供一种捷联惯性导航的误差校正方法及装置，通过对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据，根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率，将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据，根据上一次的第二姿态数据与当前的第二姿态数据，计算第二姿态变化率，跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，并通过所述第三姿态数据初始化陀螺仪姿态，以校正陀螺仪积分误差。本专利技术解算的终端姿态能避免陀螺仪因长时间运动模式积分误差带来的终端姿态失效问题。附图说明为了更清楚地说明本专利技术实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可根据这些附图获得其他的附图。图1为本专利技术实施例的一种捷联惯性导航的误差校正方法的流程图；图2为本专利技术实施例的偏差率的周期性示意图；图3为本专利技术实施例的一种捷联惯性导航的误差校正装置的结构示意图。具体实施方式为使本专利技术的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本专利技术作进一步详细的说明。如图1所示，为一种捷联惯性导航的误差校正方法，包括：步骤101、对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据；其中，步骤101具体可以包括：步骤101-1、计算第一磁力计矢量及加速度计矢量；其中，磁力计测量值＝(Xm,Ym,Zm)，磁力计模磁力计矢量(归一化)＝(Xm,Ym,Zm)/M＝(Xm1,Ym1,Zm1)；加速度计测量值＝(Xa,Ya,Za)，加速度计模加速度计矢量(归一化)＝(Xa,Ya,Za)/N＝(Xa1,Ya1,Za1)。步骤101-2、计算水平矢量，所述水平矢量等于所述第一磁力计矢量乘以所述加速度计矢量；其中，水平矢量＝磁力计矢量*加速度计矢量＝(Ym1*Za1–Zm1*Ya1,Zm1*Xa1–Xm1*Za1,Xm1*Ya1–Ym1*Xa1)＝(Xh,Yh,Zh)；水平矢量模水平矢量(归一化)＝(Xh,Yh,Zh)/L＝(Xh1,Yh1,Zh1)。步骤101-3、计算第二磁力计矢量，所述第二磁力计矢量等于所述加速度计矢量乘以所述水平矢量；其中，第二磁力计矢量＝(Xa1,Ya1,Za1)*(Xh1,Yh1,Zh1)步骤101-4、根据所述水平矢量、所述第二磁力计矢量及所述加速度计矢量，计算第一姿态矩阵；其中，步骤101-5、根据所述第一姿态矩阵，计算终端的第一姿态数据。其中，获得旋转坐标values[0]＝arctan2(Yh1,Ym2)＝θ，values[1]＝arcsin(-Ya1)＝α，values[2]＝arctan2(-Xa1,Za1)＝β，values[0]为围绕Z轴旋转，为方位角又称地平经度；values[1]为围绕X轴旋转，叫做俯仰角；values[2]为围绕Y轴旋转。步骤102、根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率；其中，步骤102具体可以包括：步骤102-1、获取终端的角度轴数据，并进行归一化；其中，θ轴：终端y轴向长度h，α轴：终端y轴向长度h，β轴：终端y轴向长度l，归一化角度轴为(θr，αr,βr)。步骤102-2、根据上一次的第一姿态数据与当前的第一姿态数据，计算角度差值；其中，上一次的第一姿态数据为(θ1,α1,β1)，当前的第一姿态数据为(θ2,α2,β2)，角度差值为(Δθ，Δα，Δβ)。步骤102-3、根据归一化的所述角度轴数据、所述角度差值、上一次的第一姿态数据及当前的第一姿态数据，计算第一姿态变化率。其中本文档来自技高网...

【技术保护点】
1.一种捷联惯性导航的误差校正方法，其特征在于，包括：对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据；根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率；将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据；根据上一次的第二姿态数据与当前的第二姿态数据，计算第二姿态变化率；跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，并通过所述第三姿态数据初始化陀螺仪姿态，以校正陀螺仪积分误差，其中，所述姿态可信度由所述偏差率及修正参数计算获得。

【技术特征摘要】
1.一种捷联惯性导航的误差校正方法，其特征在于，包括：对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据；根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率；将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据；根据上一次的第二姿态数据与当前的第二姿态数据，计算第二姿态变化率；跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，并通过所述第三姿态数据初始化陀螺仪姿态，以校正陀螺仪积分误差，其中，所述姿态可信度由所述偏差率及修正参数计算获得。2.根据权利要求1所述的捷联惯性导航的误差校正方法，其特征在于，对加速度计，磁力计进行姿态解算，以获取终端的第一姿态数据，包括：计算第一磁力计矢量及加速度计矢量；计算水平矢量，所述水平矢量等于所述第一磁力计矢量乘以所述加速度计矢量；计算第二磁力计矢量，所述第二磁力计矢量等于所述加速度计矢量乘以所述水平矢量；根据所述水平矢量、所述第二磁力计矢量及所述加速度计矢量，计算第一姿态矩阵；根据所述第一姿态矩阵，计算终端的第一姿态数据。3.根据权利要求1所述的捷联惯性导航的误差校正方法，其特征在于，所述根据上一次的第一姿态数据与当前的第一姿态数据，计算第一姿态变化率，包括：获取终端的角度轴数据，并进行归一化；根据上一次的第一姿态数据与当前的第一姿态数据，计算角度差值；根据归一化的所述角度轴数据、所述角度差值、上一次的第一姿态数据及当前的第一姿态数据，计算第一姿态变化率。4.根据权利要求1所述的捷联惯性导航的误差校正方法，其特征在于，所述将所述第一姿态数据作为陀螺仪的初始姿态，采用四元数进行陀螺仪姿态解算，以获取第二姿态数据，包括：根据所述第一姿态数据，计算第二姿态矩阵；计算陀螺仪的四元数，并根据所述四元数进行陀螺仪姿态解算，以获取第三姿态矩阵；根据所述第三姿态矩阵对所述第二姿态矩阵进行更新，以获取第二姿态数据。5.根据权利要求1所述的捷联惯性导航的误差校正方法，其特征在于，所述跟踪偏差率的周期性变化，当所述偏差率达到极值时，根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，计算第三姿态数据，包括：根据所述第一姿态变化率、所述第二姿态变化率和姿态偏差率，计算偏差率；跟踪偏差率的周期性变化，当所述偏差率达到极大值或极小值时，根据所述偏差率及修正参数，计算姿态可信度；根据所述第一姿态数据、所述第二姿态数据及所述姿态可信度，进行融合解算，以获取第三姿态数据。6.一种捷联惯性导航的误差校正装置，其特征在于，包括：第一姿态计算模块，用于对加速度计，磁力计进行姿态解算，以获取终端的第一姿...

【专利技术属性】
技术研发人员：汪勇，胡祝银，林威，胡树立，章小兵，
申请(专利权)人：深圳市博安智控科技有限公司，
类型：发明
国别省市：广东,44