CN110133700B - 一种船载综合导航定位方法 - Google Patents

Abstract

本发明公开了一种船载综合导航定位方法，包括：首先获取可见卫星信号中的导航电文；根据导航电文，获取可见卫星到船载接收机之间的伪距和可见卫星的空间位置坐标；获取船舶的航向，并获得航向与地心地固坐标向量的第一关系函数；根据可见卫星的空间位置坐标、卫星信号的发射时间与卫星信号的接收时间，获得伪距观测方程；获得地心地固坐标与大地坐标的第二关系函数；根据第一关系函数、观测方程、第二关系函数和牛顿迭代法的矩阵方程式，利用牛顿迭代法计算船舶当前位置坐标。本发明实施例，准确稳定地输出船舶的实时位置、速度，且在卫星数量不小于两个的情况下就可以直接进行定位计算，提高计算的便利性。

Description

一种船载综合导航定位方法

技术领域

本发明涉及船舶定位技术领域，尤其涉及一种船载综合导航定位方法。

背景技术

当前船舶在航行时主要利用全球定位系统(Global Positioning System，GPS)实现定位，利用雷达与船舶自动识别系统(Automatic Identification System，AIS)辅助船舶实现避障。虽然卫星定位系统能够为船舶提供全球性、全天候的导航定位授时服务，但由于卫星信号易受到欺骗与干扰，卫星定位的数据更新率低，定位结果易受到多路径效应的影响且没有姿态信息，所以船舶在定位时不能完全依赖卫星定位系统。且当前许多船舶都装有计程仪与罗经供驾驶者观测，但缺少与导航系统的结合，信息利用率不高。

发明内容

本发明的目的在于提供一种船载综合导航定位方法，旨在准确稳定地输出船舶的实时位置、速度，且在卫星数量不小于两个的情况下就可以直接进行计算，提高定位的便利性。

为了实现上述目的，本发明提供一种船载综合导航定位方法，包括步骤：

获取卫星信号，并根据所述卫星信号和多普勒频率获取其中的导航电文；

根据所述导航电文，获取可见卫星到船载接收机的伪距和可见卫星的空间位置坐标；

获取船舶的航向，并获得航向与地心地固坐标向量的第一关系函数；

根据所述卫星的空间位置坐标、卫星信号的发射时间与卫星信号的接收时间，获得伪距观测方程；

根据地心地固坐标向量、站心坐标向量以及船舶水平高度变化的关系，获得地心地固坐标与大地坐标的第二关系函数；

根据所述第一关系函数、所述观测方程、所述第二关系函数和牛顿迭代法的矩阵方程式，利用牛顿迭代法计算船舶当前位置坐标。

本发明的一种实现方式中，所述方法还包括：

获取船舶的航速；

根据转换矩阵与站心坐标系中第一速度和第二速度的关系，对应的第三关系函数作为验证函数，其中，所述转换方程为地心地固坐标和站心坐标系的转换矩阵，第一速度为站心坐标系中东方向上的速度，第二速度为站心坐标系中北方向上的速度，所述验证函数的具体表达为：

V是船舶的航速，S’是转换矩阵，x、y、z为船载接收机的坐标，t为时间参数，Δ为门限值；

根据所述验证函数确定船舶当前位置坐标的准确性。

本发明的一种实现方式中，所述根据所述卫星信号和多普勒频率获取其中的导航电文的步骤，包括：

将所述卫星信号经过射频前端处理与下变频混频，得到数字中频信号；

将所述数字中频信号与本地正弦载波和余弦载波相乘，逐步去除所述数字中频信号中的载波；

去除伪码，获取导航电文。

本发明的一种实现方式中，所述伪距观测方程的具体表达为：

其中，r⁽ⁿ⁾卫星n到接收机的几何距离，x、y、z为船载接收机的坐标，xⁿ、yⁿ、zⁿ代表第n个可见卫星的坐标，

为伪距测量值，

为伪距测量误差量，t_u为接收机时钟钟差。

本发明的一种实现方式中，所述第一关系函数的具体表达为：

其中，A是航向，Δe站心坐标轴中东向坐标、Δn站心坐标轴中北向坐标。

本发明的一种实现方式中，所述第二关系函数的具体表达为：

