Information processing device, information processing method, program, and storage medium

Abstract

Estimate the water surface height with high accuracy. [Solution] In the information processing device, a point cloud data acquisition means acquires point cloud data generated by a measurement device installed on the ship. A water surface reflection data extraction means extracts data measured at a predetermined distance from the shore, that is, data measured at a position within a predetermined distance from the measurement position of the measurement device, as water surface reflection data measured by reflection on the water surface. Then, a water surface height calculation means calculates the water surface height based on the water surface reflection data.

Description

[Technical Field]

[0001] The present disclosure relates to processing of data measured on board a vessel. [Background technology]

[0002] Conventionally, there have been known techniques for estimating the self-position of a moving object by comparing (matching) shape data of surrounding objects measured using a measurement device such as a laser scanner with map information in which the shapes of surrounding objects are stored in advance. For example, Patent Document 1 discloses an autonomous mobile system that determines whether a detected object in a voxel obtained by dividing a space according to a predetermined rule is a stationary object or a moving object, and matches the map information with the measurement data for voxels in which a stationary object exists. Furthermore, Patent Document 2 discloses a scan matching method that estimates the self-position by comparing voxel data including the mean vector and covariance matrix of stationary objects for each voxel with point cloud data output by a LIDAR. Furthermore, Patent Document 3 describes a method for controlling the attitude of a ship in an automatic docking device that automatically docks a ship so that light emitted from the LIDAR is reflected by objects around the docking position and received by the LIDAR. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] International Publication WO2013 / 076829 [Patent Document 2] International Publication No. WO2018 / 221453 [Patent Document 3] Japanese Patent Publication No. 2020-59403 Summary of the Invention [Problem to be solved by the invention]

[0004] Recently, autonomous ship-steering systems have been studied not only in the automotive field but also in ships, and highly accurate self-location estimation is equally important for safe autonomous ship steering. In the open ocean, where there are few structures in the vicinity, self-location can be determined using the Global Navigation Satellite System (GNSS). However, along urban coasts and rivers, where high-rise buildings and other structures are located nearby, the reception environment for GNSS radio waves is poor due to factors such as a reduced number of receiving satellites and multipath, making highly accurate positioning impossible. Therefore, highly accurate position estimation using the scan matching method described above is also ideal for ships.

[0005] On the other hand, when a lidar is installed on a ship, part of the lidar beam pointing downward is reflected by the water surface, resulting in detection data from locations that do not actually exist. Such data reflected by the water surface is false detection (false alarm) data, which affects the accuracy of self-localization. In order to determine whether the detected data is false detection data or not, it is necessary to estimate the water surface height with high accuracy.

[0006] The present disclosure has been made to solve the above-mentioned problems, and has as its main object to provide an information processing device that can estimate the height of the water surface with high accuracy. [Means for solving the problem]

[0007] The invention described in the claims is an information processing device comprising: a point cloud data acquisition means for acquiring point cloud data generated by a measuring device installed on a ship; a water surface reflection data extraction means for extracting data measured at a predetermined distance from the shore and within a predetermined distance from the measurement position by the measuring device as water surface reflection data measured by reflection on the water surface; and a water surface height calculation means for calculating the water surface height based on the water surface reflection data.

[0008] The claimed invention is a control method executed by a computer, which acquires point cloud data generated by a measuring device installed on a ship, extracts data measured at a position within a predetermined distance from the measurement position measured by the measuring device as water surface reflection data measured by reflection on the water surface, and calculates the water surface height based on the water surface reflection data, where the data is data that is a predetermined distance from the shore.

[0009] Furthermore, the invention described in the claims is a program that acquires point cloud data generated by a measuring device installed on a ship, extracts data that is a predetermined distance from the shore and that measures a position within a predetermined distance from the measurement position of the measuring device as water surface reflection data measured by reflection on the water surface, and causes a computer to execute a process of calculating the water surface height based on the water surface reflection data. [Brief explanation of the drawings]

[0010] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Figure 2] FIG. 1 is a block diagram showing a configuration of an information processing device. [Figure 3] 2 is a diagram showing a self-position to be estimated by a self-position estimation unit in three-dimensional orthogonal coordinates. FIG. [Figure 4] 1 shows an example of a schematic data structure of voxel data. [Figure 5] FIG. 1 is a diagram illustrating the water surface height as seen from a lidar. [Figure 6] 1A and 1B are diagrams illustrating reflection of light emitted by a lidar on the water surface. [Figure 7] FIG. 10 is a diagram illustrating point cloud data used to estimate the water surface height. [Figure 8] FIG. 10 is a diagram illustrating a method for detecting an obstacle. [Figure 9] FIG. 10 is a diagram illustrating a method for detecting an undertow. [Figure 10] FIG. 2 is a block diagram showing the functional configuration of an obstacle / undertow detection unit. [Figure 11] FIG. 10 is a diagram illustrating a method for determining a search range. [Figure 12] The results of a simulation of detecting straight lines using the Hough transform are shown below. [Figure 13] An example of Euclidean clustering is shown below. [Figure 14] The simulation results of Euclidean clustering are shown. [Figure 15] FIG. 10 is a diagram illustrating the relationship between the distance of an object and the interval of point cloud data. [Figure 16] Simulation results are shown for the cases where the grouping threshold and score threshold are fixed and where they are set adaptively. [Figure 17] This shows water surface reflection data obtained around the ship. [Figure 18] This paper shows how to remove undertow and obstacles from water surface reflection data. [Figure 19] 10 is a flowchart of an obstacle / undertow detection process. [Figure 20] 10 is a flowchart of a backwash detection process. [Figure 21] FIG. 10 is a diagram illustrating a method for detecting a line. [Figure 22] 10 is a flowchart of an obstacle detection process. [Figure 23] 10 is a flowchart of a water surface position estimation process. [Figure 24] 10A and 10B are a flowchart and an explanatory diagram of a backwash information calculation process; [Figure 25] 10 is a flowchart of an obstacle information calculation process. [Figure 26] FIG. 10 is an explanatory diagram of a water surface position estimation method according to Modification 1. [Figure 27] An example of undertow detection according to the second modification will be described. DETAILED DESCRIPTION OF THE INVENTION

