Laser radar SLAM degradation detection method and device, unmanned ship and storage medium

Abstract

The invention discloses a laser radar SLAM degradation detection method and device, an unmanned ship and a storage medium, and the method comprises the steps: obtaining continuous frames of original point cloud data of a water surface environment where the unmanned ship is located, and constructing a point cloud vector matrix of the original point cloud data for each frame of original point cloud data; detecting the SLAM degradation direction of the point cloud vector matrix through a degradation factor detection algorithm to obtain a degradation direction vector; and acquiring SLAM odometer information corresponding to the original point cloud data, and detecting whether the laser radar SLAM is degraded or not according to the degradation direction vector and the SLAM odometer information. According to the method, whether the laser radar SLAM degenerates or not can be effectively detected in real time according to the continuous frame original point cloud data and the SLAM odometer information corresponding to the continuous frame original point cloud data, so that autonomous navigation of the unmanned ship is ensured.

Description

Technical Field

[0001] The embodiments of the present invention relate to the technical field of unmanned ships, and in particular to a laser radar SLAM degradation detection method, device, unmanned ship and storage medium. Background Art

[0002] With the rapid development of unmanned ship technology, the application of simultaneous localization and mapping (SLAM) technology based on LiDAR in the field of unmanned ships has gradually increased. SLAM technology uses sensor data, such as LiDAR scanning data, to build environmental maps and locate the position of unmanned ships in real time, thereby achieving autonomous navigation of unmanned ships. However, in complex water environments, LiDAR-based SLAM often faces degradation problems due to sensor failures, severe weather conditions, etc., which leads to reduced SLAM performance or even failure.

[0003] The current degradation detection methods of LiDAR-based SLAM have certain limitations. Especially in special environments, the existing detection methods are often unable to effectively detect the performance degradation of LiDAR SLAM in real time, which affects the robustness and reliability of the system and further affects the autonomous navigation of unmanned ships. Summary of the invention

[0004] The embodiments of the present invention provide a laser radar SLAM degradation detection method, device, unmanned ship and storage medium, aiming to solve the problem that the existing SLAM degradation cannot be effectively detected, thereby affecting the autonomous navigation of the unmanned ship.

[0005] In a first aspect, an embodiment of the present invention provides a laser radar SLAM degradation detection method, which is applied to an unmanned ship, comprising:

[0006] Acquire continuous frames of original point cloud data of the water surface environment where the unmanned ship is located, and construct a point cloud vector matrix of the original point cloud data for each frame of the original point cloud data;

[0007] Detecting the SLAM degradation direction of the point cloud vector matrix by a degradation factor detection algorithm to obtain a degradation direction vector;

[0008] SLAM odometer information corresponding to the original point cloud data is obtained, and whether the laser radar SLAM is degraded is detected according to the degradation direction vector and the SLAM odometer information.

[0009] In a second aspect, an embodiment of the present invention further provides a laser radar SLAM degradation detection device, which is applied to an unmanned ship, comprising:

[0010] An acquisition unit, used for acquiring continuous frames of original point cloud data of the water surface environment where the unmanned ship is located, and constructing a point cloud vector matrix of the original point cloud data for each frame of the original point cloud data;

[0011] A degradation direction detection unit, used to detect the SLAM degradation direction of the point cloud vector matrix through a degradation factor detection algorithm to obtain a degradation direction vector;

[0012] The actual degradation direction detection unit is used to obtain SLAM odometer information corresponding to the original point cloud data, and detect whether the laser radar SLAM is degraded according to the degradation direction vector and the SLAM odometer information.

[0013] In a third aspect, an embodiment of the present invention further provides an unmanned ship, comprising a memory and a processor, wherein a computer program is stored in the memory, and the above method is implemented when the processor executes the computer program.

[0014] In a fourth aspect, an embodiment of the present invention further provides a computer-readable storage medium, wherein the storage medium stores a computer program, and the computer program can implement the above method when executed by a processor.

