一种导航方法、系统及导航终端与流程

本发明涉及导航技术领域，特别涉及一种导航方法、系统及导航终端。

背景技术：

全球卫星导航系统(gnss)可为全球范围提供全天候、连续、精密的三维定位和导航服务，已广泛应用于航空、航天、航海及地面运载工具的导航和定位。例如北斗卫星导航系统是中国正在实施的自主发展、独立运行的全球卫星导航系统，是独立自主、开放兼容、技术先进、稳定可靠的卫星导航系统，其突出优点是其导航定位精度高，并且导航定位精度基本不受时间和地域的限制。目前，当卫星信号受到遮挡或干扰时，其优势就不能发挥出来，在需要获得载体连续的位置、速度、姿态等控制信息时，也不能满足其性能要求。因此目前gnss还只能作为一种辅助导航设备，而不能作为唯一的导航设备使用。

捷联式惯性导航系统(sins)直接利用安装在载体上的惯性测量单元(imu)测量出载体沿三个轴的加速度和角速率，经过变换和积分等运算得到载体的姿态、速度、位置等信息。微型捷联惯导系统(msins)在体积、功耗、耐冲击等方面的优点使惯性系统的应用领域更加宽广，纯惯导系统的优点是不依赖于任何外界信息就能够实现完全自主的导航，抗干扰能力强，隐蔽性、实时性好。但目前sins的精度比较低，导致惯导系统误差随着时间积累而增大，因此，单独使用sins来实现较高精度的载体制导任务就难以胜任。为此，需要利用其它定位手段作为参考信息源，定期对sins进行校正和对漂移进行补偿。

因此，如何将卫星导航和惯性导航有机结合，获得优于任何单一导航系统的导航精度和可靠性，是本领域技术人员需要解决的技术问题。

技术实现要素：

本发明的目的是提供一种导航方法、系统及导航终端，将卫星导航和惯性导航有机结合，获得优于任何单一导航系统的导航精度和可靠性。

为解决上述技术问题，本发明提供一种导航方法，包括：

将接收到的卫星导航信号、内部射频数据以及基带芯片数据进行pvt解算，得到卫星导航预处理数据；

获取惯性导航的原始惯性测量数据，并利用捷联惯性导航算法对所述原始惯性测量数据进行解算，得到惯性导航解算结果；

获取外部观测信息，利用所述外部观测信息以及所述原始惯性测量数据计算误差数据；其中，所述外部观测信息包括温度和启动时间；

对所述卫星导航预处理数据、惯性导航解算结果以及所述误差数据进行组合导航滤波，输出滤波结果。

可选的，利用所述外部观测信息以及所述原始惯性测量数据计算误差数据，包括：

根据陀螺仪和加速度计各轴对应的原始测量数据，对所述原始测量数据进行数据误差补偿，并分别计算所述陀螺仪和所述加速度计各轴的位置误差；

计算所述陀螺仪的稳定零偏误差、姿态误差角以及速度误差。

可选的，利用捷联惯性导航算法对所述原始惯性测量数据进行解算，包括：

根据所述原始惯性测量数据，计算解算点的地球曲率半径，并进行速度解算、位置解算以及姿态解算。

可选的，所述pvt解算过程，包括：

利用公式q＝pq+dm+kf+sn+f+j+sa+c+rd+hdop+cn0进行pvt解算；

其中，q为组合算法估计出的定位质量系数，pq为定位质量，dm为定位模式，kf为解算模式质量因子，sn为参与解算的卫星数，f为错误模式，j为判停模式，c为载体速度，sa为方位角，hdop为水平定位精度，cn0为灵敏度质量因子，rd为载体坐标系上四元数表示的投影分量。

可选的，对所述卫星导航预处理数据、惯性导航解算结果以及所述误差数据进行组合导航滤波，包括：

根据所述误差数据建立卡尔曼滤波器的状态方程；其中，所述状态方程中的状态量包括位置误差、速度误差、稳定零偏误差、姿态误差角以及陀螺仪角度随机游走系数；

