Absolute positioning method for unmanned surface vehicle (USV), device, and storage medium

Abstract

An absolute positioning method for an unmanned surface vehicle (USV) utilises multi-source coastline measurements from a synchronously calibrated thermal infrared camera, lidar, and X-band marine rada

Description

TECHNICAL FIELD

[0001] The present disclosure relates to the technical field of positioning navigation, and in particular, to an absolute positioning method for an unmanned surface vehicle (USV) based on multi-source coastline sensing, a device, and a storage medium. BACKGROUND

[0002] At present, most existing USV positioning technologies rely on signals from the global navigation satellite system (GNSS). However, in a satellite-denied environment or a deceptive environment in which GNSS signals are interfered with, the sensors on the USV can only achieve coarse relative positioning and cannot obtain the absolute position information of the USV in maritime areas, making it difficult for the unmanned system to complete navigation tasks that depend on accurate absolute geographic position information in maritime areas.

[0003] The Patent Application No. CN202210884919.6 discloses an unmanned ship positioning method based on multi-sensor data fusion. This method utilizes latitude and longitude information, from the navigation track of the unmanned ship, collected by the positioning system sensor, that is, the global positioning system (GPS), to implement subsequent strategies such as positioning information fusion and correction. In other words, the positioning method achieves fine positioning based on existing positioning signals. In addition, GNSS includes the GPS of the USA, the Global Orbiting Navigation Satellite System (GLONASS) of Russia, and the BeiDou Navigation Satellite System of China, meaning this method cannot be applied in maritime environments without GNSS signals, that is, GPS signals.

[0004] The Patent Application No. CN202210575283.7 discloses a simultaneous localization and mapping (SLAM) method and system for an unmanned ship based on multi-sensor fusion. By fusing the sensing information from solid-state lidar and mechanical lidar, this method obtains the pose of the USV relative to the riverbank. However, the method cannot obtain the absolute pose information of the USV and lacks the capability for high-precision, long-distance mapping. SUMMARY

[0005] The present disclosure aims to provide an absolute positioning method for a USV, a device, and a storage medium, so as to resolve the problem of achieving absolute positioning of the USV in a satellite-denied condition or a deceptive environment in which GNSS signals are interfered with.

[0006] The present disclosure resolves the above technical problem by using the following technical solutions: An absolute positioning method for a USV. The USV is equipped with a thermal infrared camera, a lidar, and a marine radar that have completed synchronous calibration, and the absolute positioning method includes the following steps:

[0007] obtaining coastline area point cloud data detected by the lidar and a coastline area thermal infrared image collected by the thermal infrared camera at a same moment;

[0008] obtaining a coastline point cloud data set in a lidar coordinate system according to the coastline area point cloud data and the coastline area thermal infrared image;

[0009] performing edge feature extraction on the coastline point cloud data set in the lidar coordinate system, to obtain an edge feature point set;

[0010] estimating a pose transformation relationship of the lidar in a (k+l)th frame relative to a kth frame according to edge feature point sets of the lidar in the (k+l)th frame and the k* frame;

[0011] obtaining an estimated USV pose and a point cloud map in the (k+l)* frame according to the coastline point cloud data set in the lidar coordinate system and the pose transformation relationship of the lidar in the (k+l)th frame relative to the kth frame;

[0012] performing periodic corrections on the estimated USV pose and the point cloud map in the (k+1)* frame according to the coastline point cloud data set in the lidar coordinate system and an original echo image collected by the marine radar, to obtain a corrected estimated USV pose and a corrected point cloud map in the (k+l)* frame;

[0013] obtaining a coastal remote sensing image tile, and performing coarse-grained retrieval matching on the corrected point cloud map in the (k+l)th frame and the coastal remote sensing image tile, to obtain a two-dimensional (2D) coastline map and remote sensing data tile corresponding to the point cloud map; and

[0014] performing absolute positioning of the USV according to the corrected estimated USV pose in the (k+l)th frame, and the 2D coastline map and the remote sensing data tile corresponding to the point cloud map.

[0015] Further, said obtaining a coastline point cloud data set in a lidar coordinate system according to the coastline area point cloud data and the coastline area thermal infrared image includes:

[0016] performing semantic segmentation on the coastline area thermal infrared image, to obtain a 2D segmentation point set of a coastline area;

[0017] projecting each point in the coastline area point cloud data to an image plane of the thermal infrared camera, where a specific projection formula is as follows:

[0018] Zc

[0019] \XL ,YL ,ZL are homogeneous coordinates of the coastline area point cloud data in the lidar coordinate system L; and

[0020] [x,nfrared, Yinfrared, l]r are homogeneous coordinates of a pixel point obtained by projecting a point to the image plane of the thermal infrared camera; Zc is a scale factor; is a spatial transformation matrix between the lidar coordinate system and an image plane coordinate system; and is an intrinsic matrix of the thermal infrared camera;

[0021] determining whether a projection range of each pixel point falls into the 2D segmentation point set; and if yes, determining a point corresponding to the pixel point as a coastline point; otherwise, determining a point corresponding to the pixel point as a noncoastline point; and

[0022] forming the coastline point cloud data set in the lidar coordinate system L with all the coastline points.

[0023] Further, edge feature extraction is performed on the coastline point cloud data set in the lidar coordinate system by using a lightweight and ground-optimized lidar odometry and mapping on variable terrain (LeGO-LOAM) algorithm, and a specific implementation process includes:

[0024] for each point in the coastline point cloud data set in the lidar coordinate system, performing least squares fitting on the point and a neighborhood point to obtain curvature of the point; and

[0025] sifting out points with curvature greater than a curvature threshold as edge feature points, to obtain an edge feature point set.

[0026] Further, said estimating a pose transformation relationship of the lidar in a (k+l)th frame relative to a kth frame using edge feature point sets of the lidar in the (k+l)th frame and the k* frame includes:

[0027] finding out a matching relationship between edge feature points in the edge feature point set in the (k+l)th frame and edge feature points in the kth frame by using a nearest neighbor searching (NNS) algorithm, to obtain matching feature pairs between the (k+l)th frame and the kth frame; and

[0028] performing estimation on the matching feature pairs of the (k+l)th frame and the kth frame by using a nonlinear optimization algorithm, to obtain the pose transformation relationship in the (k+l)th frame relative to the kth frame.