令船舶的水平高度变化为0，则Δu＝0，

其中，φ，λ为大地坐标中的纬度和经度，Δx，Δy，Δz为地心地固直角坐标系的坐标。

本发明的一种实现方式中，所述利用牛顿迭代法计算船舶当前位置坐标的步骤，包括：

对船载接收机接收机到卫星n的几何距离r⁽ⁿ⁾求导；

其中，

将对船载接收机到卫星n的几何距离r⁽ⁿ⁾求导的结果、第一关系函数和第二关系函数代入牛顿迭代法的矩阵方程式；

其中，牛顿迭代法的矩阵方程的具体表达为：

设定，

其中，ρ_C为北斗处理模块所测得的伪距，k-1为第k-1次迭代。

本发明的一种实现方式中，所述方法还包括：

根据所测量的载体在惯性参考系的加速度信息，估算目标伪距；

与卫星伪距进行比较，将卫星伪距与目标伪距的差值作为EKF滤波器的输入；

对IMU的速度、位置、姿态和传感器误差进行估计，并根据估计结果对IMU进行校正。

应用本发明实施例提供的船载综合导航定位方法，准确稳定地输出船舶的实时位置、速度，且在卫星数量不小于两个的情况下就可以直接进行计算，提高计算的便利性。

附图说明

图1是本发明实施例的流程示意图。

图2是本发明实施例的一种效果示意图。

图3是本发明实施例的一种效果示意图。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。

请参阅图1-2。需要说明的是，本实施例中所提供的图示仅以示意方式说明本发明的基本构想，遂图式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

如图2本发明提供一种船载综合导航定位方法，包括步骤：

S101，获取卫星信号，并根据所述卫星信号和多普勒频率获取其中的导航电文。

本发明的具体实现方式中，船载接收机接收到的卫星信号由三部分构成：伪码、载波和导航电文。不同伪码代表接收到不同的北斗卫星发射的信号；载波是用于调制导航信息的特高频调制信号，以便于卫星信号在地表和卫星间传输；导航电文中含有卫星信号的发射时间、卫星星历、电离层延时等信息。

对接收机天线接收到的卫星信号的处理包括捕获、跟踪和定位数据处理。

捕获过程中，经天线接收到的卫星信号经过射频前端处理与下变频混频(下变频频率为f_LO)，得到数字中频信号s_IF(n)；再经过并行码相位搜索，确定可见卫星的卫星伪码，并估计接收到的卫星信号的伪码起始沿的值和载波相位的值。

跟踪过程中，对数字中频信号s_IF(n)进行处理。

s_IF(n)＝C(n)D(n)cos(ω_in+θ_i) (1)

其中，C(n)为伪码，cos(ω_in+θ_i)为载波，D(n)为导航电文。s_IF(n)经混频器，分别与本地正弦载波和余弦载波相乘，逐步剔除信号中的载波。

ω_o＝2π(f-f_LO) (4)

其中，cos(ω_on+θ_o)为接收机提供的本地载波，用来剔除数字中频信号s_IF(n)中的载波，ω_o为卫星信号从卫星上被发射时的调制角频率减去固定值中频角频率，f为射频信号的中心频率，θ_o的初始值为捕获过程中载波相位的估计值。

该船载综合定位导航终端系统中的载波环为锁相环，包括鉴相器、环路滤波器和压控振荡器，如图2所示。

由于船舶的动态性较低，接收机接收到卫星信号的载波频率f_i与卫星信号被发射时的载波频率f相差较小，即

f_i≈f-f_LO (6)

所以，接收机接收到的卫星信号的载波频率f_i为

f_i＝f-f_LO+f_d (7)

其中f_d为由卫星和接收机在连线方向上的相对运动所引起的多普勒频移。则载波环在剔除s_IF(n)中的载波时，要考虑多普勒频移的影响，即

ω₀＝2π(f-f_LO+f_d) (8)

则进入混频器的新的本地载波为u₀(n)

u₀(n)＝cos(ω₀n+θ_o) (9)

跟踪环路剔除伪码和载波后，获得s_IF(n)中的导航电文D(n)。

S102，根据所述导航电文，获取卫星伪距和卫星的空间位置坐标。

ρ(t)＝c(t_u(t)-t^s(t-τ)) (10)

