一种姿态解算算法、装置及车载惯性导航系统制造方法及图纸

本发明专利技术适用于导航技术领域，提供了一种姿态解算算法、装置及车载惯性导航系统，所述算法包括：获取载体车辆的速度相关数据；根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。本发明专利技术提供的姿态解算算法通过选择恰当的姿态更新周期可以在保证解算精度的前提下控制计算量。

An attitude calculation algorithm, device and vehicle inertial navigation system

【技术实现步骤摘要】
一种姿态解算算法、装置及车载惯性导航系统
本专利技术属于导航
，尤其涉及一种姿态解算算法、装置及车载惯性导航系统。
技术介绍
惯性导航系统是不依赖外部信息的自主式导航系统，通过监测载体的加速度信息和角速度信息，进行计算后得到载体在导航坐标系中的信息。惯性导航系统的核心是姿态解算算法。目前在惯性导航系统中主要使用的姿态解算算法有四元数法，欧拉角法及方向余弦法等，其中四元数法的计算精度不足；欧拉角法计算过程中三角运算较多，计算量较大；而方向余弦法的计算量也较大。
技术实现思路
有鉴于此，本专利技术实施例提供了一种姿态解算算法、装置及车载惯性导航系统，旨在解决现有技术中惯性导航系统的姿态解算算法在高动态环境下无法兼顾解算精度和计算量的问题。本专利技术实施例第一方面提供了一种姿态解算算法，包括：步骤一：获取载体车辆的速度相关数据；步骤二：根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；步骤三：根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；步骤四：利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。本专利技术实施例第二方面提供了一种基于车载惯性导航系统的姿态解算装置，包括：速度数据获取模块，用于获取载体车辆的速度相关数据；等效旋转矢量计算模块，用于根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；四元数更新计算模块，用于根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；姿态角计算模块，用于利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。本专利技术实施例第三方面提供了一种车载终端，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如上所述方法的各个步骤。本专利技术实施例第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上所述方法的各个步骤。本专利技术实施例第五方面提供了一种车载惯性导航系统，包括：传感器模块、自检模块、电源模块、数据传输模块和如上所述的姿态解算装置；所述自检模块分别与所述传感器模块、所述电源模块、所述数据传输模块和所述姿态解算装置连接；所述电源模块还分别与所述传感器模块、所述数据传输模块和所述姿态解算装置连接；所述传感器模块还与所述姿态解算装置连接，所述姿态解算装置还与所述数据传输模块连接；所述传感器模块用于采集载体车辆的速度相关数据，并将所述速度相关数据输出至所述姿态解算装置；所述姿态解算装置用于根据所述速度相关数据进行姿态解算；所述自检模块用于检测所述车载惯性导航系统是否存在异常；所述电源模块用于为所述车载惯性导航系统供电；所述数据传输模块用于以规范化数据包的格式实现所述姿态解算装置与第三方系统的数据传输。本专利技术实施例与现有技术相比存在的有益效果是：本专利技术实施例提供了一种姿态解算算法、装置及车载惯性导航系统，所述算法包括：获取载体车辆的速度相关数据；根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。本专利技术提供的姿态解算算法通过选择恰当的姿态更新周期可以在保证解算精度的前提下控制计算量。附图说明为了更清楚地说明本专利技术实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本专利技术的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。图1是本专利技术实施例提供的姿态解算算法的流程图；图2是本专利技术实施例提供的姿态解算装置框图；图3是本专利技术实施例提供的车载惯性导航系统的系统框图；图4是本专利技术实施例提供的一种车载终端的示意框图。具体实施方式以下描述中，为了说明而不是为了限定，提出了诸如特定系统结构、技术之类的具体细节，以便透彻理解本专利技术实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本专利技术。在其它情况中，省略对众所周知的系统、装置、电路以及方法的详细说明，以免不必要的细节妨碍本专利技术的描述。为了说明本专利技术的技术方案，下面通过具体实施例来进行说明。参见图1，本专利技术实施例第一方面提供了一种姿态解算算法，包括：S101：获取载体车辆的速度相关数据；S102：根据载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；S103：根据当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；S104：利用当前姿态更新周期的四元数计算载体车辆的姿态角。在本实施例中，姿态更新周期与等效旋转矢量计算周期呈倍数关系。在本专利技术的一个实施例中，在S102之前，本实施例中的姿态解算算法还包括：对速度相关数据进行误差补偿，并对误差补偿后的速度相关数据进行零位偏差修正；相应地，S102包括：根据载体车辆零位偏差修正后的速度相关数据计算当前姿态更新周期的等效旋转矢量。在本实施例中，误差补偿包括零偏补偿、标度因数补偿、垂直度误差补偿、零位和标度因数补偿及灵敏度补偿，实现对数据进行去除噪声、滤除干扰的处理。具体地，零位和标度因数的补偿范围为在-45℃～70℃，采用曲线拟合的方法进行补偿。补偿时每10℃一个点进行采样取点，其中零位补偿采用6阶函数进行线性拟合，标度因数补偿采用四阶函数进行线性拟合。可选的，采用曲线拟合而非插值法进行补偿，由于插值法获得的曲线包括所有提供的数据点坐标，因而在部分点数据误差较大时，将导致较大的逼近误差。而采用曲线拟合可以避免部分样本点的波动，从而可以更好的得到描述样本整体变化规律的曲线。进一步地，曲线拟合一般采用最小二乘法，但是由于MEMS(Micro-Electro-MechanicalSystem，微电子机械系统)器件在高低温实验采样过程中，数据的重复性和精确性都比较差，特别是高动态环境下，因此所得的实验拟合数据存在较为剧烈的变化。例如高低温曲线由于差距较大不能很好的衔接，在某些温度点出现异常点，这些异常情况对最小二乘法的拟合估计存在较大的干扰，因而设计采用稳健拟合。相对其他回归方法而言，稳健拟合受异常值影响较小，在回归分析中可以自动剔除异常值，得到更为稳健的回归系数。在MATLAB中调用robustfit函数作稳健回归，可以针对MEMS器件测试过程中数据变化较大，精度不高的特点，得到更加适配MEMS器件性能的拟合函数曲线，从而更好的实现零位和标度因数的补偿。在本实施例本文档来自技高网...