根据所述状态方程，建立误差协方差矩阵，并构建系统噪声矩阵；

根据所述北斗卫星导航预处理数据、所述解算结果、所述状态方程、所述误差协方差矩阵以及系统噪声矩阵建立量测方程进行组合导航卡尔曼滤波。

可选的，输出滤波结果之后，还包括：

将所述滤波结果反馈给惯性导航系统进行位置、速度以及姿态信息的修正。

本发明还提供一种导航系统，包括：

卫星导航数据处理模块，用于将接收到的卫星导航信号、内部射频数据以及基带芯片数据进行pvt解算，得到卫星导航预处理数据；

惯性导航数据处理模块，用于获取惯性导航的原始惯性测量数据，并利用捷联惯性导航算法对所述原始惯性测量数据进行解算，得到惯性导航解算结果；

误差数据计算模块，用于获取外部观测信息，利用所述外部观测信息以及所述原始惯性测量数据计算误差数据；其中，所述外部观测信息包括温度和启动时间；

组合导航滤波模块，用于对所述卫星导航预处理数据、惯性导航解算结果以及所述误差数据进行组合导航滤波，输出滤波结果。

可选的，所述组合导航滤波模块，包括：

状态方程单元，用于根据所述误差数据建立卡尔曼滤波器的状态方程；其中，所述状态方程中的状态量包括位置误差、速度误差、稳定零偏误差、姿态误差角以及陀螺仪角度随机游走系数；

系统噪声矩阵单元，用于根据所述状态方程，建立误差协方差矩阵，并构建系统噪声矩阵；

滤波单元，用于根据所述北斗卫星导航预处理数据、所述解算结果、所述状态方程、所述误差协方差矩阵以及系统噪声矩阵建立量测方程进行组合导航卡尔曼滤波。

可选的，该系统还包括：

修正模块，用于将所述滤波结果反馈给惯性导航系统进行位置、速度以及姿态信息的修正。

本发明还提供一种导航终端，其特征在于，包括：

卫星导航装置，用于获取卫星导航信号且经过pvt解算得到卫星导航预处理数据，并将卫星导航预处理数据发送给处理器；

惯性导航装置，用于获取惯性导航的原始惯性测量数据且经过捷联惯性导航算法解算得到惯性导航解算结果，并将所述原始惯性测量数据以及所述惯性导航解算结果发送给所述处理器；

外部观测部件，用于获取外部观测信息，并发送给所述处理器；其中，所述外部观测信息包括温度和启动时间；

处理器与所述卫星导航装置、所述惯性导航装置以及所述外部观测部件均相连的，用于利用所述外部观测信息以及所述原始惯性测量数据计算误差数据，并对所述卫星导航预处理数据、所述惯性导航解算结果以及所述误差数据进行组合导航滤波，输出滤波结果。

本发明所提供的一种导航方法，该导航方法包括：将接收到的卫星导航信号、内部射频数据以及基带芯片数据进行pvt解算，得到卫星导航预处理数据；获取惯性导航的原始惯性测量数据，并利用捷联惯性导航算法对原始惯性测量数据进行解算，得到惯性导航解算结果；获取外部观测信息，利用外部观测信息以及原始惯性测量数据计算误差数据；其中，外部观测信息包括温度和启动时间；对卫星导航预处理数据、惯性导航解算结果以及误差数据进行组合导航滤波，输出滤波结果；

可见，该方法将卫星导航和惯性导航有机结合，对卫星导航预处理数据、惯性导航解算结果以及误差数据进行组合导航滤波，获得优于任何单一导航系统的导航精度和可靠性；本发明还提供了一种导航系统及导航终端，具有上述有益效果，在此不再赘述。

附图说明

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

图1为本发明实施例所提供的导航方法的流程图；

图2为本发明实施例所提供的导航系统的结构框图；

图3为本发明实施例所提供的导航终端的结构框图。

具体实施方式

本发明的核心是提供一种导航方法、系统及导航终端，将卫星导航和惯性导航有机结合，获得优于任何单一导航系统的导航精度和可靠性。

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

