CN110879066A - 一种姿态解算算法、装置及车载惯性导航*** - Google Patents

Abstract

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

Description

一种姿态解算算法、装置及车载惯性导航***

技术领域

本发明属于导航技术领域，尤其涉及一种姿态解算算法、装置及车载惯性导航***。

背景技术

惯性导航***是不依赖外部信息的自主式导航***，通过监测载体的加速度信息和角速度信息，进行计算后得到载体在导航坐标系中的信息。惯性导航***的核心是姿态解算算法。

目前在惯性导航***中主要使用的姿态解算算法有四元数法，欧拉角法及方向余弦法等，其中四元数法的计算精度不足；欧拉角法计算过程中三角运算较多，计算量较大；而方向余弦法的计算量也较大。

发明内容

有鉴于此，本发明实施例提供了一种姿态解算算法、装置及车载惯性导航***，旨在解决现有技术中惯性导航***的姿态解算算法在高动态环境下无法兼顾解算精度和计算量的问题。

本发明实施例第一方面提供了一种姿态解算算法，包括：

步骤一：获取载体车辆的速度相关数据；

步骤二：根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；

步骤三：根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；

步骤四：利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。

本发明实施例第二方面提供了一种基于车载惯性导航***的姿态解算装置，包括：

速度数据获取模块，用于获取载体车辆的速度相关数据；

等效旋转矢量计算模块，用于根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；

四元数更新计算模块，用于根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；

姿态角计算模块，用于利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。

本发明实施例第三方面提供了一种车载终端，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如上所述方法的各个步骤。

本发明实施例第四方面提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如上所述方法的各个步骤。

本发明实施例第五方面提供了一种车载惯性导航***，包括：传感器模块、自检模块、电源模块、数据传输模块和如上所述的姿态解算装置；

所述自检模块分别与所述传感器模块、所述电源模块、所述数据传输模块和所述姿态解算装置连接；所述电源模块还分别与所述传感器模块、所述数据传输模块和所述姿态解算装置连接；所述传感器模块还与所述姿态解算装置连接，所述姿态解算装置还与所述数据传输模块连接；

所述传感器模块用于采集载体车辆的速度相关数据，并将所述速度相关数据输出至所述姿态解算装置；

所述姿态解算装置用于根据所述速度相关数据进行姿态解算；

所述自检模块用于检测所述车载惯性导航***是否存在异常；

所述电源模块用于为所述车载惯性导航***供电；

所述数据传输模块用于以规范化数据包的格式实现所述姿态解算装置与第三方***的数据传输。

本发明实施例与现有技术相比存在的有益效果是：

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

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1是本发明实施例提供的姿态解算算法的流程图；

图2是本发明实施例提供的姿态解算装置框图；

图3是本发明实施例提供的车载惯性导航***的***框图；

图4是本发明实施例提供的一种车载终端的示意框图。

具体实施方式

以下描述中，为了说明而不是为了限定，提出了诸如特定***结构、技术之类的具体细节，以便透彻理解本发明实施例。然而，本领域的技术人员应当清楚，在没有这些具体细节的其它实施例中也可以实现本发明。在其它情况中，省略对众所周知的***、装置、电路以及方法的详细说明，以免不必要的细节妨碍本发明的描述。

为了说明本发明的技术方案，下面通过具体实施例来进行说明。

参见图1，本发明实施例第一方面提供了一种姿态解算算法，包括：

S101：获取载体车辆的速度相关数据；

S102：根据载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；

S103：根据当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；

S104：利用当前姿态更新周期的四元数计算载体车辆的姿态角。

在本实施例中，姿态更新周期与等效旋转矢量计算周期呈倍数关系。

在本发明的一个实施例中，在S102之前，本实施例中的姿态解算算法还包括：

对速度相关数据进行误差补偿，并对误差补偿后的速度相关数据进行零位偏差修正；

相应地，S102包括：

根据载体车辆零位偏差修正后的速度相关数据计算当前姿态更新周期的等效旋转矢量。

在本实施例中，误差补偿包括零偏补偿、标度因数补偿、垂直度误差补偿、零位和标度因数补偿及灵敏度补偿，实现对数据进行去除噪声、滤除干扰的处理。

