CN117346794B - 一种用于浒苔跟踪的无人船组合导航系统及导航方法 - Google Patents

一种用于浒苔跟踪的无人船组合导航系统及导航方法 Download PDF

Info

Publication number
CN117346794B
CN117346794B CN202311648169.3A CN202311648169A CN117346794B CN 117346794 B CN117346794 B CN 117346794B CN 202311648169 A CN202311648169 A CN 202311648169A CN 117346794 B CN117346794 B CN 117346794B
Authority
CN
China
Prior art keywords
module
enteromorpha
representing
inertial navigation
image recognition
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
CN202311648169.3A
Other languages
English (en)
Other versions
CN117346794A (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.)
Institute of Oceanographic Instrumentation Shandong Academy of Sciences
Original Assignee
Institute of Oceanographic Instrumentation Shandong Academy of Sciences
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 Institute of Oceanographic Instrumentation Shandong Academy of Sciences filed Critical Institute of Oceanographic Instrumentation Shandong Academy of Sciences
Priority to CN202311648169.3A priority Critical patent/CN117346794B/zh
Publication of CN117346794A publication Critical patent/CN117346794A/zh
Application granted granted Critical
Publication of CN117346794B publication Critical patent/CN117346794B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • 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/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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • 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/393—Trajectory determination or predictive tracking, e.g. Kalman filtering
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
    • 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

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

本发明公开了一种用于浒苔跟踪的无人船组合导航系统及导航方法，涉及无人船导航技术领域，通过在接收惯导模块、浒苔近距离图像识别模块、全球定位模块输出的相关信息后使用改进的无迹卡尔曼滤波算法对三个模块数据进行融合并读取3D栅格地图信息，实时输出无人船位置信息、姿态信息和速度信息，形成无人船精确自主导航信息。本发明可实现无人船快速高精度自主导航，可明显改善自主导航的精度、输出频率及增强系统的可靠性，获得最佳无人船运行轨迹。

Description