[0011] In one preferred embodiment of the present invention, the information processing device comprises a point cloud data acquisition means for acquiring point cloud data generated by a measuring device installed on a ship, a water surface reflection data extraction means for extracting data measured at a position a predetermined distance away from the shore and within a predetermined distance from the measurement position by the measuring device as water surface reflection data measured by reflection on the water surface, and a water surface height calculation means for calculating the water surface height based on the water surface reflection data.

[0012] In the information processing device, the point cloud data acquisition means acquires point cloud data generated by a measurement device installed on the ship. The water surface reflection data extraction means extracts data measured at a position within a predetermined distance from the shore, that is, data measured at a position within a predetermined distance from the measurement position of the measurement device, as water surface reflection data measured by reflection on the water surface. The water surface height calculation means calculates the water surface height based on the water surface reflection data. This makes it possible to calculate the water surface height with high accuracy using the water surface reflection data.

[0013] In one aspect of the information processing device, the water surface height calculation means calculates the average of the height direction values ​​of the water surface reflection data as the water surface height. In another aspect, the water surface height calculation means calculates the average and variance of the height direction values ​​of the water surface reflection data, and if the variance is less than a predetermined value, calculates the average as the water surface height.

[0014] Another aspect of the information processing device includes a detection unit that sets a search range based on the measurement position and the water surface height, and detects at least one of an obstacle and an undertow based on point cloud data belonging to the search range. In this aspect, the obstacle and the undertow can be detected using the water surface height.

[0015] In another aspect of the above information processing device, the detection means determines whether the point cloud data belonging to the search range is linear or not, and if the point cloud data determined to be linear exists for a predetermined time, detects the linear point cloud data as an undertow, determines whether the point cloud data belonging to the search range forms a cluster, and if the point cloud data determined to be a cluster exists for a predetermined time, detects the cluster as an obstacle.

[0016] In another aspect of the information processing device, the detection means changes the height range of the search range based on the variance of the height direction values ​​of the water surface reflection data, thereby making it possible to set an appropriate search range depending on the wave height.

[0017] In another preferred embodiment of the present invention, a control method executed by a computer acquires point cloud data generated by a measurement device installed on a ship, extracts data measured at a position within a predetermined distance from the measurement position of the measurement device as water surface reflection data measured by reflection on the water surface, and calculates the water surface height based on the water surface reflection data. This makes it possible to calculate the water surface height with high accuracy using the water surface reflection data.

[0018] In another preferred embodiment of the present invention, a program causes a computer to acquire point cloud data generated by a measuring device installed on a ship, extract data measured at a position within a predetermined distance from the measurement position of the measuring device as water surface reflection data measured by reflection on the water surface, and calculate the water surface height based on the water surface reflection data. By executing this program on a computer, the above-mentioned information processing device can be realized. This program can be stored in a storage medium and used. [Example]

[0019] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For convenience, in this specification, a character with "^" or "-" added above any symbol will be referred to as "A^" or "A - " (where "A" is any letter).

[0020] (1) Overview of the driving assistance system Figure 1 shows a schematic configuration of a driving assistance system according to this embodiment. The driving assistance system includes an information processing device 1 that moves together with a ship, which is a moving body, and a sensor group 2 mounted on the ship. Hereinafter, the ship that moves together with the information processing device 1 will also be referred to as the "target ship."

[0021] The information processing device 1 is electrically connected to the sensor group 2, and estimates the position of the target ship on which the information processing device 1 is installed (also referred to as "self-position") based on the outputs of various sensors included in the sensor group 2. Then, the information processing device 1 performs driving assistance such as automatic driving control of the target ship based on the results of estimating its self-position. Driving assistance also includes docking assistance such as automatic docking. Here, "docking" includes not only docking the target ship at a quay, but also docking the target ship at a structure such as a pier. The information processing device 1 may be a navigation device installed on the target ship, or an electronic control device built into the ship.

[0022] The information processing device 1 also stores a map database (DB: DataBase) 10 containing voxel data "VD." The voxel data VD is data that records position information and the like of stationary structures for each voxel, which represents a cube (regular lattice), the smallest unit of three-dimensional space. The voxel data VD includes data that expresses measured point cloud data of stationary structures within each voxel using a normal distribution, and is used for scan matching using NDT (Normal Distributions Transform), as described below. The information processing device 1 estimates, for example, the planar position, height position, yaw angle, pitch angle, and roll angle of the target ship through NDT scan matching. Unless otherwise specified, the self-position is assumed to also include the attitude angle, such as the yaw angle, of the target ship.

[0023] The sensor group 2 includes various external and internal sensors provided on the target ship. In this embodiment, the sensor group 2 includes a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 3, a speed sensor 4 that detects the speed of the target ship, a GPS (Global Positioning Satellite) receiver 5, and an Inertial Measurement Unit (IMU) 6 that measures the acceleration and angular velocity of the target ship in three axial directions.

[0024] The LIDAR 3 emits a pulsed laser beam over a predetermined angular range in the horizontal and vertical directions to discretely measure the distance to an object in the external world and generate three-dimensional point cloud data indicating the position of the object. In this case, the LIDAR 3 includes an irradiation unit that irradiates laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and an output unit that outputs scan data (points that constitute the point cloud data, hereinafter referred to as "measurement points") based on the light receiving signal output by the light receiving unit. The measurement points are generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the response delay time of the laser light identified based on the above-mentioned light receiving signal. Note that, in general, the closer the distance to the object, the higher the accuracy of the LIDAR's distance measurement value, and the farther the distance, the lower the accuracy. The LIDAR 3 is an example of a "measurement device" in the present invention. The speed sensor 4 may be, for example, a Doppler-based speedometer or a GNSS-based speedometer.

[0025] Instead of the GPS receiver 5, the sensor group 2 may include a receiver that generates positioning results of a GNSS other than GPS.

[0026] (2) Configuration of information processing device 2 is a block diagram showing an example of the hardware configuration of the information processing device 1. The information processing device 1 mainly includes an interface 11, a memory 12, and a controller 13. These elements are connected to each other via a bus line.