具体地，零位和标度因数的补偿范围为在-45℃～70℃，采用曲线拟合的方法进行补偿。补偿时每10℃一个点进行采样取点，其中零位补偿采用6阶函数进行线性拟合，标度因数补偿采用四阶函数进行线性拟合。可选的，采用曲线拟合而非插值法进行补偿，由于插值法获得的曲线包括所有提供的数据点坐标，因而在部分点数据误差较大时，将导致较大的逼近误差。而采用曲线拟合可以避免部分样本点的波动，从而可以更好的得到描述样本整体变化规律的曲线。

进一步地，曲线拟合一般采用最小二乘法，但是由于MEMS(Micro-Electro-Mechanical System，微电子机械***)器件在高低温实验采样过程中，数据的重复性和精确性都比较差，特别是高动态环境下，因此所得的实验拟合数据存在较为剧烈的变化。例如高低温曲线由于差距较大不能很好的衔接，在某些温度点出现异常点，这些异常情况对最小二乘法的拟合估计存在较大的干扰，因而设计采用稳健拟合。相对其他回归方法而言，稳健拟合受异常值影响较小，在回归分析中可以自动剔除异常值，得到更为稳健的回归系数。在MATLAB中调用robustfit函数作稳健回归，可以针对MEMS器件测试过程中数据变化较大，精度不高的特点，得到更加适配MEMS器件性能的拟合函数曲线，从而更好的实现零位和标度因数的补偿。

在本实施例中，零位偏差修正的步骤为，通过加速度数据和角速度数据得到零位信息，通过卡尔曼滤波实现对零位的实时修正。

在本发明的一个实施例中，速度相关数据包括角速度数据，S102包括：

根据等效旋转矢量微分方程，对当前等效旋转矢量计算周期上的角速度数据进行二次抛物线拟合，得到拟合方程；

根据拟合方程，计算当前等效旋转矢量计算周期的等效旋转矢量及当前姿态更新周期的等效旋转矢量。

在本实施例中，角增量拟合采样周期为t,等效旋转矢量计算周期为T，姿态更新周期为h。在一个姿态变换四元数对应的等效旋转矢量计算周期T内，为了减少姿态解算算法的漂移，采用多子样算法，选取的子样数越多，计算越精确，但同时计算量也越大。在每个姿态更新周期h内更新由于载体本身的姿态变化导致的载体坐标系和导航坐标系之间的姿态变化矩阵，针对计算量较小的等效旋转矢量求解，采用高频次更新，针对计算量较大的姿态角更新四元数解算，采用低频次更新，其更新频率依据载体车辆的行驶速度设定，从而在保证计算精度的基础上控制计算量。

可选的，T＝3t，h＝2T。

在本实施例中，根据等效旋转矢量微分方程，对当前等效旋转矢量计算周期，即时间区间[t_k-1,t_k-1+T]上的角速度数据进行曲线拟合，曲线的次数由等效旋转矢量子样数确定，由于T＝3t,即为三字样等效旋转矢量，使用二次抛物线拟合，得到拟合方程如式(1)所示；

式(1)中，t为角增量拟合采样周期；t_K为第k个角增量拟合采样周期；τ为微小的时间增量；n代表导航坐标系，b代表载体车辆坐标系，

为在第k个角增量拟合采样周期后的时间增量τ处，载体车辆相对导航坐标系姿态变换角增量在载体车辆坐标系轴向的分量；

和

分别为角速度拟合方程的各阶次系数。

在本实施例中，将姿态更新周期，即时间区间[t_k-1,t_k-1+h]均分为3个时间区间，根据式(1)角速度拟合方程，得到角度增量计算方程如式(2)所示：

式(2)中，

为在姿态更新周期的第i个时间区间内的角度增量，

为式(1)所示的在第k个角增量拟合采样周期后的时间增量τ处，载体车辆相对导航坐标系姿态变换角增量在载体车辆坐标系轴向的分量。

具体的，将式(1)代入式(2)计算分别计算当前姿态更新周期内第i个时间区间内的角度增量

可得:

根据当前姿态更新周期内各个区间内的角度增量，得到当前等效旋转矢量计算周期对应的等效旋转矢量计算公式如式(3)所示：

式(3)中，

为当前等效旋转矢量计算周期对应的等效旋转矢量，

为分别式(2)所示的当前姿态更新周期内三个时间区间内的角度增量。