[0015] The embodiment of the present invention provides a laser radar SLAM degradation detection method, device, unmanned ship and storage medium. The method includes: obtaining continuous frames of original point cloud data of the water surface environment where the unmanned ship is located, and for each frame of the original point cloud data, constructing a point cloud vector matrix of the original point cloud data; detecting the SLAM degradation direction of the point cloud vector matrix through a degradation factor detection algorithm to obtain a degradation direction vector; obtaining SLAM odometer information corresponding to the original point cloud data, and detecting whether the laser radar SLAM is degraded according to the degradation direction vector and the SLAM odometer information. The technical solution of the embodiment of the present invention first obtains the original point cloud data and processes the original point cloud data to obtain the degradation direction vector, then obtains the SLAM odometer information corresponding to the original point cloud data, and then detects whether the laser radar SLAM is degraded in real time and effectively according to the degradation direction vector and the SLAM odometer information, thereby ensuring the autonomous navigation of the unmanned ship. BRIEF DESCRIPTION OF THE DRAWINGS

[0016] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying any creative work.

[0017] Figure 1A schematic diagram of a process flow of a laser radar SLAM degradation detection method provided by an embodiment of the present invention;

[0018] Figure 2 A schematic diagram of a sub-process of a laser radar SLAM degradation detection method provided by an embodiment of the present invention;

[0019] Figure 3 A schematic diagram of a sub-process of a laser radar SLAM degradation detection method provided by an embodiment of the present invention;

[0020] Figure 4 A schematic diagram of a sub-process of a laser radar SLAM degradation detection method provided by an embodiment of the present invention;

[0021] Figure 5 A schematic diagram of a sub-process of a laser radar SLAM degradation detection method provided by another embodiment of the present invention;

[0022] Figure 6 A schematic block diagram of a laser radar SLAM degradation detection device provided in an embodiment of the present invention;

[0023] Figure 7 A schematic block diagram of an unmanned ship provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0024] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0025] It should be understood that when used in this specification and the appended claims, the terms "include" and "comprises" indicate the presence of described features, integers, steps, operations, elements and / or components, but do not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or combinations thereof.

[0026] It should also be understood that the terms used in this specification of the present invention are only for the purpose of describing specific embodiments and are not intended to limit the present invention. As used in the specification of the present invention and the appended claims, unless the context clearly indicates otherwise, the singular forms "a", "an" and "the" are intended to include plural forms.

[0027] It should be further understood that the term "and / or" used in the present description and the appended claims refers to any and all possible combinations of one or more of the associated listed items, and includes these combinations.

[0028] As used in this specification and the appended claims, the term "if" may be interpreted as "when" or "upon" or "in response to determining" or "in response to detecting," depending on the context. Similarly, the phrases "if it is determined" or "if [described condition or event] is detected" may be interpreted as meaning "upon determination" or "in response to determining" or "upon detection of [described condition or event]" or "in response to detecting [described condition or event]," depending on the context.

[0029] See also Figure 1 , Figure 1 FIG. 1 is a flow chart of a laser radar SLAM degradation detection method provided by an embodiment of the present invention. The laser radar SLAM degradation detection method is described in detail below. Figure 1 As shown, the method includes the following steps S100-S300.

[0030] S100, obtaining continuous frames of original point cloud data of the water surface environment where the unmanned ship is located, and constructing a point cloud vector matrix of the original point cloud data for each frame of the original point cloud data.