[0029] Further, said obtaining an estimated USV pose and a point cloud map in the (k+l)th frame according to the coastline point cloud data set in the lidar coordinate system and the pose transformation relationship of the lidar in the (k+l)th frame relative to the kth frame includes:

[0030] obtaining a pose transformation relationship of the lidar coordinate system relative to a world coordinate system in the (k+l)th frame according to a pose transformation relationship of the lidar coordinate system relative to the world coordinate system in the kth frame and the pose transformation relationship of the lidar in the (k+l)th frame relative to the kth frame;

[0031] projecting the coastline point cloud data set in the lidar coordinate system to the world coordinate system according to a pose transformation relationship of the lidar coordinate system relative to the world coordinate system in the (k+l)th frame, to obtain a coastline point cloud data set in the world coordinate system in the (k+l)th frame;

[0032] constructing a point cloud map of the kth frame according to coastline area point cloud data from a 1st frame to the kth frame of the lidar;

[0033] matching the coastline point cloud data set in the world coordinate system with the point cloud map in the kth frame by using the NNS algorithm, to obtain matching point pairs of the coastline point cloud data set in the world coordinate system and the point cloud map in the k* frame;

[0034] optimizing the pose transformation relationship of the lidar coordinate system relative to the world coordinate system in the (k+l)th frame by using the nonlinear optimization algorithm based on the matching point pairs, to obtain an optimized pose transformation relationship of the lidar coordinate system relative to the world coordinate system in the (k+l)th frame, that is, the estimated USV pose in the (k+l)th frame; and

[0035] merging the coastline point cloud data set in the world coordinate system into the point cloud map in the kth frame based on the estimated USV pose in the (k+l)th frame, to obtain the point cloud map in the (k+l)th frame.

[0036] Further, said performing periodic corrections on the estimated USV pose and the point cloud map in the (k+l)th frame according to the coastline point cloud data set in the lidar coordinate system and an original echo image collected by the marine radar includes:

[0037] constructing a factor graph by using the pose transformation relationship of the lidar in the (k+l)th frame relative to the kth frame as an edge of the factor graph and using the estimated USV pose in the (k+l)th frame as a vertex of the factor graph, and initializing the factor graph;

[0038] extracting, according to the coastline point cloud data set in the lidar coordinate system, a coastal direction echo image from the original echo image collected by the marine radar;

[0039] setting a correction interval frame of the marine radar to h, and determining whether the (k+l)th frame of the lidar is an integer multiple of the correction interval frame h of the marine radar; and if not, outputting the estimated USV pose and the point cloud map in the (k+l)th frame, and adding to the factor graph the pose transformation relationship in the (k+l)th frame relative to the kth frame and the estimated USV pose in the (k+l)th frame; otherwise, extracting again, according to the coastline point cloud data set in the lidar coordinate system, a coastal direction echo image in the (k+l)th frame from the original echo image collected by the marine radar, and matching common coastline features in the two extracted coastal direction echo images;

[0040] estimating a pose transformation relationship of the marine radar in the (k+l)* frame of the lidar relative to previous h frames by using the nonlinear optimization algorithm according to matching feature point pairs obtained through matching;

[0041] adding to the factor graph the pose transformation relationship of the marine radar in the (k+l)th frame of the lidar relative to the previous h frames and the estimated USV pose in the (k+l)th frame; and

[0042] optimizing the factor graph, correcting an estimated USV pose between a (k+l-h)* frame and the (k+l)th frame, and readjusting a point cloud map between the (k+l-h)th frame and the (k+l)th frame by using the corrected estimated USV pose.

[0043] Further, said extracting, according to the coastline point cloud data set in the lidar coordinate system, a coastal direction echo image from the original echo image collected by the marine radar includes:

[0044] obtaining a horizontal angle range of the coastal area according to edge points on both sides of the coastline point cloud data set in the lidar coordinate system and an origin of the lidar coordinate system;

[0045] drawing an angle bisector of the horizontal angle range, passing through the origin of the lidar coordinate system; and

[0046] drawing a vertical of the angle bisector, passing through the origin of the lidar coordinate system, and retaining the original echo image within a 180° range on a coastal side, to obtain the coastal direction echo image.

[0047] Further, said matching common coastline features in the two extracted coastal direction echo images includes:

[0048] extracting feature point coordinates and a feature descriptor from the first extracted coastal direction echo image by using a scale-invariant feature transform (SIFT) operator, and extracting feature point coordinates and a feature descriptor from the second extracted coastal direction echo image by using the SIFT operator; and

[0049] matching the two extracted feature descriptors by using a K-nearest neighbors (KNN) algorithm, to obtain the matching feature point pairs between the two extracted coastal direction echo images.

[0050] Further, said performing coarse-grained retrieval matching on the corrected point cloud map in the (k+l)* frame and the coastal remote sensing image tile includes:

[0051] performing coastline segmentation on the coastal remote sensing image tile, and overlaying a segmentation mask onto the coastal remote sensing image tile along a channel dimension, to obtain fused tile image data;

[0052] based on a deviation between the world coordinate system in the point cloud map and a true north direction at an initial moment, rotating counterclockwise the corrected point cloud map in the (k+l)th frame around an x-axis of the world coordinate system, to align the point cloud map with a direction in the coastal remote sensing image tile;

[0053] projecting the rotated point cloud map to a bird's-eye view (BEV), to obtain a 2D top view map;

[0054] scaling the 2D top view map to a same receptive field scale as the coastal remote sensing image tile, to obtain a 2D coastline map;

[0055] separately performing feature extraction on the fused tile image data and the 2D coastline map, to obtain a tile feature vector and a map feature vector;

[0056] separately performing preliminary compression on the tile feature vector and the map feature vector; and

[0057] performing feature encoding and retrieval matching on the preliminarily compressed tile feature vector and map feature vector, to obtain the remote sensing data tile and geospatial information thereof corresponding to a minimum distance between feature codes.

[0058] Further, said performing absolute positioning of the USV according to the corrected estimated USV pose in the (k+l)th frame, and the 2D coastline map and the remote sensing data tile corresponding to the point cloud map includes:

[0059] performing registration on the remote sensing data tile and the 2D coastline map, and calculating plane rectangular coordinates of the USV in a projected coordinate system; and

[0060] converting the plane rectangular coordinates of the USV in the projected coordinate system into spatial geodetic coordinates in a geographic coordinate system, to obtain an absolute position of the USV.

