CN112964250B - 基于北斗和降维imu数据的船舶运动姿态估计方法 - Google Patents

基于北斗和降维imu数据的船舶运动姿态估计方法 Download PDF

Info

Publication number
CN112964250B
CN112964250B CN202110167890.5A CN202110167890A CN112964250B CN 112964250 B CN112964250 B CN 112964250B CN 202110167890 A CN202110167890 A CN 202110167890A CN 112964250 B CN112964250 B CN 112964250B
Authority
CN
China
Prior art keywords
ship
beidou
axis
motion
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110167890.5A
Other languages
English (en)
Other versions
CN112964250A (zh
Inventor
夏亮
叶楠
金家龙
王祥
刘希慧
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Shipbuilding Pengli Nanjing Atmospheric And Ocean Information System Co ltd
Original Assignee
CSIC Pride Nanjing Atmospheric and Oceanic Information System Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by CSIC Pride Nanjing Atmospheric and Oceanic Information System Co Ltd filed Critical CSIC Pride Nanjing Atmospheric and Oceanic Information System Co Ltd
Priority to CN202110167890.5A priority Critical patent/CN112964250B/zh
Publication of CN112964250A publication Critical patent/CN112964250A/zh
Application granted granted Critical
Publication of CN112964250B publication Critical patent/CN112964250B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
    • G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于北斗和降维IMU数据的船舶运动姿态估计方法，包括步骤为：首先选取合适的坐标系，定义船舶六个自由度的运动；然后融合北斗和降维IMU数据，构建船舶运动姿态模型；最后进行粒子滤波算法的递推计算，得到船舶运动姿态的精确估计值。本发明将北斗数据和降维IMU数据进行融合，实现船舶运动姿态的准确可靠估计，主要本发明方法可以在非线性、非高斯的场景下，精确估计出船舶的运动姿态，具有可靠性高、适用性好、成本低等特点。

Description

基于北斗和降维IMU数据的船舶运动姿态估计方法
技术领域
本发明涉及多源信息融合领域，特别是一种基于北斗和降维IMU数据的船舶运动姿态估计方法。
背景技术
船舶在海上航行时，由于受到海风、海浪和洋流等复杂海况的影响，不可避免地会产生存在相互耦合作用的一个六自由度摇荡运动，这使得船舶在海上航行、海上作业时具有很大的安全隐患。因此，如果能够准确可靠估计出船舶的运动姿态信息，将大幅提升船舶海上作业的安全性和稳定性，可以有力保障船舶海上安全航行、安全作业，促进我国海洋经济的高质量发展，具有显著的社会价值、经济价值和军事价值。
目前常用的船舶运动姿态测量方法有全球定位系统(Global PositioningSystem，GPS)检测法、惯性传感器检测法、机器视觉检测法等。其中，船载AIS设备都是使用美国的GPS提供船舶的位置、速度等信息，但GPS受制于美国的选择失效SD技术，该技术可通过基于陆地上的干扰源来干扰民用GPS信号，导致AIS精度大幅降低、可用性减弱。与此同时，通常使用完整的全维惯性测量单元Inertial Measurement Unit，IMU来确定船舶姿态角信息，该IMU包括3个加速度计和3个角速度陀螺仪。船舶姿态角信息可以通过全维IMU的捷联算法计算得出，然而全维IMU的价格昂贵，尤其是三个高精度的陀螺仪价格。
着眼于国家安全和经济社会发展需要，我国自主研制建设了北斗卫星导航系统(BeiDou Navigation Satellite System，BDS)，该系统不受国外政策和形势的影响，可以为用户提供全天候、高精度、强安全的服务。伴随系统服务能力提升，北斗应用领域快速扩展，政府部门的公务船只、渔船、客船、货船、深海科考船等已经逐渐使用北斗船载终端来替代船载AIS设备，该终端使用BDS为船舶提供位置、速度等信息。
传统的检测方法一般建立线性的船舶运动模型，并将信号噪声设定为高斯白噪声，但在复杂的水域环境下，不确定的海况(天气变化引起的大风浪)会使船舶运动姿态和传感器噪声统计特性发生变化，使得预先设定的船舶运动模型和噪声模型与实际情况不一致，从而导致测量精度下降、适用性变差，无法准确估计出船舶运动姿态。
发明内容
本发明要解决的技术问题是针对上述现有技术的不足，而提供一种基于北斗和降维IMU数据的船舶运动姿态估计方法，该基于北斗和降维IMU数据的船舶运动姿态估计方法将北斗数据和降维IMU数据进行融合，并使用粒子滤波算法进行递推计算，实现船舶运动姿态的准确可靠估计。本发明方法可以在非线性、非高斯的场景下，精确估计出船舶的运动姿态，具有可靠性高、适用性好、成本低等特点。
为解决上述技术问题，本发明采用的技术方案是：
一种基于北斗和降维IMU数据的船舶运动姿态估计方法，包括如下步骤。
步骤1、选取坐标系：选取北东地坐标系NED和船体坐标系，来描述船舶运动情况。其中，北东地坐标系NED的定义方法为：以船舶的质心作为北东地坐标系的原点，N轴指向正北方向，E轴指向正东方向，D轴与N，E两轴所在平面垂直并且指向地心。
船体坐标系的定义方法为：以船舶的质心作为船体坐标系的原点，x轴与海面平行且指向船艏，y轴与海面平行且垂直于x轴，z轴垂直于x、y两轴确定的平面。
步骤2、在船体坐标系中定义六个自由度的运动：在步骤1选取的船体坐标系中，定义船舶六个自由度的运动。船舶六个自由度的运动包括纵荡Surge、横荡Sway、垂荡Heave、横摇Roll、纵摇Pitch和艏摇Yaw。具体定义方法为：定义船舶沿x轴前后运动为纵荡Surge，船舶沿y轴左右运动为横荡Sway，船舶沿z轴上下运动为垂荡Heave，船舶绕x轴旋转运动为横摇Roll，船舶绕y轴旋转运动为纵摇Pitch，船舶绕z轴旋转运动为艏摇Yaw。
步骤3、安装船载传感器：船载传感器包括北斗船载终端和降维IMU。其中，降维IMU包括一个陀螺仪和两个加速度计。
北斗船载终端安装在邻近船舶质心的位置，用于获取船舶的位置及速度信息。
陀螺仪沿船体坐标系的z轴放置且邻近船舶质心，用于测量横摆角速度。
其中一个加速度计沿船体坐标系的y轴水平放置，用于测量横向加速度。
另一个加速度计沿船体坐标系的x轴水平放置，用于测量纵向加速度。
步骤4、构建船舶运动姿态模型：船舶运动姿态模型包括待求解姿态向量Xk、输入向量Uk、姿态转移方程和观测向量Zk，具体构建方法如下：
步骤41、定义船舶在离散化时刻k的待求解姿态向量Xk为：
Figure BDA0002938109780000021
式(1)中，
Figure BDA0002938109780000022
λk、hk分别表示船舶在离散化时刻k的纬度、经度和海拔高度。
Figure BDA0002938109780000023
分别表示船舶在离散化时刻k的东向速度、北向速度和垂向速度。φk、θk、ψk分别表示船舶在离散化时刻k的横摇角、纵摇角和艏摇角。
步骤42、定义船舶在离散化时刻k的输入向量Uk为：
Figure BDA0002938109780000024
式(2)中，
Figure BDA0002938109780000031
分别表示在离散化时刻k时，北斗船载终端获取到的船舶速度和船舶纵向加速度。
Figure BDA0002938109780000032
分别表示在离散化时刻k时，降维IMU测量得到的船舶横向加速度、纵向加速度和横摆角速度。
步骤43、定义船舶在离散化时刻k的的姿态转移方程为：
Figure BDA0002938109780000033
式(3)中，Δt为采样时间，RM为子午线半径，RN为基准椭球体的卯酉圈曲率半径，we为地球自转速度，g为重力加速度。
Figure BDA0002938109780000034
λk-1、hk-1分别表示船舶在离散化时刻k-1时的纬度、经度和海拔高度。
Figure BDA0002938109780000035
表示船舶在离散化时刻k-1时的东向速度。
步骤44、定义船舶在离散化时刻k的观测向量Zk为：
Figure BDA0002938109780000036
式(4)中，m表示各时刻接收到的北斗卫星数目，
Figure BDA0002938109780000037
ρBDS分别表示第1颗、第2颗、……、第m颗北斗卫星的修正过电离层延时误差和对流层延时误差的卫星伪距。
步骤5、基于粒子滤波算法的船舶运动姿态估计：采用粒子滤波算法，对步骤4构建的船舶运动姿态模型，进行求解，得到当前k时刻下船舶的运动姿态信息估计值
Figure BDA0002938109780000038
为。
Figure BDA0002938109780000039
式(6)中，
Figure BDA00029381097800000310
分别表示船舶在离散化时刻k时的纬度预估值、经度预估值和海拔高度预估值。
Figure BDA00029381097800000311
分别表示船舶在离散化时刻k的东向速度预估值、北向速度预估值和垂向速度预估值。
Figure BDA00029381097800000312
分别表示船舶在离散化时刻k的横摇角预估值、纵摇角预估值和艏摇角预估值。
在求解过程中，令离散化时刻k＝1时船舶纬度、经度、海拔高度以及东向速度
Figure BDA0002938109780000041
λk＝1、hk＝1以及
Figure BDA0002938109780000042
均采用北斗船载终端在k＝1时的位置及速度信息。
步骤6、基于运动姿态信息的六自由度运动描述：根据步骤5得到的当前k时刻下船舶的运动姿态信息估计值
Figure BDA0002938109780000043
描述当前k时刻下的船舶六自由度运动状况。具体描述方式为：
采用横摇角预估值
Figure BDA0002938109780000044
表示船舶的横摇运动状况。
采用纵摇角预估值
Figure BDA0002938109780000045
表示船舶的纵摇运动状况。
采用艏摇角预估值
Figure BDA0002938109780000046
表示船舶的艏摇运动状况。
采用δx、δy、δz分别表示船舶横荡、纵荡、垂荡运动状况。其中，δx、δy、δz的计算公式为：
Figure BDA0002938109780000047
式(7)中，xk、yk、zk分别表示
Figure BDA0002938109780000048
转换到切平面直角坐标系下的位置坐标。
xk-1、yk-1、zk-1分别表示
Figure BDA0002938109780000049
转换到切平面直角坐标系下的位置坐标。其中，
Figure BDA00029381097800000410
分别表示船舶在离散化时刻k-1时的纬度预估值、经度预估值和海拔高度预估值。
还包括步骤45，计算观测向量Zk，具体计算方法为：
Figure BDA00029381097800000411
式(5)中，xBDS，yBDS，zBDS表示北斗船载终端接收到的卫星位置坐标，
xBDS，1，yBDS，1，zBDS，1表示北斗船载终端接收到的第1颗北斗卫星的位置坐标。
xBDS，2，yBDS，2，zBDS，2表示北斗船载终端接收到的第2颗北斗卫星的位置坐标。
xBDS，m，yBDS，m，zBDS，m表示北斗船载终端接收到的第m颗北斗卫星的位置坐标，e表示椭球第一偏心率。
每个北斗船载终端均为多频接收终端，均能够接收B1I和B3I两个频段上的伪距进行电离层延时误差修正。
B1I和B3I对应的载波频率分别为1561.098MHz和1268.520MHz。
本发明具有如下有益效果：
1、本发明将北斗数据和降维IMU数据进行融合，并使用粒子滤波算法进行递推计算，实现船舶运动姿态的准确可靠估计。其中，降维IMU使用降维IMU一个陀螺仪和两个加速度计来测量船舶的姿态角，从而能够有效降低成本。
2、本发明使用粒子滤波算法对姿态模型进行递推计算，能够在非线性、非高斯的场景下，精确估计出船舶的运动姿态。由于粒子滤波算法就是为克服非线性问题被设计出来的，该算法不受模型的线性和高斯假设的约束，因而，具有可靠性高、适用性好、成本低等特点。因而，本发明能够适用于非线性、非高斯的场景，在复杂的海况环境下，运动姿态估计结果可以满足船舶海上安全航行、安全作业的要求。其中，非线性、非高斯场景，通常指的是天气变化引起的大风浪等海面状况。
3、本发明融合了北斗船载终端提供的船舶相关信息(位置、速度、艏摇)，大幅提升系统可靠性，同时也对北斗卫星导航系统在海上交通运输领域的应用普及具有一定的推广意义和实用价值；
附图说明
图1显示了本发明基于北斗和降维IMU数据的船舶运动姿态估计方法的流程示意图。
图2显示了北东地坐标系的坐标定义示意图。
图3显示了船体坐标系的坐标定义示意图。
图4显示了船舶六自由度运动示意图。
图5显示了粒子滤波示意图。
具体实施方式
下面结合附图和具体较佳实施方式对本发明作进一步详细的说明。
本发明的描述中，需要理解的是，术语“左侧”、“右侧”、“上部”、“下部”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，“第一”、“第二”等并不表示零部件的重要程度，因此不能理解为对本发明的限制。本实施例中采用的具体尺寸只是为了举例说明技术方案，并不限制本发明的保护范围。
如图1所示，一种基于北斗和降维IMU数据的船舶运动姿态估计方法，包括如下步骤。
步骤1、选取坐标系：选取北东地坐标系NED和船体坐标系，来描述船舶运动情况。
如图2所示，北东地坐标系NED(North-East-Down)，为惯性参考坐标系，其定义方法为：以船舶的质心作为坐标系原点，N轴指向正北方向，E轴指向正东方向，D轴与N，E两轴所在平面垂直并且指向地心。
如图3所示，船体坐标系的定义方法为：以船舶的质心作为坐标系原点，x轴与海面平行指向船艏，y轴与海面平行垂直于x轴，z轴垂直于x，y两轴确定的平面。
步骤2、在坐标系中定义六个自由度的运动
在步骤1选取的船体坐标系中，定义船舶六个自由度的运动。
船舶六个自由度的运动包括纵荡Surge、横荡Sway、垂荡Heave、横摇Roll、纵摇Pitch和艏摇Yaw。其中，横摇、纵摇、艏摇为三个绕轴转动的相对姿态角；横荡、纵荡、垂荡为三个相对平移量。
如图4所示，六个自由度运动的具体定义方法为：
在船体坐标系中，定义船舶沿x轴前后运动为纵荡Surge，船舶沿y轴左右运动为横荡Sway，船舶沿z轴上下运动为垂荡Heave，船舶绕x轴旋转运动为横摇Roll，船舶绕y轴旋转运动为纵摇Pitch，船舶绕z轴旋转运动为艏摇Yaw。
步骤3、安装船载传感器：船载传感器包括北斗船载终端和降维IMU。其中，降维IMU包括一个陀螺仪和两个加速度计。
北斗船载终端安装在邻近船舶质心的位置，用于获取船舶的位置及速度信息。
陀螺仪沿船体坐标系的z轴放置且邻近船舶质心，用于测量横摆角速度。
其中一个加速度计沿船体坐标系的y轴水平放置，用于测量横向加速度。
另一个加速度计沿船体坐标系的x轴水平放置，用于测量纵向加速度。
步骤4、构建船舶运动姿态模型：船舶运动姿态模型包括待求解姿态向量Xk、输入向量Uk、姿态转移方程和观测向量Zk，具体构建方法如下：
步骤41、定义船舶在离散化时刻k的待求解姿态向量Xk为：
Figure BDA0002938109780000061
式(1)中，
Figure BDA0002938109780000062
λk、hk分别表示船舶在离散化时刻k的纬度、经度和海拔高度。
Figure BDA0002938109780000063
分别表示船舶在离散化时刻k的东向速度、北向速度和垂向速度。φk、θk、ψk分别表示船舶在离散化时刻k的横摇角、纵摇角和艏摇角。
步骤42、定义船舶在离散化时刻k的输入向量Uk为：
Figure BDA0002938109780000071
式(2)中，
Figure BDA0002938109780000072
分别表示在离散化时刻k时，北斗船载终端获取到的船舶速度和船舶纵向加速度。
Figure BDA0002938109780000073
分别表示在离散化时刻k时，降维IMU测量得到的船舶横向加速度、纵向加速度和横摆角速度。
步骤43、定义船舶在离散化时刻k的的姿态转移方程为：
Figure BDA0002938109780000074
式(3)中，Δt为采样时间，RM为子午线半径，RN为基准椭球体的卯酉圈曲率半径，we为地球自转速度，g为重力加速度。
Figure BDA0002938109780000075
λk-1、hk-1分别表示船舶在离散化时刻k-1时的纬度、经度和海拔高度。
Figure BDA0002938109780000076
表示船舶在离散化时刻k-1时的东向速度。
步骤44、定义船舶在离散化时刻k的观测向量Zk为：
Figure BDA0002938109780000077
式(4)中，m表示各时刻接收到的北斗卫星数目，
Figure BDA0002938109780000078
ρBDS分别表示第1颗、第2颗、……、第m颗北斗卫星的修正过电离层延时误差和对流层延时误差的卫星伪距。
步骤45，计算观测向量zk，具体计算方法为：
Figure BDA0002938109780000081
式(5)中，xBDS，yBDS，zBDS表示北斗船载终端接收到的卫星位置坐标，xBDS，1，yBDS ，1，zBDS，1表示北斗船载终端接收到的第1颗北斗卫星的位置坐标。xBDS，2，yBDS，2，zBDS，2表示北斗船载终端接收到的第2颗北斗卫星的位置坐标。xBDS，m，yBDS，m，zBDS，m表示北斗船载终端接收到的第m颗北斗卫星的位置坐标，e表示椭球第一偏心率，为已知值。
上述北斗卫星的空间位置坐标解算过程为现有技术，可以参考文献李亚飞.北斗/GPS双模定位关键技术研究[D].中国地质大学北京,2014.。
现有方法通常采用数学模型来估算电离层延时，实验证明这些模型只能修正50％到60％的电离层延时误差。
本发明中，每个北斗船载终端均采用多频接收终端，均能够接收B1I和B3I两个频段上的伪距进行电离层延时误差修正。
对于这些多频接收终端，可以不借助任何电离层延时数学估算模型，而是直接利用北斗卫星两个频段的伪距测量值对电离层延时进行实时测定，进而完全消除电离层延时误差。
本发明方法使用北斗B1I和B3I两个频段上的伪距进行电离层延时误差修正，其载波频率分别为1561.098MHz和1268.520MHz，双频修正后的伪距值将不再含有电离层延时误差，具体过程可参考文献谢钢.GPS原理与接收机设计[M].电子工业出版社,2009。
步骤5、基于粒子滤波算法的船舶运动姿态估计：采用粒子滤波算法，对步骤4构建的船舶运动姿态模型，进行求解，得到当前k时刻下船舶的运动姿态信息估计值
Figure BDA0002938109780000082
为。
Figure BDA0002938109780000083
式(6)中，
Figure BDA0002938109780000084
分别表示船舶在离散化时刻k时的纬度预估值、经度预估值和海拔高度预估值。
Figure BDA0002938109780000085
分别表示船舶在离散化时刻k的东向速度预估值、北向速度预估值和垂向速度预估值。
Figure BDA0002938109780000091
分别表示船舶在离散化时刻k的横摇角预估值、纵摇角预估值和艏摇角预估值。
在求解过程中，令离散化时刻k＝1时船舶纬度、经度、海拔高度以及东向速度
Figure BDA0002938109780000092
λk＝1、hk＝1以及
Figure BDA0002938109780000093
均采用北斗船载终端在k＝1时的位置及速度信息。
本发明中建立的运动姿态转移方程和观测方程都是非线性的，传统的扩展卡尔曼滤波EKF、无迹卡尔曼滤波UKF等算法在处理这种非线性、非高斯问题时，因适用性差，精度会大幅降低。故本发明使用粒子滤波算法来处理，且在采样点数足够多(本实施例中，选取的采样点数为300)的情况下，精度可以逼近最优估计，是一种很有效的非线性滤波技术。
上述粒子滤波算法，为现有技术，本发明中，仅通过对粒子滤波算法中观测向量Zk的北斗伪距进行了修正。
粒子滤波的中心思想是使用具有权值的随机粒子样本集合
Figure BDA0002938109780000094
来逼近后验概率密度p(Xk|Zk)。其中
Figure BDA0002938109780000095
表示k时刻第i个代表船舶运动姿态信息的粒子，
Figure BDA0002938109780000096
表示
Figure BDA0002938109780000097
粒子的权重值。
整个滤波过程如图5所示，图中箭头表示各个粒子的转移过程，圆的面积越大表示该粒子的权值越大。最后根据权值更新后的粒子估计k时刻的状态值，并为下一时刻迭代进行重采样。经过重采样，使权值大的粒子被复制，而权值小的粒子慢慢被舍弃，且新粒子的权值被重新设置为1/N，其中，N表示粒子点数，本发明中N＝300。
针对本发明建立的船舶运动姿态模型，粒子滤波具体过程如下所示：
1)初始化
在初始0时刻时，用先验概率密度p(X0)产生粒子群
Figure BDA0002938109780000098
其中
Figure BDA0002938109780000099
其中，N表示粒子点数。
2)预测
在时刻k≥1时，用姿态转移方程更新粒子群
Figure BDA00029381097800000910
获得新的粒子群
Figure BDA00029381097800000911
在粒子滤波算法中，先验概率密度
Figure BDA00029381097800000912
被用来作为重要性密度函数；
Figure BDA00029381097800000913
表示k-1时刻第i个代表船舶运动姿态信息的粒子，
Figure BDA00029381097800000914
表示
Figure BDA00029381097800000915
粒子的权重值；
Figure BDA00029381097800000916
表示k时刻经过姿态转移方程更新过的粒子，
Figure BDA00029381097800000917
表示
Figure BDA00029381097800000918
粒子的权重值。
3)更新
依据当前的观测值，用观测似然概率函数
Figure BDA0002938109780000101
来更新粒子权重值，并且做归一化处理，获得新的权值
Figure BDA0002938109780000102
用这一系列有权重的粒子
Figure BDA0002938109780000103
来近似后验概率密度p(Xk|Zk)，其中，
Figure BDA0002938109780000104
表示归一化之后的权重值。
4)输出
获得k时刻的姿态估计
Figure BDA0002938109780000105
由此确定在当前k时刻下船舶的运动姿态
Figure BDA0002938109780000106
其中，∑为求和符号，即：将该时刻所有粒子的值与其权重相乘并求和得到最终的估计值。
5)重采样
为下一时刻迭代，获取新的粒子群
Figure BDA0002938109780000107
步骤6、基于运动姿态信息的六自由度运动描述：根据步骤5得到的当前k时刻下船舶的运动姿态信息估计值
Figure BDA0002938109780000108
描述当前k时刻下的船舶六自由度运动状况。具体描述方式为：
采用横摇角预估值
Figure BDA0002938109780000109
表示船舶的横摇运动状况。
采用纵摇角预估值
Figure BDA00029381097800001010
表示船舶的纵摇运动状况。
采用艏摇角预估值
Figure BDA00029381097800001011
表示船舶的艏摇运动状况。
采用δx、δy、δz分别表示船舶横荡、纵荡、垂荡运动状况，则δx、δy、δz计算公式为：
Figure BDA00029381097800001012
式(7)中，xk、yk、zk分别表示
Figure BDA00029381097800001013
转换到切平面直角坐标系下的位置坐标。
xk-1、yk-1、zk-1分别表示
Figure BDA00029381097800001014
转换到切平面直角坐标系下的位置坐标。
其中，
Figure BDA00029381097800001015
分别表示船舶在离散化时刻k-1时的纬度预估值、经度预估值和海拔高度预估值。
以上详细描述了本发明的优选实施方式，但是，本发明并不限于上述实施方式中的具体细节，在本发明的技术构思范围内，可以对本发明的技术方案进行多种等同变换，这些等同变换均属于本发明的保护范围。

