基于RTK/SINS的高动态定位定姿系统及方法与流程

本发明涉及导航领域，特别是涉及一种适用于高动态场景下的高精度定位定姿导航系统及方法。

背景技术：

导航技术多年来一直受到各个国家的重视且发展迅速，并取得了一定的成果，但在加速度达到20g～100g的高动态下的导航一直是一大难题。除了民用领域的具体市场价值，现代战争中，技术对抗巳经成为了关键，导弹、战斗机等高动态载体对现代导航技术的可靠性、抗干扰能力等都提出了新的要求。

对于高动态条件中的载体，如何更好地解决其导航定位一直是一大难题，因其对现代国防军事等的重要作用而一直被众多专家学者所关注。高动态环境下载体的精确导航，首先对于GNSS(Global Navigation Satellite System,全球卫星导航系统)而言，卫星和运动载体间的相对运动会引发多普勒频移与多普勒频移率，直接导致跟踪环路失锁。而增大带宽跟踪高动态信号的做法则会引入更大的噪声。对于惯性导航，高动态环境对其初始对准等技术提出了更高的要求，且定位误差会变大。鉴于单纯对SINS(Strapdown Inertial Navigation System,捷联惯性导航系统)的提升一般主要依靠提升硬件，其性价比不高及提升有限等原因，目前针对高动态导航，主要研究的方向在于高动态组合导航技术。GNSS/SINS超紧耦合正是一种新型的组合导航技术，能够全面提升导航系统的各项性能，尤其在高动态等恶劣条件下表现尤为明显。

当前对于深耦合或超紧耦合导航技术的研究已取得一些进展，《一种GNSS/INS深耦合系统环路辅助切换方法》(申请号：201510697375.2)提出了一种INS出现故障时，对深耦合系统的环路辅助状态进行切换的方法，避免了INS故障造成的系统精度发散，提升了系统的鲁棒性。《基于UKF滤波的BDS与GIS深耦合定位方法和系统》(申请号：201310733134.X)提出了一种基于UKF(Unscented Kalman Filter，无迹卡尔曼滤波)的定位方法，采用地理信息系统技术，对定位系统的修正来源于精确的GIS模型，解决了定位误差随时间推移而增大的技术问题。这些技术解决了组合导航系统的鲁棒性和精度问题，但其应用场景并未在高动态之下，GNSS定位方式也基本采用单点定位的方式，而组合导航若有高精度的需求，RTK(Real Time Kinematic，载波相位差分技术)技术是非常有必要的。但是在高动态环境下，RTK通信数据链通常也存在巨大的多普勒频移，难以维持数据链的正常通信，因此现有技术通常难以兼顾高精度和高动态的需求。

技术实现要素：

为了解决上述的技术问题，本发明提供一种基于RTK/SINS的高动态定位定姿技术方案。

本发明所采用的技术方案提供一种基于RTK/SINS的高动态定位定姿系统，包括GNSS模块、SINS模块和RTK通信模块，GNSS模块和SINS模块之间建立了通信连接，GNSS模块和RTK通信模块之间建立了通信连接；系统执行以下步骤，

步骤一，系统上电启动；

步骤二，GNSS模块初始化，期间GNSS跟踪环路使用初始带宽进行卫星信号跟踪，初始带宽包括载波环带宽T1和码环带宽T2；

步骤三，GNSS模块完成初始化后，获得载体位置速度信息，并将相关信息传递给SINS模块，辅助SINS完成初始化；

步骤四，由GNSS模块提供载体位置速度，计算基站视线方向的数据链传输多普勒频移，并送入RTK通信模块进行跟踪辅助；

步骤五，RTK通信模块接收基站播发的差分数据，传递给GNSS模块，GNSS模块进行实时RTK解算，将RTK解算后的卫星载波相位与多普勒频移作为卡尔曼滤波的观测量；

步骤六，GNSS模块通过卫星星历计算，向SINS模块提供当前卫星位置/速度，同时SINS模块根据自身递推的位置和速度，计算得到卫星伪距和多普勒频移，作为卡尔曼滤波的预测量；

步骤七，将SINS的预测量与GNSS的观测量送入卡尔曼滤波器，进行卡尔曼滤波；

步骤八，利用卡尔曼滤波结果对GNSS误差与SINS误差进行校正，输出组合导航结果，并修正SINS的加速度计与陀螺仪；

步骤九，利用校正后的SINS位置速度，以及GNSS提供的卫星位置速度，计算出新的被校正后的载波多普勒频移，并送入GNSS跟踪环路进行辅助，GNSS接收机开始使用工作带宽进行卫星信号跟踪，工作带宽包括载波环带宽t1和码环带宽t2，t1小于T1，t2小于T2；