[0061] Further, a formula for calculating the plane rectangular coordinates of the USV in the projected coordinate system is: w^1 z x = xk + wk + r sin (arctan

[0062] h?™1 z y = y* ~T^ihk ~r cos (arctan^-0 ) K yw (X-X^ / o^X^-X, (x + x^ / ^x^ + x, X ’max 'max max max other Xy-y^Wy^l-y^ y>y^ (y+y^<2ymJ+y^ y<- y^ y other

[0064] (x, y) represent the plane rectangular coordinates of the USV in the projected coordinate system before overflow treatment, (x',^') represent the plane rectangular coordinates of the USV in the projected coordinate system after overflow treatment, and represent maximum values of the plane rectangular coordinates of the USV in the projected coordinate system; ^k,yk,-wk,h^) represent the geospatial information of remote sensing data tile; and hkxel respectively represent a pixel width and height of the remote sensing data tile; hkxel represents a pixel offset of an upper left comer of the 2D coastline map relative to an upper left comer of the remote sensing data tile on an y axis of the projected coordinate system; represents a pixel offset of the upper left comer of the 2D coastline map relative to the upper left comer of the remote sensing data tile on an x-axis of the projected coordinate system; r represents a distance between the USV and the origin of the world coordinate system; 0 represents a counterclockwise rotation angle of the corrected point cloud map in the (k+l)th frame around the x-axis of the world coordinate system; 0w represents a heading angle of the USV in the world coordinate system, and (yw,zw) represent coordinates of the USV in the world coordinate system.

[0065] Based on the same concept, the present disclosure further provides a terminal device, including:

[0066] a memory configured to store a computer program; and

[0067] a processor configured to execute the computer program to implement the above absolute positioning method for a USV.

[0068] Based on the same concept, the present disclosure further provides a computer-readable storage medium that stores a computer program, and the computer program, when executed by a processor, implements the above absolute positioning method for a USV.

[0069] Beneficial effects

[0070] Compared with the prior art, the present disclosure has the following advantages:

[0071] According to the present disclosure, the USV can obtain the spatial geodetic coordinates in the world geographic coordinate system and the heading angle through multi-source coastline perception solely by sensors mounted on the USV, without using GNSS signals. This capability expands the mission scope of the USV, allowing the USV to perform more complex maritime tasks based on the geospatial coordinates, such as autonomous coastal inspections, island inspections, and autonomous landings. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] To describe the technical solutions in the embodiments of the present disclosure more clearly, the following briefly introduces the accompanying drawings required for describing the embodiments. Apparently, the accompanying drawings in the following description show merely some embodiments of the present disclosure, and those of ordinary skill in the art may still derive other drawings from these accompanying drawings without creative efforts.

[0073] FIG. 1 is a flowchart of an absolute positioning method for a USV according to an embodiment of the present disclosure;

[0074] FIG. 2 is a schematic diagram of coastline sensing from a USV view according to an embodiment of the present disclosure;

[0075] FIG. 3 is a schematic diagram of cross-view registration and positioning according to an embodiment of the present disclosure;

[0076] FIG. 4 is a block diagram of correcting a point cloud map constructed by a lidar and a USV pose under guidance of a short-wave marine radar according to an embodiment of the present disclosure;

[0077] FIG. 5 is a schematic diagram of constructing a point cloud map by a lidar according to an embodiment of the present disclosure;

[0078] FIG. 6 is a schematic diagram of pose correction of a short-wave marine radar according to an embodiment of the present disclosure;

[0079] FIG. 7 is a flowchart of pose correction of a short-wave marine radar according to an embodiment of the present disclosure;

[0080] FIG. 8 is a schematic diagram of echo image collection of a short-wave marine radar according to an embodiment of the present disclosure;

[0081] FIG. 9 is an architecture diagram of cross-view USV absolute positioning according to an embodiment of the present disclosure;

[0082] FIG. 10A is a schematic diagram of rotating a point cloud map according to an embodiment of the present disclosure; and

[0083] FIG. 10B is a schematic diagram of registration between a remote sensing tile segmentation mask and a horizontal projection of a point cloud map according to an embodiment of the present disclosure. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0084] The following clearly and completely describes the technical solutions in the embodiments of the present disclosure with reference to the accompanying drawings in the embodiments of the present disclosure. Apparently, the described examples are merely a part rather than all of the examples of the present disclosure. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present disclosure without creative efforts shall fall within the scope of protection of the present disclosure.

[0085] The technical solution of the present disclosure will be described in detail below with reference to specific embodiments. The following specific embodiments may be combined with each other, and the same or similar concepts or processes may not be repeatedly described in some embodiments.

[0086] Embodiment 1

[0087] In order to resolve the problem of autonomous geographic positioning for a USV in a satellite-denied environment, the present disclosure fuses multi-source sensor information, specifically from a thermal infrared camera, a lidar, an X-band marine radar, and coastal remote sensing image tile, to perform multi-source coastline perception processing. Specifically, the present disclosure proposes a method for long-distance coastal point cloud map construction and cross-view registration and positioning between the long-distance coastal point cloud map and remote sensing tile images, aiming to obtain spatial geodetic coordinate information (latitude and longitude) and USV pose information (yaw angle) in the geographic coordinate system.

[0088] The USV in the present disclosure is equipped with a thermal infrared camera, a lidar, and an X-band short-wave marine radar that have completed synchronous calibration, to achieve coastline perception from the USV view. Coastline perception from a satellite view is achieved using offline remote sensing image tile. Specifically, for the USV view, a lidar positioning and mapping framework based on the guidance of a short-wave marine radar is designed. On the basis of autonomous regulation and control along the coast, the USV can collect and construct a long-distance coastal point cloud map, and obtain pose information of the USV relative to the point cloud map. For the remote sensing view, a global fused coastal remote sensing tile database is established, and a cross-view coastline registration and positioning framework based on the remote sensing images and the point cloud map is designed for coastline perception data in different modalities and different views, to obtain the absolute pose information of the USV. The flowchart of the method according to the present disclosure is shown in FIG. 1, and a schematic diagram of USV positioning under a denied condition is shown in FIG. 2 and FIG. 3.

[0089] The absolute positioning method for a USV provided in the embodiments of the present disclosure includes two parts. The first part is lidar positioning and mapping based on the guidance of a short-wave marine radar, while the second part utilizes remote sensing images and the point cloud map from the first part for cross-view coastline registration and positioning.