Claims (4)

1.一种基于北斗和降维IMU数据的船舶运动姿态估计方法，其特征在于：包括如下步骤：
步骤1、选取坐标系：选取北东地坐标系NED和船体坐标系，来描述船舶运动情况；其中，北东地坐标系NED的定义方法为：以船舶的质心作为北东地坐标系的原点，N轴指向正北方向，E轴指向正东方向，D轴与N，E两轴所在平面垂直并且指向地心；
船体坐标系的定义方法为：以船舶的质心作为船体坐标系的原点，x轴与海面平行且指向船艏，y轴与海面平行且垂直于x轴，z轴垂直于x、y两轴确定的平面；
步骤2、在船体坐标系中定义六个自由度的运动：在步骤1选取的船体坐标系中，定义船舶六个自由度的运动；船舶六个自由度的运动包括纵荡Surge、横荡Sway、垂荡Heave、横摇Roll、纵摇Pitch和艏摇Yaw；具体定义方法为：定义船舶沿x轴前后运动为纵荡Surge，船舶沿y轴左右运动为横荡Sway，船舶沿z轴上下运动为垂荡Heave，船舶绕x轴旋转运动为横摇Roll，船舶绕y轴旋转运动为纵摇Pitch，船舶绕z轴旋转运动为艏摇Yaw；
步骤3、安装船载传感器：船载传感器包括北斗船载终端和降维IMU；其中，降维IMU包括一个陀螺仪和两个加速度计；
北斗船载终端安装在邻近船舶质心的位置，用于获取船舶的位置及速度信息；
陀螺仪沿船体坐标系的z轴放置且邻近船舶质心，用于测量横摆角速度；
其中一个加速度计沿船体坐标系的y轴水平放置，用于测量横向加速度；
另一个加速度计沿船体坐标系的x轴水平放置，用于测量纵向加速度；
步骤4、构建船舶运动姿态模型：船舶运动姿态模型包括待求解姿态向量Xk、输入向量Uk、姿态转移方程和观测向量Zk，具体构建方法如下：
步骤41、定义船舶在离散化时刻k的待求解姿态向量Xk为：
Figure FDA0002938109770000011
式(1)中，
Figure FDA0002938109770000012
λk、hk分别表示船舶在离散化时刻k的纬度、经度和海拔高度；
Figure FDA0002938109770000013
分别表示船舶在离散化时刻k的东向速度、北向速度和垂向速度；φk、θk、ψk分别表示船舶在离散化时刻k的横摇角、纵摇角和艏摇角；
步骤42、定义船舶在离散化时刻k的输入向量Uk为：
Figure FDA0002938109770000014
式(2)中，
Figure FDA0002938109770000015
分别表示存离散化时刻k时，北斗船载终端获取到的船舶速度和船舶纵向加速度；
Figure FDA0002938109770000016
分别表示在离散化时刻k时，降维IMU测量得到的船舶横向加速度、纵向加速度和横摆角速度；
步骤43、定义船舶在离散化时刻k的姿态转移方程为：
Figure FDA0002938109770000021
式(3)中，Δt为采样时间，RM为子午线半径，RN为基准椭球体的卯酉圈曲率半径，we为地球自转速度，g为重力加速度；
Figure FDA0002938109770000022
λk-1、hk-1分别表示船舶在离散化时刻k-1时的纬度、经度和海拔高度；
Figure FDA0002938109770000023
表示船舶在离散化时刻k-1时的东向速度；
步骤44、定义船舶在离散化时刻k的观测向量Zk为：
Figure FDA0002938109770000024
式(4)中，m表示各时刻接收到的北斗卫星数目，
Figure FDA0002938109770000025
ρBDS分别表示第1颗、第2颗、……、第m颗北斗卫星的修正过电离层延时误差和对流层延时误差的卫星伪距；
步骤5、基于粒子滤波算法的船舶运动姿态估计：采用粒子滤波算法，对步骤4构建的船舶运动姿态模型，进行求解，得到当前k时刻下船舶的运动姿态信息估计值
Figure FDA0002938109770000026
为；
Figure FDA0002938109770000027
式(6)中，
Figure FDA0002938109770000028
分别表示船舶在离散化时刻k时的纬度预估值、经度预估值和海拔高度预估值；
Figure FDA0002938109770000029
分别表示船舶在离散化时刻k的东向速度预估值、北向速度预估值和垂向速度预估值；
Figure FDA00029381097700000210
分别表示船舶在离散化时刻k的横摇角预估值、纵摇角预估值和艏摇角预估值；
在求解过程中，令离散化时刻k＝1时船舶纬度、经度、海拔高度以及东向速度
Figure FDA0002938109770000031
λk＝1、hk＝1以及
Figure FDA0002938109770000032
均采用北斗船载终端在k＝1时的位置及速度信息；
步骤6、基于运动姿态信息的六自由度运动描述：根据步骤5得到的当前k时刻下船舶的运动姿态信息估计值
Figure FDA0002938109770000033
描述当前k时刻下的船舶六自由度运动状况；具体描述方式为：
采用横摇角预估值
Figure FDA0002938109770000034
表示船舶的横摇运动状况；
采用纵摇角预估值
Figure FDA0002938109770000035
表示船舶的纵摇运动状况；
采用艏摇角预估值
Figure FDA0002938109770000036
表示船舶的艏摇运动状况；
采用δx、δy、δz分别表示船舶横荡、纵荡、垂荡运动状况；其中，δx、δy、δz的计算公式为：
Figure FDA0002938109770000037
式(7)中，xk、yk、zk分别表示
Figure FDA0002938109770000038
转换到切平面直角坐标系下的位置坐标；xk-1、yk-1、zk-1分别表示
Figure FDA0002938109770000039
转换到切平面直角坐标系下的位置坐标；其中，
Figure FDA00029381097700000310
分别表示船舶在离散化时刻k-1时的纬度预估值、经度预估值和海拔高度预估值。
2.根据权利要求1所述的基于北斗和降维IMU数据的船舶运动姿态估计方法，其特征在于：还包括步骤45，计算观测向量Zk，具体计算方法为：
Figure FDA00029381097700000311
式(5)中，xBDS，yBDS，zBDS表示北斗船载终端接收到的卫星位置坐标，xBDS，1，yBDS，1，zBDS，1表示北斗船载终端接收到的第1颗北斗卫星的位置坐标；xBDS，2，yBDS，2，zBDS，2表示北斗船载终端接收到的第2颗北斗卫星的位置坐标；xBDS，m，yBDS，m，zBDS，m表示北斗船载终端接收到的第m颗北斗卫星的位置坐标；
e表示椭球第一偏心率。
3.根据权利要求1或2所述的基于北斗和降维IMU数据的船舶运动姿态估计方法，其特征在于：每个北斗船载终端均为多频接收终端，均能够接收B1I和B3I两个频段上的伪距进行电离层延时误差修正。
4.根据权利要求3所述的基于北斗和降维IMU数据的船舶运动姿态估计方法，其特征在于：B1I和B3I对应的载波频率分别为1561.098MHz和1268.520MHz。
CN202110167890.5A 2021-02-07 2021-02-07 基于北斗和降维imu数据的船舶运动姿态估计方法 Active CN112964250B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110167890.5A CN112964250B (zh) 2021-02-07 2021-02-07 基于北斗和降维imu数据的船舶运动姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110167890.5A CN112964250B (zh) 2021-02-07 2021-02-07 基于北斗和降维imu数据的船舶运动姿态估计方法