请参考图1，图1为本发明实施例所提供的导航方法的流程图；该导航方法可以包括：

s100、将接收到的卫星导航信号、内部射频数据以及基带芯片数据进行pvt解算，得到卫星导航预处理数据；

具体的，本实施例并不限定卫星导航的种类，例如可以是北斗卫星导航，即接收北斗卫星导航信号，接收内部射频和基带芯片数据，经pvt(位置、速度、时间)解算处理后，得到北斗卫星导航预处理数据。本实施例也不限定具体的pvt解算过程。下面提供一种具体的针对北斗卫星导航的特点的pvt解算过程：即利用下面公式1进行。

利用公式q＝pq+dm+kf+sn+f+j+sa+c+rd+hdop+cn0进行pvt解算；

其中，q为组合算法估计出的定位质量系数，pq为定位质量，dm为定位模式，kf为解算模式质量因子，sn为参与解算的卫星数，f为错误模式，j为判停模式，c为载体速度，sa为方位角，hdop为水平定位精度，cn0为灵敏度质量因子，rd为载体坐标系上四元数表示的投影分量。上述的各个参数都可以从标准的pvt解算结果中获取。

由于北斗卫星导航信号的pvt解算的质量对导航系统误差影响很大，本实施中针对北斗卫星导航的特点设计了pvt解算质量估计模型(公式1)，通过模型可以有效的提高pvt解算的数据质量，提高组合导航的精度。经过公式1得到的定位质量系数将作为组合导航滤波时的量测误差量之一。

s110、获取惯性导航的原始惯性测量数据，并利用捷联惯性导航算法对原始惯性测量数据进行解算，得到惯性导航解算结果；

具体的，本实施例并不限定具体的原始惯性测量数据种类，可以根据实际的惯性导航系统进行确定。即始惯性测量数据可以但不限于包括：陀螺仪原始测量数据、加速度计原始测量数据、磁力计原始测量数据。

本实施例中利用捷联惯性导航算法对原始惯性测量数据进行解算可以包括：根据原始惯性测量数据，计算解算点的地球曲率半径，并进行速度解算、位置解算以及姿态解算。

具体的，利用公式计算解算点的地球曲率半径；

其中，a为地球长半轴半径，e为地球扁率，lat为捷联导航解算点的纬度。

进行速度解算，计算旋转矢量的变化值，即经度、纬度、高度的变化量测值，并转化为导航坐标系到载体坐标系上四元数表示的投影分量值。通过对投影分量值的时间历元处理转化为旋转矢量上投影分量，最后利用上一次迭代的速度和当前观测加速度的积分值，求出当前时刻的导航坐标系统的速度分量，主要解算公式如下：

rd＝vn×0.5×dt/(rm+dr)

v1＝vn+0.5×dv

其中，rd为载体坐标系上四元数表示的投影分量，vn为当前坐标系下的速度分量，v1为迭代后的三轴速度分量，dt是时间历元，dv为导航系下的速度向量，dr为地理坐标系下的位置向量，rm为地球子午线对应的曲率半径。

进行位置解算，主要解算公式如下：

wen＝vn×k(mv+rn×dv)

r＝r1–dt×dv

其中，wen为导航系下的转换速率，vn为当前坐标系下的速度分量，rn为卯酉圈的曲率半径，mv为差值速度向量，k为转换系数，可以根据经验值或实验值确定；r1为上一解算时刻的位置矢量，r为解算后的位置矢量。

采用等效转动矢量法辅助姿态算法，进行姿态解算，姿态解算公式以及等效转动矢量最终结果公式如下所示：

其中，为转动矢量矩阵，ψ为载体的航向角，θ均为俯仰角，γ为横滚角，φ为等效转动矢量，ω角速度矢量，i为算法的i阶，j为算法的j阶，m为姿态个数，n为算法的阶数，kij为算法系数。