[0027] The interface 11 performs interface operations related to the exchange of data between the information processing device 1 and an external device. In this embodiment, the interface 11 acquires output data from each sensor in the sensor group 2, such as the lidar 3, speed sensor 4, GPS receiver 5, and IMU 6, and supplies the data to the controller 13. The interface 11 also supplies, for example, signals related to the control of the target vessel generated by the controller 13 to each component of the target vessel that controls the operation of the target vessel. For example, the target vessel includes a drive source such as an engine or electric motor, a screw that generates a forward thrust based on the drive force of the drive source, a thruster that generates a lateral thrust based on the drive force of the drive source, and a rudder, which is a mechanism for freely determining the direction of travel of the vessel. During automatic operation, such as automatic docking, the interface 11 supplies control signals generated by the controller 13 to each of these components. If the target vessel is equipped with an electronic control device, the interface 11 supplies the control signal generated by the controller 13 to the electronic control device. The interface 11 may be a wireless interface such as a network adapter for wireless communication, or may be a hardware interface for connecting to an external device via a cable, etc. The interface 11 may also perform interface operations with various peripheral devices such as an input device, a display device, and a sound output device.

[0028] The memory 12 is configured by various types of volatile and non-volatile memory, such as a RAM (Random Access Memory), a ROM (Read Only Memory), a hard disk drive, and a flash memory. The memory 12 stores programs for the controller 13 to execute predetermined processes. The programs executed by the controller 13 may be stored in a storage medium other than the memory 12.

[0029] The memory 12 also stores a map DB 10 including the voxel data VD. In addition to the voxel data VD, the map DB 10 also includes, for example, information on docking locations (including shores and piers) and information on waterways that ships can navigate. The map DB 10 may be stored in an external storage device of the information processing device 1, such as a hard disk connected to the information processing device 1 via the interface 11. The storage device may be a server device that communicates with the information processing device 1. The storage device may also be composed of multiple devices. The map DB 10 may also be updated periodically. In this case, for example, the controller 13 receives partial map information on an area to which its own position belongs from a server device that manages map information via the interface 11, and reflects the partial map information in the map DB 10.

[0030] The memory 12 also stores information necessary for the processing executed by the information processing device 1 in this embodiment, in addition to the map DB 10. For example, the memory 12 stores information used to set the downsampling size when downsampling is performed on point cloud data obtained when the LIDAR 3 performs one scanning cycle.

[0031] The controller 13 includes one or more processors such as a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), and a TPU (Tensor Processing Unit), and controls the entire information processing device 1. In this case, the controller 13 executes programs stored in the memory 12 or the like to perform processing related to self-position estimation, driving assistance, and the like.

[0032] Furthermore, the controller 13 functionally has a self-position estimation unit 15 and an obstacle / undertow detection unit 16. The controller 13 functions as a "point cloud data acquisition means," "water surface reflection data extraction means," "water surface height calculation means," "detection means," and a computer that executes a program.

[0033] The self-position estimation unit 15 estimates its own position by performing scan matching based on NDT (NDT scan matching) based on the point cloud data based on the output of the LIDAR 3 and the voxel data VD corresponding to the voxels to which the point cloud data belongs. Here, the point cloud data to be processed by the self-position estimation unit 15 may be point cloud data generated by the LIDAR 3, or may be point cloud data obtained by downsampling the point cloud data.

[0034] The obstacle / wake detection unit 16 uses the point cloud data output by the lidar 3 to detect obstacles and wakes around the ship.

[0035] (3) Self-location estimation Next, the self-position estimation based on NDT scan matching executed by the self-position estimation unit 15 will be described.

[0036] FIG. 3 is a diagram showing the self-position to be estimated by the self-position estimation unit 15 in three-dimensional Cartesian coordinates. As shown in FIG. 3, the self-position on a plane defined on the three-dimensional Cartesian coordinates of xyz is expressed by the coordinates "(x, y, z)", the roll angle "φ", pitch angle "θ", and yaw angle (azimuth) "ψ" of the target ship. Here, the roll angle φ is defined as the rotation angle around the axis of the target ship's traveling direction, the pitch angle θ is defined as the elevation angle of the traveling direction of the target ship relative to the xy plane, and the yaw angle ψ is defined as the angle between the traveling direction of the target ship and the x-axis. The coordinates (x, y, z) are, for example, absolute positions corresponding to a combination of latitude, longitude, and altitude, or world coordinates indicating a position with a predetermined point as the origin. The self-position estimation unit 15 then performs self-position estimation using these x, y, z, φ, θ, and ψ as estimation parameters.

[0037] Next, we will explain the voxel data VD used in NDT scan matching. The voxel data VD includes data in which measured point cloud data of a stationary structure in each voxel is expressed using a normal distribution.

[0038] Fig. 4 shows an example of a schematic data structure of the voxel data VD. The voxel data VD includes parameter information when expressing a point group in a voxel using a normal distribution, and in this embodiment, as shown in Fig. 4, includes a voxel ID, voxel coordinates, a mean vector, and a covariance matrix.

[0039] "Voxel coordinates" indicate the absolute three-dimensional coordinates of a reference position, such as the center position of each voxel. Each voxel is a cube that divides space into a grid, and since its shape and size are predetermined, it is possible to identify the space of each voxel using its voxel coordinates. Voxel coordinates may also be used as a voxel ID.

[0040] The "mean vector" and "covariance matrix" refer to the mean vector and covariance matrix, which are parameters when expressing the point cloud in the target voxel as a normal distribution. Note that the coordinates of an arbitrary point "i" in an arbitrary voxel "n" are X n (i)=[x n (i), y n (i), z n (i)] T and the number of points in voxel n is defined as "N n ", then the mean vector at voxel n is "μ n ” and the covariance matrix “V n " are expressed by the following formulas (1) and (2), respectively.

[0041]

number

[0042]

number

[0043] Next, an overview of NDT scan matching using voxel data VD will be explained.

[0044] Scan matching using NDT assuming a ship is performed using estimated parameters that are based on the amount of movement in the horizontal plane (here, xy coordinates) and the orientation of the ship. P=[t x , t y , t z , t φ , t θ , t ψ ] T Here, "t x " is the amount of movement in the x direction, "t y " is the amount of movement in the y direction, "t z " is the amount of movement in the z direction, "t φ ” is the roll angle, “t θ ” is the pitch angle, “t ψ " indicates the yaw angle.

[0045] In addition, the coordinates of the point cloud data output by the lidar 3 are X L (j)=[x n (j), y n (j), z n (j)] T Then, X L The average value of (j) "L' n " is expressed by the following equation (3).

[0046]

number