步骤十，重复步骤四至九，直至导航结束。

而且，步骤四中，基站视线方向的数据链传输多普勒频移f_RTK计算方法为，

其中，Px_gnss、Py_gnss、Pz_gnss为地球坐标系下的GNSS坐标，Vx_gnss、Vy_gnss、Vz_gnss为GNSS速度，Px_base、Py_base、Pz_base为基站坐标，λ_RTK为RTK通信数据链载波波长。

而且，步骤六中，

卫星伪距预测量计算公式为，

卫星载波多普勒频移预测量计算公式为，

其中，Px_ins、Py_ins、Pz_ins为地球坐标系下的SINS坐标，Vx_ins、Vy_ins、Vz_ins为SINS速度，Px_sat、Py_sat、Pz_sat为卫星坐标，Vx_sat、Vy_sat、Vz_sat为卫星速度，λ_sat为卫星载波波长。

而且，步骤九中，采用卫星载波多普勒频移预测量计算公式计算出新的被校正后的载波多普勒频移。

而且，t1为T1的1/100～1/10，t2为T2的1/100～1/10。

本发明还相应提供一种基于RTK/SINS的高动态定位定姿方法，设置GNSS模块、SINS模块和RTK通信模块，GNSS模块和SINS模块之间建立通信连接，GNSS模块和RTK通信模块之间建立通信连接；执行以下步骤，

步骤一，系统上电启动；

步骤二，GNSS模块初始化，期间GNSS跟踪环路使用初始带宽进行卫星信号跟踪，初始带宽包括载波环带宽T1和码环带宽T2；

步骤三，GNSS模块完成初始化后，获得载体位置速度信息，并将相关信息传递给SINS模块，辅助SINS完成初始化；

步骤四，由GNSS模块提供载体位置速度，计算基站视线方向的数据链传输多普勒频移，并送入RTK通信模块进行跟踪辅助；

步骤五，RTK通信模块接收基站播发的差分数据，传递给GNSS模块，GNSS模块进行实时RTK解算，将RTK解算后的卫星载波相位与多普勒频移作为卡尔曼滤波的观测量；

步骤六，GNSS模块通过卫星星历计算，向SINS模块提供当前卫星位置/速度，同时SINS模块根据自身递推的位置和速度，计算得到卫星伪距和多普勒频移，作为卡尔曼滤波的预测量；

步骤七，将SINS的预测量与GNSS的观测量送入卡尔曼滤波器，进行卡尔曼滤波；

步骤八，利用卡尔曼滤波结果对GNSS误差与SINS误差进行校正，输出组合导航结果，并修正SINS的加速度计与陀螺仪；

步骤九，利用校正后的SINS位置速度，以及GNSS提供的卫星位置速度，计算出新的被校正后的载波多普勒频移，并送入GNSS跟踪环路进行辅助，GNSS接收机开始使用工作带宽进行卫星信号跟踪，工作带宽包括载波环带宽t1和码环带宽t2，t1小于T1，t2小于T2；

步骤十，重复步骤四至九，直至导航结束。

而且，步骤四中，基站视线方向的数据链传输多普勒频移f_RTK计算方法为，

其中，Px_gnss、Py_gnss、Pz_gnss为地球坐标系下的GNSS坐标，Vx_gnss、Vy_gnss、Vz_gnss为GNSS速度，Px_base、Py_base、Pz_base为基站坐标，λ_RTK为RTK通信数据链载波波长。

而且，步骤六中，

卫星伪距预测量计算公式为，

卫星载波多普勒频移预测量计算公式为，

其中，Px_ins、Py_ins、Pz_ins为地球坐标系下的SINS坐标，Vx_ins、Vy_ins、Vz_ins为SINS速度，Px_sat、Py_sat、Pz_sat为卫星坐标，Vx_sat、Vy_sat、Vz_sat为卫星速度，λ_sat为卫星载波波长。

而且，步骤九中，采用卫星载波多普勒频移预测量计算公式计算出新的被校正后的载波多普勒频移。

而且，t1为T1的1/100～1/10，t2为T2的1/100～1/10。

本发明相较于现有的组合导航技术，引入了RTK技术，一方面通过基站视线方向的载波多普勒频移辅助，获得高精度的GNSS RTK数据；一方面通过卫星视线方向的载波多普勒频移辅助，减小GNSS跟踪环路带宽提升了跟踪精度，两种辅助方式均有效提升了高动态下的定位定姿精度。