等效转动矢量的迭代计算采用较高的频率，而用了等效转动矢量后的方向余弦矩阵计算则用较低迭代频率，两种方式的结合构成了捷联矩阵的计算。

s120、获取外部观测信息，利用外部观测信息以及原始惯性测量数据计算误差数据；其中，外部观测信息包括温度和启动时间；

具体的，本实施例并不对外部观测信息的内容进行限定。可以但不限于包括温度和启动时间。本实施例中利用外部观测信息以及原始惯性测量数据计算误差数据可以包括：

根据陀螺仪和加速度计各轴对应的原始测量数据，对原始测量数据进行数据误差补偿(即分别对陀螺仪和加速度计各轴的原始测量数据进行数据误差补偿)；分别计算陀螺仪和加速度计各轴的位置误差；计算陀螺仪的稳定零偏误差、姿态误差角以及速度误差。

具体的，位置误差c1具体可以是：c1＝(z–b)/(1+s)；

其中，z为所计算的轴的原始观测数据，b为所计算的轴的标定偏置误差，s为所计算的轴的刻度因子误差，其中，b和s可以根据经验值或仪器厂商提供的参数得到。

稳定零偏误差可以利用公式w稳定零偏＝a0+a1t+a2t+a3t²+a4t³+a5δt计算；

其中，t为陀螺仪的启动时间，t为陀螺仪的内部温度，δt为温度变化梯度，a0为常数项，a1～a5为系数；具体的，δt、a0、a1～a5可以根据经验或仿真值设定。

本实施中在导航开始后，评估高频imu原始测量数据的零偏误差、刻度因子误差，角度随机游走系数，并据此补偿高频imu原始测量数据，可提高原始测量数据输出的精度。mems(microelectromechanicalsystems，微电子机械系统)陀螺仪的零偏误差同时受到多个变量因素影响，温度是其影响因素之一，本实施方式针对mems陀螺仪特点，设计了mems陀螺的温度区间零偏计算模型，可以据此进行零偏补偿。

计算由于圆锥运动导致等效陀螺仪等输出漂移，本实施方式采用圆锥补偿算法来消除圆锥误差的影响，从而提高姿态计算的精度；计算姿态误差角c2为：c2＝a1×b1–b2×a2。

其中，a1为解算前上一个采样点陀螺仪的原始观测数据，b1为解算前上一个采样点的加速度计的原始观测数据，a2为解算时当前采样点陀螺仪的观测数据，b2为解算时当前采样点的加速度计的观测数据。

计算由于scullingmotion(划船运动)导致等效陀螺仪等输出漂移，对划船效应误差进行补偿，这样能有效提高速度更新中比力的转换精度，减少了速度误差；计算速度误差c3为：c3＝1/2×(a2×b2+b1×a1+b1×a2)/12.0；

其中，a1为解算前上一个采样点陀螺仪的原始观测数据，b1为解算前上一个采样点的加速度计的原始观测数据，a2为解算时当前采样点陀螺仪的观测数据，b2为解算时当前采样点的加速度计的观测数据。

进一步本实施方式中，零偏补偿、数据误差补偿、圆锥误差补偿及划船效应补偿(即数据误差补偿、位置误差、稳定零偏误差、姿态误差角以及速度误差)的先后顺序可以不限，也可以按照预定顺序进行。

s130、对卫星导航预处理数据、惯性导航解算结果以及误差数据进行组合导航滤波，输出滤波结果。

具体的，本实施例并不限定具体的组合导航滤波方式，可选的利用卡尔曼滤波器进行滤波。具体过程可以是：

根据误差数据建立卡尔曼滤波器的状态方程；其中，状态方程中的状态量包括位置误差、速度误差、稳定零偏误差、姿态误差角以及陀螺仪角度随机游走系数；

其中，根据上述步骤计算的到的位置误差、速度误差、姿态误差角、以及陀螺仪角度随机游走系数和零偏作为状态量。对应的建立的卡尔曼滤波器的状态方程(状态向量)如下：