同样的，计算两个等效旋转矢量计算周期2T内，即一个姿态更新周期h内的等效旋转矢量

在本发明的一个实施例中，S103包括：

根据当前姿态更新周期的等效旋转矢量构造当前姿态更新周期对应的姿态变化四元数；

根据当前姿态更新周期对应的姿态变化四元数及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数。

在本实施例中，根据当前姿态更新周期内的等效旋转矢量构造当前姿态更新周期对应的姿态变化四元数，得到姿态变化四元数计算公式如式(4)所示；

式(4)中，

为姿态变化四元数，

为一个姿态更新周期h内的等效旋转矢量。

根据

及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数的公式如式(5)所示:

式(5)中，

为当前姿态更新周期的四元数，

为前一姿态更新周期的四元数，

为姿态变化四元数。

在本发明的一个实施例中，S104包括：

根据当前姿态更新周期的四元数，确定当前姿态更新周期的姿态变换矩阵；

根据当前姿态更新周期的姿态变换矩阵，确定当前姿态更新周期对应的姿态角。

在本实施例中，由当前姿态更新周期的四元数计算出当前姿态更新周期的姿态变换矩阵A为：

式(6)中，A_xy为当前姿态更新周期的姿态变换矩阵A中下标为xy的元素，q₀、q₁、q₂、q₃为当前姿态更新周期内姿态变化四元数

的四个分量。

在本实施例中，当前姿态更新周期对应的姿态角包括俯仰角θ，偏航角ψ和横滚角γ，由式(7)和表1计算：

式(7)和表1中，θ为俯仰角，γ_主为横滚角的计算中间值，ψ_主为偏航角的计算中间值，A_xy为当前姿态更新周期的姿态变换矩阵A中下标为xy的元素，由式(6)提供。

其中俯仰角由式(7)直接计算，偏航角和横滚角由表1提供的真值表计算。

表1

在本发明的一个实施例中，速度相关数据包括角速度数据和加速度数据，姿态解算算法还包括：

根据载体车辆的角速度数据和加速度数据，确定当前导航坐标更新周期的旋转矢量；

根据当前导航坐标更新周期的旋转矢量，确定当前导航坐标更新周期的姿态变换矩阵；

根据当前导航坐标更新周期的姿态变换矩阵，计算载体车辆坐标系和导航坐标系在当前导航坐标更新周期内的转换矩阵；

根据载体车辆坐标系和导航坐标系在当前导航坐标更新周期内的转换矩阵，对当前导航坐标更新周期内的导航坐标系进行更新。

在本实施例中，导航坐标系的更新周期为D，由于载体车辆姿态变化相比，导航坐标系的变化缓慢，所以导航坐标更新周期D相较姿态周期更长，可依据车辆行驶速度选择姿态更新周期的若干倍。

在一个导航坐标更新周期D内，导航坐标系等效旋转矢量由式(8)计算

式(8)中，ηⁿ为导航坐标系的等效旋转矢量；Δλ为经度增量，ΔL为纬度增量，Δλ和ΔL分别由角速度数据和加速度数据计算得到；L为纬度。

对应的，由j_k-1时刻导航坐标系到j_k-1+D时刻导航坐标系的姿态变换矩阵由式(9)计算：

式(9)中，

为由j_k-1时刻导航坐标系到j_k-1+D时刻导航坐标系的姿态变换矩阵；Δλ为经度增量，ΔL为纬度增量，Δλ和ΔL分别由角速度数据和加速度数据计算得到；L为纬度。

j_k-1+D时刻载体坐标系到j_k-1时刻导航坐标系的转换矩阵由式(10)计算；

式(10)中，

为由姿态四元数计算j_k-1+D时刻载体坐标系到j_k-1时刻导航坐标系的转换矩阵，

为当前姿态更新周期的姿态变换矩阵，A_xy为当前姿态更新周期的姿态变换矩阵A中下标为xy的元素，由式(6)提供。

在j_k-1+D时刻由载体坐标系向导航坐标系转换的转换矩阵由式(11)计算：

式(11)中，

为在j_k-1+D时刻由载体坐标系向导航坐标系转换的转换矩阵；

为由j_k-1时刻导航坐标系到j_k-1+D时刻导航坐标系的姿态变换矩阵，由式(9)计算；

为j_k-1+D时刻载体坐标系到j_k-1时刻导航坐标系的转换矩阵，由式(10)计算。