[0090] The coastline area that can be detected by the lidar mounted on the USV is usually much smaller than that covered by a remote sensing tile image. Therefore, to ensure a balance in the receptive fields between the registered data, it is necessary to conduct a long-distance mapping operation on the coastline point cloud (that is, to construct a long-distance coastline point cloud map) and simultaneously obtain the relative pose of the USV in relative to the constructed point cloud map. To ensure the accuracy of the constructed point cloud map, the traditional SLAM framework eliminates the accumulated errors from the odometer during the mapping process through loop detection. However, the collection path of the USV in unfamiliar seas is usually not closed, and there is no a priori map to serve as a guide, which means the traditional SLAM framework cannot guarantee the precision of system localization and mapping. The receptive field refers to the size of the area imaged by the sensor and mapped to the real world. The radius of the area that the marine radar can detect is about 40 nautical miles, while the effective detection range of the lidar is only about 150 meters, meaning the marine radar has a larger receptive field than the lidar.

[0091] In view of this, the first part of the present disclosure proposes a localization and mapping framework for fine-grained information from the lidar, based on the coarse-grained information from the short-wave marine radar. The framework uses the coastline profile from the large receptive field detected by the short-wave marine radar as a priori guidance information to effectively suppress the accumulated errors in the localization and mapping of the lidar. The first part involves two coordinate systems: a lidar coordinate system L, a world coordinate system W, and a marine radar coordinate system R. In the lidar coordinate system L, the x-y-z axes point respectively to the upper-right-front of the USV. The position of the world coordinate system W is fixed, coinciding with the lidar coordinate system L at the initial time k=0 of point cloud mapping. Since the size of the USV is much smaller than the scale of the long-distance coastline point cloud map, the lidar coordinate system L and the marine radar coordinate system R are set to coincide. The dashed box in FIG. 4 represents utilizing the original echo image collected by the marine radar to correct the point cloud map constructed based on the lidar. This process helps to suppress the accumulated errors in both the point cloud map constructed based on the lidar and the pose information. The area outside the dashed box in FIG. 4 represents constructing a long-distance coastline point cloud map based on the lidar.

[0092] As shown in FIG. 1, FIG. 4, and FIG. 9, an absolute positioning method for a USV according to an embodiment of the present disclosure includes the following steps.

[0093] Step SI: Obtain coastline area point cloud data detected by the lidar and a coastline area thermal infrared image collected by the thermal infrared camera at a same moment.

[0094] Step S2: Obtain a coastline point cloud data set in a lidar coordinate system according to the coastline area point cloud data and the coastline area thermal infrared image.

[0095] Because thermal infrared can sense temperature, the water temperature and coastline (that is, the boundary between the coastline and the water area) are more distinct than in visible light images. Therefore, the coastline area thermal infrared image collected by the thermal infrared camera can be used to filter out irrelevant background point clouds and noise in the coastline area point cloud data, while preserving the coastline point clouds. In a specific implementation of the present disclosure, a specific implementation process of the step S2 includes:

[0096] Step S2.1: Perform semantic segmentation on the coastline area thermal infrared image collected by the thermal infrared camera, to obtain a 2D segmentation point set , )} of a coastline area.

[0097] In this embodiment, the semantic segmentation network U-net is used to perform semantic segmentation on the 2D coastline of the coastline area thermal infrared image collected by the thermal infrared camera. For details about the semantic segmentation network U-net, refer to: Ronneberger O, Fischer P, Brox T.U-net: Convolutional networks for biomedical image segmentation [C]. Medical Image Computing and Computer-Assisted Intervention-MICCAI 2015: 18th International Conference, Munich, Germany, October 5-9, 2015, Proceedings, Part III 18, 2015:234-41.

[0098] Step S2.2: Project each point in the coastline area point cloud data in the lidar coordinate system L to an image plane of the thermal infrared camera, where a specific projection formula is as follows:

[0099] Zc v0 1 (1)

[0100] [Xl,Yl,Zl,1]t are homogeneous coordinates of the coastline area point cloud data in the lidar coordinate system L; and

[0101] ^x'nfrared, Yinfrared, l]r are homogeneous coordinates of a pixel point obtained by projecting a point to the image plane of the thermal infrared camera; Zc is a scale factor; is a spatial transformation matrix between the lidar coordinate system and an image plane coordinate system; T'"frared includes a 3x3 rotation matrix R and a 3x1 translation vector T; and 4„ / rared 311 intrinsic matrix of the thermal infrared camera. T^frared and 4» / ™^ 31-6 obtained by calibrating the thermal infrared camera.

[0102] Step S2.3: Determine whether a projection range of a pixel point corresponding to a spatial point cloud falls into the 2D segmentation point set {(XinpZ<)| >and if yes, determine a point corresponding to the pixel point as a coastline point; otherwise, determine a point corresponding to the pixel point as a non-coastline point. Traverse all points in the coastline area point cloud data, to eliminate irrelevant background point clouds and noise and retain real coastline points.

[0103] Step S2.4: All the coastline points form the coastline point cloud data set coast’^cLoaSf^coast)} the lidar coordinate system L, where the superscript L represents the lidar coordinate system and (Xfoast,YcLoast,Z^oasl) represent 3D coordinates of the coastline point in the lidar coordinate system L.

[0104] The step S2 is implemented by a thermal infrared coastline auxiliary extraction submodule in FIG. 4, to form the coastline point cloud data set t(XL ,,71 „ZL ,)) in the lidar coordinate system L.

[0105] Step S3: Perform edge feature extraction on the coastline point cloud data set in the lidar coordinate system, to obtain an edge feature point set of the lidar.

[0106] In a specific implementation of the present disclosure, edge feature extraction is performed on the coastline point cloud data set in the lidar coordinate system by using a LeGO-LOAM algorithm, and a specific implementation process includes:

[0107] Step S3.1: For each point in the coastline point cloud data set {(X^oast,YcLoast, Zfoasl)} in the lidar coordinate system, perform least squares fitting on the point and a neighborhood point to obtain curvature of the point.

[0108] Step S3.2: Sift out points with curvature greater than a curvature threshold as edge feature points, to obtain the edge feature point set {(^dge’^dge’^dge)} ■