[0047] Then, the self-position estimation unit 15 searches for voxel data VD that is associated with the point cloud data converted into an absolute coordinate system (also called a "world coordinate system") that is the same coordinate system as the map DB 10, and calculates the mean vector μ n and the covariance matrix V n Using the above, the evaluation function value of voxel n (also called "individual evaluation function value") "E n In this case, the self-position estimation unit 15 calculates the individual evaluation function value E of the voxel n based on the following equation (4): n Calculate.

[0048]

number

[0049] Then, the self-position estimation unit 15 calculates a comprehensive evaluation function value (also called a "score value") "E(k)" for all voxels to be matched, as shown in the following formula (5). The score value E is an index showing the compatibility of the matching.

[0050]

number

[0051]

number

[0052] (4) Obstacle / undertow detection Next, the detection of obstacles and undertows by the obstacle / undertow detection unit 16 will be described. The obstacle / undertow detection unit 16 uses the water surface height calculated in processing up to one time before to detect obstacles and undertows. If there is an obstacle near the ship, it is necessary to navigate while avoiding collision or contact with the obstacle. Examples of obstacles include other ships, piles, bridge piers, buoys, nets, and garbage. Furthermore, if there is a undertow caused by another ship nearby, the ship must navigate carefully to avoid increasing its rocking due to the influence of the undertow. Therefore, the obstacle / undertow detection unit 16 uses the water surface height to detect the presence of obstacles or undertows near the ship.

[0053] (4-1) Estimation of water surface position FIG. 5 is a diagram illustrating the water surface height as seen from the lidar 3. The draft position of a ship changes depending on the number of passengers and cargo volume. In other words, the height to the water surface as seen from the lidar 3 changes. As shown in FIG. 5(A), when the draft position of the ship is low, the water surface position as seen from the lidar 3 is low. On the other hand, as shown in FIG. 5(B), when the draft position of the ship is high, the water surface position as seen from the lidar 3 is high. Therefore, by setting a search range with a predetermined width based on the water surface position as shown in FIG. 5(C), it becomes possible to correctly detect obstacles and wakes.

[0054] FIG. 6 is a diagram illustrating the reflection of light emitted from the LIDAR 3 on the water surface. When the LIDAR 3 is facing downward, some of the emitted light may be reflected by the water surface and return to the LIDAR 3. Now, as shown in FIG. 6(A), assume that the LIDAR 3 on a ship is emitting laser light. FIG. 6(B) shows light received by the LIDAR 3 on a ship near a quay. In FIG. 6(B), beam 101 is light that is not reflected by the water surface, but is a part of the scattered light of light that is directly irradiated onto an object and returns to the LIDAR 3 and is received. Beam 102 is light that is emitted from the LIDAR 3, reflected by the water surface, and returned directly to the LIDAR 3 and is one type of light reflected from the water surface (hereinafter also referred to as "light directly reflected from the water surface"). Beam 103 is light emitted from LIDAR 3, reflected by the water surface, and hits a cliff or the like, causing part of the scattered light to reflect off the water surface again, which returns to LIDAR 3 and is received; it is one type of water surface reflection light (hereinafter also referred to as "indirect water surface reflection light"). Since LIDAR 3 cannot recognize that this is light reflected by the water surface, when it receives beam 102, it recognizes that there is an object at that water surface position. On the other hand, when it receives beam 103, it recognizes that there is an object below the water surface. Therefore, when a LIDAR receives beam 103, it outputs incorrect point cloud data indicating a position inside the cliff, as shown in the figure.

[0055] FIG. 7 is a diagram illustrating point cloud data used to estimate the water surface height (hereinafter also referred to as the "water surface position"). FIG. 7(A) is a view of the ship seen from the rear, and FIG. 7(B) is a view of the ship seen from above. Around the ship, water surface turbulence sometimes causes the beam from the LIDAR 3 to be almost perpendicular to the water surface, resulting in direct water surface reflection light such as the beam 102 described above. On the other hand, when the ship is close to the shore, the beam from the LIDAR 3 is reflected by the shore or the like, resulting in indirect water surface reflection light such as the beam 103 described above. Therefore, the obstacle / wake detection unit 16 acquires multiple point cloud data of direct water surface reflection light near the ship and averages their z coordinate values ​​to estimate the water surface position. Because a ship floats on the water, the amount of water submerged varies depending on the number of passengers and cargo, and the height from the LIDAR 3 to the water surface changes. Therefore, the above method makes it possible to constantly calculate the distance from the LIDAR 3 to the water surface.

[0056] Specifically, the obstacle / undertow detection unit 16 extracts point cloud data measured at a position far from the shore and close to the vessel from the point cloud data output by the lidar 3. Here, a position far from the shore refers to a position that is a predetermined distance or more away from the shore. The position of the shore can be a docking location (including a shore or a pier) stored in the map DB 10. The shore may also be a ground location or structure other than a docking location. By using point cloud data measured at a position far from the shore, it is possible to exclude point cloud data of indirect water surface reflection light.

[0057] Furthermore, a position close to the ship refers to a position within a predetermined range from the ship's own position. By using point cloud data measured at a position close to the ship, it becomes possible to estimate the water surface position with high accuracy using point cloud data that measures the light directly reflected from the water surface (hereinafter also referred to as "direct water surface reflection data").

[0058] (4-2) Obstacle detection Next, an obstacle detection method will be described. FIG. 8 is a diagram illustrating the obstacle detection method. After the water surface position is estimated as described above, the obstacle / undertow detection unit 16 performs Euclidean clustering processing on the point cloud data at heights near the water surface position. As shown in FIG. 8(A), if a "lump" (hereinafter also referred to as a "cluster") is detected by the Euclidean clustering processing, the obstacle / undertow detection unit 16 provisionally determines that cluster as an obstacle candidate. The obstacle / undertow detection unit 16 similarly detects clusters in frames at multiple times, and if it detects clusters of similar sizes at each time, it determines that the cluster is some kind of obstacle.

[0059] Note that when detecting small obstacles on the water, such as buoys, the water surface reflection component can also be valuable information from the viewpoint of detection. In FIG. 8(B), beam 111 is emitted from LIDAR 3, reflected by the buoy, and returned to LIDAR 3. On the other hand, beam 112 is emitted from LIDAR 3, reflected by the water surface, hits the buoy, and some of the scattered light is reflected again by the water surface, returns to LIDAR 3, and is received. In the case of small obstacles such as buoys, the amount of data that is reflected directly from the buoy and returns as beam 111 is small. Therefore, by including data of the component reflected by the water surface, such as beam 112, the amount of data used for analysis can be increased and utilized for clustering. This increases the amount of data to be subjected to clustering processing, thereby improving clustering performance.