t^s(t-τ)为卫星信号的发射时间，t_u(t)为卫星信号的接收时间，ρ(t)为t时刻的伪距。

导航电文D(n)中含有卫星信号的发射时间、卫星星历、电离层延时等信息。其中，卫星星历能精确描述卫星在各个时刻的空间位置和运行速度，该部分为现有技术，本发明实施例在此不做赘述。

S103，获取船舶的航向，并获得航向与地心地固坐标向量的第一关系函数。

可以理解的是，随动球与陀螺球在方位上出现偏角时，基于惠斯通工作原理的信号电桥失去平衡，对称变压器输出与之相对应的交流随动电压信号。交流随动电压信号依次经过运算放大器、模拟开关电路、电容器和另一运算放大器变换成直流电压信号。该直流电压信号直接输至CPU的模拟输入端，经处理获得航向A，本发明实施例中，如图2所示，通过罗经获得航向，航向为由经线北端顺时针测量至船舶航向线的夹角，大小为0至360度。

站心坐标系以用户所在位置为坐标原点，三个坐标轴分别相互垂直指向东向、北向和天向。地心地固坐标系中的向量[Δx Δy Δz]^T与站心坐标系中的向量[Δe Δn Δu]^T满足关系

其中，

大地坐标(φ λ h)与地心地固直角坐标(x y z)的关系

则

带入式(11)得

S104，根据所述卫星的空间位置坐标、卫星信号的发射时间与卫星信号的接收时间，获得伪距观测方程。

首先，根据卫星位置和伪距，构造牛顿迭代法的基本方程。

卫星信号的发射时间t^(s)与接收机接收到卫星信号时的接收时间t_u的时间差乘以光速为伪距ρ，即该段卫星信号从卫星传播至接收机所走过的距离。

ρ＝c(t_u-t^(s)) (18)

由式(18)推导出伪距观测方程式：

其中，

为伪距测量值(均已知)，

为伪距测量误差量，未知量[x,y,z]和δt_u均在伪距观测方程式(19)等号的左边。

利用牛顿迭代法和最小二乘法，求解伪距观测方程组。通过将非线性方程不断线性化，利用最小二乘法求解每次牛顿迭代循环中的线性矩阵方程，求得最佳解。

根据各可见卫星的空间位置坐标(x⁽ⁿ⁾,y⁽ⁿ⁾,z⁽ⁿ⁾)，接收机到卫星n的几何距离为

其中，x＝[x,y,z]^T为未知的接收机在地心地固坐标系中位置坐标向量，x⁽ⁿ⁾＝[x⁽ⁿ⁾,y⁽ⁿ⁾,z⁽ⁿ⁾]^T为卫星n在地心地固坐标系中的位置坐标向量，x⁽ⁿ⁾、y⁽ⁿ⁾、Z⁽ⁿ⁾代表第n个可见卫星的坐标。

求函数r⁽ⁿ⁾对x、y、z的偏导，得

其中，

在牛顿迭代法的第k(k＝2，3，4…)次迭代中，假设伪距观测方程组中的各个非线性方程可在[x_k-1,δt_u,k-1]^T线性化，第一次迭代时，可将接收机初始位置和接收机时钟钟差初始值均设为0。

S105，根据地心地固坐标向量、站心坐标向量以及船舶水平高度变化的关系，获得地心地固坐标与大地坐标的第二关系函数。

船舶在航行过程中，一般船舶的水平高度为海平面高度，即水平高度不变，Δu＝0，则根据式(11)，得

S106，根据所述第一关系函数、所述观测方程、所述第二关系函数和牛顿迭代法的矩阵方程式，利用牛顿迭代法计算船舶当前位置坐标。

利用牛顿迭代法的矩阵方程式得：

将式(17)、式(22)、式(24)带入式(25)，其中

矩阵G只与各颗卫星相对于用户的几何位置有关，且λ、φ与船舶位置(x y z)满足式(13-15)。利用(x y z)确定λ、φ时，由于φ中存在h，故也需进行多次迭代，求解近似值。

由式(25)可得矩阵G的秩必须大于等于4，故除了满足式(17)、(24)，至少还需要两可见卫星才能进行矩阵解算，在n>＝2即可实现定位。