由载体车辆坐标系和导航坐标系的转换矩阵完成对导航坐标系的更新。

在本发明实施例中，实际应用时，低动态环境下的姿态解算效果与传统的四元数算法精度相当，但在高动态环境下，本发明实施例在算法复杂程度稍有增加的情况下，解算精度比传统的四元数法的解算精度提高了一个数量级。本发明实施例提供的车载惯性导航***适用于高冲击高动态环境下的汽车等运输工具，用以提供高精度的导航数据。在实际跑车测试中，本发明实施例的改进姿态解算算法在高动态环境下，相对于常规毕卡法求解四元数算法，能够减小输出的姿态角误差，同时偏航角、俯仰角和横滚角的精度可以提高一个数量级，从5°左右提升至0.5°左右。

参照图2，图2示出了本发明实施例提供的基于车载惯性导航***的姿态解算装置100的结构，包括：

速度数据获取模块110，用于实时获取载体车辆的速度相关数据；

等效旋转矢量计算模块120，用于根据载体车辆在当前等效旋转矢量计算周期的速度相关数据计算当前等效旋转矢量计算周期对应的等效旋转矢量；

四元数更新计算模块130，用于根据当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，对四元数进行更新，姿态更新周期与等效旋转矢量计算周期呈倍数关系；

姿态角计算模块140，用于利用当前姿态更新周期的四元数确定姿态变换矩阵，并根据姿态变换矩阵计算载体车辆的姿态角。

姿态解算装置还包括补偿模块150，补偿模块150用于对速度相关数据进行误差补偿，并对误差补偿后的速度相关数据进行零位偏差修正；将零位偏差修正后的速度相关数据输入等效旋转矢量计算计算模块120。

在一个实施例中，图2中的等效旋转矢量计算模块120包括：

拟合单元，用于根据等效旋转矢量微分方程，对当前等效旋转矢量计算周期上的角速度数据进行二次抛物线拟合，得到拟合方程；

等效旋转矢量计算单元，用于根据拟合方程，计算当前等效旋转矢量计算周期的等效旋转矢量及当前姿态更新周期的等效旋转矢量。

在一个实施例中，图2中的四元数更新计算模块130包括：

姿态变化四元数构造单元，用于根据当前姿态更新周期的等效旋转矢量构造当前姿态更新周期对应的姿态变化四元数；

四元数计算单元，用于根据当前姿态更新周期对应的姿态变化四元数及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数。

在一个实施例中，图2中的姿态角计算模块140包括：

姿态变换矩阵计算单元，用于根据当前姿态更新周期的四元数，确定当前姿态更新周期的姿态变换矩阵；

姿态角计算单元，用于根据当前姿态更新周期的姿态变换矩阵，确定当前姿态更新周期对应的姿态角。

在一个实施例中，姿态解算装置100还包括：

导航坐标系更新周期旋转矢量计算单元，用于根据载体车辆的角速度数据和加速度数据，确定当前导航坐标更新周期的旋转矢量；

导航坐标系更新周期的姿态变换矩阵计算单元，用于根据当前导航坐标更新周期的旋转矢量，确定当前导航坐标更新周期的姿态变换矩阵；

坐标系转换矩阵计算单元，用于根据当前导航坐标更新周期的姿态变换矩阵，计算载体车辆坐标系和导航坐标系在当前导航坐标更新周期内的转换矩阵；

导航坐标系更新计算单元，用于根据载体车辆坐标系和导航坐标系在当前导航坐标更新周期内的转换矩阵，对当前导航坐标更新周期内的导航坐标系进行更新。

参见图3，本发明实施例提供了一种车载惯性导航***，包括：传感器模块200、自检模块500、电源模块400、数据传输模块300和上述姿态解算装置100；

自检模块500分别与传感器模块200、电源模块400、数据传输模块300和姿态解算装置100连接；电源模块400还分别与传感器模块200、数据传输模块300和姿态解算装置100连接；传感器模块200还与姿态解算装置100连接，姿态解算装置100还与数据传输模块300连接；

传感器模块200用于采集载体车辆的速度相关数据，并将速度相关数据输出至姿态解算装置100；

姿态解算装置100用于根据速度相关数据进行姿态解算；

自检模块500用于检测车载惯性导航***是否存在异常；