[0031] In an embodiment of the present invention, continuous k frames of original point cloud data of the water surface environment where the unmanned ship is located are obtained by laser radar, where k≥2. It should be noted that each frame of the original point cloud data is a point cloud cluster, and the point cloud cluster includes n point clouds, where n≥1. Specifically, step S100 includes: performing clustering filtering on the original point cloud data to obtain a non-water surface point cloud corresponding to the original point cloud data; performing normal vector estimation on the non-water surface point cloud and constructing the point cloud vector matrix corresponding to the non-water surface point cloud. More specifically, after obtaining the original point cloud data of consecutive k frames, cluster filtering is performed on the original point cloud data of consecutive k frames to remove the water surface point cloud to obtain the non-water surface point cloud of consecutive k frames, wherein each frame of the non-water surface point cloud includes n point clouds; normal vector estimation is performed on the non-water surface point clouds of consecutive k frames, that is, the normal vector of each point cloud in the non-water surface point clouds of consecutive k frames is calculated, wherein the components of the normal vector of the i-th point cloud in the positive directions of the X-axis, the Y-axis and the Z-axis are respectively expressed as as well as i≥1; construct k point cloud vector matrices corresponding to the non-water surface point clouds of consecutive k frames according to the normal vector of each point cloud, that is, a point cloud vector matrix N kCorresponding to a frame of the non-water surface point cloud, that is, a point cloud vector matrix N k Describes the normal vectors of n point clouds in a frame of the non-water surface point cloud, the point cloud vector matrix N k As shown in formula (1), i∈{1, 2, ..., n}, n is the number of point clouds after filtering. It should be noted that in the embodiment of the present invention, the algorithm used for clustering filtering is the DBSCAN clustering algorithm, and other clustering algorithms may also be used in other embodiments. It should also be noted that the water surface point cloud is a special point cloud on the water surface close to the laser radar sensor. The special point cloud will affect the effect of laser radar SLAM. In addition, the water surface point cloud is a plane, and the feature information is not significant enough, so it is necessary to first filter the k frames of the original point cloud data to filter out the water surface point cloud.

[0032]

[0033] S200, detecting the SLAM degradation direction of the point cloud vector matrix by using a degradation factor detection algorithm to obtain a degradation direction vector.

[0034] In the embodiment of the present invention, Figure 2 As shown, step S200 may specifically include steps S210-S230: S210, using principal component analysis to reduce the dimension of the point cloud vector matrix to obtain a target point cloud vector matrix; S220, using singular value decomposition to solve the eigenvalues ​​of the target point cloud vector matrix to obtain an eigenvalue set of the target point cloud vector matrix and an eigenvector set corresponding to the eigenvalue set; S230, detecting the SLAM degradation direction according to the eigenvalue set, the eigenvector set and a preset eigenvalue threshold to obtain the degradation direction vector. Specifically, the principal component analysis method is used to reduce the dimensions of k point cloud vector matrices to obtain k target point cloud vector matrices, that is, the normal vectors with similar directions in the k point cloud vector matrices are merged to reduce repeated or similar normal vectors. It should be noted that the principal component analysis method can generate a target point cloud vector matrix with a small number of features and retain most of the effective information during the dimensionality reduction process; the singular value decomposition method is used to solve the eigenvalues ​​of the k target point cloud vector matrices according to calculation formula (2) to obtain k eigenvalue sets and k eigenvector sets corresponding to the k eigenvalue sets, wherein the eigenvalue set of the kth target point cloud vector matrix is ​​λ k ={λ1,…,λ3}, the eigenvector set corresponding to the eigenvalue set is f k ={f1,…,f3}, the calculation formula (2) is as follows.

[0035] N k TN k =UΣ V T (2)