一种用于浒苔跟踪的无人船组合导航系统及导航方法
技术领域
本发明涉及无人船导航技术领域，尤其是一种用于浒苔跟踪的无人船组合导航系统及导航方法。
背景技术
近年来，我国近海海域屡次遭受浒苔侵袭，据自然资源部北海预报中心监测，黄海海域浒苔灾害规模已创历史最大值。针对当前监测手段对浒苔绿潮监测的时空分辨率低、数据时效性不足的问题，融合卫星遥感、人工智能、海洋科学、高端装备制造等多学科优势技术力量，建立多源浒苔绿潮观测体系显得尤为重要。无人船作为一种高新技术手段，很适合作为多源浒苔绿潮观测体系的载体。目前，无人船多以半自动化形式运行，具有完全自主航行能力的无人船是国内外研究的热点之一。为实现无人船自主化航行，自主导航技术是必须具备的一项关键技术，也是无人船实现浒苔绿潮观测的必然需求。现阶段应用于无人船自主导航的传感器主要有:声纳、地磁、惯导等。但是，这些传感器均有其自身的局限性。若采用传统传感器进行自主导航，其精度、可靠性均很难满足大范围长时间浒苔绿潮观测的要求。
发明内容
为了克服现有技术中存在的上述问题，本发明提出一种用于浒苔跟踪的无人船组合导航系统及导航方法。
本发明解决其技术问题所采用的技术方案是：一种用于浒苔跟踪的无人船组合导航系统，包括导航系统、控制系统、定位系统，所述导航系统、控制系统、定位系统均安装于无人船上，所述导航系统、控制系统、定位系统通信连接，所述定位系统包括惯导模块、浒苔近距离图像识别模块、全球定位模块，所述惯导模块与无人船质心重合，所述全球定位模块安装于无人船船尾中心线上，所述浒苔近距离图像识别模块高于海平面1.5米以上，正面摄像头与无人船推进器前进方向一直，正下方与惯导模块位于一条直线。
上述的一种用于浒苔跟踪的无人船组合导航系统，所述全球定位模块、浒苔近距离图像识别模块的输出信息直接接入惯导模块，由惯导模块内部进行初步的组合运算输出组合导航信息，并将其作为无人船的主导航信息；所述全球定位模块作为备份坐标定位，用于输出无人船在海上位置信息；所述全球定位模块和惯导模块组成双备份方位角测量，输出无人船在海上方位角信息；所述浒苔近距离图像识别模块、全球定位模块组成多信息融合制导系统，输出无人船在浒苔绿潮团中相对位置坐标、环境光照度、浒苔绿潮团中心方向信息。
上述的一种用于浒苔跟踪的无人船组合导航系统，所述浒苔近距离图像识别模块包括不少于4个海面浒苔绿潮监测视觉图像传感器，视场覆盖360°；所述惯导模块内部集成加速度计、光纤陀螺仪。
一种用于浒苔跟踪的无人船组合导航方法，应用于上述的一种用于浒苔跟踪的无人船组合导航系统，包括如下步骤：
步骤1，对无人船导航系统进行初始校准；
步骤2，接收惯导模块、浒苔近距离图像识别模块、全球定位模块输出的信息后，使用改进的无迹卡尔曼滤波算法对三个模块数据进行融合，利用滤波器估算的误差校正相应的导航参数；
步骤3，使用SLAM算法读取3D栅格地图信息，将无人船的运动状态从载体坐标系转换到3D栅格地图坐标系。
上述的一种用于浒苔跟踪的无人船组合导航方法，所述步骤1具体包括：无人船初始对准、浒苔近距离图像识别模块初始校准、惯导模块的粗校准和精校准，所述无人船初始对准目的是精确确定载体坐标系和真实导航坐标系之间的初始方向余弦矩阵；所述浒苔近距离图像识别模块初始校准的目的是获得相机坐标系和真实导航坐标系之间的初始方向余弦矩阵。
上述的一种用于浒苔跟踪的无人船组合导航方法，所述步骤1具体初始校准过程包括：无人船在水面静止通电，对惯导模块进行粗校准；浒苔近距离图像识别模块、全球定位模块数据反馈给惯导模块的条件下，通过走阶梯状轨迹，对惯导模块精校准；浒苔近距离图像识别模块、惯导模块、全球定位模块均开始工作，全球定位模块数据不反馈给惯导模块，浒苔近距离图像识别模块数据反馈给惯导模块的条件下，模拟在恶劣天气环境下，无法使用全球定位模块时的导航能力和导航精度；无人船使用浒苔近距离图像识别模块与惯导模块进行组合导航，根据8字形轨迹设定航迹，进行校准效果验证。
上述的一种用于浒苔跟踪的无人船组合导航方法，所述步骤2用改进的无迹卡尔曼滤波算法对三个模块数据进行融合的具体过程包括：将惯导模块作为公共参考模块，判断全球定位模块、浒苔近距离图像识别模块采集的信息是否有效，将惯导模块采集的信息与全球定位模块、浒苔近距离图像识别模块采集的有效信息进行两两融合，组成子滤波器，各子滤波器并行运行，在子滤波器局部量测的基础上获得子最优估计，在主滤波器内按照数据融合算法得到当前量测基础上的全局估计。
上述的一种用于浒苔跟踪的无人船组合导航方法，所述改进的无迹卡尔曼滤波算法具体过程包括：
以地理坐标系n作为导航坐标系，改进的无迹卡尔曼滤波算法的状态向量为：
;
其中，δL为纬度误差，δλ为经度误差，δh为深度误差，δν E 为东向速度误差，δν N 为北向速度误差，δV u 为纵向速度误差，为横滚角误差，/>为航向角误差，/>为纵倾角误差；
惯导模块与全球定位模块构成的改进的无迹卡尔曼滤波算法子滤波器状态方程为：
;
测量方程为：
;
其中，,/>,/>表示滤波后惯导模块状态向量，/>表示滤波后全球定位模块状态向量，/>表示惯导模块状态更新向量，/>表示全球定位模块状态更新向量，/>表示导航模块状态向量，/>表示全球定位模块状态向量，/>表示全球定位模块中的白噪声，/>表示观测矩阵，/>表示导航模块所测纬度值，/>表示全球定位模块所测纬度值，/>表示惯导模块所测经度值，/>表示全球定位模块所测经度值，/>表示惯导模块纬度误差，/>表示全球定位模块纬度误差，/>表示惯导模块经度误差，表示全球定位模块精度误差，VL表示沿纬度方向速度，/>表示沿经度方向速度；
惯导模块与浒苔近距离图像识别模块构成的改进的无迹卡尔曼滤波算法子滤波器状态方程为：
；
测量方程为：
；
其中，，/>，/>表示滤波后浒苔近距离图像识别模块状态向量，/>表示浒苔近距离图像识别模块状态更新量，/>表示浒苔近距离图像识别模块状态向量，/>表示浒苔近距离图像识别模块中的白噪声，/>表示惯导模块东向速度误差，/>表示浒苔近距离图像识别模块东向速度误差，/>表示惯导模块北向速度误差，/>表示浒苔近距离图像识别模块北向速度误差，/>表示惯导模块纵向速度误差，/>表示浒苔近距离图像识别模块纵向速度误差，VE表示动向速度，VN表示北向速度，VU表示纵向速度，HDVL表示浒苔近距离图像识别模块观测向量；
子滤波器单独进行时间更新和状态更新，获得子滤波器的最优估计，输入到主滤波器；主滤波器根据时间更新获取误差的最优估计，并应用到导航定位系统中。
本发明的有益效果是，本发明的技术方案通过使用惯导系统、浒苔近距离图像识别系统、全球定位系统等部件，实现了高精度自主导航。在接收惯导系统、浒苔近距离图像识别系统、全球定位系统输出的无人船在海上位置信息、无人船在海上方位角信息、无人船在浒苔绿潮团中相对位置坐标、环境光照度、浒苔绿潮团中心方向等信息后使用改进的无迹卡尔曼滤波算法对三种传感器数据进行融合并读取3D栅格地图信息，实时输出无人船位置信息、姿态信息和速度信息，形成无人船精确自主导航信息。本发明可实现无人船快速高精度自主导航，可明显改善自主导航的精度、输出频率及增强系统的可靠性，获得最佳无人船运行轨迹。
附图说明
下面结合附图和实施例对本发明进一步说明。
图1为本发明实施例无人组合导航系统结构部署图；
图2为本发明实施例中多传感器校准航迹图，其中（a)为阶梯状轨迹，（b）为8字形轨迹；
图3为本发明实施例中多传感器初始校准流程图；
图4为本发明实施例中捷联惯导系统工作原理图；
图5为本发明实施例中Kalman滤波算法示意图；
图6为本发明实施例中导航定位实现流程图；
图7为本发明实施例中改进的无迹卡尔曼滤波算法流程图；
图8为本发明实施例中SVD-MUKF子滤波器结构图。
具体实施方式
为使本领域技术人员更好的理解本发明的技术方案，下面结合附图和具体实施方式对本发明作详细说明。
本实施例公开了一种用于浒苔跟踪的无人船组合导航系统，结构部署图如图1所示，包括导航系统、控制系统、定位系统，所述导航系统、控制系统、定位系统均安装于无人船上，所述导航系统、控制系统、定位系统通信连接，所述定位系统包括惯导模块、浒苔近距离图像识别模块、全球定位模块，所述惯导模块与无人船质心重合，所述全球定位模块安装于无人船船尾中心线上，所述浒苔近距离图像识别模块高于海平面1.5米以上，正面摄像头与无人船推进器前进方向一直，正下方与惯导模块位于一条直线。
所述全球定位模块、浒苔近距离图像识别模块的输出信息将被直接接入到惯导模块，由惯导模块内部进行初步的组合运算输出组合导航信息，并将其作为无人船的主导航信息。所述全球定位模块、浒苔近距离图像识别模块的输出信息也会接入无人船组合导航系统。所述全球定位模块作为备份坐标定位，输出无人船在海上位置信息；所述全球定位模块和惯导模块组成双备份方位角测量，输出无人船在海上方位角信息；所述浒苔近距离图像识别模块、全球定位模块组成多信息融合制导系统，输出无人船在浒苔绿潮团中相对位置坐标、环境光照度、浒苔绿潮团中心方向等信息。
所述惯导模块（附图中简称为PHINS），选择iXBlue公司的PHINS Compact7，导航精度可达0.1%。选择如此高精度的惯导器件，除了在无人船运行过程中提供高精度的导航数据外，还可用于标定其它精度较低的惯导模块。所述浒苔近距离图像识别模块（附图中简称为图像识别模块），选择海面浒苔绿潮监测视觉图像传感器不少于4支，视场覆盖360°；浒苔绿潮监测数据内容包括相对位置坐标、环境光照度、浒苔绿潮团聚中心方向等关键指标，采样率周期1分钟-1小时可调，系统使用寿命不少于1年，数据本地保存最大容量1年数据量。所述全球定位模块（附图中简称为GPS），选择Trimble公司的BD970模块，垂直定位精度为0.5m+1ppm，水平定位精度为0.25+1ppm。
基于上述用于浒苔跟踪的无人船组合导航系统，本实施例还公开了一种用于浒苔跟踪的无人船组合导航方法，无人船组合导航方法的主要功能是实现浒苔轨迹跟踪监测保障中的无人船自主导航。其中涉及的功能有无人船的初始对准，惯导系统的初始校准，浒苔近距离图像识别系统的初始校准，传感器数据采集，无人船导航定位，数据管理功能等。
步骤1，对无人船导航系统进行初始校准。
多传感器数据的采集和处理被抽象成独立的进程，在MOOS平台上运行，采用统一标准的软件接口，通过统一的TCP/IP通信协议在MOOSDB上实现模块间的信息交互功能。MOOSDB作为MOOS的数据管理站和服务站，是整个软件框架的信息中心和信息交互平台。
多传感器初始校准需要对波特率、数据协议、端口、启动设置，涉及的传感器包括PHINS惯导、浒苔近距离图像识别模块、GPS。多传感器初始校准不仅需要对传感器本身进行校准，还需要消除传感器安装偏差，将多传感器的坐标系旋转到载体坐标系上，作为后期各传感器信息融合的基础，为导航系统提供可靠稳定的数据支持。其中涉及的功能有无人船的初始对准，惯导系统的初始校准，浒苔近距离图像识别系统的初始校准，传感器数据采集，无人船导航定位，数据管理功能等。
无人船初始对准的主要目的是精确确定载体坐标系和真实导航坐标系之间的初始方向余弦矩阵。主要通过Φ角法或者Ψ角法导出惯导系统的误差方程；建立基于位置误差或者速度误差的平动误差方程，以及基于载体坐标系与真实地理坐标系之间的误差角Φ或者基于载体坐标系与计算坐标系之间的误差角Ψ的姿态误差方程。在本系统中，采用Φ角法建立动基座捷联式惯导模块的误差模型，且其平动误差方程取速度误差的形式完成系统的初始对准。
通过Φ角法或者Ψ角法导出惯导系统的误差方程；建立基于位置误差或者速度误差的平动误差方程，以及基于载体坐标系与真实地理坐标系之间的误差角Φ或者基于载体坐标系与计算坐标系之间的误差角Ψ的姿态误差方程。在本系统中，拟采用Φ角法建立动基座捷联式惯导系统的误差模型，且其平动误差方程取速度误差的形式。捷联惯导系统工作原理如图4所示。
捷联惯导系统动基座初始对准的卡尔曼滤波模型：
由于加速度计误差、/>和陀螺漂移/>、/>、/>并不完全为白噪声，将加速度计误差和陀螺漂移扩充为状态变量。现假定加速度计误差和陀螺漂移为随机常值，则系统运动方程为：
；
式中：X为系统状态向量，X具体公式为：
。
W为系统噪声矢量，W具体公式为：
；
其中，均为零均值的高斯白噪声，分别看做加速度计误差和陀螺漂移中的白噪声成分，A为系统转移矩阵，A的具体公式如下：
；
其中，为2×3维的零矩阵；/>为3×2维的零矩阵；/>为2×2维的零矩阵；/>为3×3维的零矩阵；
；
其中，Wie为地球自转角速率；
；
其中，。
虽然L在对准过程中随时间变化，但变化很小，因此和/>阵随时间变化较小。
；
式中，、/>、/>分别为捷联惯导系统加速计的比力在东向、北向和天向的分量，它们随时间变化。
为捷联式惯导系统初始对准卡尔曼滤波模型的系统方程。若要求对系统状态变量δVE、δVN、φE、φN、φU、/>及εE、εN、εU进行估计，还需要建立系统观测方程。选取不同的观测量对系统状态估计有不同的效果。在此取水平速度误差δVE和δVN为观测量。系统观测方程为：
；
其中、/>是系统观测噪声矢量，为N(0,R)的高斯白噪声。Kalman滤波算法如图5所示。
浒苔近距离图像识别模块的初始校准主要目的是获得相机坐标系和真实导航坐标系之间的初始方向余弦矩阵。主要通过相机标定和坐标变换导出图像识别系统的误差方程；建立基于位置误差的平动误差方程，以及基于相机坐标系与真实地理坐标系之间的误差角Φ或者基于相机坐标系与计算坐标系之间的误差角Ψ的姿态误差方程。在本系统中，采用张正友单目相机标定法获得相机内、外参数，建立浒苔近距离图像识别模块的误差模型，完成系统的初始校准。
惯性元件误差模型的建立和实时补偿主要是进行惯性仪表（陀螺仪和加速度计）的测飘和标定，需要在惯性仪表性能稳定的情况下进行测试，不需要每次上电启动时都进行，即可以海试前，在陆地上的稳定环境中进行测试。初始对准完成前，还需对惯导系统进行校准，主要分为粗校准和精校准两步。鉴于惯导模块在短期内精度较高，但是误差会随时间累积，需要将惯导模块与外部导航系统组合起来，以此来提高导航精度。
组合导航系统使用进行对各传感器进行初始校准过程如下：
1)惯导模块在水面粗校准：无人船上电五分钟内自动执行粗校准，此时无人船尽可能保持静止，禁止有加速度值。惯导模块根据内部光纤陀螺和加速度计粗略估计航向、纵倾和横滚角度信息。
2)无人船精校准过程：粗校准过程获取了姿态信息等数据，但姿态误差在小级别上仍然是不稳定的，在浒苔近距离图像识别模块、GPS数据反馈给惯导模块的条件下，通过走图2中（a）的阶梯形轨迹，实现INS的精准过程。
3)校准效果验证：航迹如图2中（b）的8字形轨迹所示，在浒苔近距离图像识别模块、惯导模块、GPS全球定位模块均开始工作，GPS数据不反馈给惯导模块，浒苔近距离图像识别模块数据反馈给惯导模块的条件下，模拟在恶劣天气环境下，无法使用GPS时的导航能力和导航精度。根据图2中（b）的8字形轨迹设定航迹，无人船使用浒苔近距离图像识别模块与惯导模块进行组合导航，校准的流程图如图3所示。
步骤2，接收惯导模块、浒苔近距离图像识别模块、全球定位模块输出的信息后，使用改进的无迹卡尔曼滤波算法（SVD-MUKF算法）对三个模块数据进行融合，利用滤波器估算的误差校正相应的导航参数。
无人船组合导航系统的非线性程度较高时，无迹卡尔曼滤波算法的估计精度下降甚至发散。分析无迹卡尔曼滤波算法的运行过程可知，UT变换过程中，生成Sigma点集时使用Cholesky分解运算误差协方差矩阵，需保证其是非负定矩阵，但在无人船运行过程中，系统协方差矩阵易受噪声影响，导致其成为奇异矩阵而无法处理，从而结束滤波过程，使滤波结果精度下降甚至发散。本发明在UT变换中引入奇异值分解 ( singular valuedecomposition，SVD)替代Cholesky分解，仅改变产生点集的过程，使算法具有更强的鲁棒性。
假设惯导模块、浒苔近距离图像识别模块、全球定位模块输出信号为，其中，1，2，…，N分别是采样点，则可将惯导模块、浒苔近距离图像识别模块、全球定位模块输出信号写成如下式所示的矩阵形式：
。
对协方差矩阵的奇异值分解过程为：
；
；
其中，U = (u 1, u 2,…, u m )和V = (v 1, v 2, …, v n ) ∈R n×n 都是正交矩阵，n的范围为1<n<N，表示信号的平均值，/>表示信号的协方差，且σ 1 ≥σ 2≥…≥σ r ≥0是矩阵B的秩，r=min(m, n)。
SVD-MUKF算法的具体实施步骤如下：
PHINS惯导模块内部集成了加速度计和光纤陀螺仪，根据初始的GPS经纬度可以计算出无人船的位置、速度和姿态等信息。导航定位实现流程图如图6。当无人船在水面航行时，PHINS可以通过GPS矫正惯性漂移，并根据浒苔近距离图像识别模块的数据来调整在浒苔中的位置。当无人船仅依靠PHINS和浒苔近距离图像识别模块工作，无GPS调整导航定位信息，位置误差将随时间的增加而加大。
通过使用多种子滤波器对同一导航参数输出的差值作为观测量，系统的状态方程和测量方程简化为线性方程，提高计算效率，改进的无迹卡尔曼算法流程图如图7所示。系统利用滤波器估算的误差校正相应的导航参数，从而计算出无人船的位置、速度和姿态等信息，无人船的位置信息可通过UTM和经纬两种方式表示。
以地理坐标系n作为导航坐标系，SVD-MUKF算法的状态向量为：
；
式中δL为纬度误差，δλ为经度误差，δh为深度误差，式中δν E 为东向速度误差，δν N 为北向速度误差，δV u 为纵向速度误差，为横滚角误差，/>为航向角误差，/>为纵倾角误差。
PHINS与GPS构成的SVD-MUKF算法子滤波器状态方程为：
；
测量方程为：
；
其中，。
PHINS与浒苔近距离图像识别模块构成的SVD-MUKF算法子滤波器状态方程为：
；
测量方程为：
；
其中，。
子滤波器根据SVD-MUKF算法原理，单独进行时间更新和状态更新，子滤波器的结构图如图8所示。以Phins+浒苔近距离图像识别模块子滤波器为例，子滤波器包含时间更新和状态更新两部分。其中时间更新过程系统根据当前的状态预测下一时刻Phins和浒苔近距离图像识别模块的状态，并更新协方差系数；状态更新则根据最新的Phins和浒苔近距离图像识别模块对惯导模块的差值作为观测值，同时使用测量协方差去修正预测值，从而获得子滤波器的最优估计，输入到主滤波器。主滤波器根据时间更新获取误差的最优估计，并应用到导航定位系统中。
步骤3，使用SLAM算法读取3D栅格地图信息，将无人船的运动状态从载体坐标系转换到3D栅格地图坐标系。
以上实施例仅为本发明的示例性实施例，不用于限制本发明，本发明的保护范围由权利要求书限定。本领域技术人员可以在本发明的实质和保护范围内，对本发明做出各种修改或等同替换，这种修改或等同替换也应视为落在本发明的保护范围内。

