基于北斗、GPS和罗经数据的导航雷达运动状态估计方法与流程

本发明涉及多源信息融合领域，特别是一种基于北斗、gps和罗经数据的导航雷达运动状态估计方法。

背景技术：

1、随着全球船舶交通流量的不断增加，船舶安全和航行效率问题日益凸显。其中，精确可靠的船舶航行态势感知对于提高航行安全、增强避碰决策能力以及提升水域通航能力具有至关重要的作用。船载导航雷达探测感知就是一种高效的水面态势感知手段。然而现有的船载导航雷达的运动状态估计方法往往依赖于单一传感器，如惯性测量单元(imu)或全球定位系统(gps)，这使得在某些情况下，如gps信号被遮挡或imu出现故障，船载导航雷达的运动状态估计会变得不精确，不可靠。

2、因此，研究一种基于多传感器数据融合的运动状态估计方法，提高状态估计的准确性和鲁棒性，同时克服单一传感器的局限性，成为了当前水上态势感知领域亟待解决的问题。

3、与此同时，我国自主研制建设了北斗卫星导航系统(beidou navigationsatellite system，bds)，其可以为用户提供全天候、高精度、强安全的服务。伴随系统服务能力提升，北斗应用领域快速扩展，政府部门的公务船只、渔船、客船、货船、深海科考船等已经逐渐使用支持北斗系统的船载卫星终端设备来替代船载ais设备，这类终端设备可以同时接收北斗、gps等数据为船舶提供位置、速度等信息。

技术实现思路

1、本发明要解决的技术问题是针对上述现有技术的不足，而提供一种基于北斗、gps和罗经数据的导航雷达运动状态估计方法，该基于北斗、gps和罗经数据的导航雷达运动状态估计方法将北斗数据、gps数据和罗经数据进行融合，并使用卡尔曼滤波算法进行递推计算，实现船载导航雷达运动状态的精确可靠估计。本发明能在复杂海况场景下，精确估计出船载导航雷达的运动状态，具有精度高、容错性好、成本低等特点。

2、为解决上述技术问题，本发明采用的技术方案是：

3、一种基于北斗、gps和罗经数据的导航雷达运动状态估计方法，包括如下步骤。

4、步骤1、选取参考坐标系：选取北东地坐标系ned作为惯性参考坐标系，并定义载体坐标系；其中，北东地坐标系ned的原点为船载导航雷达的基座平面中心点；n轴指向正北方向，e轴指向正东方向，d轴与n和e两轴所在平面垂直且指向地心。

5、载体坐标系的原点为船载导航雷达的基座平面中心点；x轴与海面平行且指向船艏，y轴与海面平行且垂直于x轴，z轴垂直于x和y两轴确定的平面。

6、步骤2、时空转换与统一：通过船载卫星终端接收m颗北斗卫星和n颗gps卫星的星历数据并解算得到每颗卫星的空间位置坐标，将gps卫星的空间坐标转换到北斗坐标系下，同时将gps时间系统转换到北斗时间系统下，从而实现北斗和gps的时空转换与统一。

7、步骤3、安装船载卫星终端设备和船载罗经设备：将船载卫星终端设备和船载罗经设备安装于船载导航雷达的基座平面中心点位置附近。

8、步骤4、构建船载导航雷达运动状态估计模型，具体包括如下步骤：

9、步骤4-1、构建船载导航雷达运动状态估计模型：基于船载卫星终端设备获取到的北斗、gps数据和船载罗经设备获取到的罗经数据，构建船载导航雷达在k时刻的运动状态估计模型xk，具体表达式为：

10、

11、式中，λk、hk分别表示船载导航雷达在k时刻的纬度、经度和海拔高度，与k时刻的输入向量uk和观测向量zk相关。

12、分别表示船载导航雷达在k时刻的东向速度、北向速度和垂向速度，与k时刻的输入向量uk相关；

13、步骤4-2、定义输入向量uk，具体表达式为：

14、uk＝[vk pk ak]t (4-2)

15、式中，vk表示船载卫星终端设备在k时刻获取到的船载导航雷达速度。

