CN114046792A - 一种无人船水面定位与建图方法、装置及相关组件 - Google Patents

一种无人船水面定位与建图方法、装置及相关组件 Download PDF

Info

Publication number
CN114046792A
CN114046792A CN202210012655.5A CN202210012655A CN114046792A CN 114046792 A CN114046792 A CN 114046792A CN 202210012655 A CN202210012655 A CN 202210012655A CN 114046792 A CN114046792 A CN 114046792A
Authority
CN
China
Prior art keywords
ship
data
positioning
moment
time
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.)
Granted
Application number
CN202210012655.5A
Other languages
English (en)
Other versions
CN114046792B (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.)
Shaanxi Orca Electronic Intelligent Technology Co ltd
Original Assignee
Shaanxi Orca Electronic Intelligent Technology 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 Shaanxi Orca Electronic Intelligent Technology Co ltd filed Critical Shaanxi Orca Electronic Intelligent Technology Co ltd
Priority to CN202210012655.5A priority Critical patent/CN114046792B/zh
Publication of CN114046792A publication Critical patent/CN114046792A/zh
Application granted granted Critical
Publication of CN114046792B publication Critical patent/CN114046792B/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/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
    • G01C21/1652—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 ranging devices, e.g. LIDAR or RADAR
    • 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
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/06—Systems determining position data of a target
    • G01S13/42—Simultaneous measurement of distance and other co-ordinates
    • 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/46—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being of a radio-wave signal type
    • 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)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种无人船水面定位与建图方法、装置及相关组件。该方法包括判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足，则计算绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，得到第一定位数据，若不满足，则返回；数据融合过程如下：获取船只的位置初值、姿态初值、雷达3D点云信息，并进行数据融合；计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据；将第一定位数据与第二定位数据进行数据融合，得到目标定位数据，对点云地图进行实时更新。该方法可以适应GNSS信号时强时弱的水面场景，有效提供船只当前位置，并基于第一定位数据与第二定位数据进行地图构建。

Description