[0109] For the coastline area point cloud data of each frame detected by the lidar, the edge feature point set of the corresponding frame can be obtained through the processing in the steps SI to S3. The step S3 is implemented by an edge feature extraction sub-module in FIG. 4, to output the edge feature point set {(A^,Yedge’zedge)} of the lidar.

[0110] Step S4: Estimate a pose transformation relationship of the lidar in a current frame k+1 (that is, a (k+l)th frame) relative to a previous frame k (that is, a kth frame) according to edge feature point sets edge’Yedge’Zedge)} °f ^e lidar in the current frame k+1 and the previous frame k.

[0111] In a specific implementation of the present disclosure, a specific implementation process of the step S4 includes:

[0112] Step S4.1: For edge feature point sets of two neighboring frames of the lidar, find out a matching relationship between edge feature points in the edge feature point sets of the two frames by using an NNS algorithm, to obtain matching feature pairs of the current frame k+1 and the previous frame k.

[0113] Step S4.2: Estimate the matching feature pairs of the current frame k+1 and the previous frame k obtained in the step S4.1 by using a nonlinear optimization algorithm (such as Levenberg-Marquardt (L-M)), to obtain the pose transformation relationship Tk+l in the current frame k+1 relative to the previous frame k, where the superscript represents the lidar coordinate system and the subscript represents the current frame k+1.

[0114] The pose transformation relationship Tk+l in the current frame k+1 relative to the previous frame k includes a translation vector and a rotation matrix. The step S4 is implemented by a lidar odometer sub-module in FIG. 4, to output the pose transformation relationship Tk+l of the lidar in the current frame k+1 relative to the previous frame k, with outputting frequency being 10 Hz.

[0115] The NNS algorithm and the nonlinear optimization algorithm are both prior art. For the NNS algorithm, refer to: Arya S, Mount D M, Netanyahu N S, et al. An Optimal Algorithm for Approximate Nearest Neighbor Searching in Fixed Dimensions [J]. Journal of the ACM, 1998, (6):45. For the nonlinear optimization algorithm, refer to: Marquardt D W. An Algorithm for Least-Squares Estimation of Nonlinear Parameters: Journal of the Society for Industrial and Applied Mathematics: Vol.ll, No.2 (Society for Industrial and Applied Mathematics) [J]. Journal of the Society for Industrial &Applied Mathematics, 1963,11 (2): 431 -41.

[0116] Step S5: Obtain an estimated USV pose Tk^ and a point cloud map Qk+i Qk+X in the current frame k+1 according to the coastline point cloud data set {(Nfoasl,YcLoa5t,Zfoast)} in the lidar coordinate system and the pose transformation relationship TkL+1 of the lidar in the current frame k+1 relative to the previous frame k.

[0117] In a specific implementation of the present disclosure, as shown in FIG. 5, a specific implementation process of the step S5 includes: —w

[0118] Step S5.1: Obtain a pose transformation relationship Tk^ of the lidar coordinate system relative to a world coordinate system in the current frame k+1 according to a pose transformation relationship Tf of the lidar coordinate system relative to the world coordinate system in the previous frame k and the pose transformation relationship Tk+X in the current frame k+1 relative to the previous frame k.

[0119] Step S5.2: Project each point in the coastline point cloud data set {(X^aast,YcLoast,Zfoast)} in the lidar coordinate system to the world coordinate system through the pose transformation relationship Tk+i, to obtain a coastline point cloud data set {(^asl,Yc^asl,Z^ast)} in the world coordinate system, that is, project each point in the coastline point cloud data set {(^cOast>^cLoast>^cOasi)} to the world coordinate system through the pose transformation —w relationship Tk+i.

[0120] Step S5.3: Construct a point cloud map of the kth frame according to coastline area point cloud data from a 1st frame to the kth frame of the lidar.

[0121] Step S5.4: Match the coastline point cloud data set {(^Oast>Y^ast>z7oast)} the world coordinate system with the point cloud map in the previous frame k by using the NNS algorithm, to obtain matching point pairs of the coastline point cloud data set {(xLsf YL»zLst)} and the P°int cloud maP in the previous frame k. —w

[0122] Step S5.5: Optimize a pose transformation relationship Tk+i of the lidar coordinate system relative to the world coordinate system in the current frame k+1 by using the nonlinear optimization algorithm based on the matching point pairs obtained in the step S5.4, to obtain an optimized pose transformation relationship of the lidar coordinate system relative to the world coordinate system in the current frame k+1, that is, obtain the estimated USV pose in the current frame k+1, to be specific, the USV pose in the lidar coordinate system in the current frame k+1.

[0123] Step S5.6: Merge the coastline point cloud data set {(^ast,Yc^asl,Z^asl)} in the world coordinate system into the point cloud map Qk in the previous frame k based on the estimated USV pose in the current frame k+1, to update the point cloud map and obtain a point cloud map Qk+1 in the current frame k+1.

[0124] In the step S5.1, an initial value of Tk is a null matrix, and Tk in each frame is iteratively output. In the step S5.3, after the end of the kth frame of the lidar, the point cloud map Q. from the 1st frame to the kth frame is constructed, thus the point cloud data in the (k+l)th frame can be merged in the (k+l)th frame (that is, the current frame) into the point cloud map Qi, thus updating the point cloud map in the (k+l)th frame. The point cloud map is updated every frame.

[0125] The frequency of outputting the estimated USV pose Tk+l and the point cloud map Qk+X is 2 Hz. Through the steps SI to S5, the purpose of obtaining the estimated USV pose and the point cloud map in real time by the lidar is realized. However, when using the lidar to construct the point cloud map, accumulated errors from the odometer during the mapping process prevents accurate representation of the constructed point cloud map. To ensure the accuracy of the point cloud map, the present disclosure utilizes an original echo image collected by an X-band marine radar, to periodically correct the estimated USV pose and the point cloud map obtained based on the lidar-detected point cloud data.

[0126] Step S6: Perform periodic corrections on the estimated USV pose and the point cloud map of the current frame k+1 according to the coastline point cloud data set coast’YcLoast,Z^oast)} in the lidar coordinate system and an original echo image collected by the marine radar, to obtain a corrected estimated USV pose and a corrected point cloud map Qt+1 in the current frame k+1.

[0127] In a specific implementation of the present disclosure, as shown in FIG. 6 and FIG. 7, a specific implementation process of the step S6 includes:

[0128] Step S6.1: Construct a factor graph by using an open-source library GTS AM, where an edge of the factor graph is set as the pose transformation relationship of the lidar in the current frame k+1 relative to the previous frame k, and the vertex of the factor graph is set as the estimated USV pose T^j in the current frame k+1, that is, the parameters to be corrected; and initialize the factor graph according to the pose transformation relationship of the lidar and the estimated USV pose T* at the initial time of mapping (that is, k=0), and all elements in the pose transformation relationship and the estimated USV pose T* are 0.

[0129] Step S6.2: Extract, according to the coastline point cloud data set in the lidar coordinate system, a coastal direction echo image from the original echo image collected by the marine radar.

[0130] As shown in FIG. 8, a specific implementation process of extracting the coastal direction echo image Iradar_Bef is as follows:

[0131] Step S6.21: Obtain a horizontal angle range of the coastal area, that is, ZELOUER, based on the edge points EL and ER on both sides of the coastline point cloud data set {(Xcoast’Yfoast’Zcoast)} in the hdar coordinate system and the origin Ou of the lidar coordinate system; since the receptive field of the marine radar is much larger than that of the lidar, the origin Ou of lidar coordinate system can be approximately used as the origin of the marine radar coordinate system.

[0132] Step S6.22: Draw an angle bisector of the horizontal angle range Z.ELOUER , passing through the origin Ou of the lidar coordinate system, to obtain OuEm.

[0133] Step S6.23: Draw a vertical of the angle bisector OuEm, passing through the origin Ou of the lidar coordinate system, and retain the original echo image within a 180° range on a coastal side, to obtain the coastal direction echo image, denoted as Iradar_Bef.

[0134] Step S6.3: Set a correction interval frame of the marine radar to be h, and determine whether the current frame of the lidar is an integer multiple of the correction interval frame h of the marine radar.

[0135] The correction interval frame h of the marine radar is a sampling interval frame of the marine radar relative to the lidar. In other words, every time the lidar collects h frames, the marine radar collects one frame. In FIG. 4, the frequency of the lidar is 10 Hz and the frequency of the marine radar is 1 Hz. In this case, the correction interval frame h is 10.

[0136] In the process of mapping, if the current frame k+1 of the lidar is not an integer multiple of the correction interval frame h of the marine radar, the estimated USV pose and the point cloud map Qk+1 in the current frame k+1 obtained in the step S5 are directly output, and the pose transformation relationship of the current frame k+1 relative to the previous frame k and the estimated USV pose Tk+l in the current frame are added to the factor map as binary pose change factors and initial calibration values thereof. If the current frame k+1 of the lidar is an integer multiple of the correction interval frame h of the marine radar, the coastal direction echo image is extracted again in the way of the step 6.2, denoted as Iradar_Aft, and common coastline features in the coastal direction echo image Iradar_Bef and the coastal direction echo image Iradar_Aft are matched.

[0137] In a specific implementation of the present disclosure, a specific implementation process of matching the common coastline features in the coastal direction echo image Iradar_Bef and the coastal direction echo image Iradar_Aft is as follows:

[0138] Step S6.31: Extract feature point coordinates and a feature descriptor from the coastal direction echo image Iradar_Bef by using a SIFT operator, and extract feature point coordinates and a feature descriptor from the coastal direction echo image Iradar_Aft by using the SIFT operator, where each feature descriptor is a 128-dimensional vector.

[0139] Step S6.32: Match the extracted feature descriptors by using a KNN algorithm, to obtain the matching feature point pairs between the two extracted coastal direction echo images.

[0140] The SIFT operator and the KNN algorithm are prior art. For the SIFT operator, refer to: Lowe DG. Distinctive image features from scale-invariant keypoints [J]. International journal of computer vision, 2004, 60:91-110. For the KNN algorithm, refer to: Cover TM, Hart P E. Nearest neighbor pattern classification [J]. IEEE Transinftheory, 1953, 13(1):21-7.

[0141] Step S6.4: According to the matching feature point pairs obtained in the step S6.3, the pose transformation relationship of the marine radar in the current frame k+1 of the lidar relative to the previous h frames (that is, the k+l-h frame) is estimated by using the nonlinear optimization algorithm, and includes a rotation matrix and a translation vector. The translation vector needs to be scaled to the real world scale according to the receptive field of the marine radar, and the pose transformation relationship is obtained after scaling, that is, the motion change of the sensor is calculated according to the change of the environmental perception information.

[0142] Step S6.5: Add to the factor graph the pose transformation relationship of the marine radar in the current frame k+1 of the lidar relative to the previous h frames and the estimated USV pose in the current frame k+1 as the binary pose change factors and initial calibration values thereof.

[0143] Step S6.6: Optimize the factor graph using iSAM2, correct the estimated USV pose between the (k+l-h)* frame to the current frame k+1, and readjust the point cloud map between the (k+l-h)* frame to the current frame k+1 by using the corrected estimated USV Pose

[0144] The positioning information from the large receptive field of the marine radar is used to periodically correct the positioning and mapping results of the lidar, and the corrected estimated USV pose and point cloud map Qk+l can be output by the lidar odometer sub-module. iSAM2 is the prior art. For details, refer to: Kaess M, Ranganathan A, Dellaert F.iSAM: Incremental Smoothing and Mapping [J]. IEEE Transactions on Robotics: A publication of the IEEE Robotics and Automation Society, 2008, (6):24.

[0145] After the correction, the factor graph is cleared, and the pose transformation relationship in the current frame k+1 and the previous frame is calculated by using the corrected estimated USV pose , and are used as the a priori factor and optimized initial value thereof to reinitialize the factor graph, and Iradar_Bef is updated to Iradar_Aft for the next factor graph optimization.

[0146] In a specific implementation of the present disclosure, a pose combination sub-module in FIG. 4 combines and outputs the estimated USV pose before correction (that is, the pose obtained in the step S5) and the corrected estimated USV pose after correction (that is, the pose obtained in the step S6), and preferentially outputs the corrected estimated USV pose with low frequency since the frequency of generating the estimated USV poses before and after correction are different. If no estimated USV pose is generated in the frame, the estimated USV pose before correction is output.

[0147] The estimated USV pose obtained in the steps SI to S6 is the relative USV pose, and the absolute positioning of the USV can be realized using the remote sensing image tile.