电源模块400用于为车载惯性导航***供电；

数据传输模块300用于以规范化数据包的格式实现姿态解算装置与第三方***的数据传输。

在本实施例中，传感器模块200包括陀螺仪和加速度计；

陀螺仪用于检测载体车辆在三个轴向上的角速度数据；

加速度计用于检测载体车辆在三个轴向上的加速度数据。

可选的，陀螺仪为三个，包括X轴陀螺仪、Y轴陀螺仪和Z轴陀螺仪，分别用于检测三个轴向的角速度信息；

可选的，加速度计为三个，分别用于检测三个轴向的加速度信息；

可选的，陀螺仪为一个三轴陀螺仪，用于检测三个轴向的角速度信息；

可选的，加速度计为一个三轴加速度计，用于检测三个轴向的加速度信息。

在本实施例中，陀螺仪和加速度计直接固连在载体车辆上，陀螺仪和加速度计输出采用速度增量和角增量的形式，以降低陀螺仪和加速度计的输出噪声对***精度的影响。

在本实施例中，电源模块404采用低功耗直流结构，其中为传感器模块401供电时，先将28V直流电源转换为12V电源，再将正12V电源转换成正5V电源，为传感器模块401提供5V的电压偏置。

在本实施例中，自检模块500的功能具体包括对传感器异常、工作电压异常、超量程、大过载等情况的告警和异常处理。

在本实施例中，车载惯性导航***还包括操作维护模块，操作维护模块用于实现版本查询、标定数据录入、固件烧录等功能。

在本实施例中，车载惯性导航***还包括信号转换模块，信号转换模块用于将传感器模块200获取的加速度数据和角速度数据转化为数字信号，并将转换后的角速度数据和加速度数据传输至姿态解算装置100。

在本实施例中，车载惯性导航***内的传感器模块200和信号转换模块通过螺钉固定于壳体，其中信号转换模块为PCB板，螺钉的固定位置通过模态分析和仿真实验确定。同时在传感器模块200的芯片周围设置了橡胶减震部分，橡胶材质硬度通过仿真实验进行优化选取。通过这些手段保证了车载惯性导航***的抗高冲击的能力。

图4是本发明一实施例提供的车载终端的示意图。如图4所示，该实施例的车载终端40包括：处理器41、存储器42以及存储在所述存储器42中并可在所述处理器41上运行的计算机程序43。所述处理器41执行所述计算机程序43时实现上述方法实施例中的步骤，例如图1所示的步骤S101至S104。或者，所述处理器41执行所述计算机程序43时实现上述各装置实施例中各模块/单元的功能，例如图2所示模块110至140的功能。

示例性的，所述计算机程43可以被分割成一个或多个模块/单元，所述一个或者多个模块/单元被存储在所述存储器42中，并由所述处理器41执行，以完成本发明。所述一个或多个模块/单元可以是能够完成特定功能的一系列计算机程序指令段，该指令段用于描述所述计算机程序43在所述车载终端40中的执行过程。

所述车载终端40可以是桌上型计算机、笔记本、掌上电脑及云端服务器等计算设备。所述车载终端可包括，但不仅限于，处理器41、存储器42。本领域技术人员可以理解，图4仅仅是车载终端40的示例，并不构成对车载终端40的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件，例如所述车载终端还可以包括输入输出设备、网络接入设备、总线等。