因此，本发明相较于现有技术具有以下优点：

(1)加入了RTK通信多普勒辅助

在几十至数百g的加速度应力下，现有的大部分无线通信模块都会失锁，因此现有高动态下的组合导航系统多为单点定位，而本系统由GNSS为RTK通信模块提供数据链的多普勒频移辅助，可在高动态应力下仍能稳定接收基站差分信息，从而获得高精度的载波相位信息，为高精度的组合解算提供数据基础。

(2)加入了卫星载波多普勒辅助

SINS提供的卫星多普勒频移辅助GNSS跟踪环路，可以减小环路跟踪载体高动态所需的大带宽，保证以小带宽捕获并锁定GNSS卫星信号，从而减小由载体动态所引起的跟踪误差，带宽的缩小还可以进一步减小弱噪声引起的跟踪误差，提高GNSS的跟踪精度。

通常情况下未进行多普勒辅助的定位定姿系统难以在高于20g的高动态环境下正常工作，而且因为未进行RTK解算，单点定位精度通常也在10m～30m左右。而在上述两种技术的辅助下，本发明中的定位定姿系统能够同时兼顾高动态与高精度的需求，可在20g～100g的加速度场景下正常工作，且定位精度可达分米级，具有重要的市场价值。

附图说明

图1为本发明实施例的系统构架图；

图2为本发明实施例的流程图。

具体实施方式

以下结合附图和实施例对本发明技术方案进行具体描述。

本发明提出了一种基于RTK/SINS的高动态定位定姿系统，其结构如图1所示，主要包括：GNSS模块1、SINS模块2和RTK通信模块3。GNSS模块和SINS模块之间建立了通信连接，GNSS模块和RTK通信模块之间建立了通信连接。其中GNSS模块进行卫星导航解算，并向SINS模块提供初始化数据与卫星星历数据，向RKT通信模块提供数据链多普勒辅助；SINS模块进行惯性导航解算，并向GNSS模块的跟踪环路提供多普勒辅助；RTK通信模块进行与基站的数据链通信，并向GNSS模块提供差分数据供RTK解算。

本发明通过在GNSS/SINS超紧耦合系统中加入RTK通信模块并加以多普勒辅助，在高动态条件下维持稳定的RTK数据链通信，并将组合方式升级为RTK/SINS超紧耦合，同时兼顾了高动态和高精度的需求。本发明将GNSS/SINS的导航结果与已知坐标基站进行视线向量计算，估算出通信模块的多普勒频移，并送入通信模块的跟踪环路进行辅助，保证RTK通信模块在高动态下稳定跟踪，为GNSS/SINS组合结算提供高精度的数据基础。同时，GNSS板卡将RTK解算后的卫星载波相位与多普勒频移作为卡尔曼滤波的观测量，将SINS计算出的卫星伪距与伪距率作为卡尔曼滤波的预估量，进行卡尔曼滤波，利用滤波结果对SINS与GNSS进行误差校正，输出校正后的定位定姿结果，并将校正后的卫星载波多普勒频移量送入GNSS跟踪环路进行辅助，GNSS转而使用更小的带宽进行跟踪以获得更精确的跟踪结果。

本发明实施例的工作流程如图2所示，实施例中GNSS模块简称GNSS，SINS模块简称SINS，其主要步骤包括：

步骤一：系统上电启动。

步骤二：GNSS初始化，期间GNSS跟踪环路使用初始的大带宽进行卫星信号跟踪，其中载波环带宽T1范围约50～1000Hz，码环带宽T2范围约0.5～10Hz；实施例中期间GNSS载波环带宽为250Hz，码环带宽范围为5Hz。

步骤三：GNSS完成初始化后，获得载体位置速度信息，并将位置速度航向时间等信息传递给SINS，辅助SINS完成初始化。具体SINS初始化为现有技术，本发明不予赘述。

步骤四：由GNSS提供载体位置速度，计算基站视线方向的数据链传输多普勒频移，并将其送入RTK通信模块进行跟踪辅助，保障通信模块在高动态下不会失锁。

实施例中，由GNSS提供载体位置坐标为[Px_gnss,Py_gnss,Pz_gnss]，速度为[Vx_gnss,Vy_gnss,Vz_gnss]，已知基站坐标为[Px_base,Py_base,Pz_base]，已知RTK通信数据链载波波长为λ_RTK，计算基站视线方向的数据链传输多普勒频移，并将其送入RTK通信模块的跟踪环路进行辅助；

基站视线方向的数据链传输多普勒频移f_RTK计算方法为：