计算最小二乘法解

第一次迭代时，若接收机为首次定位，则可将接收机初始位置和接收机时钟钟差初始值均设为0，经多次迭代循环可得到收敛；若有近期的定位记录，则将最后一次接收机的定位结果和接收机时钟钟差作为接收机初始位置和接收机时钟钟差初始值。

更新接收机位置坐标x_k和钟差值δt_u,k，进行下一次迭代。

δt_u,k＝δt_u,k-1+Δδt_u (30)

其中，

进行下一次迭代，直到最小二乘法的解满足一定的精度要求时为止。

利用航速验证求解出的x_k的准确性。船舶位置x＝[x,y,z]^T随时间变化的值均被记录，拟合出x、y、z随时间t变化的曲线函数x(t)、y(t)、z(t)。

利用声相关计程仪测量航速。声相关计程仪利用发射换能器垂直向海底发射超声波，相互有一定间距的两个接受换能器接收到的回波来测量船舶速度V。沿船底纵向等间距安装前向接收换能器R_f、发射换能器T和后向接收换能器R_a，前后两接收换能器的间距为S。发射换能器T以一定的时间间隔向海底垂直发射较大波束宽带的超声波脉冲，在t＝t₁时刻，经海底A反射回来的回波被前向换能器R_f所接收，经过时间间隔τ，在t＝t₂时刻，回波被后向换能器所接收，船舶航行的航程为S/2。

则航速为：

在站心坐标系中，船舶的航速V满足：

利用式(11)，得

Δ为门限值，若式(35)满足，则可将x_k作为船舶的初始位置，并对惯性导航处理模块和扩展卡尔曼滤波器进行初始化；否则，将重新进行定位解算，其中，t为时间参数。

惯性导航处理模块以牛顿力学为基础，通过测量载体在惯性参考系的加速度信息，估算伪距ρ_I，并与北斗处理模块输出的伪距进行比较，将差值作为EKF滤波器的输入，同时向EKF滤波器输入船舶航行过程中载体系三轴上陀螺仪漂移、加速度计零偏等。

利用北斗处理模块获得的初始位置信息对捷联式惯性导航系统(Strap-downInertial Navigation System，SINS)进行初始化。当SINS进入导航状态时，陀螺仪测量的角速度对时间进行积分可获得船舶当前的姿态信息；加速度计测量到的加速度对时间进行一次积分可获得船舶当前的速度信息，二次积分可获得船舶当前的空间位置(x_I,y_I,z_I)，并得到伪距：

其中，(x_I,y_I,z_I)为惯性测量单元(Inertial measurement unit，IMU)解算的船舶在地球坐标系中的位置，(x_s,y_s,z_s)为卫星在地球坐标系中的位置。

以IMU线性化的误差方程为系统方程，通过EKF滤波器对IMU的速度、位置、姿态和传感器误差进行最优估计，并根据估计结果对IMU进行校正。与IMU相关的系统误差和器件误差包括三维姿态误差、三维速度误差、三维位置误差、三维陀螺漂移和三维加速度计零偏。

系统状态方程为：

其中，

W＝[ω_x ω_y ω_z a_x a_y a_z]^T (40)

X为状态变量，W为系统噪声、F为系统矩阵、G为噪声矩阵。

为姿态误差，δV_i(i＝e,n,u)为速度误差，δLδλδh为位置误差，ε_i(i＝x,y,z)为陀螺仪漂移，

为加速度计零偏。

量测方程为：

Z＝HX-V (41)

式中

Z＝[δρ¹δρ²…δρⁿ]^T (42)

δρ＝ρ_I-ρ_C (43)

其中，H为量测矩阵，V为测量噪声矩阵，X为状态变量，Z为IMU测得的伪距(伪距率)和卫星模块测得的伪距(伪距率)的差值。ρ_I为惯性导航处理模块所测得的伪距，ρ_C为北斗处理模块所测得的伪距。

更新的陀螺仪漂移和加速度零偏能对IMU进行修正。

扩展卡尔曼滤波能给出船舶的实时速度信息v，结合卫星星历中卫星的运动速度v^s和空间位置，计算接收机与卫星间的多普勒频移f_d，将多普勒频率估计值带入跟踪环路中，辅助载波环快速剔除载波。