Claims (6)

1.一种用于浒苔跟踪的无人船组合导航方法，其特征在于，包括如下步骤：
步骤1，对无人船导航系统进行初始校准；
步骤2，接收惯导模块、浒苔近距离图像识别模块、全球定位模块输出的信息后，使用改进的无迹卡尔曼滤波算法对三个模块数据进行融合，利用滤波器估算的误差校正相应的导航参数；
步骤3，使用SLAM算法读取3D栅格地图信息，将无人船的运动状态从载体坐标系转换到3D栅格地图坐标系；
所述步骤2用改进的无迹卡尔曼滤波算法对三个模块数据进行融合的具体过程包括：将惯导模块作为公共参考模块，判断全球定位模块、浒苔近距离图像识别模块采集的信息是否有效，将惯导模块采集的信息与全球定位模块、浒苔近距离图像识别模块采集的有效信息进行两两融合，组成子滤波器，各子滤波器并行运行，在子滤波器局部量测的基础上获得子最优估计，在主滤波器内按照数据融合算法得到当前量测基础上的全局估计；
所述改进的无迹卡尔曼滤波算法具体过程包括：以地理坐标系n作为导航坐标系，改进的无迹卡尔曼滤波算法的状态向量为：
其中，δL为纬度误差，δλ为经度误差，δh为深度误差，δνE为东向速度误差，δνN为北向速度误差，δVu为纵向速度误差，为横滚角误差，/>为航向角误差，/>为纵倾角误差；
惯导模块与全球定位模块构成的改进的无迹卡尔曼滤波算法子滤波器状态方程为：
测量方程为：
其中，H1＝[I2×2 02×7]，HGPS＝[-I2×2]，表示滤波后惯导模块状态向量，/>表示滤波后全球定位模块状态向量，FINS表示惯导模块状态更新向量，FGPS表示全球定位模块状态更新向量，XINS表示惯导模块状态向量，XGPS表示全球定位模块状态向量，ωGPS表示全球定位模块中的白噪声，I2×2表示单位矩阵，LINS表示惯导模块所测纬度值，LGPS表示全球定位模块所测纬度值，λINS表示惯导模块所测经度值，λGPS表示全球定位模块所测经度值，δLINS表示惯导模块纬度误差，δLGPS表示全球定位模块纬度误差，δλINS表示惯导模块经度误差，δλGPS表示全球定位模块精度误差，VL表示沿纬度方向速度，Vλ表示沿经度方向速度；
惯导模块与浒苔近距离图像识别模块构成的改进的无迹卡尔曼滤波算法子滤波器状态方程为：
测量方程为：
其中，H3＝[03×3 I3×3 03×3]，HDVL＝[-I3×3]，表示滤波后浒苔近距离图像识别模块状态向量，I3×3表示单位矩阵；τ表示浒苔近距离图像识别模块状态更新量，Xdepth表示浒苔近距离图像识别模块状态向量，ωd表示浒苔近距离图像识别模块中的白噪声，δvINSE表示惯导模块东向速度误差，δvDE表示浒苔近距离图像识别模块东向速度误差，δvINSN表示惯导模块北向速度误差，δvDN表示浒苔近距离图像识别模块北向速度误差，δvINSU表示惯导模块纵向速度误差，δνDU表示浒苔近距离图像识别模块纵向速度误差，VE表示东向速度，VN表示北向速度，VU表示纵向速度，HDVL表示浒苔近距离图像识别模块观测向量；
子滤波器单独进行时间更新和状态更新，获得子滤波器的最优估计，输入到主滤波器；主滤波器根据时间更新获取误差的最优估计，并应用到导航定位系统中。
2.根据权利要求1所述的一种用于浒苔跟踪的无人船组合导航方法，其特征在于，所述步骤1具体包括：无人船初始对准、浒苔近距离图像识别模块初始校准、惯导模块的粗校准和精校准，所述无人船初始对准目的是精确确定载体坐标系和真实导航坐标系之间的初始方向余弦矩阵；所述浒苔近距离图像识别模块初始校准的目的是获得相机坐标系和真实导航坐标系之间的初始方向余弦矩阵。
3.根据权利要求1所述的一种用于浒苔跟踪的无人船组合导航方法，其特征在于，所述步骤1具体初始校准过程包括：无人船在水面静止通电，对惯导模块进行粗校准；浒苔近距离图像识别模块、全球定位模块数据反馈给惯导模块的条件下，通过走阶梯状轨迹，对惯导模块精校准；浒苔近距离图像识别模块、惯导模块、全球定位模块均开始工作，全球定位模块数据不反馈给惯导模块，浒苔近距离图像识别模块数据反馈给惯导模块的条件下，模拟在恶劣天气环境下，无法使用全球定位模块时的导航能力和导航精度；无人船使用浒苔近距离图像识别模块与惯导模块进行组合导航，根据8字形轨迹设定航迹，进行校准效果验证。
4.一种用于浒苔跟踪的无人船组合导航系统，使用权利要求1-3任一项所述的一种用于浒苔跟踪的无人船组合导航方法，其特征在于，包括导航系统、控制系统、定位系统，所述导航系统、控制系统、定位系统均安装于无人船上，所述导航系统、控制系统、定位系统通信连接，所述定位系统包括惯导模块、浒苔近距离图像识别模块、全球定位模块，所述惯导模块与无人船质心重合，所述全球定位模块安装于无人船船尾中心线上，所述浒苔近距离图像识别模块高于海平面1.5米以上，正面摄像头与无人船推进器前进方向一直，正下方与惯导模块位于一条直线。
5.根据权利要求4所述的一种用于浒苔跟踪的无人船组合导航系统，其特征在于，所述全球定位模块、浒苔近距离图像识别模块的输出信息直接接入惯导模块，由惯导模块内部进行初步的组合运算输出组合导航信息，并将其作为无人船的主导航信息；所述全球定位模块作为备份坐标定位，用于输出无人船在海上位置信息；所述全球定位模块和惯导模块组成双备份方位角测量，输出无人船在海上方位角信息；所述浒苔近距离图像识别模块、全球定位模块组成多信息融合制导系统，输出无人船在浒苔绿潮团中相对位置坐标、环境光照度、浒苔绿潮团中心方向信息。
6.根据权利要求4所述的一种用于浒苔跟踪的无人船组合导航系统，其特征在于，所述浒苔近距离图像识别模块包括不少于4个海面浒苔绿潮监测视觉图像传感器，视场覆盖360°；所述惯导模块内部集成加速度计、光纤陀螺仪。
CN202311648169.3A 2023-12-05 2023-12-05 一种用于浒苔跟踪的无人船组合导航系统及导航方法 Active CN117346794B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311648169.3A CN117346794B (zh) 2023-12-05 2023-12-05 一种用于浒苔跟踪的无人船组合导航系统及导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311648169.3A CN117346794B (zh) 2023-12-05 2023-12-05 一种用于浒苔跟踪的无人船组合导航系统及导航方法