[0060] When the obstacle / undertow detection unit 16 determines that the detected cluster is an obstacle, it subtracts the water surface position from the z coordinate of the highest point of the obstacle and calculates the height Ho of the obstacle above the water surface as shown in Figure 8(B).

[0061] (4-3) How to detect undertow Next, a method for detecting undertow will be described. FIG. 9 is a diagram illustrating the method for detecting undertow. After the water surface position is calculated as described above, the obstacle / undertow detection unit 16 performs a Hough transform on the point cloud data at heights near the water surface position, ignoring the z coordinate, and converts the data into a point cloud on a two-dimensional plane. As shown in FIG. 9(A), if a "straight line" is detected by the Hough transform process, the obstacle / undertow detection unit 16 provisionally determines that the straight line is a candidate undertow. The obstacle / undertow detection unit 16 similarly detects straight lines in multiple frames at different times, and if it detects a straight line with similar coefficients at each time, it determines that the straight line is a undertow.

[0062] When detecting backwash, the component reflected from the water surface can also be valuable information from the viewpoint of detection. In FIG. 9(B), beam 113 is emitted from LIDAR 3, reflected by the backwash, and returned to LIDAR 3. On the other hand, beam 114 is emitted from LIDAR 3, reflected by the water surface, hits the backwash, and some of the scattered light is reflected again by the water surface, returns to LIDAR 3, and is received. In the case of backwash, the amount of data reflected directly from the backwash as beam 113 is small, so by including data of the component reflected from the water surface, such as beam 114, the amount of data used for analysis is increased and used in the Hough transform. This increases the amount of data to be subjected to the Hough transform process, thereby improving the performance of the Hough transform.

[0063] After determining whether a wave is undertow using the two-dimensional data as described above, the obstacle / undertow detection unit 16 re-evaluates the z coordinates of points determined to be part of a wave undertow. Specifically, the obstacle / undertow detection unit 16 calculates the average z coordinate using only points whose z coordinate values ​​are higher than the water surface height, and then subtracts the water surface position from this average to calculate the height Hw of the undertow from the water surface.

[0064] (4-4) Example of Obstacle / Wake Detection Unit Next, an embodiment of the obstacle / wake detection unit 16 will be described. In the following embodiment, the obstacle / wake detection unit 16 performs processing in the order of wake detection → obstacle detection → water surface position estimation, thereby facilitating subsequent processing. Specifically, the water surface position estimated by the water surface position estimation block 132 is used to determine the heights of the wake and obstacles from the water surface, and this is used to set the search range for the point cloud data at the next time.

[0065] 10 is a block diagram showing the functional configuration of the obstacle / undertow detection unit 16. The obstacle / undertow detection unit 16 receives point cloud data measured by the lidar 3 and outputs undertow information and obstacle information. The obstacle / undertow detection unit 16 includes a search range setting block 121, a line extraction block 122, a undertow detection block 123, a undertow information calculation block 124, a undertow data removal block 125, a Euclidean clustering block 126, an obstacle detection block 127, an obstacle information calculation block 128, an obstacle data removal block 129, a mean / variance calculation block 130, a time filter block 131, and a water surface position estimation block 132.

[0066] The search range setting block 121 extracts point cloud data of direct water surface reflection light from the input point cloud data and sets a search range for obstacles and backwash in the height direction. The obstacle / backwash detection unit 16 detects obstacles and backwash by extracting and analyzing point cloud data belonging to a search range set around the water surface as shown in Figure 5(C). However, if the ship is moving heavily or the waves are large, obstacles and backwash floating on the water surface may fall outside the search range and become undetectable. On the other hand, if the search range is enlarged to avoid this, irrelevant data may be included when the waves are small, reducing detection accuracy.

[0067] Therefore, the search range setting block 121 calculates the standard deviation of the z-coordinate values ​​of the direct water surface reflection data obtained around the ship as described above, and sets the search range using this standard deviation value. Specifically, the search range setting block 121 estimates the wave height (wave height) using the standard deviation of the z-coordinate values ​​of the direct water surface reflection data, and sets the search range according to the wave height. If the standard deviation of the z-coordinate values ​​of the direct water surface reflection data is small, the wave height is estimated to be small, as shown in Figure 11(A). In this case, the search range setting block 121 narrows the search range. For example, the search range setting block 121 sets the search range near the average value of the z-coordinate values ​​of the direct water surface reflection data. This reduces the inclusion of noise, thereby improving the accuracy of detecting obstacles and backwash.

[0068] On the other hand, if the standard deviation of the z coordinate values ​​of the direct water surface reflection is large, it is estimated that the wave height is large, as shown in Figure 11(B). Therefore, the search range setting block 121 widens the search range. In other words, the search range setting block 121 sets a wider search range than when the wave height is small, centered on the average value of the z coordinate values ​​of the direct water surface reflection data.

[0069] As an example, as shown in Figure 11(C), the search range setting block 121 may use the standard deviation σ of the z coordinate values ​​of the direct water surface reflection data to set the search range to a range of ±3σ centered on the average value of the z values ​​of the direct water surface reflection data. This allows the search range to be widened even when the waves are high, preventing obstacles and backwash from being missed. The search range setting block 121 outputs the set search range to the line extraction block 122.

[0070] The line extraction block 122 uses a Hough transform to extract a line from direct water surface reflection data (hereinafter also referred to as "search data") measured within the search range around the ship. The line extraction block 122 outputs the extracted line to the wake detection block 123. When detecting a line using the Hough transform, a discretized two-dimensional array is used, so the resulting line is an approximation. Therefore, the line extraction block 122 and the wake detection block 123 use the following procedure to find a more accurate line. (Step 1) Calculate an approximate line using the Hough transform. (Step 2) Extract data whose distance to the approximate line is within a predetermined threshold (straight-line distance threshold). (Step 3) Principal component analysis is performed using the extracted data, and a straight line is recalculated to represent the backwash line.