其中，x为状态向量，表示东、北、天方向的姿态误差角，δve、δvn、δvu和δl、δλ、δh分别表示东、北、天方向的速度误差和纬度、经度、高度的位置误差，εx、εy、εz表示沿载体系xyz轴上的角度随机游走系数，表示沿载体系xyz轴上的陀螺仪零偏。

根据状态方程，建立误差协方差矩阵，并构建系统噪声矩阵；

具体的，根据上述卡尔曼滤波状态方程，建立误差协方差矩阵a(t)，a(t)为15×15维的矩阵；a(t)表示为如下形式：

其中，0表示零矩阵，下标表示该零矩阵是几乘几的矩阵。

根据北斗卫星导航预处理数据、解算结果、状态方程、误差协方差矩阵以及系统噪声矩阵建立量测方程进行组合导航卡尔曼滤波。

其中，f11、f12、f13、f14、f21、f22、f23、f24、f32、f33、f43分别为：

其中，l为纬度，h为高度，vn为导航坐标系下的速度分量；rn为卯酉面曲率半径，rm为子午面曲率半径，ve为地球坐标系下的速度矢量，ωie为地球自转角速率。

其中，α为速度分量方差，β为位置分量方差。

其中，fu、fn、fe为分别为地理坐标系下天向、北向、东向方向上的比力。

其中，为转动矢量矩阵。

进一步的构建系统噪声矩阵r，对于北斗(bd)量测卡尔曼更新，r是mn×mn维的矩阵，其表示形式速度和位置标准差的对角阵，如下：

其中，x为状态向量；m为代表速度和位置标准差的6维向量，n为算法的阶数；h为转置共轭。

根据所述卫星导航(如北斗卫星导航)预处理数据及所述解算结果，以及所述状态方程建立量测方程。以北斗卫星导航和sins捷联惯性导航为例，该过程具体为：取北斗pvt解算位置信息与sins捷联解算位置信息的差值以及北斗pvt解算速度信息与sins捷联解算的速度信息的差值作为观测量值，量测方程公式如下：

z(t)＝h(t)x(t)+w(t)

其中，x(t)为上述求得的状态向量，w(t)为系统过程白噪声，h(t)为观测矩阵。

h(t)为观测矩阵定义如下：

建立非完整性约束组合导航滤波方程，加入载体约束条件，在不增加硬件的基础上提高导航精度、重新建立量测方程，选取两轴上的速度直接误差作为外部观测量z1；其中，为载体系上迭代计算后的x轴与z轴的速度误差。

根据速度坐标转换得到量测方程如下：

其中，量测噪声γn是零均值的白噪声，量测矩阵h1为：

其中，vⁿ为导航坐标系下的速度矢量，v^e为地球坐标系下的速度矢量，后括号中的数字是指在矩阵中相应行和列上的元素，比如就是指中第1行第2列的元素；其它可以类推。

最后可以根据组合导航卡尔曼滤波的结果反馈给sins系统做位置、速度、姿态信息的修正。

进一步还可以同步输出卫星导航、组合导航两种解算结果，两种导航结果通过组合滤波后能够相互辅助，在北斗信号失锁后，能继续提供全姿态的导航数据，最终提高全天候的导航结果的精度和可靠性。

进一步，本实施例在输出滤波结果之后，还可以包括：

将滤波结果反馈给惯性导航系统进行位置、速度以及姿态信息的修正。

具体的，可以将组合导航滤波的结果反馈给惯性导航系统做位置、速度、姿态信息的修正。以提高导航准确度。

基于上述技术方案，本发明实施例提的导航方法，利用卫星导航系统不随时间积累的定位能力和惯性导航系统短时高精度自主导航能力，将卫星导航系统和惯性导航系统有机地结合起来，利用两者性能互补的特点，不仅可充分发挥各自的特长，还可以相互弥补存在的不足，具备良好的发展前景和广泛的使用价值。且通过改进组合导航算法及采用辅助约束信息如零速修正等技术，可以大幅度的提高导航的效果。