[0036] Further, step S230 specifically includes: taking the smallest eigenvalue in the eigenvalue set as the comparison eigenvalue; judging whether the comparison eigenvalue is less than the preset eigenvalue threshold; if the comparison eigenvalue is less than the preset eigenvalue threshold, taking the eigenvector corresponding to the comparison eigenvalue as the degradation direction vector. Specifically, taking the smallest eigenvalue in the k eigenvalue sets as the comparison eigenvalue, and the comparison eigenvalue is denoted as λ min_k , that is, min_k =min(λ k )=min{λ1,…,λ3}, and the eigenvector corresponding to the comparative eigenvalue is recorded as Determine whether the comparison characteristic value is less than the preset characteristic value threshold, that is, determine whether λ is satisfied. min_k <λ threshold If the comparison characteristic value is less than the preset characteristic value threshold, then λ is satisfied. min_k <λ threshold , indicating that the LiDAR SLAM may be degraded in the current scene, then f min_k As the degradation direction vector, f min_k direction as the possible degradation direction of SLAM. It should be noted that the projection of the target point cloud vector matrix on the eigenvector is a fixed expansion and contraction of the eigenvector itself, and the expansion and contraction ratio is the eigenvalue corresponding to the eigenvector, that is, the size of the eigenvalue can reflect the size of the projection ratio of the target point cloud vector matrix in the direction of the eigenvector; the smaller the eigenvalue, the smaller the constraint of the target normal vector matrix in the direction of the eigenvector corresponding to the eigenvalue, and the easier it is to degenerate; therefore, judging the size of the smallest eigenvalue in the eigenvalue set, that is, judging the size of the comparative eigenvalue, can detect the possible degradation direction of SLAM. It should also be noted that in an embodiment of the present invention, the eigenvalue threshold can be reasonably set according to actual needs, and no specific limitation is made here.

[0037] S300, obtaining SLAM odometer information corresponding to the original point cloud data, and detecting whether the laser radar SLAM is degraded according to the degradation direction vector and the SLAM odometer information.

[0038] In an embodiment of the present invention, the SLAM odometer information corresponding to each frame of the original point cloud data in k consecutive frames is collected, that is, the SLAM odometer information output by each frame of the original point cloud data in k consecutive frames under the lidar SLAM algorithm is collected.

[0039] Furthermore, if Figure 3 As shown, step S300 specifically includes steps S310-S330: S310, constructing an odometer matrix according to the SLAM odometer information, and constructing an odometer vector matrix according to the odometer matrix; S320, using the odometer direction vector corresponding to each frame of the original point cloud data in the odometer vector matrix as the target direction vector; S330, detecting whether the laser radar SLAM is degraded according to the degraded direction vector and the target direction vector. Specifically, the odometer matrix Odo is constructed according to the SLAM odometer information. m , and construct the odometer vector matrix ON_m, Odo according to the odometer matrix m As shown in formula (3), as well as They represent the components of the SLAM odometer in the X-axis, Y-axis and Z-axis directions corresponding to the original point cloud data of the mth frame, respectively. ON_m is shown in formula (4), where: Represents the direction vector of the SLAM odometer corresponding to the original point cloud data of the mth frame, and s m As the target direction vector, where m=1, 2, ..., k.

[0040]

[0041]

[0042] Furthermore, if Figure 4 As shown, step S330 specifically includes S331-S333: S331, calculate the angle between the degraded direction vector and the target direction vector, and use the angle as the first direction angle; S332, determine whether the first direction angle is less than a preset angle threshold, if the first direction angle is less than the preset angle threshold, execute step S333; S333, use the direction of the degraded direction vector as the actual SLAM degraded direction, and determine whether the laser radar SLAM is degraded. Specifically, calculate the angle between the degraded direction vector and the target direction vector, that is, calculate f min_k With s m The angle between the first direction angle R_diff is taken as the first direction angle R_diff; if the first direction angle is less than the preset angle threshold, R_diff<R threshold , then the degenerate direction vector f min_k The direction of is taken as the actual degradation direction of the SLAM, and the degradation of the laser radar SLAM is determined, wherein the calculation formula of R_diff is shown in formula (5). It should be noted that the direction vector of the SLAM odometer corresponding to the original point cloud data of the mth frame is The vector of the SLAM odometer coordinates corresponding to the original point cloud data of the m-1th frame pointing to the SLAM odometer coordinates corresponding to the original point cloud data of the mth frame. It should also be noted that in the embodiment of the present invention, the preset angle threshold can be reasonably set according to actual needs and is not specifically limited here.

[0043]

[0044] R_diff=arcsin(sin<f min_k ,s m >) (5)

[0045] See also Figure 5 , Figure 5 A schematic diagram of a process flow of a laser radar SLAM degradation detection method provided by another embodiment of the present invention is shown as follows: Figure 5 As shown, the method further includes steps S334-S336 after step S333 of the above embodiment:

[0046] S334: Acquire a throttle direction coordinate corresponding to the throttle direction of the unmanned ship, and obtain a throttle direction vector according to the throttle direction coordinate, wherein the throttle direction coordinate is represented by the coordinate of the unmanned ship in the initial coordinate system.

[0047] It should be noted that, in the embodiment of the present invention, the throttle direction describes the direction of movement of the unmanned boat, and the throttle direction coordinate is expressed as pedal_direction = (x_p, y_p, z_p), where x_p, y_p and z_p represent the values ​​of the unmanned boat in the X-axis, Y-axis and Z-axis directions, respectively. It should also be noted that the initial coordinate system uses the starting position of the unmanned boat when the lidar SLAM starts as the coordinate origin, the front of the starting position of the unmanned boat as the X-axis, the left direction of the starting position of the unmanned boat as the Y-axis, and the Z-axis directly above the starting position of the unmanned boat.

[0048] S335: If the modulus of the target direction vector is smaller than a preset modulus length threshold and the modulus of the throttle direction vector is larger than the preset modulus length threshold, then the angle between the degraded direction vector and the throttle direction vector is calculated, and the angle is used as the second direction angle.

[0049] In the embodiment of the present invention, if the modulus of the target direction vector is less than the preset modulus length threshold and the modulus of the throttle direction vector is greater than the preset modulus length threshold, the angle between the degenerate direction vector and the throttle direction is calculated, that is, f is calculated. min_kThe angle between the pedal_direction and the pedal_direction is taken as the second direction angle R_pedal, and the calculation formula of R_pedal is shown in formula (6). It should be noted that the preset module length threshold can be reasonably set according to actual needs and is not specifically limited here.

[0050]

[0051] R_pedal=arcsin(sin<f min_k ,pedal_direction>) (6)

[0052] S336: If the second direction angle is less than the preset angle threshold, it is determined that the unmanned boat is moving but the odometer is stationary.

[0053] In the embodiment of the present invention, if the second direction angle is less than the preset angle threshold, that is, R_pedal<R threshold , it is determined that the unmanned ship is moving but the odometer is stationary.

[0054] In summary, in this embodiment, firstly, continuous k frames of raw point cloud data are obtained by laser radar and the SLAM odometer information corresponding to each frame of raw point cloud data in the continuous k frames of raw point cloud data is collected. The possible SLAM degradation direction is detected by constructing a point cloud vector matrix and a degradation factor detection algorithm based on the obtained continuous k frames of raw point cloud data. The odometer information of SLAM is combined to assist in detecting the actual SLAM degradation direction. At the same time, it is detected whether the unmanned ship moves but the odometer is stationary. Whether the laser radar SLAM is degraded is determined based on whether the actual SLAM degradation direction is detected and whether the unmanned ship moves but the odometer is stationary. If the laser radar SLAM is degraded, the unmanned ship exits the laser radar SLAM method and adopts other navigation methods for autonomous navigation. It can detect the quality of the laser radar data in real time and effectively detect the performance degradation of the laser radar SLAM in real time, improve the robustness and reliability of the system, and ensure the autonomous navigation of the unmanned ship.

[0055] Figure 6 4 is a schematic block diagram of a laser radar SLAM degradation detection device 400 provided in an embodiment of the present invention. Figure 6 As shown, corresponding to the above laser radar SLAM degradation detection method, the present invention also provides a laser radar SLAM degradation detection device 400. The laser radar SLAM degradation detection device 400 includes a unit for executing the above laser radar SLAM degradation detection method, and the device can be configured in an unmanned ship. Specifically, please refer to Figure 6The laser radar SLAM degradation detection device 400 includes an acquisition unit 401, a degradation direction detection unit 402 and an actual degradation direction detection unit 403.