[0071] The results of a simulation performed to detect a straight line using the above procedure are shown in Figure 12. As shown, the straight line 141 obtained by the Hough transform is an approximate straight line, and therefore there is a slight deviation from the data. By extracting data within the straight line distance threshold from the straight line 141 (marked with a square in Figure 12), and then using the extracted data to recalculate a straight line using principal component analysis, an accurate undertow straight line 142 can be obtained.

[0072] The backwash detection block 123 determines that the newly calculated straight line is a backwash, and outputs backwash data indicating the backwash to the backwash information calculation block 124 and the backwash data removal block 125. The backwash information calculation block 124 calculates the position, distance, angle, and height of the backwash based on the equation of the line indicating the backwash and the ship's own position, and outputs this as backwash information.

[0073] The backwash data removal block 125 removes backwash data from the search data measured within the search range around the ship and outputs the data to the Euclidean clustering block 126. The Euclidean clustering block 126 performs Euclidean clustering processing on the input search data, detects clusters of the search data, and outputs the clusters to the obstacle detection block 127.

[0074] In Euclidean clustering, the distance between every target point and every other point is first calculated, and points whose inter-point distance is shorter than a predetermined value (hereinafter referred to as the "grouping threshold") are grouped together. Next, among each group, groups containing points with a predetermined number of points (hereinafter referred to as the "score threshold") or more are considered to be clusters. Note that groups with a low number of points are not considered to be clusters, as they are likely to be noise.

[0075] FIG. 13 shows an example of Euclidean clustering. FIG. 13(A) shows multiple points to be subjected to Euclidean clustering. The inter-point distances shown in FIG. 13(A) were calculated and compared with a grouping threshold to perform grouping. Since the distances indicated by the arrows in FIG. 13(B) are greater than the grouping threshold, five groups A to E, indicated by dashed lines in FIG. 13(B), were obtained. Next, the scores belonging to each group were compared with the score threshold (set to "6" in this example), and only groups A and C, which contain more points than the score threshold, were ultimately determined to be clusters, as shown in FIG. 13(C).

[0076] Figure 14 shows the simulation results of Euclidean clustering. Figure 14(A) shows the simulation results when backwash data remains during Euclidean clustering. In Euclidean clustering, when group determination is performed using a grouping threshold, if backwash data remains, there is a risk that the obstacle and backwash will be determined to be in the same cluster. In the example of Figure 14(A), the backwash and the obstacle are close to each other, so the data for the obstacle and backwash are grouped together, and because the score for that group is higher than the score threshold, they are detected as being in the same cluster.

[0077] Figure 14(B) shows the simulation results when Euclidean clustering was performed after removing the backwash data. To distinguish obstacles from backwash, backwash detection was performed first, and data determined to be backwash data was removed before Euclidean clustering was performed. In this case, obstacles were correctly detected as clusters without being affected by the backwash data.

[0078] Generally, because the light beam of a lidar is output radially, the farther away the data is, the greater the distance between the positions of the data. Therefore, as shown in Figure 15, the farther away the data is, the greater the distance between adjacent data. Also, even for objects of the same size, the number of detected points will be greater if they are close, and fewer if they are far away. Therefore, in Euclidean clustering processing, by setting a grouping threshold and a point threshold according to the distance value of the data, it is possible to perform clustering determination under as similar conditions as possible for objects close to and far from the lidar.

[0079] FIG. 16 shows the results of a simulation in which the grouping threshold is increased as the distance of the data increases, and the score threshold is decreased as the distance to the center of gravity of the group increases.

[0080] Figure 16(A) shows Grouping threshold = 2.0 m Score threshold = 6 points The simulation results are shown below.

[0081] Figure 16(B) shows Grouping threshold = a × (data distance), Score threshold = b / (distance to center of gravity of group) In this simulation, a=0.2 and b=80, but in practice these are set to values ​​suitable for the characteristics of the lidar 3.

[0082] Comparing Figure 16(A) and Figure 16(B) reveals that in Figure 16(B), in addition to Cluster 1, which is located near the ship, Cluster 2, which is located farther from the ship, was also detected. In Cluster 2, the distance between each data point was approximately 3 m, but the grouping threshold calculated using the distance from the ship to the data was approximately 4.5 m. Because the distance was closer than this threshold, the data points were grouped together. Furthermore, although the number of data points was four, the score threshold calculated using the distance to the center of gravity of the group was approximately 3.2. Therefore, the number of points was greater than this threshold, and the data points were determined to be part of a cluster. Using the above formula, the grouping threshold calculated using the distance from the ship to the data was approximately 2.5 m, and the score threshold was approximately 7.1, which is not significantly different from the fixed values ​​in Figure 16(A). This adaptive threshold setting can minimize missed and false detections of clusters, thereby improving obstacle detection performance.

[0083] The obstacle detection block 127 outputs point cloud data (hereinafter referred to as "obstacle data") indicating obstacles detected by Euclidean clustering to the obstacle information calculation block 128 and the obstacle data removal block 129. Based on the obstacle data, the obstacle information calculation block 128 calculates the position, distance, angle, size, and height of the obstacle relative to the ship's own position, and outputs the calculated data as obstacle information.

[0084] The obstacle data removal block 129 removes obstacle data from the search data measured within the search range around the ship, and outputs the data to the mean and variance calculation block 130. This is because when estimating the water surface position from the direct water surface reflection data around the ship, if there are backwash or obstacles, the water surface position cannot be estimated correctly.

[0085] FIG. 17(A) shows direct water surface reflection data obtained when there are backwashes or obstacles around the ship. In this case, data from a position higher than the water surface and indirect water surface reflection light due to obstacles or backwashes (such as beam 112 in FIG. 8(B) and beam 114 in FIG. 9(B)) can cause errors in estimating the water surface position. Therefore, the backwash data removal block 125 and the obstacle data removal block 129 remove backwashes and obstacles as shown in FIG. 17(B) to estimate the water surface position using search data. Specifically, as shown in FIG. 18, from State 1, where there are backwashes and obstacles around the ship, backwashes are detected and removed as shown in State 2, resulting in State 3. Next, obstacles are detected and removed as shown in State 4, and direct water surface reflection data that does not include backwashes or obstacles is obtained as shown in State 5.