其中：Px_gnss、Py_gnss、Pz_gnss为地球坐标系下的GNSS坐标，Vx_gnss、Vy_gnss、Vz_gnss为GNSS速度，Px_base、Py_base、Pz_base为基站坐标，λ_RTK为RTK通信数据链载波波长。

具体实施时，辅助方法为在RTK通信模块的跟踪环路中，在锁相环(或锁频环)的环路滤波器之后、晶体振荡器频率字变更前，将环路滤波器结果加入上述多普勒频移数值，即完成辅助跟踪。

步骤五：RTK通信模块接收基站播发的差分数据，传递给GNSS，后者进行实时RTK解算，得到精确的卫星载波相位/多普勒频移，即将RTK解算后的卫星载波相位与多普勒频移作为卡尔曼滤波的观测量。

步骤六：GNSS通过卫星星历计算，向SINS提供当前卫星位置/速度，同时SINS根据自身递推的位置/速度，计算得到卫星伪距/多普勒频移，即将SINS计算出的卫星伪距与伪距率作为卡尔曼滤波的预测量。

实施例中，GNSS通过卫星星历计算，向SINS提供当前卫星位置坐标为[Px_sat,Py_sat,Pz_sat]，速度为[Vx_sat,Vy_sat,Vz_sat]，同时SINS由惯性递推得到其位置坐标为[Px_ins,Py_ins,Pz_ins]，速度为[Vx_ins,Vy_ins,Vz_ins]，再由已知的卫星载波波长λ_sat计算得到卫星伪距/多普勒频移，即预测量；

卫星伪距预测量计算公式为：

卫星载波多普勒频移预测量计算公式为：

其中：Px_ins、Py_ins、Pz_ins为地球坐标系下的SINS坐标，Vx_ins、Vy_ins、Vz_ins为SINS速度，Px_sat、Py_sat、Pz_sat为卫星坐标，Vx_sat、Vy_sat、Vz_sat为卫星速度，λ_sat为卫星载波波长。

步骤七：将SINS的预测量与GNSS的观测量送入卡尔曼滤波器，进行卡尔曼滤波，卡尔曼滤波结果为GNSS与SINS的误差状态向量，具体包含SINS的位置误差、速度误差与姿态角误差、加速度计误差、陀螺仪误差以及GNSS的时钟误差。卡尔曼滤波器为现有技术，本发明不予赘述。

步骤八：利用卡尔曼滤波结果对GNSS误差与SINS误差进行校正，校正方法为将步骤七输出的误差状态向量中分量同GNSS与SINS的对应状态值进行相加，输出组合导航结果，包含SINS位置、速度与姿态角与GNSS时钟，并修正SINS的加速度计与陀螺仪。

步骤九：利用校正后的SINS位置/速度，以及GNSS提供的卫星位置速度，参考步骤六的方法(即采用步骤六的卫星载波多普勒频移预测量计算公式)，计算出新的被校正后的载波多普勒频移，并送入GNSS跟踪环路进行辅助，GNSS接收机开始使用小的工作带宽进行卫星信号跟踪，工作带宽包括载波环带宽t1和码环带宽t2，t1小于T1，t2小于T2。建议载波环带宽t1范围约0.5～10Hz，码环带宽t2范围约0.01～0.1Hz。优选地，t1为T1的1/100～1/10，t2为T2的1/100～1/10，例如t1为T1的1/50，t2为T2的1/50。实施例中载波环带宽降至2.5Hz，码环带宽降至0.05Hz，进一步提升跟踪精度。

载波多普勒频移辅助GNSS跟踪环路的方法为：在载波跟踪环的环路滤波器之后、数控振荡器频率字更新之前，将载波环滤波器结果加上载波多普勒频移值；同时，在码跟踪环的环路滤波器之后、数控振荡器频率字更新之前，将载波多普勒频移值除以1540后加入码环滤波器结果。完成多普勒辅助后，通过调整跟踪环路的滤波器参数，可将载波环与码环的带宽降低为原带宽的1/10～1/100，并在后续导航中不再调整。

步骤十：返回步骤四，重复步骤四至九，直至导航结束。重复时保持使用工作带宽，无需调整带宽。

具体实施时，以上流程可采用计算机软件技术实现自动运行。本发明所提供系统及相应方法都应在保护范围内。

应当理解的是，上述针对较佳实施例的描述较为详细，并不能因此而认为是对本发明专利保护范围的限制，本领域的普通技术人员在本发明的启示下，在不脱离本发明权利要求所保护的范围情况下，还可以做出替换或变形，均落入本发明的保护范围之内，本发明的请求保护范围应以所附权利要求为准。