[0148] Firstly, a global fused coastal remote sensing tile database is established. Secondly, the long-distance coastal point cloud map and the fused coastal remote sensing tile database are utilized for coarse-grained retrieval and matching, to obtain remote sensing data tile corresponding to the area of the point cloud map. Then, based on the 2D coastline projection of the point cloud map and the matching remote sensing data tile, fine registration and positioning are performed. The absolute positioning of the USV is achieved by combining the USV relative pose information (that is, the estimated USV pose obtained in the step S6) with the geospatial information of the remote sensing tiles.

[0149] Step S7: Obtain coastal remote sensing image tile, and perform coarse-grained retrieval matching on the corrected point cloud map in the current frame and the coastal remote sensing image tile, to obtain a 2D coastline map and remote sensing data tile corresponding to the point cloud map.

[0150] As shown in FIG. 9, firstly, the coastal remote sensing image tile Z4mote, / =1,2,...,m is obtained through channels such as "National Remote Sensing Data and Application Service Platform", where m represents the number of coastal remote sensing tile images and remote stands for remote sensing. Secondly, the coastal remote sensing image tile is segmented by using the semantic segmentation network U-net. The segmentation mask is then overlaid onto the original coastal remote sensing image tile in the channel dimension to obtain fused tile image data IJseg / =1,2,...,m. Subsequently, geospatial information (x^y^w^hj) of each piece of fused tile image data is saved, Xj and yj respectively represent the plane rectangular coordinates of the upper left comer of the area corresponding to the fused tile image data in the projected coordinate system, w. and h} represent the width and height of the geographic space corresponding to the fused tile image data. This process completes the establishment of the fused coastal remote sensing tile database. The purpose of segmentation is to capture more pronounced coastline feature information, thereby enhancing the feasibility of retrieval in the subsequent steps.

[0151] The long-distance coastal point cloud map generated in the steps SI to S6 is denoted as Pcoast ■ To facilitate subsequent retrieval, based on a deviation between the world coordinate system in the point cloud map and a true north direction at an initial moment, the corrected point cloud map gi+1 in the current frame is rotated counterclockwise by 0ycm around an x-axis of the world coordinate system, to align the point cloud map with a direction in the coastal remote sensing image tile (where the y axis points south and the z axis points east). The process diagram is shown in FIG. 10A. Before the rotation of the point cloud map 2t+1, the plane pose of the USV in the world coordinate system is represented as Xw = (yw,zw,0w). After the rotation of the point cloud map , the plane pose Xrot of the USV in the world coordinate system is: z z

[0152] ^=(rcos(arctan^-0^),rsin(arctan^-0^),^ (2) yw y.

[0153] r = yjyj + zw2 represents the distance between the USV and the origin of the world coordinate system. 0w -0yaw represents the heading angle of the USV in the rotated point cloud map, which is recorded as Pcoasl . 0yaw represents the angle by which the corrected point cloud map Qk+l in the current frame k+1 is rotated counterclockwise around the x-axis of the world coordinate system, that is, the angle between the world coordinate system and the direction of the remote sensing data tile. 0W represents the heading angle of the USV in the world coordinate system.

[0154] The rotated point cloud map Pcoast is projected to the BEV to obtain a 2D top view map. Then, the 2D top view map is scaled to a same receptive field scale as the coastal remote sensing image tile, to obtain a 2D coastline map Ibev.

[0155] A convolutional neural network is employed to extract features from the fused tile image data 1' i=l,2,...,m and the 2D coastline map Ibev. This process yields a 1024-dimensional tile feature vector {v‘emoIe,i = 1,2,...,m} and a 1024-dimensional map feature vector v . The convolutional neural network utilizes the ResNet-50 backbone model pre-trained on ImageNet.

[0156] Principal component analysis (PCA) is then applied to perform preliminary compression on both the tile feature vector {v^emate,i = 1,2,...,m} and the map feature vector vmap from 1024 dimensions to 80 dimensions, resulting in the compressed feature vector {v'remote,i = 1,2,..., m} and v map

[0157] For PCA technologies, refer to: Pearson K. LIII. On lines and planes of closest fit to systems of points in space [J]. The London, Edinburgh, and Dublin philosophical magazine and journal of science, 1901, 2(11): 559-572.

[0158] To further reduce redundant information and improve retrieval efficiency, the bag of visual words (BoVW) model is employed for feature encoding and retrieval matching on the preliminarily compressed tile feature vector {v^mote,z = 1,2,...,m} and map feature vector vmap v • This process yields the remote sensing data tile and geospatial information thereof corresponding to a minimum distance between feature codes.

[0159] Specifically, the 80-dimensional feature vector corresponding to each image is encoded into a fixed-length vector using Hard Assignment based on a similarity to the visual vocabulary. During retrieval matching, a Hamming distance between the two codes is calculated, and the remote sensing data tile Ik with the smallest Hamming distance, along with the geospatial information (xk,yk,wk,hk), is selected as the retrieval matching result.

[0160] The Bo VW model is the prior art. For details thereof, refer to: Yang Y, Newsam S. Bag-of-visual-words and spatial extensions for land-use classification [C]. Proceedings of the 18th SIGSPATIAL international conference on advances in geographic information systems, 2010: 270-9.

[0161] Step S8: Perform absolute positioning of the USV according to the corrected estimated USV pose in the current frame, and the 2D coastline map Ibev and the remote sensing data tile Ik corresponding to the point cloud map.

[0162] In a specific implementation of the present disclosure, a specific implementation process of the step S8 includes:

[0163] Step S8.1: Perform registration on the remote sensing data tile Ik fus and the 2D coastline map Ibev, and calculate plane rectangular coordinates of the USV in a projected coordinate system. As shown in FIG. 10B, specifically, for the 2D coastline map Ibev, let the upper left comer point be the base point, and the rectangular coordinates of the upper left comer of the remote sensing data tile Ik in the projected coordinate system are (xk,yk), and the registration result is the pixel offset of the base point in the 2D coastline map Ibev relative to the upper left comer of the remote sensing data tile Ik fus in the x and y axes of the projected coordinate system, that is, w?™1 and hk'xel . According to the registration result, the plane rectangular coordinates of the USV in the projected coordinate system can be calculated by using Formula (3): z x = xk + wk + r sin (arctan - Qyaw)

[0164] y’ (3) y = yk —T^hk ~r cos (arctan—-"k y*

[0165] and hkxel represent the pixel width and height of the remote sensing data tile ^eg / uS ’ (x> y) represent the plane rectangular coordinates of the USV in the projected coordinate system before the overflow treatment, and the values of x and y fall in the range of {-20037508.3427892, 20037508.3427892}. Formula (3) may lead to an overflow in the operation result, which is addressed by Formulas (4) and (5):

[0166] x' = ■ (x — x )%(2x ) —x x^ max / x max / max (x + Xm}° / o(2xm} + xm„ x max / x max / max X X> X max x< — X max other (4)

[0167] y'=< (y — y ) — y V / max / x / max / / max (y "I" ymax) max.) + J^max y J^Xnax other (5)

[0168] (xmaxj^max) represents the maximum values of the plane rectangular coordinates of the USV in the projected coordinate system, Xmax=ymax=20037508.3427892. (x',^') represent the plane rectangular coordinates of the USV in the projected coordinate system after overflow treatment. When the coordinates exceed the upper limit, the coordinates are reset to the lower limit (since the Earth is round, 361° is equivalent to 1°); otherwise, the coordinates are set to the upper limit.

[0169] Step S8.2: Convert the plane rectangular coordinates of the USV in the projected coordinate system into spatial geodetic coordinates in a geographic coordinate system, to obtain an absolute position of the USV.

[0170] Based on the China Geodetic Coordinate System 2000 (CGCS2000), the ellipsoid has a major axis a=6378137 m, a minor axis b=6356752.314 m, the oblateness f=l / 298.257222101, the linear eccentricity c=6399593.6259, the first eccentricity e=0.0066943800229, and the second eccentricity e-0.00673949677548. According to the inverse calculation formula of Gaussian projection, the plane rectangular coordinates (x, y) of the USV in the projected coordinate system are transformed into the spatial geodetic coordinates in the geographic coordinate system. B = Bf- 2MfNf y2 +—-—7(5+3 / +77)-977) / )) / 24MfNf f f f f