下面对本发明实施例提供的导航系统进行介绍，下文描述的导航系统与上文描述的导航方法可相互对应参照。

请参考图2，图2为本发明实施例所提供的导航系统的结构框图；该系统可以包括：

卫星导航数据处理模块110，用于将接收到的卫星导航信号、内部射频数据以及基带芯片数据进行pvt解算，得到卫星导航预处理数据；

惯性导航数据处理模块120，用于获取惯性导航的原始惯性测量数据，并利用捷联惯性导航算法对原始惯性测量数据进行解算，得到惯性导航解算结果；

具体的，惯性导航数据处理模块130除了用于采用捷联惯性导航算法进行解算的处理单元之外，还可以包括测量单元；所述测量单元可以但不限于包括三轴陀螺仪、三轴加速度计、三轴磁力计。相应的，原始惯性测量数据包括：陀螺仪原始测量数据、加速度计原始测量数据、磁力计原始测量数据。

误差数据计算模块140，用于获取外部观测信息，利用外部观测信息以及原始惯性测量数据计算误差数据；其中，外部观测信息包括温度和启动时间；

具体的，外部观测信息可以但不限于包括：温度传感器单元，用于提供计算误差数据所需的温度。时间等其它外部观测信息可以直接从相应仪器或单元中读取，或预先输入/保存在误差数据计算模块130中。

组合导航滤波模块140，用于对卫星导航预处理数据、惯性导航解算结果以及误差数据进行组合导航滤波，输出滤波结果。

基于上述实施例，该系统还可以包括：

修正模块，用于将滤波结果反馈给惯性导航系统进行位置、速度以及姿态信息的修正。

请参考图3，图3为本发明实施例所提供的导航终端的结构框图；该导航终端可以包括：

卫星导航装置100，用于获取卫星导航信号且经过pvt解算得到卫星导航预处理数据，并将卫星导航预处理数据发送给处理器；

惯性导航装置200，用于获取惯性导航的原始惯性测量数据且经过捷联惯性导航算法解算得到惯性导航解算结果，并将所述原始惯性测量数据以及所述惯性导航解算结果发送给所述处理器；

外部观测部件300，用于获取外部观测信息，并发送给所述处理器；其中，所述外部观测信息包括温度和启动时间；

处理器400与所述卫星导航装置100、所述惯性导航装置200、所述外部观测部件300均相连的，用于利用所述外部观测信息以及所述原始惯性测量数据计算误差数据，并对所述卫星导航预处理数据、所述惯性导航解算结果以及所述误差数据进行组合导航滤波，输出滤波结果。

请参考图3，基于上述实施例，该导航终端还可以包括：

存储芯片500，用于存储处理器400中计算所需程序。

基于上述实施例，所述外部观测部件300可以包括：温度传感器和计时器。

基于上述实施例，所述卫星导航装置100具体为北斗卫星导航装置。

基于上述实施例，所述惯性导航装置200具体为捷联式惯性导航装置。

基于上述实施例，所述存储芯片500具体为eeprom。

基于上述任意实施例，该导航终端还可以包括：

与所述处理器相连的低压差稳压源，用于为所述导航终端提供稳定的输入电源。

基于上述任意实施例，该导航终端还可以包括：

与所述处理器相连的i²c硬件接口，用于输出所述滤波结果。

基于上述任意实施例，该导航终端还可以包括：

与所述i²c硬件接口相连的显示器，用于显示所述滤波结果。

说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的装置而言，由于其与实施例公开的方法相对应，所以描述的比较简单，相关之处参见方法部分说明即可。

专业人员还可以进一步意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。

结合本文中所公开的实施例描述的方法或算法的步骤可以直接用硬件、处理器执行的软件模块，或者二者的结合来实施。软件模块可以置于随机存储器(ram)、内存、只读存储器(rom)、电可编程rom、电可擦除可编程rom、寄存器、硬盘、可移动磁盘、cd-rom、或技术领域内所公知的任意其它形式的存储介质中。

以上对本发明所提供的一种导航方法、系统及导航终端进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。