Publications (2)

Publication Number Publication Date
CN117346794A CN117346794A (zh) 2024-01-05
CN117346794B true CN117346794B (zh) 2024-02-23

Family

ID=89357865

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311648169.3A Active CN117346794B (zh) 2023-12-05 2023-12-05 一种用于浒苔跟踪的无人船组合导航系统及导航方法

Country Status (1)

Country Link
CN (1) CN117346794B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110926466A (zh) * 2019-12-14 2020-03-27 大连海事大学 一种面向无人船组合导航信息融合的多尺度数据分块算法
CN115265532A (zh) * 2022-07-28 2022-11-01 大连海事大学 一种用于船用组合导航中的辅助滤波方法
CN115824250A (zh) * 2022-10-14 2023-03-21 大连海事大学 一种智能船舶增强现实助航信息显示方法
CN116045970A (zh) * 2023-03-06 2023-05-02 北京航空航天大学 一种基于外部条件约束的多平台信息协同导航增强方法
CN116839591A (zh) * 2023-07-12 2023-10-03 哈尔滨天枢问道技术有限公司 一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110487301B (zh) * 2019-09-18 2021-07-06 哈尔滨工程大学 一种雷达辅助机载捷联惯性导航系统初始对准方法

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109376785A (zh) * 2018-10-31 2019-02-22 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
WO2020087846A1 (zh) * 2018-10-31 2020-05-07 东南大学 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法
CN110579740A (zh) * 2019-09-17 2019-12-17 大连海事大学 一种基于自适应联邦卡尔曼滤波的无人船组合导航方法
CN110926466A (zh) * 2019-12-14 2020-03-27 大连海事大学 一种面向无人船组合导航信息融合的多尺度数据分块算法
CN115265532A (zh) * 2022-07-28 2022-11-01 大连海事大学 一种用于船用组合导航中的辅助滤波方法
CN115824250A (zh) * 2022-10-14 2023-03-21 大连海事大学 一种智能船舶增强现实助航信息显示方法
CN116045970A (zh) * 2023-03-06 2023-05-02 北京航空航天大学 一种基于外部条件约束的多平台信息协同导航增强方法
CN116839591A (zh) * 2023-07-12 2023-10-03 哈尔滨天枢问道技术有限公司 一种轨迹跟踪定位滤波系统及救援无人机的融合导航方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Application research in integrated navigation system of measurement ship Based on multiple data fusion technology;Zhong Lin et al.;2017 16TH INTERNATIONAL CONFERENCE ON OPTICAL COMMUNICATIONS & NETWORKS;全文 *
Autonomous unmanned surface vehicle: a microcontroller based approach;Ahmad, N. et al.;2011 International Conference on Electric Information and Control Engineering;20111231;全文 *
一种小型无人船导航-制导-控制系统设计与验证;王宁;吴亚;杨毅;高云程;;大连海事大学学报(第04期);全文 *
区间平滑在船用INS/GPS姿态组合导航系统事后评估中的应用;陈俊杰;信冠杰;王战军;严航;;船电技术(第10期);全文 *
应用RB无迹卡尔曼滤波组合导航提高GPS重获信号后的导航精度;张百强;储海荣;孙婷婷;贾宏光;郭立红;ZHANG Yue;;光学精密工程;20160430(第04期);全文 *