[0171] 12QMfNf5 (61 + 90 / ) + 45 / )) / (6) -------------(1 + 2 / )+77)) Nfcos Bf 6N} cos Bf f f

[0172] H--:-------(5 + 28 / ) + 24 / + 677 <+ ft r) + A 120V) cos Bf f f f f f 1 (7)

[0173] B represents the calculated latitude of the point, L represents the calculated longitude of the point, Lo represents the central meridian, w = -e2 sin2 B Nf=—, tf = \anBf , I]? = e'cos275y . Bf is the latitude of the base point, and is iteratively calculated according to the meridian arc length Formula (8). At the beginning of iteration, B'f = x / a0.

[0174]

[0175] FiB' / ) = sin2Bt +—sin4B'f sin6B'f +—sin8B'f v / / 2 f 4 f 6 f 8 f B' / 1 = (X - F(B‘fy) / a0 (8) a0,a2,a4,a6,ag are basic constants,and are calculated according to Formula (9):

[0176]

[0177] m7 3 5 35 an = mn d—- +—m. d--m, d--ms 0 0 2 8 4 16 6 128 8 m7 m. 15 7 a7 =^-d—-d--m, d-- 2 2 2 32 6 16 w4 3 7 a, =—— d--m, d--m. 4 8 16 6 32 8 <2, =— + — 6 32 16 Wo a8 =—- 8 128 (9) m0, m2, , m6, ms are basic constants, and are calculated according to Formula (10): mQ =a(l-e2) 3 2 w2 = -e m0 = 5e2w2 7 2 =~e 0

[0178] 9 2 w. = — e m, 8 8 6 (10)

[0179] In this case, the USV utilizes the perceived multi-source coastline information to obtain the spatial geodetic coordinates (B, L) in the world geographic coordinate system (that is, the absolute position of the USV) and the heading angle d- 6yaw.

[0180] According to the present disclosure, the USV can obtain the spatial geodetic coordinates in the world geographic coordinate system and the heading angle through multi-source coastline perception solely by sensors mounted on the USV, without using GNSS signals. This capability expands the mission scope of the USV, allowing the USV to perform more complex maritime tasks based on the geospatial coordinates, such as autonomous coastal inspections, island inspections, and autonomous landings.

[0181] Embodiment!

[0182] The present disclosure further provides a terminal device. The terminal device includes a processor and a memory storing a computer program. The processor is configured to execute the computer program to implement the absolute positioning method for a USV.

[0183] Although not shown, the terminal device includes a processor, which can perform various suitable actions and processing according to a program / data stored in a read-only memory (ROM) or a program / data loaded from a storage part to a random access memory (RAM) . The processor may be a multi-core processor or include a plurality of processors. In some embodiments, the processor may include a general primary processor and one or more special coprocessors, such as a central processing unit, a graphics processing unit (GPU), a neural network processor (NPU), or a digital signal processor (DSP). The RAM further stores various programs and data required for operations of the terminal device. The processing unit, the ROM, and the RAM are connected to each other through a bus. An input / output (I / O) interface is also connected to the bus.

[0184] The processor and the memory are jointly configured to execute the program stored in the memory, and the program, when executed by a computer, can realize the methods, steps or functions described in the above embodiments.

[0185] Although not shown, an embodiment of the present disclosure further provides a computer-readable storage medium. The computer-readable storage medium stores a computer program, and the computer program is executed by a processor to implement the above absolute positioning method for a USV.

[0186] The storage medium in the embodiments of the present disclosure includes both persistent, non-persistent, removable, and non-removable media, and storage of information may be implemented by any method or technology. Examples of a computer storage medium include, but are not limited to, a phase-change random access memory (PRAM), a static random access memory (SRAM), a dynamic random access memory (DRAM), other types of RAMs, a ROM, an electrically erasable programmable read-only memory (EEPROM), a flash memory or another memory technology, a compact disc read-only memory (CD-ROM), a digital versatile disk (DVD) or another optical storage device, a magnetic cassette tape, and a magnetic tape disk storage device or another magnetic storage device or any other non-transmission medium, which can be used to store information that can be accessed by a computing device.

[0187] The above are merely specific implementations of the present disclosure, and the protection scope of the present disclosure is not limited thereto. Any modification or replacement easily conceived by those skilled in the art within the technical scope of the present disclosure should fall within the protection scope of the present disclosure.

Images