其中，I⁽ⁿ⁾为卫星在接收机处的单位观测矢量，同式(23)。

同时，扩展卡尔曼滤波向外输出船舶的位置速度和姿态信息。

本发明的原理及其功效，而非用于限制本发明。任何熟悉此技术的人士皆可在不违背本发明的精神及范畴下，对上述实施例进行修饰或改变。因此，举凡所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等效修饰或改变，仍应由本发明的权利要求所涵盖。

Claims (7)

1.一种船载综合导航定位方法，其特征在于，包括步骤：

获取卫星信号，并根据所述卫星信号和多普勒频率获取其中的导航电文；

根据所述导航电文，获取伪距和卫星的空间位置坐标；

获取船舶的航向，并获得航向与地心地固坐标向量的第一关系函数；

根据所述卫星的空间位置坐标、卫星信号的发射时间与卫星信号的接收时间，获得伪距观测方程；

根据地心地固坐标向量、站心坐标向量以及船舶水平高度变化的关系，获得地心地固坐标与大地坐标的第二关系函数；

根据所述第一关系函数、所述观测方程、所述第二关系函数和牛顿迭代法的矩阵方程式，利用牛顿迭代法计算船舶当前位置坐标；

所述伪距观测方程的具体表达为：

其中，r⁽ⁿ⁾为卫星n到船载接收机的几何距离，x、y、z为船载接收机的坐标，x⁽ⁿ⁾、y⁽ⁿ⁾、z⁽ⁿ⁾代表第n个可见卫星的坐标，

为伪距测量值，

为伪距测量误差量，δt_u为接收机时钟钟差。

2.根据权利要求1所述的一种船载综合导航定位方法，其特征在于，所述方法还包括：

获取船舶的航速；

根据转换矩阵与站心坐标系中第一速度和第二速度的关系，对应的第三关系函数作为验证函数，其中，转换方程为地心地固坐标和站心坐标系的转换矩阵，第一速度为站心坐标系中东方向上的速度，第二速度为站心坐标系中北方向上的速度，所述验证函数的具体表达为：

V是船舶的航速，S’是转换矩阵，x、y、z为船载接收机的坐标，t为时间参数，Δ为门限值；

根据所述验证函数确定船舶当前位置坐标的准确性。

3.根据权利要求1或2所述的一种船载综合导航定位方法，其特征在于，所述根据所述卫星信号和多普勒频率获取其中的导航电文的步骤，包括：

将所述卫星信号经过射频前端处理与下变频混频，得到数字中频信号；

将所述数字中频信号与本地正弦载波和余弦载波相乘，逐步去除所述数字中频信号中的载波；

去除伪码，获取导航电文。

4.根据权利要求1所述的一种船载综合导航定位方法，其特征在于，所述第一关系函数的具体表达为：

其中，A是航向，Δe站心坐标轴中东向坐标、Δn站心坐标轴中北向坐标。

5.根据权利要求4所述的一种船载综合导航定位方法，其特征在于，所述第二关系函数的具体表达为：

令船舶的水平高度变化为0，则Δu＝0，

其中，φ，λ为大地坐标的纬度和经度，Δx，Δy，Δz为地心地固直角坐标系的坐标。

6.根据权利要求5所述的一种船载综合导航定位方法，其特征在于，所述利用牛顿迭代法计算船舶当前位置坐标的步骤，包括：

对船载接收机到卫星n的几何距离r⁽ⁿ⁾求导；

其中，

将对船载接收机到卫星n的几何距离r⁽ⁿ⁾求导的结果、第一关系函数和第二关系函数代入牛顿迭代法的矩阵方程式；

其中，牛顿迭代法的矩阵方程的具体表达为：

设定：

其中，ρ_C为北斗处理模块所测得的伪距，k-1为第k-1次迭代。

7.根据权利要求1所述的一种船载综合导航定位方法，其特征在于，所述方法还包括：

根据所测量的载体在惯性参考系的加速度信息，估算目标伪距；

与卫星伪距进行比较，将卫星伪距与目标伪距的差值作为EKF滤波器的输入；

对IMU的速度、位置、姿态和传感器误差进行估计，并根据估计结果对IMU进行校正。

Images