一种组合导航系统及方法
阅读说明：本技术 一种组合导航系统及方法 (Combined navigation system and method ) 是由 曹宜 张杨勇 梅雪松 王琦思 刘谋荣 刘庆 于 2021-10-28 设计创作，主要内容包括：本发明公开了一种组合导航系统及方法。该系统包括：无线电导航组件、惯导组件、磁力计和计算模块；所述计算模块用于求解载体当前的姿态、位置与地球参考物理量之间的最小二乘解,利用磁力计等传感器稳定性好,高频噪声低的特点,降低无线电导航组件定位误差,同时可以校准其它惯导组件的零点偏移。本发明不依赖GNSS系统,具有运算量小、解算精度高、测量准确、稳定性好的特点。(The invention discloses a combined navigation system and a method. The system comprises: the system comprises a radio navigation component, an inertial navigation component, a magnetometer and a calculation module; the calculation module is used for solving a least square solution between the current attitude and position of the carrier and the georeferenced physical quantity, and the positioning error of the radio navigation assembly is reduced by utilizing the characteristics of good stability and low high-frequency noise of sensors such as a magnetometer and the like, and meanwhile, the zero offset of other inertial navigation assemblies can be calibrated. The method does not depend on a GNSS system, and has the characteristics of small calculation amount, high resolving precision, accurate measurement and good stability.)
技术领域
本发明属于导航技术领域，更具体地，涉及一种组合导航系统及方法。
背景技术
在现代定位、导航和授时PNT体系内GNSS都占据了核心地位，在覆盖范围、精度和使用成本上相对其它导航系统都具有明显的综合优势。但由于其脆弱性和易受干扰的缺点，过度依赖GNSS的PNT体系，在社会的经济安全、生产安全和国防安全的角度上来说均具有巨大的风险，因此，需要建立不依赖于GNSS的导航系统。
发明内容
针对现有技术的至少一个缺陷或改进需求，本发明提供了一种组合导航系统及方法，不依赖GNSS系统，具有运算量小、解算精度高、测量准确、稳定性好的特点。
为实现上述目的，按照本发明的第一方面，提供了一种组合导航系统，包括：
无线电导航组件、惯导组件、磁力计和计算模块；
所述计算模块用于在每个时间步进行以下步骤：
根据上一时间步到当前时间步的时间区间所述惯导组件的测量数据、以及上一时间步的载体姿态预测值及载体位置预测值，确定当前时间步的载体姿态预测值及载体位置预测值；
计算当前时间步所述磁力计的测量数据与当前时间步的载体姿态预测值的第一误差，计算当前时间步所述惯导组件的测量数据与当前时间步的载体姿态预测值的第二误差，计算当前时间步的载体位置预测值与当前时间步所述无线电导航组件的测量数据的第三误差，采用非线性最小二乘算法根据所述第一误差、第二误差、第三误差对当前时间步的载体姿态预测值及载体位置预测值进行修正。
进一步地，所述计算当前时间步所述磁力计的测量数据与当前时间步的载体姿态预测值的第一误差包括步骤：
根据当前时间步的载体姿态预测值确定当前时间步所述磁力计的预测测量值；
计算当前时间步所述磁力计的预测测量值与当前时间步所述磁力计的测量数据的差，作为第一误差。
进一步地，所述惯导组件包括陀螺仪和加速度计。
进一步地，计算当前时间步所述惯导组件的测量数据与当前时间步的载体姿态预测值的第二误差包括步骤：
根据当前时间步的载体姿态预测值确定当前时间步所述加速度计的预测测量值；
计算当前时间步所述加速度计的预测测量值与当前时间步所述加速度计的测量数据的差，作为第二误差。
进一步地，所述计算模块用于在每个时间步进行以下步骤：
对所述惯导组件进行误差矫正。
进一步地，所述无线电导航组件通过RS422接口与所述计算模块电连接，所述惯导组件通过RS422接口与所述计算模块电连接，所述磁力计通过I2C接口与所述计算模块电连接。
进一步地，还包括与所述计算模块电连接的上位机。
按照本发明的第二方面，提供了一种组合导航方法，包括步骤：
分别采用设置在载体的无线电导航组件、惯导组件、磁力计进行测量；
在每个时间步进行以下步骤：
根据上一时间步到当前时间步的时间区间所述惯导组件的测量数据、以及上一时间步的载体姿态预测值及载体位置预测值，确定当前时间步的载体姿态预测值及载体位置预测值；
计算当前时间步所述磁力计的测量数据与当前时间步的载体姿态预测值的第一误差，计算当前时间步所述惯导组件的测量数据与当前时间步的载体姿态预测值的第二误差，计算当前时间步的载体位置预测值与当前时间步所述无线电导航组件的测量数据的第三误差，采用非线性最小二乘算法根据所述第一误差、第二误差、第三误差对当前时间步的载体姿态预测值及载体位置预测值进行修正。
进一步地，所述计算当前时间步所述磁力计的测量数据与当前时间步的载体姿态预测值的第一误差包括步骤：
根据当前时间步的载体姿态预测值确定当前时间步所述磁力计的预测测量值；
计算当前时间步所述磁力计的预测测量值与当前时间步所述磁力计的测量数据的差，作为第一误差。
进一步地，所述惯导组件包括陀螺仪和加速度计，计算当前时间步所述惯导组件的测量数据与当前时间步的载体姿态预测值的第二误差包括步骤：
根据当前时间步的载体姿态预测值确定当前时间步所述加速度计的预测测量值；
计算当前时间步所述加速度计的预测测量值与当前时间步所述加速度计的测量数据的差，作为第二误差。
总体而言，本发明与现有技术相比，具有有益效果：通过多种传感器数据融合，提高了信息的冗余度，即通过相对无线电导航组件本身额外的姿态和速度信息对当前位置的变化量做出估计，由于磁力计系统短时的稳定性极佳，因而可以有效抑制无线电导航组件由于噪声干扰导致的定位误差，可代替GNSS定位系统的成为可靠的高精度导航定位手段，其穿透力强，不易受人为蓄意干扰，在军事和经济领域等对安全性要求较高的领域具有较高的应用价值，且计算速度相对传统的卡尔曼滤波算法大幅提升。
附图说明
图1是本发明实施例的一种组合导航系统的结构示意图；
图2是本发明实施例的计算模块的计算原理示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
无线电导航组件(eLoran导航系统)起源于二次世界大战期间，其有效范围可达2000km以上，被认为是GNSS系统的最佳代替方案，因此，本发明实施例采用eLoran导航接收机、惯导组件(Inertial Navigation System，简称INS模块)与磁力计，利用非线性最小二成法进行数据融合，通过传感器相互之间取长补短，从而达到导航精度的提升和计算量下降的目的。
如图1所示，本发明实施例的一种组合导航系统，包括：无线电导航组件、惯导组件、磁力计和计算模块。
进一步地，组合导航系统还包括与所述计算模块电连接的上位机。
进一步地，组合导航系统还包括电源模块，电源模块负责为无线电导航组件、惯导组件、磁力计和计算模块提供5.5V电压。
进一步地，所述惯导组件包括陀螺仪和加速度计。
在一个实施例中，计算模块采用微型计算单元；无线电导航组件采用河南尚禹科技提供的北斗-eLoran导航接收机，其与微型计算单元通过RS422相连，并通过通信转换模块与微型计算单元的ARM模块进行通信；INS模块采用湖北三江红峰公司的HZ-1型光纤惯性测量系统，与微型计算单元通过RS422相连，并通过通信转换模块与微型计算单元的ARM模块进行通信。磁力计采用PNI公司的RM3100三轴磁场传感器，与ARM芯片通过SPI方式进行通信，其噪声RMS大小为15nT。微型计算单元的采样率为10Hz，采集到eLoran/INS/磁力计的数据后，利用基于非线性最小二乘估计的数据融合算法计算当前的姿态和位置。
进一步地，所述计算模块的原理如图2所示，其用于在每个时间步进行以下步骤：
S1，根据上一时间步到当前时间步的时间区间所述惯导组件的测量数据、以及上一时间步的载体姿态预测值及载体位置预测值，确定当前时间步的载体姿态预测值及载体位置预测值。
具体地，首先根据陀螺仪数据得到载体姿态预测值：
其中：为利用陀螺仪估计的该时间步t载体姿态预测值，为上一个时间步t-1的载体姿态预测值，为时间步t-1～t陀螺仪测量到的数据，Δt为时间步间隔，表示四元数的直积运算。
再利用加速度计数据计算的速度可以得到位置的估计量：
其中，为该时间步t-1～t加速度计的测量数据，为该时间步t速度的估计量，是该时间步t的载体位置预测值，为时间步t-1的载体位置预测值，*表示共轭四元数。
S2，计算当前时间步所述磁力计的测量数据与当前时间步的载体姿态预测值的第一误差，计算当前时间步所述惯导组件的测量数据与当前时间步的载体姿态预测值的第二误差，计算当前时间步的载体位置预测值与当前时间步所述无线电导航组件的测量数据的第三误差，采用非线性最小二乘算法根据所述第一误差、第二误差、第三误差对当前时间步的载体姿态预测值及载体位置预测值进行修正。
进一步地，所述计算当前时间步所述磁力计的测量数据与当前时间步的载体姿态预测值的第一误差包括步骤：
(1)根据当前时间步的载体姿态预测值确定当前时间步所述磁力计的预测测量值；
(2)计算当前时间步所述磁力计的预测测量值与当前时间步所述磁力计的测量数据的差，作为第一误差。
进一步地，计算当前时间步所述惯导组件的测量数据与当前时间步的载体姿态预测值的第二误差包括步骤：
(1)根据当前时间步的载体姿态预测值确定当前时间步所述加速度计的预测测量值；
(2)计算当前时间步所述加速度计的预测测量值与当前时间步所述加速度计的测量数据的差，作为所述第二误差。
具体地，依据载体姿态预测值反推的重力矢量和地磁场矢量在载体坐标系中的表示，与载体坐标系中加速度计实际测量值和磁力计实际测量值之间的误差：
公式(4)中，为当前时间步所述加速度计的预测测量值与当前时间步所述加速度计的测量数据的差，即上述的第二误差。前时间步所述磁力计的预测测量值与当前时间步所述磁力计的测量数据的差，即上述的第一误差。
的雅克比行列式Jg，Jm为：
上述的第三误差的计算方法具体可以是：
公式(6)中，为当前时间步的载体位置预测值与当前时间步所述无线电导航组件的测量数据的差，即上述第三误差。的雅克比行列式Jp为：
这三个误差的整体的误差函数可以构造为：
上式的雅克比行列式为：
再利用非线性最小二乘算法，通过迭代，求使该误差数值最小的载体姿态，即可得到当前的载体姿态。为提高迭代效率，采用高斯牛顿法计算迭代量：
和分别代表位置和姿态的迭代量，完整的递推公式为：
其中，为载体的姿态与位置，为估计的载体姿态，为迭代量，μ是收敛步长因子。
进一步地，所述计算模块用于在每个时间步进行以下步骤：
S3，对所述惯导组件进行误差矫正。
加速度计和陀螺仪的对载体状态的计算结果存在累积误差，因此，需要通过位置和姿态数据进行校准。可利用预测的姿态和位置与测量的姿态位置之间的误差通过低通滤波的方式计算得到INS系统的自身零点偏移。
由于陀螺仪的零偏误差为一个缓变量，因此可以通过累计估计姿态与测量姿态之间的误差得到：
公式(7)中，表示速度的偏置，表示上一时刻姿态的共轭四元数，表示单次运算中由姿态的变化反推得到的陀螺仪输出的估计量，γ为积分常数。同理，速度的偏置可由类似的方法得到：
公式(8)中，表示速度的偏置，表示单次运算中由位置的变化反推得到的速度的估计量，δ为积分常数。
因此，校准后的角速度和加速度分别为：
公式(9)中，表示校准后的角速度，表示校准后的速度。
微型计算单元与上位机通过RS422接口通信，在接收到上位机发送的“开始工作”的指令后，将计算得到的姿态和位置信息发送给上位机，并由上位机进行存储、呈现。
本发明实施例的一种组合导航方法，包括步骤：
分别采用设置在载体的无线电导航组件、惯导组件、磁力计进行测量；
在每个时间步进行以下步骤：
根据上一时间步到当前时间步的时间区间所述惯导组件的测量数据、以及上一时间步的载体姿态预测值及载体位置预测值，确定当前时间步的载体姿态预测值及载体位置预测值；
计算当前时间步所述磁力计的测量数据与当前时间步的载体姿态预测值的第一误差，计算当前时间步所述惯导组件的测量数据与当前时间步的载体姿态预测值的第二误差，计算当前时间步的载体位置预测值与当前时间步所述无线电导航组件的测量数据的第三误差，采用非线性最小二乘算法根据所述第一误差、第二误差、第三误差对当前时间步的载体姿态预测值及载体位置预测值进行修正。
进一步地，所述计算当前时间步所述磁力计的测量数据与当前时间步的载体姿态预测值的第一误差包括步骤：
根据当前时间步的载体姿态预测值确定当前时间步所述磁力计的预测测量值；
计算当前时间步所述磁力计的预测测量值与当前时间步所述磁力计的测量数据的差，作为第一误差。
进一步地，所述惯导组件包括陀螺仪和加速度计，计算当前时间步所述惯导组件的测量数据与当前时间步的载体姿态预测值的第二误差包括步骤：
根据当前时间步的载体姿态预测值确定当前时间步所述加速度计的预测测量值；
计算当前时间步所述加速度计的预测测量值与当前时间步所述加速度计的测量数据的差，作为第二误差。
方法的实现原理、技术效果与上述系统类似，此处不再赘述。
必须说明的是，上述任一实施例中，方法并不必然按照序号顺序依次执行，只要从执行逻辑中不能推定必然按某一顺序执行，则意味着可以以其他任何可能的顺序执行。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
- 上一篇：一种医用注射器针头装配设备
- 下一篇：基于数字孪生和用户个性化需求的车辆路径规划方法