Publications (2)

Publication Number Publication Date
CN112964250A CN112964250A (zh) 2021-06-15
CN112964250B true CN112964250B (zh) 2022-04-05

Family

ID=76275064

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110167890.5A Active CN112964250B (zh) 2021-02-07 2021-02-07 基于北斗和降维imu数据的船舶运动姿态估计方法

Country Status (1)

Country Link
CN (1) CN112964250B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114659496B (zh) * 2022-05-25 2022-08-02 南京恒舟准导航科技有限公司 一种用于船载北斗一体机倾斜监测的方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10809384B2 (en) * 2017-02-09 2020-10-20 Jackson Labs Technologies, Inc. Method and apparatus to retrofit legacy global positioning satellite (GPS) and other global navigation satellite system (GNSS) receivers
CN109911110B (zh) * 2019-03-27 2020-04-21 武汉理工大学 变稳船
AU2020101525A4 (en) * 2020-07-28 2020-09-03 Xi`an University of Architecture and Technology Inertial basis combined navigation method for indirect grid frame in polar zone

Also Published As

Publication number Publication date
CN112964250A (zh) 2021-06-15

Similar Documents

Publication Publication Date Title
CN112987066B (zh) 基于多系统多源定位数据融合的海上目标定位方法
CN102918416B (zh) 用于确定车辆的方位角的系统和方法
US20070040737A1 (en) Apparatus and method for carrier phase-based relative positioning
CN103744098A (zh) 基于sins/dvl/gps的auv组合导航系统
CN110133700B (zh) 一种船载综合导航定位方法
CN112859133B (zh) 一种基于雷达与北斗数据的船舶深度融合定位方法
CN111829512A (zh) 一种基于多传感器数据融合的auv导航定位方法及系统
CN113466912B (zh) 一种基于多频gnss双天线海上船舶姿态确定方法
CN112964250B (zh) 基于北斗和降维imu数据的船舶运动姿态估计方法
Zhang et al. An INS-aided MASS autonomous navigation algorithm considering virtual motion constraints and the leeway and drift angle
Talwani et al. Navigation at sea by satellite
Godhavn High quality heave measurements based on GPS RTK and accelerometer technology
CN113155134A (zh) 一种基于惯性信息辅助的水声信道跟踪与预测方法
CN114659496B (zh) 一种用于船载北斗一体机倾斜监测的方法
Li et al. Adaptively robust filtering algorithm for maritime celestial navigation
CN201716421U (zh) 小型水下航行器组合导航装置
CN115390120A (zh) 一种海上平台与船舶避碰预警方法及存储介质
JP2023548513A (ja) 多義性の解明によって少なくとも１つのｇｎｓｓ衛星信号を評価する方法
RU2319930C2 (ru) Корректируемая система инерциальной навигации и стабилизации
Liu Robust Multi-sensor Data Fusion for Practical Unmanned Surface Vehicles (USVs) Navigation
Inazu et al. Extracting clearer tsunami currents from shipborne Automatic Identification System data using ship yaw and equation of ship response
JP4417812B2 (ja) 船舶姿勢角測定装置
US20230393289A1 (en) Method for detecting masking of one or more satellites, electronic detection device and associated computer program product
Gelin A High-Rate Virtual Instrument of Marine Vehicle Motions for Underwater Navigation and Ocean Remote Sensing
Gharib et al. Error analysis of dead reckoning navigation system by considering uncertainties in an underwater vehicle's sensors

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant
CP01 Change in the name or title of a patent holder

Address after: 211153 No.32 Changqing street, Jiangning Development Zone, Nanjing City, Jiangsu Province

Patentee after: China Shipbuilding Pengli (Nanjing) Atmospheric and Ocean Information System Co.,Ltd.

Address before: 211153 No.32 Changqing street, Jiangning Development Zone, Nanjing City, Jiangsu Province

Patentee before: CSIC PRIDE (NANJING) ATMOSPHERE MARINE INFORMATION SYSTEM Co.,Ltd.

CP01 Change in the name or title of a patent holder