所称处理器41可以是中央处理单元(Central Processing Unit，CPU)，还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application Specific Integrated Circuit，ASIC)、现成可编程门阵列(Field-Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

所述存储器42可以是所述车载终端40的内部存储单元，例如车载终端40的硬盘或内存。所述存储器42也可以是所述车载终端40的外部存储设备，例如所述车载终端40上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(Secure Digital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器42还可以既包括所述车载终端40的内部存储单元也包括外部存储设备。所述存储器42用于存储所述计算机程序以及所述车载终端所需的其他程序和数据。所述存储器42还可以用于暂时地存储已经输出或者将要输出的数据。

本发明实施例提供了一种计算机可读存储介质，计算机可读存储介质存储有计算机程序，计算机程序被处理器执行时实现上述方法的步骤。

所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述***中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。

本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个***，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。

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

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。所述计算机可读介质可以包括：能够携带所述计算机程序代码的任何实体或装置、记录介质、U盘、移动硬盘、磁碟、光盘、计算机存储器、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、电载波信号、电信信号以及软件分发介质等。需要说明的是，所述计算机可读介质包含的内容可以根据司法管辖区内立法和专利实践的要求进行适当的增减，例如在某些司法管辖区，根据立法和专利实践，计算机可读介质不包括电载波信号和电信信号。

以上所述实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围，均应包含在本发明的保护范围之内。

Claims (10)

1.一种姿态解算算法，其特征在于，包括：

步骤一：获取载体车辆的速度相关数据；

步骤二：根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；

步骤三：根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；

步骤四：利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。

2.如权利要求1所述的姿态解算算法，其特征在于，在所述步骤二之前，所述姿态解算算法还包括：

对所述速度相关数据进行误差补偿，并对所述误差补偿后的速度相关数据进行零位偏差修正；

相应地，所述步骤二包括：

根据所述载体车辆零位偏差修正后的速度相关数据计算当前姿态更新周期的等效旋转矢量。

3.如权利要求1所述的姿态解算算法，其特征在于，所述速度相关数据包括角速度数据，所述步骤二包括：

根据等效旋转矢量微分方程，对当前等效旋转矢量计算周期上的角速度数据进行二次抛物线拟合，得到拟合方程；

根据所述拟合方程，计算所述当前等效旋转矢量计算周期的等效旋转矢量及当前姿态更新周期的等效旋转矢量。

4.如权利要求1所述的姿态解算算法，其特征在于，步骤三包括：

根据所述当前姿态更新周期的等效旋转矢量构造所述当前姿态更新周期对应的姿态变化四元数；

根据所述当前姿态更新周期对应的姿态变化四元数及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数。

5.如权利要求1所述的姿态解算算法，其特在于，步骤四包括：

根据所述当前姿态更新周期的四元数，确定所述当前姿态更新周期的姿态变换矩阵；

根据所述当前姿态更新周期的姿态变换矩阵，确定所述当前姿态更新周期对应的姿态角。

6.如权利要求1所述的姿态解算算法，其特征在于，所述速度相关数据包括角速度数据和加速度数据，所述姿态解算算法还包括：

根据所述载体车辆的角速度数据和加速度数据，确定当前导航坐标更新周期的旋转矢量；

根据所述当前导航坐标更新周期的旋转矢量，确定所述当前导航坐标更新周期的姿态变换矩阵；

根据所述当前导航坐标更新周期的姿态变换矩阵，计算载体车辆坐标系和导航坐标系在所述当前导航坐标更新周期内的转换矩阵；

根据所述载体车辆坐标系和所述导航坐标系在当前导航坐标更新周期内的转换矩阵，对所述当前导航坐标更新周期内的导航坐标系进行更新。

7.一种基于车载惯性导航***的姿态解算装置，其特征在于，包括：

速度数据获取模块，用于获取载体车辆的速度相关数据；

等效旋转矢量计算模块，用于根据所述载体车辆的速度相关数据计算当前姿态更新周期的等效旋转矢量；

四元数更新计算模块，用于根据所述当前姿态更新周期的等效旋转矢量及前一姿态更新周期的四元数，计算当前姿态更新周期的四元数；

姿态角计算模块，用于利用所述当前姿态更新周期的四元数计算所述载体车辆的姿态角。

8.一种车载终端，包括存储器、处理器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至6任一项所述姿态解算算法的步骤。

9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述姿态解算算法的步骤。

10.一种车载惯性导航***，其特征在于，包括：传感器模块、自检模块、电源模块、数据传输模块和如权利要求7所述的姿态解算装置；

所述自检模块分别与所述传感器模块、所述电源模块、所述数据传输模块和所述姿态解算装置连接；所述电源模块还分别与所述传感器模块、所述数据传输模块和所述姿态解算装置连接；所述传感器模块还与所述姿态解算装置连接，所述姿态解算装置还与所述数据传输模块连接；

所述传感器模块用于采集载体车辆的速度相关数据，并将所述速度相关数据输出至所述姿态解算装置；

所述姿态解算装置用于根据所述速度相关数据进行姿态解算；

所述自检模块用于检测所述车载惯性导航***是否存在异常；

所述电源模块用于为所述车载惯性导航***供电；

所述数据传输模块用于以规范化数据包的格式实现所述姿态解算装置与第三方***的数据传输。

Images