Also Published As

Publication number Publication date
CN117346794A (zh) 2024-01-05

Similar Documents

Publication Publication Date Title
CN109556632B (zh) 一种基于卡尔曼滤波的ins/gnss/偏振/地磁组合导航对准方法
CN110487301B (zh) 一种雷达辅助机载捷联惯性导航系统初始对准方法
CN107270893B (zh) 面向不动产测量的杆臂、时间不同步误差估计与补偿方法
CN110779521A (zh) 一种多源融合的高精度定位方法与装置
CN113203418B (zh) 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及系统
CN102829777B (zh) 自主式水下机器人组合导航系统及方法
CN106767752B (zh) 一种基于偏振信息的组合导航方法
CN106405670B (zh) 一种适用于捷联式海洋重力仪的重力异常数据处理方法
CN103674030B (zh) 基于天文姿态基准保持的垂线偏差动态测量装置和方法
CN109556631B (zh) 一种基于最小二乘的ins/gnss/偏振/地磁组合导航系统对准方法
CN110501024A (zh) 一种车载ins/激光雷达组合导航系统的量测误差补偿方法
CN110274588A (zh) 基于无人机集群信息的双层嵌套因子图多源融合导航方法
CN111947652A (zh) 一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法
CN101788296A (zh) 一种sins/cns深组合导航系统及其实现方法
CN111024064A (zh) 一种改进Sage-Husa自适应滤波的SINS/DVL组合导航方法
CN104635251A (zh) 一种ins/gps组合定位定姿新方法
CN104422948A (zh) 一种嵌入式组合导航系统及其方法
CN111982106A (zh) 导航方法、装置、存储介质及电子装置
CN114018242B (zh) 一种基于偏振/太阳/惯性信息智能匹配的自主定姿方法
CN104698486A (zh) 一种分布式pos用数据处理计算机系统实时导航方法
Yan et al. Performance assessment of the android smartphone’s IMU in a GNSS/INS coupled navigation model
CN105091907A (zh) Sins/dvl组合中dvl方位安装误差估计方法
CN113503892A (zh) 一种基于里程计和回溯导航的惯导系统动基座初始对准方法
CN116222551A (zh) 一种融合多种数据的水下导航方法及装置
CN104061930A (zh) 基于捷联惯性制导和多普勒计程仪的导航方法

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