[0056] Among them, the acquisition unit 401 is used to acquire continuous frame original point cloud data of the water surface environment where the unmanned boat is located, and for each frame of the original point cloud data, construct a point cloud vector matrix of the original point cloud data; the degradation direction detection unit 402 is used to detect the SLAM degradation direction of the point cloud vector matrix through a degradation factor detection algorithm to obtain a degradation direction vector; the actual degradation direction detection unit 403 is used to acquire the SLAM odometer information corresponding to the original point cloud data, and detect whether the lidar SLAM is degraded based on the degradation direction vector and the SLAM odometer information.

[0057] In some embodiments, such as this embodiment, the acquisition unit 401 includes a filtering unit and a normal vector estimation unit.

[0058] Among them, the filtering unit is used to perform cluster filtering processing on the original point cloud data to obtain a non-water surface point cloud corresponding to the original point cloud data; the normal vector estimation unit is used to perform normal vector estimation on the non-water surface point cloud and construct the point cloud vector matrix corresponding to the non-water surface point cloud.

[0059] In some embodiments, such as this embodiment, the degradation direction detection unit 402 includes a dimensionality reduction unit, a solution unit, and a degradation direction detection sub-unit.

[0060] Among them, the filtering unit is used to use the principal component analysis method to reduce the dimension of the point cloud vector matrix to obtain the target point cloud vector matrix; the solving unit is used to adopt the singular value decomposition method to solve the eigenvalues ​​of the target point cloud vector matrix to obtain the eigenvalue set of the target point cloud vector matrix and the eigenvector set corresponding to the eigenvalue set; the degradation direction detection subunit is used to detect the SLAM degradation direction according to the eigenvalue set, the eigenvector set and the preset eigenvalue threshold to obtain the degradation direction vector.

[0061] In some embodiments, such as this embodiment, the degradation direction detection subunit includes a first acting unit, a judging unit, and a second acting unit.

[0062] Among them, the first as unit is used to take the smallest eigenvalue in the eigenvalue set as the comparison eigenvalue; the judgment unit is used to judge whether the comparison eigenvalue is less than the preset eigenvalue threshold; the second as unit is used to take the eigenvector corresponding to the comparison eigenvalue as the degradation direction vector if the comparison eigenvalue is less than the preset eigenvalue threshold.

[0063] In some embodiments, such as this embodiment, the actual degradation direction detection unit 403 includes a first construction unit, a third operation unit, and a detection subunit.

[0064] Among them, the first construction unit is used to construct an odometer matrix according to the SLAM odometer information, and to construct an odometer vector matrix according to the odometer matrix; the third as unit is used to use the odometer direction vector corresponding to each frame of the original point cloud data in the odometer vector matrix as the target direction vector; the detection subunit is used to detect whether the lidar SLAM is degraded according to the degraded direction vector and the target direction vector.

[0065] In some embodiments, such as this embodiment, the detection subunit includes a first calculation unit, a judgment unit, and a determination unit.

[0066] Among them, the first calculation unit is used to calculate the angle between the degraded direction vector and the target direction vector, and use the angle as the first direction angle; the judgment unit is used to judge whether the first direction angle is less than a preset angle threshold; the judgment unit is used to use the direction of the degraded direction vector as the actual degradation direction of SLAM if the first direction angle is less than the preset angle threshold, and judge whether the lidar SLAM has been degraded.

[0067] In some embodiments, such as another embodiment of the present invention, the detection subunit further includes an acquisition subunit, a second calculation unit, and a second determination unit.

[0068] Among them, the acquisition subunit is used to obtain the throttle direction coordinates corresponding to the throttle direction of the unmanned ship, and obtain the throttle direction vector according to the throttle direction coordinates, wherein the throttle direction coordinates are represented by the coordinates of the unmanned ship in the initial coordinate system; the second calculation unit is used to calculate the angle between the degenerate direction vector and the throttle direction vector if the modulus of the target direction vector is less than a preset modulus length threshold and the modulus of the throttle direction vector is greater than the preset modulus length threshold, and use the angle as the second direction angle; the second judgment unit is used to judge that the unmanned ship is moving but the odometer is stationary if the second direction angle is less than the preset angle threshold.