一种无人船水面定位与建图方法、装置及相关组件
技术领域
本发明涉及运载道具的航道的控制领域，尤其涉及一种无人船水面定位与建图方法、装置及相关组件。
背景技术
近年来，随着科技的进步，自动驾驶和机器人领域相关技术得到了飞速发展，各个行业对无人化作业的需求也逐步被激发出来。近几年，无人船的应用价值被不断挖掘，其应用场景覆盖了海面、港口、内河、湖泊等等，无人船可执行如巡检监察、垃圾清理、物流运输、水中建筑安全性监测等任务。
对于无人船，准确的定位对于无人船执行和完成任务必不可少，只有知道自己当前所处位置，才能知道如何向目标位置行进；而依据其他传感器数据，如激光雷达、毫米波雷达等建立的环境地图，则可提供丰富的环境信息，为无人船的安全行驶提供保障，因此，对于无人船，准确的定位与建图起着至关重要的作用。
传统的无人船行驶于海上场景，场地开阔，依赖于卫星定位即可获得较为准确的定位。而当无人船在内河等区域行驶时，由于信号遮挡，卫星定位精度下降，此时不仅船只定位出现问题，由于建图结果往往依赖于定位，因此最终建立的环境地图也会受到影响而不准确。
发明内容
本发明的目的是提供一种无人船水面定位与建图方法、装置及相关组件，旨在解决在卫星定位精度不高的情景下，导致建立的环境地图不准确的问题。
为解决上述技术问题，本发明的目的是通过以下技术方案实现的：提供一种无人船水面定位与建图方法，其包括：
基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据，判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，得到第一定位数据，若不满足计算条件，则返回判断船只下一时刻的实时数据是否满足计算绝对方向角的条件；
其中，所述数据融合过程如下：
获取船只当前时刻的位置初值、姿态初值以及雷达3D点云信息，并将所述船只当前时刻的位置初值、姿态初值以及雷达3D点云信息进行数据融合；
基于所述实时数据以及经纬高坐标系与北东地坐标系的转换关系，计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据；
将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，并基于所述目标定位数据以及观测到的雷达3D点云，对点云地图进行实时更新。
另外，本发明要解决的技术问题是还在于提供一种无人船水面定位与建图装置，其包括：
识别单元，用于基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据，判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，若不满足计算条件，则返回判断船只下一时刻的实时数据是否满足计算绝对方向角的条件；
其中，数据融合单元，用于所述数据融合，过程如下：
获取船只当前时刻的位置初值、姿态初值以及雷达3D点云信息，并将所述船只当前时刻的位置初值、姿态初值以及雷达3D点云信息进行数据融合，得到第一定位数据；
转换单元，用于基于所述实时数据以及经纬高坐标系与北东地坐标系的转换关系，计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据；
更新单元，用于将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，并基于所述目标定位数据以及观测到的雷达3D点云，对点云地图进行实时更新。
另外，本发明实施例又提供了一种计算机设备，其包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述第一方面所述的无人船水面定位与建图方法。
另外，本发明实施例还提供了一种计算机可读存储介质，其中所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行上述第一方面所述的无人船水面定位与建图方法。
本发明实施例公开了一种无人船水面定位与建图方法、装置及相关组件，其中，方法包括：基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据，判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，得到第一定位数据，若不满足计算条件，则返回判断船只下一时刻的实时数据是否满足计算绝对方向角的条件；所述数据融合过程如下：获取船只当前时刻的位置初值、姿态初值以及雷达3D点云信息，并将所述船只当前时刻的位置初值、姿态初值以及雷达3D点云信息进行数据融合；基于所述实时数据以及经纬高坐标系与北东地坐标系的转换关系，计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据；将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，并基于所述目标定位数据以及观测到的雷达3D点云，对点云地图进行实时更新。该方法可以适应GNSS信号时强时弱的水面场景，有效提供船只当前位置，并基于第一定位数据与第二定位数据进行地图构建。
附图说明
为了更清楚地说明本发明实施例技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的无人船水面定位与建图方法的流程示意图；
图2为本发明实施例提供的无人船水面定位与建图方法的子流程示意图；
图3为本发明实施例提供的无人船水面定位与建图装置的示意性框图；
图4为本发明实施例提供的计算机设备的示意性框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和 “包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/ 或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
请参阅图1，图1为本发明实施例提供的无人船水面定位与建图方法的流程示意图；
如图1所示，该方法包括步骤S101~S105：
S101、基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据；
S102、判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则执行步骤S103，若不满足计算条件，则返回步骤S101；
S103、计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，得到第一定位数据，其中，所述数据融合过程如下：
获取船只当前时刻的位置初值、姿态初值以及雷达3D点云信息，并将所述船只当前时刻的位置初值、姿态初值以及雷达3D点云信息进行数据融合，并执行下一步骤S104；
S104、基于所述实时数据以及经纬高坐标系与北东地坐标系的转换关系，计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据，并执行下一步骤S105；
S105、将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，并基于所述目标定位数据以及观测到的雷达3D点云，对点云地图进行实时更新。
本申请提供的无人船水面定位与建图方法是一种水面环境下高精度的船只定位与建图方法，可以适应GNSS信号时强时弱的水面场景，有效提供船只当前位置，并基于第一定位数据与第二定位数据进行地图构建。
需要说明的是，卫星导航系统即GNSS卫星导航系统，惯性传感器即IMU惯性传感器，接收GNSS卫星导航系统以及IMU惯性传感器发出的实时监测数据，计算船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，即说明可以启动后续的定位和建图系统；值得注意的是，本申请所说的雷达3D点云为多线激光雷达3D点云。
具体一实施例中，所述步骤S101中基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据，包括：
S10、利用GNSS双天线系统获取船只当前时刻的GNSS双天线信息，其中，所述GNSS双天线信息包括双天线之间的连接线在世界坐标系下的GNSS方向角、GNSS精度指标、双天线测角可用性评价指标；
S11、基于前k个时刻的GNSS双天线信息，计算船只当前时刻的IMU方向角补偿值；
S12、利用惯性传感器获取船只当前时刻的IMU信息，并基于船只当前时刻的IMU信息以及上一时刻的第一IMU方向角，计算得到当前时刻的第一IMU方向角，其中，所述IMU信息包括IMU角速度、IMU加速度；
S13、基于当前时刻的IMU方向角补偿值以及第一IMU方向角，计算船只当前时刻行驶的绝对方向角。
在本实施例中，GNSS双天线系统装配在船上，且需要说明的是，GNSS方向角用
Figure 100002_DEST_PATH_IMAGE001
表示，GNSS精度指标用
Figure 541045DEST_PATH_IMAGE002
表示，双天线测角可用性评价指标用
Figure 100002_DEST_PATH_IMAGE003
表示，其 中，
Figure 326599DEST_PATH_IMAGE002
的数值越小代表当前定位误差越小，
Figure 782988DEST_PATH_IMAGE002
与
Figure 381197DEST_PATH_IMAGE003
的值均由GNSS双天线系统 直接输出。
如图2所示，为了评估当前时刻的GNSS方向角的可用性，具体一实施例中，所述步骤S11包括：
S20、获取当前时刻t以及前k个时刻的GNSS双天线信息，判断集合
Figure 52481DEST_PATH_IMAGE004
中的每个值是否均小于RMSthre；其中，
Figure 100002_DEST_PATH_IMAGE005
分别表示第t-k+1时刻、第t-k+2时 刻、……、当前时刻t的GNSS精度指标的值，RMSthre为预设值；
若集合
Figure 710733DEST_PATH_IMAGE006
中的每个值均小于RMSthre，则执行 步骤S21；若集合
Figure 100002_DEST_PATH_IMAGE007
中的每个值不满足均小于
Figure 592101DEST_PATH_IMAGE008
，则执行步骤S23；
S21、判断集合
Figure 100002_DEST_PATH_IMAGE009
中的每个值是否均为
Figure 698729DEST_PATH_IMAGE010
， 若
Figure 100002_DEST_PATH_IMAGE011
中的每个值均为
Figure 683626DEST_PATH_IMAGE010
，则执行步骤S22；若
Figure 76561DEST_PATH_IMAGE011
中的每个值有不为
Figure 382908DEST_PATH_IMAGE010
，则执行步骤S23；
S22、判定当前时刻的GNSS方向角精度高，并对船只当前时刻的IMU方向角补偿值 进行更新；其中，
Figure 480177DEST_PATH_IMAGE012
表示当前时刻t的 GNSS精度指标的值，
Figure 922791DEST_PATH_IMAGE013
分别表示第t-k+1时刻、第t-k+2时 刻、……、当前时刻t的双天线测角可用性评价指标的值，
Figure 985163DEST_PATH_IMAGE010
为预设值；
S23、判定当前时刻的GNSS方向角精度低，并不对船只当前时刻的IMU方向角补偿值进行更新。
在本实施例中，当判定当前时刻的GNSS方向角精度高，则可以更新IMU方向角补偿 值，否则认为不满足条件，对IMU方向角补偿值不予以更新。需要说明的是，
Figure DEST_PATH_IMAGE014
的值为
Figure 841123DEST_PATH_IMAGE015
之一，值为
Figure 555133DEST_PATH_IMAGE010
代表双天线测角可用且精度高，值为
Figure DEST_PATH_IMAGE016
代表双 天线测角可用但精度低，值为No代表双天线测角不可用。
具体一实施例中，所述步骤S12包括以下步骤：
S24、获取当前时刻以及前k个时刻的GNSS方向角，以及获取当前时刻以及前k个时刻的第一IMU方向角，并对两者作差并求均值，得到当前时刻的IMU方向角补偿值；
S25、利用欧拉角解算方法，解算船只当前时刻的欧拉角，得到船只当前时刻的第一IMU方向角；
在步骤S24中，按下式计算当前时刻的IMU方向角补偿值：
Figure 413105DEST_PATH_IMAGE017
；
其中，mean代表求均值的操作，
Figure DEST_PATH_IMAGE018
表示在第（t0-k+1）时刻的GNSS方向角，
Figure 288788DEST_PATH_IMAGE019
表示在第t0-k+1时刻的第一IMU方向角。
在步骤S25中，通过常用的捷联惯导解算中的方法，例如方向余弦法，进行欧拉角计算，计算出来的第一IMU方向角结果更加精确。
具体一实施例中，所述步骤S13包括：
S26、将船只当前时刻的第一IMU方向角与当前时刻的IMU方向角补偿值相加，得到当前时刻的船只行驶的绝对方向角。
具体的，按下式计算当前时刻的船只行驶的绝对方向角：
Figure 428783DEST_PATH_IMAGE020
；
具体一实施例中，所述步骤S102包括：
S30、判断是否对当前时刻的IMU方向角补偿值进行更新，若对当前时刻的IMU方向角补偿值进行更新，则执行步骤S31，若未对当前时刻的IMU方向角补偿值进行更新，则返回执行步骤S101；
S31、将计算得到的当前时刻的船只行驶的绝对方向角作为起始时刻的绝对方向角，并执行数据融合；
在本实施例中，根据步骤S11得到的结果，判断第一IMU方向角是否已经进行过角度补偿，若判定为步骤S23的结果，则说明不满足角度补偿条件，未对船只当前时刻的IMU方向角补偿值进行更新，则不进行后续的步骤，等待判定结果为步骤S22时，再开启后续定位与建图系统，从而可以进行后续的步骤。
若是已经出现过满足步骤S20和S21中的条件，则说明已经对第一IMU方向角进行 了补偿，则认为，定位系统起始时刻的绝对方向角
Figure 100002_DEST_PATH_IMAGE021
，即绝对方向角补偿的时 刻即为定位系统起始时刻，并开启后续定位与建图系统。
具体一实施例中，所述步骤S103包括：
S40、获取当前时刻t对应的时间
Figure 710597DEST_PATH_IMAGE022
和上一时刻对应的时间
Figure 100002_DEST_PATH_IMAGE023
之间的m个IMU测 量值
Figure 268748DEST_PATH_IMAGE024
中绕z轴的角速度
Figure DEST_PATH_IMAGE025
，并按下式对所述角速度
Figure 469660DEST_PATH_IMAGE025
进行积分，得到 平台方向角yaw的变化量：
Figure 706738DEST_PATH_IMAGE026
S41、基于所述平台方向角yaw的变化量，按下式计算当前时刻t的第二IMU方向角：
Figure DEST_PATH_IMAGE027
；
其中，
Figure 559287DEST_PATH_IMAGE028
表示上一时刻t-1的第二IMU方向角；
S42、基于所述当前时刻t的第二IMU方向角，按下式计算得到当前时刻t的姿态初值：
Figure DEST_PATH_IMAGE029
；
其中，
Figure 595114DEST_PATH_IMAGE030
为IMU测量值中
Figure DEST_PATH_IMAGE031
对应的滚转角，
Figure 406075DEST_PATH_IMAGE032
为IMU测量值中
Figure 396028DEST_PATH_IMAGE031
对应的俯仰角；
S43、基于船只速度近似不变的关系，按下式计算当前时刻t的位置初值
Figure DEST_PATH_IMAGE033
， 由此得到当前时刻t的位置和姿态初值
Figure 81962DEST_PATH_IMAGE034
：
Figure 100002_DEST_PATH_IMAGE035
；
其中，
Figure 4919DEST_PATH_IMAGE036
表示上一时刻t-1的位置；
S44、基于当前时刻t的位置初值和姿态初值
Figure 100002_DEST_PATH_IMAGE037
，以及上 一时刻t-1最终求得的历史位置和历史姿态
Figure 252360DEST_PATH_IMAGE038
，计 算两者之间的第一变化量
Figure 100002_DEST_PATH_IMAGE039
；
S45、基于所述第一变化量
Figure 696986DEST_PATH_IMAGE040
、当前 时刻t的激光雷达3D点云
Figure 100002_DEST_PATH_IMAGE041
与上一时刻t-1的激光雷达3D点云
Figure 156917DEST_PATH_IMAGE042
，利用点云匹配算法， 计算得到融合雷达点云后的第二变化量
Figure 100002_DEST_PATH_IMAGE043
；
S46、将所述第二变化量与上一时刻的历史位置和历史姿态对应相加，得到当前时 刻t的第一定位数据
Figure 839440DEST_PATH_IMAGE044
。
需要说明的是，在步骤S42中，由于IMU测量得到的滚转和俯仰角，不存在累积误 差，因此直接取
Figure 585679DEST_PATH_IMAGE031
对应的IMU测量的俯仰角
Figure 815804DEST_PATH_IMAGE032
和滚转角
Figure 876163DEST_PATH_IMAGE030
。在步骤S43中， 基于船只速度近似不变的假设，认为第t时刻kt对应的位置与第t-1时刻kt-1对应的位置之 差，等于kt-1时对应的位置与kt-2时对应的位置之差。
具体的，计算当前时刻t的位置和姿态初值
Figure 944351DEST_PATH_IMAGE037
，与上一 时刻t-1最终求得的位置和姿态
Figure 2437DEST_PATH_IMAGE038
之间的变化量初 值
Figure 100002_DEST_PATH_IMAGE045
，基于此变化量初值，以及当前时刻t的 激光雷达3D点云
Figure 923120DEST_PATH_IMAGE041
与上一时刻t-1的激光雷达3D点云
Figure 488968DEST_PATH_IMAGE042
，利用点云匹配算法，求得最 终的第二变化量
Figure 568920DEST_PATH_IMAGE046
。
在步骤S45中，点云匹配算法可采用最近点迭代算法（Iterative Closest Point，ICP）或正态分布变化算法（Normal Distributions Transform，NDT）。在本申请中，优先采用NDT进行点云匹配。
将最终求得的当前时刻t的第二变化量
Figure 100002_DEST_PATH_IMAGE047
施加于前一时刻t-1求得的第二变化量
Figure 594644DEST_PATH_IMAGE048
，即可计算出当前时刻t的第一定位结 果
Figure 100002_DEST_PATH_IMAGE049
。
具体的，在所述步骤104中，先记录当前时刻t的GNSS定位信息即记录当前时刻t的 GNSS定位精度判别的多个指标，包含GNSS定位精度因子
Figure 471465DEST_PATH_IMAGE050
，GNSS经度定位误差
Figure 100002_DEST_PATH_IMAGE051
，GNSS纬度定位误差
Figure 106583DEST_PATH_IMAGE052
， GNSS高度定位误差
Figure 100002_DEST_PATH_IMAGE053
，基于当前时刻t的GNSS测量的 经纬度，计算当前时刻相对起始时刻，在北东坐标系下的坐标，得到第二定位结果
Figure 447566DEST_PATH_IMAGE054
，具体的，记录当前时刻t的GNSS定位经纬度与海拔
Figure 100002_DEST_PATH_IMAGE055
，基于起 始时刻经纬度和海拔
Figure 644192DEST_PATH_IMAGE056
，利用经纬高-北东地坐标系的转换，计算当前时刻 t，相对起始时刻在北东坐标系下的坐标
Figure 100002_DEST_PATH_IMAGE057
。具体的，经纬高-北东地坐标系的转换 可使用如墨卡托投影等现有的转换方法。
基于当前时刻t与其之前数个时刻共m个时刻的GNSS定位精度信息，判决当前时刻t应使用的定位结果，具体一实施例中，所述步骤S104包括：
S50、利用GNSS双天线系统获取船只当前时刻的经度信息、纬度信息，得到经纬高坐标系下的经纬坐标；
S51、将经纬高坐标系下的经纬坐标转换到北东地坐标系下的坐标，得到第二定位 数据
Figure 336204DEST_PATH_IMAGE058
；
所述步骤S105中将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，包括：
S52、判断当前时刻t的第二定位数据是否满足精度条件；
S53、其中，所述精度条件包括：
集合
Figure 100002_DEST_PATH_IMAGE059
中的每个值均小于
Figure 243855DEST_PATH_IMAGE060
，且集 合
Figure 100002_DEST_PATH_IMAGE061
的每 个值均小于
Figure 236082DEST_PATH_IMAGE062
，其中，
Figure 100002_DEST_PATH_IMAGE063
分别是当前时刻t的所 述GNSS精度指标中的GNSS定位精度因子、GNSS经度定位误差、GNSS纬度定位误差、GNSS高度 定位误差，
Figure 275713DEST_PATH_IMAGE064
分别表示第t-m+1时刻、第t-m+2时 刻、……、第t时刻的GNSS定位精度因子，
Figure 100002_DEST_PATH_IMAGE065
分别表示第t-m+1时刻的GNSS经度定位误差、GNSS纬度定位误差、GNSS高度定位误差，
Figure 422399DEST_PATH_IMAGE066
分别表示第t时刻的GNSS经度定位误差、GNSS纬度定位误差、 GNSS高度定位误差，
Figure 494260DEST_PATH_IMAGE060
为定位精度因子阈值，
Figure 75414DEST_PATH_IMAGE062
为定位误差阈值；若当前时刻t 的第二定位数据满足精度条件，则执行步骤S54，若当前时刻t的第二定位数据不满足精度 条件，则执行步骤S55；
S54、判定当前时刻t的所述第二定位数据的定位精度高，并按下式计算当前时刻的中间位置和姿态数据：
Figure 100002_DEST_PATH_IMAGE067
；
其中，
Figure 20367DEST_PATH_IMAGE068
表示当前时刻t的第二定位数据，
Figure 100002_DEST_PATH_IMAGE069
表示当 前时刻t的第一定位数据中的姿态；
S55、判定当前时刻t的所述第二定位数据的定位精度低，并执行步骤S56；
S56、基于当前时刻t的第一定位数据以及上一时刻t-1的第一定位数据，计算两者 之间的第三变化量，并将所述第三变化量与上一时刻t-1的历史中间位置和姿态数据相加， 得到当前时刻t的中间位置和姿态数据
Figure 185507DEST_PATH_IMAGE070
；
S57、基于当前时刻t的中间位置和姿态数据
Figure 100002_DEST_PATH_IMAGE071
、经纬高 坐标系与北东地坐标系的转换关系，将当前时刻t相对起始时刻在北东坐标系下的坐标
Figure 202005DEST_PATH_IMAGE072
转换成当前时刻t对应的经纬高
Figure 100002_DEST_PATH_IMAGE073
，并按下式计算得到目标定位数 据：
Figure 637665DEST_PATH_IMAGE074
；
其中，
Figure DEST_PATH_IMAGE075
表示当前时刻t的中间位置和姿态数据。
在本实施例中，
Figure 815837DEST_PATH_IMAGE076
，
Figure 563213DEST_PATH_IMAGE077
。
在步骤S53中，最终得到的当前时刻t相对起始时刻的北东坐标系下坐标
Figure DEST_PATH_IMAGE078
直接等于计算得到的第二定位数据
Figure 881937DEST_PATH_IMAGE079
，而
Figure DEST_PATH_IMAGE080
则直接等于S46中求得 的第一定位数据中的姿态
Figure 172104DEST_PATH_IMAGE081
，进而得到当前时刻t的中间位置和姿态数据
Figure DEST_PATH_IMAGE082
。
在步骤S54中，在得到当前时刻t的第一定位数据
Figure 52335DEST_PATH_IMAGE083
后，先计算当前时刻t的第一定位数据
Figure 896794DEST_PATH_IMAGE083
，相对上一时刻t-1的第一定位数据
Figure DEST_PATH_IMAGE084
的第三变化量
Figure 573805DEST_PATH_IMAGE085
；
再将该第三变化量，施加于上一时刻t-1的历史中间位置和姿态数据
Figure 452899DEST_PATH_IMAGE086
，得到当前时刻t的中间位置和姿态数据
Figure 831928DEST_PATH_IMAGE087
。
然后利用当前时刻t相对起始时刻在北东地坐标系下的坐标
Figure DEST_PATH_IMAGE088
，通过北东 地-经纬高坐标系的转化，求出当前时刻t对应的经纬高
Figure 694841DEST_PATH_IMAGE089
，最后将当前时刻 t的经纬高和当前时刻t的中间位置和姿态数据
Figure DEST_PATH_IMAGE090
串起来， 得到最终的目标定位数据
Figure 856833DEST_PATH_IMAGE091
。
在一具体实施例中，所述步骤S105中基于所述目标定位数据，对点云地图进行实时更新，包括：
S60、按下式计算当前时刻t的目标定位数据与历史目标定位数据组合的绝对差值：
Figure DEST_PATH_IMAGE092
；
其中，所述
Figure 620127DEST_PATH_IMAGE093
由前n个历史时刻中所有插入点云地图的对应的历 史目标定位数据构建的集合；
S61、遍历所述历史目标定位数据的集合，确定是否满足判断条件；
其中，所述判断条件如下：
Figure DEST_PATH_IMAGE094
；
Figure 842161DEST_PATH_IMAGE095
；
若满足所述判断条件，则执行步骤S62，若不满足所述判断条件，则执行步骤S63；
S62、判定有历史目标定位数据与当前时刻t的目标定位数据相似，并不对当前时刻t的点云地图进行实时更新；
S63、判定没有历史目标定位数据与当前时刻t的目标定位数据相似，并执行步骤S64：
S64、将当前时刻t的激光雷达3D点云插入全局地图中，并将当前时刻t的目标定位数据加入到历史目标定位数据组合；
S65、对插入所述全局地图中的当前时刻t的激光雷达3D点云进行降采样处理，并执行坐标点的转换；
S66、按下式将转换后的点云加入全局地图：
Figure DEST_PATH_IMAGE096
；
其中，
Figure 192371DEST_PATH_IMAGE097
表示全局地图，
Figure DEST_PATH_IMAGE098
，代表 每个雷达点，每一雷达点包含对应的坐标x，y，z以及intensity强度信息，
Figure 892473DEST_PATH_IMAGE099
表示当前时刻t的点云集合；
若满足判断条件，则认为当前时刻t的Pt与已插入点云地图中的某时刻的目标定位数据较为相似，即两个时刻的场景相近，当前时刻t的激光雷达3D点云无需插入全局地图中，跳过后续的步骤。反之，若不满足所述判断条件，则认为当前时刻的点云和之前时刻已插入的点云差别较大，需要将当前时刻的点云插入至全局地图中。
需要说明的是，当点云地图PMAPn为空时，无需判断，直接认为当前时刻的激光雷达3D点云需要插入全局地图中。
在步骤S65中，设插入当前时刻t的点云前，环境地图为
Figure DEST_PATH_IMAGE100
， 其中
Figure 244695DEST_PATH_IMAGE101
，代表每个激光雷达点，包含每个点的坐标x，y，z以及 intensity强度信息，对当前时刻t的激光雷达3D点云
Figure DEST_PATH_IMAGE102
进行降采样，得到降采样后的点云
Figure 637630DEST_PATH_IMAGE103
，在本申请中，采用体素降采样，对激光雷达进行降采样，体素大小设置为0.3m。
基于目标定位数据Pt，将当前时刻的激光雷达3D点云
Figure DEST_PATH_IMAGE104
转化至全局坐标下，得到
Figure 475136DEST_PATH_IMAGE105
，具体一实施例中，所述步骤S65中的坐标点的转换，包括：
S70、按下式计算所述目标定位数据中的位姿
Figure DEST_PATH_IMAGE106
对应的旋转矩阵
Figure 978930DEST_PATH_IMAGE107
：
Figure DEST_PATH_IMAGE108
；
Figure 451237DEST_PATH_IMAGE109
；
组合旋转矩阵Rt与平移矩阵Tt为转换矩阵
Figure 483915DEST_PATH_IMAGE110
，其中，平移矩阵
Figure DEST_PATH_IMAGE111
；
S71、基于所述转换矩阵Transt，按下式将所述目标点在雷达坐标系下的坐标转换至在全局坐标系下的坐标：
Figure 808718DEST_PATH_IMAGE112
；
其中，
Figure DEST_PATH_IMAGE113
表示目标点在全局坐标系下的坐标，
Figure 850623DEST_PATH_IMAGE114
表示 目标点在雷达坐标系下的坐标；
S72、将当前时刻t的点云集合转换至全局坐标系下的历史点云集合，其中，当前时刻t的点云集合由所有经过转换后的点云构成。
通过本申请的定位与建图方法即可得到每一时刻对应的目标定位数据
Figure DEST_PATH_IMAGE115
，并不断更新点云地图
Figure 239754DEST_PATH_IMAGE116
。
本发明实施例还提供一种无人船水面定位与建图装置，该无人船水面定位与建图装置用于执行前述无人船水面定位与建图方法的任一实施例。具体地，请参阅图3，图3是本发明实施例提供的无人船水面定位与建图装置的示意性框图。
如图3所示，无人船水面定位与建图装置500，包括：
识别单元501，用于基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据，判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，得到第一定位数据，若不满足计算条件，则返回判断船只下一时刻的实时数据是否满足计算绝对方向角的条件；
数据融合单元502，用于所述数据融合，过程如下：
获取船只当前时刻的位置初值、姿态初值以及激光雷达3D点云信息，并将所述船只当前时刻的位置初值、姿态初值以及激光雷达3D点云信息进行数据融合；
转换单元503，用于基于所述实时数据以及经纬高坐标系与北东地坐标系的转换关系，计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据；
更新单元504，用于将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，并基于所述目标定位数据以及观测到的激光雷达3D点云，对点云地图进行实时更新。
该装置可以适应GNSS信号时强时弱的水面场景，有效提供船只当前位置，并基于定位结果与观测结果进行地图构建。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
上述无人船水面定位与建图装置可以实现为计算机程序的形式，该计算机程序可以在如图4所示的计算机设备上运行。
请参阅图4，图4是本发明实施例提供的计算机设备的示意性框图。该计算机设备1100是服务器，服务器可以是独立的服务器，也可以是多个服务器组成的服务器集群。
参阅图4，该计算机设备1100包括通过系统总线1101连接的处理器1102、存储器和网络接口1105，其中，存储器可以包括非易失性存储介质1103和内存储器1104。
该非易失性存储介质1103可存储操作系统11031和计算机程序11032。该计算机程序11032被执行时，可使得处理器1102执行无人船水面定位与建图方法。
该处理器1102用于提供计算和控制能力，支撑整个计算机设备1100的运行。
该内存储器1104为非易失性存储介质1103中的计算机程序11032的运行提供环境，该计算机程序11032被处理器1102执行时，可使得处理器1102执行无人船水面定位与建图方法。
该网络接口1105用于进行网络通信，如提供数据信息的传输等。本领域技术人员可以理解，图4中示出的结构，仅仅是与本发明方案相关的部分结构的框图，并不构成对本发明方案所应用于其上的计算机设备1100的限定，具体的计算机设备1100可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
本领域技术人员可以理解，图4中示出的计算机设备的实施例并不构成对计算机设备具体构成的限定，在其他实施例中，计算机设备可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。例如，在一些实施例中，计算机设备可以仅包括存储器及处理器，在这样的实施例中，存储器及处理器的结构及功能与图4所示实施例一致，在此不再赘述。
应当理解，在本发明实施例中，处理器1102可以是中央处理单元 (CentralProcessing Unit，CPU)，该处理器1102还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路 (Application Specific IntegratedCircuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。其中，通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
在本发明的另一实施例中提供计算机可读存储介质。该计算机可读存储介质可以为非易失性的计算机可读存储介质。该计算机可读存储介质存储有计算机程序，其中计算机程序被处理器执行时实现本发明实施例的无人船水面定位与建图方法。
所述存储介质为实体的、非瞬时性的存储介质，例如可以是U盘、移动硬盘、只读存储器（Read-Only Memory，ROM）、磁碟或者光盘等各种可以存储程序代码的实体存储介质。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的设备、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种无人船水面定位与建图方法，其特征在于，包括：
基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据，判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，得到第一定位数据，若不满足计算条件，则返回判断船只下一时刻的实时数据是否满足计算绝对方向角的条件；
其中，所述数据融合过程如下：
获取船只当前时刻的位置初值、姿态初值以及雷达3D点云信息，并将所述船只当前时刻的位置初值、姿态初值以及雷达3D点云信息进行数据融合；
基于所述实时数据以及经纬高坐标系与北东地坐标系的转换关系，计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据；
将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，并基于所述目标定位数据以及观测到的雷达3D点云，对点云地图进行实时更新。
2.根据权利要求1所述的无人船水面定位与建图方法，其特征在于，所述基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据，包括：
利用GNSS双天线系统获取船只当前时刻的GNSS双天线信息，其中，所述GNSS双天线信息包括双天线之间的连接线在世界坐标系下的GNSS方向角、GNSS精度指标、双天线测角可用性评价指标；
基于前k个时刻的GNSS双天线信息，计算船只当前时刻的IMU方向角补偿值；
利用惯性传感器获取船只当前时刻的IMU信息，并基于船只当前时刻的IMU信息以及上一时刻的第一IMU方向角，计算得到当前时刻的第一IMU方向角，其中，所述IMU信息包括IMU角速度、IMU加速度；
基于当前时刻的IMU方向角补偿值以及第一IMU方向角，计算船只当前时刻行驶的绝对方向角。
3.根据权利要求2所述的无人船水面定位与建图方法，其特征在于，所述基于前k时刻的GNSS双天线信息，计算船只当前时刻的IMU方向角补偿值，包括：
获取当前时刻t以及前k个时刻的GNSS双天线信息，判断集合
Figure DEST_PATH_IMAGE001
中的每个值是否均小于
Figure 567418DEST_PATH_IMAGE002
；其中，
Figure DEST_PATH_IMAGE003
分别表示第t-k+1时刻、第t-k+2时刻、……、当 前时刻t的GNSS精度指标的值，
Figure 848357DEST_PATH_IMAGE004
为预设值；
若集合
Figure DEST_PATH_IMAGE005
中的每个值均小于
Figure 959533DEST_PATH_IMAGE006
，则 继续判断集合
Figure DEST_PATH_IMAGE007
中的每个值是否均为
Figure 907897DEST_PATH_IMAGE008
，若
Figure DEST_PATH_IMAGE009
中的每个值均为
Figure 312071DEST_PATH_IMAGE008
，则判定当前时刻t 的GNSS方向角精度高，并对船只当前时刻的IMU方向角补偿值进行更新；其中，
Figure 939362DEST_PATH_IMAGE010
表示 当前时刻t的 GNSS精度指标的值，
Figure DEST_PATH_IMAGE011
分 别表示第t-k+1时刻、第t-k+2时刻、……、当前时刻t的双天线测角可用性评价指标的值，
Figure 995174DEST_PATH_IMAGE008
为预设值；
若集合
Figure 657099DEST_PATH_IMAGE012
中的每个值不满足均小于
Figure DEST_PATH_IMAGE013
，或集合
Figure 733640DEST_PATH_IMAGE009
中的每个值有不为
Figure 956548DEST_PATH_IMAGE008
的，则判定当前时刻的GNSS方向角精度低，并不对船只当前时刻的IMU方向角补偿值进行更 新；
所述利用惯性传感器获取船只当前时刻的IMU信息，并基于船只当前时刻的IMU信息以及上一时刻的第一IMU方向角，计算得到当前时刻的第一IMU方向角，包括：
获取当前时刻以及前k个时刻的GNSS方向角，以及获取当前时刻以及前k个时刻的第一IMU方向角，并对两者作差并求均值，得到当前时刻的IMU方向角补偿值；
利用欧拉角解算方法，解算船只当前时刻的欧拉角，得到船只当前时刻的第一IMU方向角；
所述基于当前时刻的IMU方向角补偿值以及第一IMU方向角，计算船只当前时刻行驶的绝对方向角，包括：
将船只当前时刻的第一IMU方向角与当前时刻的IMU方向角补偿值相加，得到船只当前时刻t的行驶的绝对方向角。
4.根据权利要求3所述的无人船水面定位与建图方法，其特征在于，所述判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，若不满足计算条件，则返回判断船只下一时刻的实时数据是否满足计算绝对方向角的条件，包括：
判断是否对当前时刻的IMU方向角补偿值进行更新，若对当前时刻的IMU方向角补偿值进行更新，则将计算得到的当前时刻的船只行驶的绝对方向角作为起始时刻的绝对方向角，并执行数据融合，若未对当前时刻的IMU方向角补偿值进行更新，则返回计算船只下一时刻行驶的绝对方向角。
5.根据权利要求1所述的无人船水面定位与建图方法，其特征在于，所述数据融合过程如下：获取船只当前时刻的位置初值、姿态初值以及雷达3D点云信息，并将所述船只当前时刻的位置初值、姿态初值以及雷达3D点云信息进行数据融合，得到第一定位数据，包括：
获取当前时刻t对应的时间
Figure 737422DEST_PATH_IMAGE014
和上一时刻对应的时间
Figure DEST_PATH_IMAGE015
之间的m个IMU测量值
Figure 660379DEST_PATH_IMAGE016
中绕z轴的角速度
Figure DEST_PATH_IMAGE017
，并按下式对所述角速度
Figure 907821DEST_PATH_IMAGE018
进行积分，得到平 台方向角
Figure DEST_PATH_IMAGE019
的变化量：
Figure 86867DEST_PATH_IMAGE020
；
基于所述平台方向角
Figure 671432DEST_PATH_IMAGE019
的变化量，按下式计算当前时刻t的第二IMU方向角：
Figure DEST_PATH_IMAGE021
；
其中，
Figure 448895DEST_PATH_IMAGE022
表示上一时刻t-1的第二IMU方向角；
基于所述当前时刻t的第二IMU方向角，按下式计算得到当前时刻的姿态初值，：
Figure DEST_PATH_IMAGE023
；
其中，
Figure 601659DEST_PATH_IMAGE024
为IMU测量值中
Figure 300625DEST_PATH_IMAGE025
对应的滚转角，
Figure DEST_PATH_IMAGE026
为IMU测量值中
Figure 390678DEST_PATH_IMAGE025
对 应的俯仰角；
基于船只速度近似不变的关系，按下式计算当前时刻t的位置初值
Figure 491490DEST_PATH_IMAGE027
，由此得 到当前时刻t的位置和姿态初值
Figure DEST_PATH_IMAGE028
：
Figure 815155DEST_PATH_IMAGE029
；
其中，
Figure DEST_PATH_IMAGE030
表示上一时刻t-1的位置；
基于当前时刻t的位置初值和姿态初值
Figure 798154DEST_PATH_IMAGE031
，以及上一时刻t- 1最终求得的历史位置和历史姿态
Figure DEST_PATH_IMAGE032
，计算两者之 间的第一变化量
Figure 629582DEST_PATH_IMAGE033
；
基于所述第一变化量
Figure 709533DEST_PATH_IMAGE034
、当前时刻t的 雷达3D点云
Figure DEST_PATH_IMAGE035
与上一时刻t-1的雷达3D点云
Figure 407362DEST_PATH_IMAGE036
，利用点云匹配算法，计算得到融合雷 达点云后的第二变化量
Figure DEST_PATH_IMAGE037
；
将所述第二变化量与上一时刻的历史位置和历史姿态对应相加，得到当前时刻t的第 一定位数据
Figure 612078DEST_PATH_IMAGE038
。
6.根据权利要求2所述的无人船水面定位与建图方法，其特征在于，所述基于所述实时数据以及经纬高坐标系与北东地坐标系的转换关系，计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据，包括：
利用GNSS双天线系统获取船只当前时刻的经度信息、纬度信息，得到经纬高坐标系下的经纬坐标；
将经纬高坐标系下的经纬坐标转换到北东地坐标系下的坐标，得到第二定位数据
Figure DEST_PATH_IMAGE039
；
所述将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，包括：
判断当前时刻t的第二定位数据是否满足精度条件；
其中，所述精度条件包括：
集合
Figure 981617DEST_PATH_IMAGE040
中的每个值均小于
Figure DEST_PATH_IMAGE041
，且集合
Figure 322600DEST_PATH_IMAGE042
的每个 值均小于
Figure DEST_PATH_IMAGE043
，其中，
Figure 50385DEST_PATH_IMAGE044
分别是当前时刻t的所述 GNSS精度指标中的GNSS定位精度因子、GNSS经度定位误差、GNSS纬度定位误差、GNSS高度定 位误差，
Figure DEST_PATH_IMAGE045
分别表示第t-m+1时刻、第t-m+2时 刻、……、第t时刻的GNSS定位精度因子，
Figure 742397DEST_PATH_IMAGE046
分别表示第t-m+1时刻的GNSS经度定 位误差、GNSS纬度定位误差、GNSS高度定位误差，
Figure DEST_PATH_IMAGE047
分别表示第 t时刻的GNSS经度定位误差、GNSS纬度定位误差、GNSS高度定位误差，
Figure 915627DEST_PATH_IMAGE041
为定位精度因 子阈值，
Figure 845537DEST_PATH_IMAGE043
为定位误差阈值；
若当前时刻t的第二定位数据满足精度条件，则判定当前时刻t的所述第二定位数据的定位精度高，并按下式计算当前时刻的中间位置和姿态数据：
Figure 540961DEST_PATH_IMAGE048
；
其中，
Figure DEST_PATH_IMAGE049
表示当前时刻t的第二定位数据，
Figure 392373DEST_PATH_IMAGE050
表示当前时 刻t的第一定位数据中的姿态；
若当前时刻t的第二定位数据不满足精度条件，则判定当前时刻t的所述第二定位数据的定位精度低，且执行以下步骤：
基于当前时刻t的第一定位数据以及上一时刻t-1的第一定位数据，计算两者之间的第三变化量；
将所述第三变化量与上一时刻t-1的历史中间位置和姿态数据相加，得到当前时刻t的 中间位置和姿态数据
Figure DEST_PATH_IMAGE051
；
基于当前时刻t的中间位置和姿态数据
Figure 369294DEST_PATH_IMAGE052
、经纬高 坐标系与北东地坐标系的转换关系，将当前时刻t相对起始时刻在北东坐标系下的坐标
Figure DEST_PATH_IMAGE053
转换成当前时刻t对应的经纬高
Figure 12765DEST_PATH_IMAGE054
，并按下式计算得到目标定位数 据：
Figure DEST_PATH_IMAGE055
；
其中，
Figure 223298DEST_PATH_IMAGE056
表示当前时刻t的中间位置和姿态数据。
7.根据权利要求6所述的无人船水面定位与建图方法，其特征在于，所述基于所述目标定位数据，对点云地图进行实时更新，包括：
按下式计算当前时刻t的目标定位数据与历史目标定位数据组合的绝对差值：
Figure DEST_PATH_IMAGE057
；
其中，所述
Figure 624323DEST_PATH_IMAGE058
由前n个历史时刻中所有插入点云地图的对应的历史目 标定位数据构建的集合；
遍历所述历史目标定位数据的集合，确定是否满足判断条件；
其中，所述判断条件如下：
Figure DEST_PATH_IMAGE059
；
Figure 404935DEST_PATH_IMAGE060
；
若满足所述判断条件，则判定有历史目标定位数据与当前时刻t的目标定位数据相似，并不对当前时刻t的点云地图进行实时更新；
若不满足所述判断条件，则判定没有历史目标定位数据与当前时刻t的目标定位数据相似，并执行以下步骤：
将当前时刻t的雷达3D点云插入全局地图中，并将当前时刻t的目标定位数据加入到历史目标定位数据组合；
对插入所述全局地图中的当前时刻t的雷达3D点云进行降采样处理，并执行坐标点的转换；
按下式将转换后的点云加入全局地图：
Figure DEST_PATH_IMAGE061
；
其中，
Figure 575016DEST_PATH_IMAGE062
表示全局地图，
Figure DEST_PATH_IMAGE063
，代表每个雷 达点，每一雷达点包含对应的坐标x，y，z以及intensity强度信息，
Figure 815505DEST_PATH_IMAGE064
表 示当前时刻t的点云集合；
其中，所述坐标点的转换过程如下：
按下式计算所述目标定位数据中的位姿
Figure DEST_PATH_IMAGE065
对应的旋转矩阵
Figure 969406DEST_PATH_IMAGE066
：
Figure DEST_PATH_IMAGE067
Figure 288129DEST_PATH_IMAGE068
；
组合旋转矩阵Rt与平移矩阵Tt为转换矩阵
Figure DEST_PATH_IMAGE069
，其中，平移矩阵
Figure 578296DEST_PATH_IMAGE070
；
基于所述转换矩阵Transt，按下式将所述目标点在雷达坐标系下的坐标转换至在全局坐标系下的坐标：
Figure DEST_PATH_IMAGE071
；
其中，
Figure 192948DEST_PATH_IMAGE072
表示目标点在全局坐标系下的坐标，
Figure DEST_PATH_IMAGE073
表示目标 点在雷达坐标系下的坐标；
将当前时刻t的点云集合转换至全局坐标系下的历史点云集合，其中，当前时刻t的点云集合由所有经过转换后的点云构成。
8.一种无人船水面定位与建图装置，其特征在于，包括：
识别单元，用于基于卫星导航系统以及船只上设置的惯性传感器获取的实时数据，判断船只当前时刻的实时数据是否满足计算绝对方向角的条件，若满足计算条件，则计算船只当前时刻行驶的绝对方向角，设定当前时刻为定位与建图的起始时刻，并执行数据融合，得到第一定位数据，若不满足计算条件，则返回判断船只下一时刻的实时数据是否满足计算绝对方向角的条件；
数据融合单元，用于所述数据融合，过程如下：
获取船只当前时刻的位置初值、姿态初值以及雷达3D点云信息，并将所述船只当前时刻的位置初值、姿态初值以及雷达3D点云信息进行数据融合；
转换单元，用于基于所述实时数据以及经纬高坐标系与北东地坐标系的转换关系，计算船只当前时刻相对起始时刻在北东地坐标系下的坐标，以得到第二定位数据；
更新单元，用于将所述第一定位数据与所述第二定位数据进行数据融合，得到目标定位数据，并基于所述目标定位数据以及观测到的雷达3D点云，对点云地图进行实时更新。
9.一种计算机设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的无人船水面定位与建图方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行如权利要求1至7任一项所述的无人船水面定位与建图方法。
CN202210012655.5A 2022-01-07 2022-01-07 一种无人船水面定位与建图方法、装置及相关组件 Active CN114046792B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210012655.5A CN114046792B (zh) 2022-01-07 2022-01-07 一种无人船水面定位与建图方法、装置及相关组件

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210012655.5A CN114046792B (zh) 2022-01-07 2022-01-07 一种无人船水面定位与建图方法、装置及相关组件

Publications (2)

Publication Number Publication Date
CN114046792A true CN114046792A (zh) 2022-02-15
CN114046792B CN114046792B (zh) 2022-04-26

Family

ID=80213471

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210012655.5A Active CN114046792B (zh) 2022-01-07 2022-01-07 一种无人船水面定位与建图方法、装置及相关组件

Country Status (1)

Country Link
CN (1) CN114046792B (zh)

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114545400A (zh) * 2022-04-27 2022-05-27 陕西欧卡电子智能科技有限公司 基于毫米波雷达的水面机器人的全局重定位方法
CN114966790A (zh) * 2022-06-02 2022-08-30 苏州庄舟智能科技有限公司 无人船定位方法及系统
CN115629374A (zh) * 2022-12-16 2023-01-20 陕西欧卡电子智能科技有限公司 基于毫米波雷达的无人船桥下定位方法及相关设备
CN117419690A (zh) * 2023-12-13 2024-01-19 陕西欧卡电子智能科技有限公司 一种无人船的位姿估计方法、装置及介质
CN117690194A (zh) * 2023-12-08 2024-03-12 北京虹湾威鹏信息技术有限公司 一种多源ai生物多样性观测的方法和采集系统

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008083777A (ja) * 2006-09-26 2008-04-10 Tamagawa Seiki Co Ltd 無人搬送車の誘導方法及び装置
CN107024210A (zh) * 2017-03-09 2017-08-08 深圳市朗空亿科科技有限公司 一种室内机器人避障方法、装置及导航系统
CN107527382A (zh) * 2017-08-16 2017-12-29 北京京东尚科信息技术有限公司 数据处理方法以及装置
CN111457902A (zh) * 2020-04-10 2020-07-28 东南大学 基于激光slam定位的水域测量方法及系统
CN111708038A (zh) * 2020-06-23 2020-09-25 上海埃威航空电子有限公司 基于姿态传感器和gnss的无人船激光雷达点云数据校正方法
CN112415558A (zh) * 2021-01-25 2021-02-26 腾讯科技（深圳）有限公司 行进轨迹的处理方法及相关设备
CN113238576A (zh) * 2021-05-07 2021-08-10 北京中科遥数信息技术有限公司 用于无人机的定位方法以及相关装置
CN113763549A (zh) * 2021-08-19 2021-12-07 同济大学 融合激光雷达和imu的同时定位建图方法、装置和存储介质

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2008083777A (ja) * 2006-09-26 2008-04-10 Tamagawa Seiki Co Ltd 無人搬送車の誘導方法及び装置
CN107024210A (zh) * 2017-03-09 2017-08-08 深圳市朗空亿科科技有限公司 一种室内机器人避障方法、装置及导航系统
CN107527382A (zh) * 2017-08-16 2017-12-29 北京京东尚科信息技术有限公司 数据处理方法以及装置
CN111457902A (zh) * 2020-04-10 2020-07-28 东南大学 基于激光slam定位的水域测量方法及系统
CN111708038A (zh) * 2020-06-23 2020-09-25 上海埃威航空电子有限公司 基于姿态传感器和gnss的无人船激光雷达点云数据校正方法
CN112415558A (zh) * 2021-01-25 2021-02-26 腾讯科技（深圳）有限公司 行进轨迹的处理方法及相关设备
CN113238576A (zh) * 2021-05-07 2021-08-10 北京中科遥数信息技术有限公司 用于无人机的定位方法以及相关装置
CN113763549A (zh) * 2021-08-19 2021-12-07 同济大学 融合激光雷达和imu的同时定位建图方法、装置和存储介质

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
朱健楠等: "无人水面艇感知技术发展综述", 《哈尔滨工程大学学报》 *

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114545400A (zh) * 2022-04-27 2022-05-27 陕西欧卡电子智能科技有限公司 基于毫米波雷达的水面机器人的全局重定位方法
CN114545400B (zh) * 2022-04-27 2022-08-05 陕西欧卡电子智能科技有限公司 基于毫米波雷达的水面机器人的全局重定位方法
CN114966790A (zh) * 2022-06-02 2022-08-30 苏州庄舟智能科技有限公司 无人船定位方法及系统
CN114966790B (zh) * 2022-06-02 2024-05-03 苏州庄舟智能科技有限公司 无人船定位方法及系统
CN115629374A (zh) * 2022-12-16 2023-01-20 陕西欧卡电子智能科技有限公司 基于毫米波雷达的无人船桥下定位方法及相关设备
CN117690194A (zh) * 2023-12-08 2024-03-12 北京虹湾威鹏信息技术有限公司 一种多源ai生物多样性观测的方法和采集系统
CN117419690A (zh) * 2023-12-13 2024-01-19 陕西欧卡电子智能科技有限公司 一种无人船的位姿估计方法、装置及介质
CN117419690B (zh) * 2023-12-13 2024-03-12 陕西欧卡电子智能科技有限公司 一种无人船的位姿估计方法、装置及介质

Also Published As

Publication number Publication date
CN114046792B (zh) 2022-04-26

Similar Documents

Publication Publication Date Title
CN114046792B (zh) 一种无人船水面定位与建图方法、装置及相关组件
CN108732603B (zh) 用于定位车辆的方法和装置
CN111208492B (zh) 车载激光雷达外参标定方法及装置、计算机设备及存储介质
Specht et al. Application of an autonomous/unmanned survey vessel (ASV/USV) in bathymetric measurements
US10197400B2 (en) Calibration methods and systems for an autonomous navigation vehicle
CN107504981B (zh) 一种基于激光测高数据的卫星姿态误差修正方法及设备
CN112835085B (zh) 确定车辆位置的方法和装置
WO2020189079A1 (ja) 自己位置推定装置、それを備えた自動運転システム、および、自己生成地図共有装置
CN113899375B (zh) 车辆定位方法和装置、存储介质及电子设备
CN114252082B (zh) 车辆定位方法、装置和电子设备
CN112596089B (zh) 融合定位方法、装置、电子设备及存储介质
CN112800159B (zh) 地图数据处理方法及装置
CN113984044A (zh) 一种基于车载多感知融合的车辆位姿获取方法及装置
CN111241224B (zh) 目标距离估计的方法、系统、计算机设备和存储介质
CN110596741A (zh) 车辆定位方法、装置、计算机设备和存储介质
CN112859133A (zh) 一种基于雷达与北斗数据的船舶深度融合定位方法
CN114063622B (zh) 无人船自主停泊定位方法、装置及相关组件
CN112154303A (zh) 高精度地图定位方法、系统、平台及计算机可读存储介质
CN115792985A (zh) 一种车辆定位方法、装置、电子设备、存储介质及车辆
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN114088104B (zh) 一种自动驾驶场景下的地图生成方法
CN112835086B (zh) 确定车辆位置的方法和装置
CN114897942A (zh) 点云地图的生成方法、设备及相关存储介质
CN111340952B (zh) 一种移动测量失锁区域制图方法和装置
CN114812447B (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