[0086] Specifically, the mean / variance calculation block 130 calculates the mean value and variance of the z-coordinate values ​​of the direct water surface reflection data obtained around the ship and outputs them to the time filter block 131. The time filter block 131 averages or filters the mean value of the z-coordinate values ​​of the input direct water surface reflection data with past water surface positions. The water surface position estimation block 132 estimates the water surface position using the mean value of the z-coordinate values ​​after averaging or filtering and the variance of the z-coordinate values ​​of the search data.

[0087] When estimating the water surface position, if the variance of the direct water surface reflection data around the ship is large, it can be assumed that waves are high due to the passage of other ships or that floating objects that were not detected as obstacles are present. Therefore, if the variance is smaller than a predetermined value, the water surface position estimation block 132 estimates and updates the water surface position using the average value of the direct water surface reflection data. On the other hand, if the variance is greater than or equal to the predetermined value, the water surface position estimation block 132 does not update the water surface position and maintains the previous value. Here, the "predetermined value" may be a fixed value or a value set based on the average value of past variance values, such as twice the average value of the variance. The water surface position estimation block 132 then outputs the estimated water surface position to the search range setting block 121, the backwash information calculation block 124, and the obstacle information calculation block 128. In this way, the water surface position is updated based on the newly obtained direct water surface reflection data, and backwash and obstacles are detected.

[0088] (4-5) Obstacle / undertow detection processing Next, we will explain the obstacle / wake detection process executed by the obstacle / wake detection unit 16. Figure 19 is a flowchart of the obstacle / wake detection process. This process is realized by the controller shown in Figure 2 executing a pre-prepared program and operating as each element shown in Figure 10.

[0089] First, the obstacle / wake detection unit 16 acquires point cloud data measured by the LIDAR 3 (step S11). Next, the search range setting block 121 determines a search range from the estimated water surface position up to one time before and the standard deviation σ of the z coordinate values ​​of the direct water surface reflection data obtained around the ship (step S12). For example, when the standard deviation σ is Search range = Estimated water surface position ±3σ Let's say. Then, the search range setting block 121 extracts point cloud data within the determined search range and sets it as search data for detecting backwash (step S13).

[0090] Next, the obstacle / backwash detection unit 16 executes backwash detection processing (step S14). FIG. 20 is a flowchart of the backwash detection processing. First, the line extraction block 122 temporarily ignores the z value for each point of the search data obtained from the search range and converts it into two-dimensional data of x and y (step S101). Next, the line extraction block 122 calculates (θ,ρ) for all search points by changing θ in the range of 0 to 180 degrees using the following equation (7) (step S102). To create a discretized two-dimensional array with (θ,ρ) as elements, "θ" and "ρ" are expressed as integers.

[0091]

number