[0069] The above-mentioned laser radar SLAM degradation detection device can be implemented in the form of a computer program, which can be used in Figure 7 Operation on board the unmanned vessel shown.

[0070] See also Figure 7 , Figure 7It is a schematic block diagram provided by an embodiment of the present invention. The unmanned ship 500 is a device with a laser radar SLAM degradation detection function.

[0071] See also Figure 7 The unmanned ship 500 includes a processor 502, a memory and a network interface 505 connected via a system bus 501, wherein the memory may include a non-volatile storage medium 503 and an internal memory 504.

[0072] The non-volatile storage medium 503 may store an operating system 5031 and a computer program 5032. When the computer program 5032 is executed, the processor 502 may execute a laser radar SLAM degradation detection method.

[0073] The processor 502 is used to provide computing and control capabilities to support the operation of the entire unmanned ship 500.

[0074] The internal memory 504 provides an environment for the operation of the computer program 5032 in the non-volatile storage medium 503. When the computer program 5032 is executed by the processor 502, the processor 502 can execute a laser radar SLAM degradation detection method.

[0075] The network interface 505 is used to communicate with other devices over the network. Figure 7 The structure shown in the figure is merely a block diagram of a partial structure related to the scheme of the present invention, and does not constitute a limitation on the unmanned ship 500 to which the scheme of the present invention is applied. The specific unmanned ship 500 may include more or fewer components than those shown in the figure, or combine certain components, or have a different arrangement of components.

[0076] The processor 502 is used to run a computer program 5032 stored in a memory to implement any embodiment of the above-mentioned laser radar SLAM degradation detection method.

[0077] It should be understood that in the embodiment of the present invention, the processor 502 may be a central processing unit (CPU), and the processor 502 may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. Among them, the general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0078] It is understood by those skilled in the art that all or part of the processes in the method for implementing the above embodiment can be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a storage medium, which is a computer-readable storage medium. The computer program is executed by at least one processor in the computer system to implement the process steps of the embodiment of the above method.

[0079] Therefore, the present invention also provides a storage medium. The storage medium may be a computer-readable storage medium. The storage medium stores a computer program. When the computer program is executed by a processor, the processor executes any embodiment of the above-mentioned laser radar SLAM degradation detection method.

[0080] The storage medium may be a USB flash drive, a mobile hard disk, a read-only memory (ROM), a magnetic disk, or an optical disk, or any other computer-readable storage medium that can store program codes.

[0081] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in terms of function in the above description. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0082] In the several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of each unit is only a logical function division, and there may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0083] The steps in the method of the embodiment of the present invention can be adjusted in order, combined and deleted according to actual needs. The units in the device of the embodiment of the present invention can be combined, divided and deleted according to actual needs. In addition, the functional units in the various embodiments of the present invention can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.

[0084] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a storage medium. Based on this understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including several instructions for an unmanned ship to perform all or part of the steps of the method described in each embodiment of the present invention.

[0085] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0086] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalents, the present invention is also intended to include these modifications and variations.

[0087] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of various equivalent modifications or replacements within the technical scope disclosed by the present invention, and these modifications or replacements should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention shall be based on the protection scope of the claims.

Claims

1. A laser radar SLAM degradation detection method, applied to an unmanned ship, characterized in that: include: Acquire continuous frames of original point cloud data of the water surface environment where the unmanned ship is located, and construct a point cloud vector matrix of the original point cloud data for each frame of the original point cloud data; Detecting the SLAM degradation direction of the point cloud vector matrix by using a degradation factor detection algorithm to obtain a degradation direction vector; SLAM odometer information corresponding to the original point cloud data is obtained, and whether the laser radar SLAM is degraded is detected according to the degradation direction vector and the SLAM odometer information.

2. The method according to claim 1, characterized in that The step of constructing a point cloud vector matrix of the original point cloud data includes: Performing clustering filtering on the original point cloud data to obtain a non-water surface point cloud corresponding to the original point cloud data; A normal vector is estimated for the non-water surface point cloud and the point cloud vector matrix corresponding to the non-water surface point cloud is constructed.