【技术保护点】
1.一种姿态解算算法，其特征在于，包括：/n步骤一：获取载体车辆的速度相关数据；/n步骤二：根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；/n步骤三：根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；/n步骤四：利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。/n

【技术特征摘要】
1.一种姿态解算算法，其特征在于，包括：
步骤一：获取载体车辆的速度相关数据；
步骤二：根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；
步骤三：根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；
步骤四：利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。


2.如权利要求1所述的姿态解算算法，其特征在于，在所述步骤二之前，所述姿态解算算法还包括：
对所述速度相关数据进行误差补偿，并对所述误差补偿后的速度相关数据进行零位偏差修正；
相应地，所述步骤二包括：
根据所述载体车辆零位偏差修正后的速度相关数据计算当前姿态更新周期的等效旋转矢量。


3.如权利要求1所述的姿态解算算法，其特征在于，所述速度相关数据包括角速度数据，所述步骤二包括：
根据等效旋转矢量微分方程，对当前等效旋转矢量计算周期上的角速度数据进行二次抛物线拟合，得到拟合方程；
根据所述拟合方程，计算所述当前等效旋转矢量计算周期的等效旋转矢量及当前姿态更新周期的等效旋转矢量。


4.如权利要求1所述的姿态解算算法，其特征在于，步骤三包括：
根据所述当前姿态更新周期的等效旋转矢量构造所述当前姿态更新周期对应的姿态变化四元数；
根据所述当前姿态更新周期对应的姿态变化四元数及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数。


5.如权利要求1所述的姿态解算算法，其特在于，步骤四包括：
根据所述当前姿态更新周期的四元数，确定所述当前姿态更新周期的姿态变换矩阵；
根据所述当前姿态更新周期的姿态变换矩阵，确定所述当前姿态更新周期对应的姿态角。


6.如权利要求1所述的姿态解算算法，其特征在于，所述速度相关数据包括角速度数据和加速度数据，所述姿态解算算法还包括：
根据所述载体车辆的角速度数据和加速度数据，确定当前导航坐标更新周期的旋转矢量；
根据所述当前导航坐标更新周期的旋转矢量，确定所述当前导航坐标更新周期的姿态变换矩阵；

【专利技术属性】
技术研发人员：谭广艺，张高举，付迪，刘健，
申请(专利权)人：河北美泰电子科技有限公司，
类型：发明
国别省市：河北;13