16、pk、ak分别表示船载罗经设备在k时刻获取到的船载导航雷达的俯仰角和航向角。

17、步骤4-3、定义观测向量zk，具体表达式为：

18、

19、式中，表示在k时刻修正过电离层延时误差和对流层延时误差的第1颗北斗卫星伪距。

20、表示在k时刻修正过电离层延时误差和对流层延时误差的第m颗北斗卫星伪距。

21、表示在k时刻修正过电离层延时误差和对流层延时误差的第1颗gps卫星伪距。

22、表示在k时刻修正过电离层延时误差和对流层延时误差的第n颗gps卫星伪距。

23、步骤5、基于卡尔曼滤波算法的运动状态估计：对步骤4构建的船载导航雷达运动状态估计模型xk，进行卡尔曼滤波算法估计，从而得到船载导航雷达在k时刻的运动状态估计值

24、步骤5中，采用卡尔曼滤波算法获取的方法，包括如下步骤：

25、步骤5-1、构建运动状态预测模型，具体表达式为：

26、

27、式中，表示k时刻运动状态的预测值。

28、a表示状态转移矩阵，为经验矩阵。

29、表示k-1时刻运动状态的估计值。

30、b表示船载导航雷达运动状态估计模型xk的输入矩阵。

31、uk表示k时刻模型的输入向量。

32、步骤5-2、构建运动状态协方差矩阵预测模型，具体表达式为：

33、

34、式中，表示k时刻运动状态协方差矩阵的预测值。

35、表示k-1时刻运动状态协方差矩阵的估计值。

36、q表示系统噪声矩阵，为经验矩阵。

37、步骤5-3、更新卡尔曼滤波增益，具体为：

38、

39、式中，kk表示k时刻船载导航雷达运动状态估计模型xk的卡尔曼滤波增益。

40、h表示船载导航雷达运动状态估计模型xk的观测矩阵。

41、r表示过程噪声矩阵，为经验矩阵。

42、步骤5-4、卡尔曼滤波状态估计，具体表达式为：

43、

44、还包括步骤5-5、运动状态协方差矩阵估计，具体表达式为：

45、

46、式中，表示k时刻运动状态协方差矩阵的估计值。

47、步骤4-3中，北斗卫星使用北斗b1i和b3i两个频段上的伪距进行电离层延时误差修正，其载波频率分别为1561.098mhz和1268.520mhz。

48、步骤4-3中，gps卫星使用gps的l1和l5两个频段上的伪距进行电离层延时误差修正，其载波频率分别为1575.42mhz和1176.45mhz。

49、步骤4中，船载导航雷达运动状态估计模型的构建方法，还包括步骤4-4、构建船载导航雷达运动状态的转移方程，具体表达式为：

50、

51、式中，λk-1、hk-1分别表示船载导航雷达在k-1时刻的纬度、经度和海拔高度。

52、δt为采样时间，rm为子午线半径，rn为基准椭球体的卯酉圈曲率半径。

53、步骤4中，船载导航雷达运动状态估计模型的构建方法，还包括步骤4-5、构建观测方程，具体为：

54、

55、式中，表示船载卫星终端在k时刻接收到的第1颗北斗卫星位置坐标。

56、表示船载卫星终端在k时刻接收到的第m颗北斗卫星位置坐标。

57、表示船载卫星终端在k时刻接收到的第1颗gps卫星位置坐标。

58、表示船载卫星终端在k时刻接收到的第n颗gps卫星位置坐标。

59、εk表示k时刻的观测噪声矩阵。

60、本发明具有如下有益效果：

61、1、本发明提出一种基于北斗、gps和罗经数据的运动状态估计方法，该方法具有精度高、容错性好、成本低等特点，可以得到船载导航雷达运动状态的精确估计值。

62、2、本发明提出的方法融合了北斗、gps和罗经提供的相关信息(位置、速度、航向)，大幅提升了系统可靠性，同时也对北斗系统在水上交通运输领域的应用普及具有一定的推广意义和实用价值。