[0092] Next, the line extraction block 122 checks the number of (θ, ρ) and extracts the maximum value that is greater than a predetermined value (step S103). When n (θ, ρ) values ​​are extracted, (θ1, ρ1) to (θ n ,ρ n Next, the line extraction block 122 extracts the extracted (θ1, ρ1) to (θ n ,ρ n ) into equation (7), and the n lines L1 to L n is generated (step S104).

[0093] Next, the undertow detection block 123 again detects the n straight lines L1 to L2 generated for all the search points. nThe backwash detection block 123 then calculates the distance to the target point, and sets the distance less than a predetermined distance as backwash data (step S105). Next, the backwash detection block 123 sets the three-dimensional data, including the z-value, of the backwash data as backwash data (step S106). Next, the backwash detection block 123 uses the extracted backwash data to newly calculate the equations of the n straight lines by the least squares method or principal component analysis (step S107). Then, the process returns to the main routine of FIG. 19.

[0094] Next, the backwash data removal block 125 removes the backwash data from the search data to obtain search data for obstacle detection (step S15).

[0095] Next, the obstacle / wake detection unit 16 executes obstacle detection processing (step S16). FIG. 22 is a flowchart of the obstacle detection processing. First, the Euclidean clustering block 126 calculates the inter-point distances between all search data and all other search data (step S111). If there are n pieces of search data, n(n-1) inter-point distances are calculated. Next, the Euclidean clustering block 126 selects the first target data (step S112), calculates the distance r1 from the ship to the target data, and calculates a grouping threshold T1 using a predetermined coefficient a (step S113). For example, T1 = a·r1. In other words, the grouping threshold T1 is a different value for each piece of target data.

[0096] Next, the Euclidean clustering block 126 groups together data whose inter-point distance from the target data is smaller than the grouping threshold T1 (step S114). Next, the Euclidean clustering block 126 determines whether all search data has been targeted (step S115). If not all search data has been targeted (step S115: No), the Euclidean clustering block 126 selects the next target data (step S116) and returns to step S113.

[0097] On the other hand, if all search data is targeted (step S115: Yes), the Euclidean clustering block 126 obtains the center of gravity position for each extracted group and calculates the distance r2 to the center of gravity position. Then, a predetermined coefficient b is used to set a score threshold T2 (step S117). For example, T2 = b / r2. In other words, the score threshold T2 is a different value for each group.

[0098] Next, the Euclidean clustering block 126 determines that each group contains a data number equal to or greater than the score threshold T2 as a cluster, and the obstacle detection block 127 determines that cluster as an obstacle (step S118). Then, the process returns to the main routine of FIG.

[0099] Next, the obstacle data removal block 129 removes data determined to be obstacles from the search data, and uses the data as data for estimating the water surface position (step S17).

[0100] Next, the obstacle / wake detection unit 16 executes a water surface position estimation process (step S18). FIG. 23 is a flowchart of the water surface position estimation process. First, the mean and variance calculation block 130 determines that data that is far from the shore, close to the ship position, and exists near the water surface position is water surface reflection data (step S121). Next, the mean and variance calculation block 130 obtains water surface reflection data for multiple scan frames, and when a predetermined number of data is obtained, it obtains the mean value and variance value in the z direction of the data (step S122).

[0101] Next, the mean / variance calculation block 130 determines whether the variance value is less than a predetermined value (step S123). If the variance value is not less than the predetermined value (step S123: No), the process proceeds to step S125. On the other hand, if the variance value is less than the predetermined value (step S123: Yes), the time filter block 131 performs filtering processing on the calculated average value of the z values ​​and the past estimated water surface positions, and updates the water surface position (step S124). Next, the water surface position estimation block 132 outputs the calculated water surface position and variance value (step S125). Then, the process returns to the main routine of FIG. 19.

[0102] Next, the obstacle / backwash detection unit 16 executes backwash information calculation processing (step S19). Figure 24(A) is a flowchart of the backwash information calculation processing. First, the backwash information calculation block 124 uses the ship's own position as a reference and calculates the shortest distance to the line detected by the backwash detection block 123, and sets this as the distance to the backwash. It also finds the position at this distance and sets this as the position of the backwash. It also calculates the slope from the coefficient of this line and sets this as the angle of the backwash (step S131).

[0103] As shown in Figure 24(B), the shortest distance from the ship's own position to a straight line is the distance to the foot of a perpendicular line dropped on the straight line. However, because the straight line detected as a backwash is a line segment, the endpoint of the data detected as a backwash may be the shortest distance, as shown in Figure 24(C). Therefore, the backwash information calculation block 124 checks whether the coordinates to the foot of the perpendicular line are within the line segment, and if not, the distance to the endpoint of the line segment is the shortest distance.

[0104] Next, the backwash information calculation block 124 calculates the average z coordinate value using only points with a higher z value than the estimated water surface position, and calculates the height of the backwash from the water surface using the estimated water surface position (step S132). Note that instead of the average z coordinate value, the maximum value may be used as the backwash height. Then, the process returns to the main routine in FIG. 19.

[0105] Next, the obstacle / wake detection unit 16 executes obstacle information calculation processing (step S20). FIG. 25 is a flowchart of the obstacle information calculation processing. First, the obstacle information calculation block 128 uses the ship's own position as a reference and extracts the cluster data detected as an obstacle that is the shortest distance away, and sets this as the position of the obstacle. It also calculates the distance to this data and sets this as the distance to the obstacle. It also calculates the angle of the obstacle from the coordinates of this data (step S141).

[0106] Next, the obstacle information calculation block 128 extracts the two points in the cluster data that are the furthest apart on the two-dimensional plane of x and y, and sets this distance as the horizontal size of the obstacle. It also subtracts the estimated water surface position from the z coordinate of the highest point in the cluster data, and sets this as the height of the obstacle from the water surface (step S142). Then, the process returns to the main routine of FIG. 19.

[0107] Next, the obstacle / undertow detection unit 16 determines whether similar undertows have been detected in multiple frames (step S21). Although exact matches will not occur if the ship itself or the undertow is moving, if the values ​​calculated in step S19 differ only slightly, it is determined to be similar undertows. If similar undertows are not detected (step S21: No), the process proceeds to step S23. On the other hand, if similar undertows are detected (step S21: Yes), the undertow information calculation block 124 determines that the data is undertows and outputs the undertow information to the hull system (step S22).

[0108] Next, the obstacle / wake detection unit 16 determines whether a similar obstacle has been detected in multiple frames (step S23). Although there will not be an exact match if the ship itself or the obstacle is moving, if the values ​​calculated in step S20 differ only slightly, it is determined to be a similar obstacle. If a similar obstacle is not detected (step S23: No), the process ends. On the other hand, if a similar obstacle is detected (step S23: Yes), the obstacle information calculation block 128 determines that the data is an obstacle and outputs obstacle information to the hull system (step S24). Then, the process ends.

[0109] (4-6) Variations (Variation 1) The above water surface position estimation uses the variance value of the water surface reflection data, but as shown in Figure 26, if the hull is statically tilted in the roll direction due to uneven loading or the like, the variance value of the water surface reflection data will also become large. When estimating the water surface position in such a situation, the water surface position estimation block 132 may process the starboard side water surface reflection data and the port side water surface reflection data separately to determine the starboard side water surface positions and the port side water surface positions separately. Alternatively, the water surface position estimation block 132 may perform coordinate transformation on the water surface reflection data to rotate the roll angle so that the difference between the starboard side average value of the water surface reflection data and the port side average value becomes small, making it possible to estimate the water surface position without distinguishing between the starboard and port sides.

[0110] (Variation 2) In the above embodiment, the straight line extraction block 122 extracts straight lines of the undertow according to the following steps 1 to 3. (Step 1) Calculate an approximate line using the Hough transform. (Step 2) Extract data whose distance to the approximate line is within a predetermined threshold (straight-line distance threshold). (Step 3) Principal component analysis is performed using the extracted data, and a straight line is recalculated to represent the backwash line.

[0111] Alternatively, the following procedure 4 may be added, and steps 2 and 3 may be repeatedly executed depending on the determination made in step 4. (Step 4) If the extracted data changes and the equation of the line changes, return to step 2. When the equation of the line no longer changes, use it as the line of the undertow.

[0112] The graph on the left side of Figure 27 shows an example of a straight line obtained without performing step 4 above, while the graph on the right side shows an example of a straight line obtained by performing step 4 and converging. By adding step 4, undertow data is not left out of the extraction, and as a result, the accuracy of the straight line can be improved.

[0113] Although the present invention has been described above with reference to the examples, the present invention is not limited to the above examples. Various modifications within the scope of the present invention that would be understood by those skilled in the art can be made to the configuration and details of the present invention. In other words, the present invention naturally includes various modifications and alterations that would be possible for those skilled in the art in accordance with the entire disclosure, including the claims, and the technical ideas. Furthermore, the disclosures of the above-cited patent documents and other documents are incorporated herein by reference. [Explanation of symbols]

[0114] 1. Information processing equipment 2 Sensor group 3 Rider 4 Speed ​​Sensor 5 GPS receiver 6 IMU 10 Map DB 13 Controller 15 Self-position estimation part 16 Obstacle / undertow detector 121 Search range setting block 122 Line Extraction Block 123 Undertow detection block 124 Undertow Information Calculation Block 125 Undertow data removal block 126 Euclidean clustering blocks 127 Obstacle Detection Block 128 Obstacle Information Calculation Block 129 Obstacle Data Removal Block 130 Mean and Variance Calculation Block 131 Time Filter Block 132 Water surface position estimation block

Claims

[Claim 1] a point cloud data acquisition means for acquiring point cloud data generated by a measuring device installed on the ship; a water surface reflection data extraction means for extracting data measured at a position within a predetermined distance from the measurement position of the measuring device as water surface reflection data measured by reflection on the water surface; and a water surface height calculation means for calculating a water surface height based on the water surface reflection data; An information processing device having the above.

Images