3. The method according to claim 1, characterized in that The step of detecting the SLAM degradation direction of the point cloud vector matrix by a degradation factor detection algorithm to obtain a degradation direction vector includes: Using principal component analysis, reducing the dimension of the point cloud vector matrix to obtain a target point cloud vector matrix; Using a singular value decomposition method, solving the eigenvalues ​​of the target point cloud vector matrix to obtain an eigenvalue set of the target point cloud vector matrix and an eigenvector set corresponding to the eigenvalue set; The degradation direction vector is obtained by detecting the SLAM degradation direction according to the eigenvalue set, the eigenvector set and a preset eigenvalue threshold.

4. The method according to claim 3, characterized in that: The detecting the SLAM degradation direction according to the eigenvalue set, the eigenvector set and a preset eigenvalue threshold to obtain the degradation direction vector comprises: Taking the smallest eigenvalue in the eigenvalue set as a comparison eigenvalue; Determine whether the comparison characteristic value is less than the preset characteristic value threshold; If the comparison eigenvalue is less than the preset eigenvalue threshold, the eigenvector corresponding to the comparison eigenvalue is used as the degradation direction vector.

5. The method according to claim 1, characterized in that The detecting whether the laser radar SLAM is degraded according to the degradation direction vector and the SLAM odometer information includes: Constructing an odometer matrix according to the SLAM odometer information, and constructing an odometer vector matrix according to the odometer matrix; Taking the odometer direction vector corresponding to each frame of the original point cloud data in the odometer vector matrix as the target direction vector; Whether the laser radar SLAM is degraded is detected according to the degradation direction vector and the target direction vector.

6. The method according to claim 5, characterized in that The detecting whether the laser radar SLAM is degraded according to the degradation direction vector and the target direction vector comprises: Calculating an angle between the degenerate direction vector and the target direction vector, and using the angle as a first direction angle; Determine whether the first direction angle is less than a preset angle threshold; If the first direction angle is less than the preset angle threshold, the direction of the degradation direction vector is used as the actual degradation direction of SLAM, and it is determined that the laser radar SLAM is degraded.

7. The method according to claim 6, characterized in that If the first direction angle is less than the preset angle threshold, the direction of the degradation direction vector is used as the actual degradation direction of SLAM, and after determining that the laser radar SLAM is degraded, the method further includes: Acquire a throttle direction coordinate corresponding to the throttle direction of the unmanned ship, and obtain a throttle direction vector according to the throttle direction coordinate, wherein the throttle direction coordinate is represented by the coordinate of the unmanned ship in the initial coordinate system; If the modulus of the target direction vector is less than a preset modulus length threshold and the modulus of the throttle direction vector is greater than the preset modulus length threshold, calculating an angle between the degraded direction vector and the throttle direction vector, and taking the angle as a second direction angle; If the second direction angle is less than the preset angle threshold, it is determined that the unmanned ship is moving but the odometer is stationary.

8. A laser radar SLAM degradation detection device, applied to an unmanned ship, characterized in that: include: An acquisition unit, used for acquiring continuous frames of original point cloud data of the water surface environment where the unmanned ship is located, and constructing a point cloud vector matrix of the original point cloud data for each frame of the original point cloud data; A degradation direction detection unit, used for detecting the SLAM degradation direction of the point cloud vector matrix by a degradation factor detection algorithm to obtain a degradation direction vector; The actual degradation direction detection unit is used to obtain SLAM odometer information corresponding to the original point cloud data, and detect whether the laser radar SLAM is degraded according to the degradation direction vector and the SLAM odometer information.

9. An unmanned ship, characterized in that: The unmanned ship includes a memory and a processor, the memory stores a computer program, and the processor implements the method according to any one of claims 1 to 7 when executing the computer program.

10. A computer-readable storage medium, characterized in that: The storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 7 can be implemented.

Images