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

Abstract

To inform the presence of a target object in the vicinity of a vessel to an operator in an intuitively easy-to-understand mode.SOLUTION: In an information processing device, target object detecting means detects a target object based on point group data generated by a measurement device installed in a vessel. Positional relationship acquiring means acquires a relative positional relationship between the target object and the vessel. Display control means displays information relating to the positional relationship on display means in a display mode corresponding to the positional relationship.SELECTED DRAWING: Figure 10

Description

[Technical field]

[0001] The present disclosure relates to processing of measurement data on ships. [Background technology]

[0002] Conventionally, a technique for estimating the self-position of a moving object by collating (matching) shape data of a surrounding object measured by a measuring device such as a laser scanner with map information in which the shapes of the surrounding objects are stored in advance has been known. For example, Patent Document 1 discloses an autonomous moving 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 map information with measurement data for voxels in which stationary objects exist. In addition, Patent Document 2 discloses a scan matching method that estimates the self-position by collating voxel data including the mean vector and covariance matrix of a stationary object for each voxel with point cloud data output by a lidar.

[0003] Furthermore, Patent Document 3 discloses a docking assistance device that detects obstacles around a ship when the ship is docking, and outputs a determination result as to whether or not docking is possible based on the obstacle detection results. [Prior art documents] [Patent documents]

[0004] [Patent Document 1] International Publication WO2013 / 076829 [Patent Document 2] International Publication WO2018 / 221453 [Patent Document 3] JP 2020-19372 A Summary of the Invention [Problem to be solved by the invention]

[0005] In maneuvering a ship, it is important to be aware of the surrounding situation, not only when docking. For example, if there are obstacles around the ship, it is necessary to navigate around the obstacles. Also, if there are wakes around the ship, the impact and rocking caused to the ship can be reduced by navigating at an appropriate angle to the wakes. Therefore, it is required to detect obstacles and wakes around the ship and convey them to the pilot in an intuitive and easy-to-understand manner.

[0006] The present disclosure has been made to solve the above-mentioned problems, and has as its main objective the provision of an information processing device capable of informing the pilot of the presence of objects around the ship in an intuitive and easy-to-understand manner. [Means for solving the problem]

[0007] The invention described in the claims is an information processing device comprising an object detection means for detecting an object based on point cloud data generated by a measuring device installed on a ship, a positional relationship acquisition means for acquiring the relative positional relationship between the object and the ship, and a display control means for displaying information regarding the positional relationship on a display means in a display mode corresponding to the positional relationship.

[0008] The invention described in the claims is a control method executed by a computer, which detects an object based on point cloud data generated by a measuring device installed on a ship, obtains a relative positional relationship between the object and the ship, and displays information regarding the positional relationship on a display means in a display mode corresponding to the positional relationship.

[0009] In addition, the invention described in the claims is a program that causes a computer to execute a process of detecting an object based on point cloud data generated by a measuring device installed on a ship, obtaining a relative positional relationship between the object and the ship, and displaying information regarding the positional relationship on a display means in a display mode corresponding to the positional relationship. [Brief description of the drawings]

[0010] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Diagram 2] FIG. 1 is a block diagram showing a configuration of an information processing device. [Diagram 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] 2 shows an example of a schematic data structure of voxel data. [Diagram 5] FIG. 2 is a diagram illustrating the water surface height as seen by a lidar. [Figure 6] 1A and 1B are diagrams illustrating the reflection of light emitted by a lidar on the water surface. [Figure 7] FIG. 1 is a diagram illustrating point cloud data used to estimate the water surface height. [Figure 8] FIG. 2 is a diagram illustrating a method for detecting an obstacle. [Figure 9] FIG. 1 is a diagram illustrating a method for detecting an undertow. [Figure 10] FIG. 4 is a block diagram showing the functional configuration of an obstacle / undertow detection unit. [Figure 11] FIG. 13 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] 1 shows an example of Euclidean clustering. [Figure 14] 4 shows the simulation results of Euclidean clustering. [Figure 15] FIG. 13 is a diagram showing 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 a ship. [Figure 18] We present a method to remove wake and obstacles from water surface reflection data. [Figure 19] An example of how obstacles and undertow are displayed is shown below. [Figure 20] 13 is a flowchart of an obstacle / undertow detection process. [Figure 21] 13 is a flowchart of a backwash detection process. [Figure 22] FIG. 13 is a diagram illustrating a method for detecting a straight line. [Figure 23] 13 is a flowchart of an obstacle detection process. [Figure 24] 13 is a flowchart of a water surface position estimation process. [Diagram 25] 1 is a flowchart and an explanatory diagram of a backwash information calculation process. FIG. [Figure 26] 13 is a flowchart of an obstacle information calculation process. [Figure 27] 13 is a flowchart of a process for displaying backwash information on a screen. [Figure 28] FIG. 13 is a diagram illustrating an emphasis parameter. [Figure 29] 13 is a flowchart of a process for displaying obstacle information on a screen. [Diagram 30] FIG. 11 is an explanatory diagram of a water surface position estimation method according to the first modified example. [Diagram 31] 13 shows an example of undertow detection according to the second modification. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

[0011] In one preferred embodiment of the present invention, the information processing device comprises an object detection means for detecting an object based on point cloud data generated by a measuring device installed on the ship, a positional relationship acquisition means for acquiring the relative positional relationship between the object and the ship, and a display control means for displaying information regarding the positional relationship on a display means in a display mode corresponding to the positional relationship.

[0012] In the above information processing device, the object detection means detects the object based on point cloud data generated by a measuring device installed on the ship. The positional relationship acquisition means acquires the relative positional relationship between the object and the ship. The display control means displays information regarding the positional relationship on the display means in a display mode according to the positional relationship. This makes it possible to display information regarding the positional relationship between the ship and the object in an appropriate display mode.

[0013] In one aspect of the information processing device, the display control means changes the display mode of the information on the positional relationship based on the degree of danger of the object to the ship determined based on the positional relationship. In this aspect, the display mode is changed according to the degree of danger. In a preferred example, the display control means emphasizes the information on the positional relationship as the degree of danger increases.

[0014] In another aspect of the information processing device, the information on the positional relationship includes a position of the ship, a position of the object, a moving direction of the object, a moving speed of the object, a height of the object, and a distance between the ship and the object, thereby enabling a pilot to easily grasp a positional relationship with respect to the object.

[0015] In another aspect of the information processing device, the object includes at least one of an obstacle and an undertow, and the information on the positional relationship includes information indicating whether the object is an obstacle or an undertow. In a preferred example in this case, when the object is an undertow, the display control means displays at least one of the height of the undertow and the angle of the direction in which the undertow extends as the information on the positional relationship. This enables the pilot to navigate appropriately with respect to the undertow.

[0016] In another preferred embodiment of the present invention, a control method executed by a computer includes detecting an object based on point cloud data generated by a measuring device provided on a ship, acquiring a relative positional relationship between the object and the ship, and displaying information regarding the positional relationship on a display means in a display mode corresponding to the positional relationship, thereby making it possible to display information regarding the positional relationship between the ship and the object in an appropriate display mode.

[0017] In another preferred embodiment of the present invention, the program causes a computer to execute a process of detecting an object based on point cloud data generated by a measuring device provided on a ship, acquiring a relative positional relationship between the object and the ship, and displaying information regarding the positional relationship on a display means in a display mode according to the positional relationship. The above-mentioned information processing device can be realized by executing the program on a computer. The program can be stored in a storage medium and handled. EXAMPLES

[0018] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For the sake of convenience, in this specification, any symbol followed by "^" or "-" is referred to as "A^" or "A - ” (where “A” is any letter).

[0019] (1) Overview of the driving assistance system Fig. 1 shows a schematic configuration of a driving assistance system according to the present 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."

[0020] The information processing device 1 is electrically connected to the sensor group 2, and estimates the position (also called "self-position") of the target ship on which the information processing device 1 is installed based on the output 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 result of estimating the 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.

[0021] The information processing device 1 also stores a map database (DB: DataBase) 10 including voxel data "VD". The voxel data VD is data that records position information of stationary structures for each voxel that indicates a cube (regular lattice) that is the smallest unit of three-dimensional space. The voxel data VD includes data that expresses measured point cloud data of stationary structures in each voxel using 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. Note that unless otherwise specified, the self-position also includes the attitude angle such as the yaw angle of the target ship.

[0022] 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 Global Positioning Satellite (GPS) receiver 5, and an Inertial Measurement Unit (IMU) 6 that measures the acceleration and angular velocity of the target ship in three axial directions.

[0023] The lidar 3 measures the distance to an object in the outside world discretely by emitting a pulsed laser in a predetermined angular range in the horizontal and vertical directions, and generates three-dimensional point cloud data indicating the position of the object. In this case, the lidar 3 has an irradiation unit that irradiates a laser beam while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser beam, 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 beam received by the light receiving unit and the response delay time of the laser beam specified based on the above-mentioned light receiving signal. In general, the closer the distance to the object, the higher the accuracy of the distance measurement value of the lidar, 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 speedometer using Doppler or a speedometer using GNSS.

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

[0025] (2) Configuration of information processing device 2 is a block diagram showing an example of a 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.

[0026] The interface 11 performs an interface operation for sending and receiving data between the information processing device 1 and an external device. In this embodiment, the interface 11 acquires output data from each sensor of the sensor group 2, such as the lidar 3, the speed sensor 4, the GPS receiver 5, and the IMU 6, and supplies the data to the controller 13. The interface 11 also supplies, for example, a signal related to the control of the target ship generated by the controller 13 to each component of the target ship that controls the operation of the target ship. For example, the target ship includes a drive source such as an engine or an electric motor, a screw that generates a propulsive force in the forward direction based on the driving force of the drive source, a thruster that generates a lateral propulsive force based on the driving force of the drive source, and a rudder, which is a mechanism for freely determining the forward direction of the ship. During automatic operation such as automatic docking, the interface 11 supplies a control signal generated by the controller 13 to each of these components. In addition, when an electronic control device is provided in the target ship, the interface 11 supplies a 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 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.

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

[0028] 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 includes, for example, information on docking locations (including shores and piers) and information on waterways that ships can move through. 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 be composed of a plurality of devices. The map DB 10 may be updated periodically. In this case, for example, the controller 13 receives partial map information on an area to which the controller's 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.

[0029] 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 a downsampling size when downsampling is performed on point cloud data obtained when the LIDAR 3 performs one scanning cycle.

[0030] The controller 13 includes one or more processors such as a central processing unit (CPU), a graphics processing unit (GPU), and a tensor processing unit (TPU), and controls the entire information processing device 1. In this case, the controller 13 executes a program stored in the memory 12 or the like to perform processes related to self-position estimation, driving assistance, and the like.

[0031] Moreover, functionally, the controller 13 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," a "water surface reflection data extraction means," a "water surface height calculation means," a "detection means," a computer that executes a program, and the like.

[0032] The self-position estimation unit 15 estimates the self-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 after the point cloud data is downsampled.

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

[0034] The display device 17 displays information about detected obstacles and wakes around the ship on a device such as a monitor.

[0035] (3) Self-location estimation Next, the self-location estimation based on NDT scan matching executed by the self-location estimator 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 orthogonal coordinates. As shown in FIG. 3, the self-position on a plane defined on the three-dimensional orthogonal coordinates of xyz is expressed by the coordinates "(x, y, z)", the roll angle "φ", the pitch angle "θ", and the yaw angle (azimuth) "ψ" of the target ship. Here, the roll angle φ is defined as the rotation angle around the traveling direction of the target ship, the pitch angle θ is defined as the elevation angle of the traveling direction of the target ship with respect 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. Then, the self-position estimation unit 15 performs self-position estimation using these x, y, z, φ, θ, and ψ as estimation parameters.

[0037] Next, the voxel data VD used in the NDT scan matching will be described. The voxel data VD includes data in which measured point cloud data of a stationary structure in each voxel is expressed by 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 by a normal distribution, and in this embodiment, as shown in Fig. 4, includes a voxel ID, a voxel coordinate, 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 determined in advance, it is possible to identify the space of each voxel by its voxel coordinates. The voxel coordinates may be used as a voxel ID.

[0040] The "mean vector" and "covariance matrix" refer to the mean vector and covariance matrix that correspond to the parameters when expressing the point group 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 the voxel data VD will be described.

[0044] Scan matching using NDT assuming a ship is based on the estimated parameters, which are the amount of movement in the horizontal plane (here, xy coordinates) and the direction 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-location estimation unit 15 searches for voxel data VD 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 average vector μ n and the covariance matrix V n Using the above, the evaluation function value of voxel n (also called the "individual evaluation function value") "E nIn this case, the self-position estimation unit 15 calculates an individual evaluation function value E 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, detection of obstacles and undertow by the obstacle / undertow detection unit 16 will be described. The obstacle / undertow detection unit 16 uses the water surface height calculated by processing up to one time before to detect obstacles and undertow. If there is an obstacle near the ship, it is necessary to navigate while avoiding collision or contact with the obstacle. Obstacles include other ships, piles, bridge piers, buoys, nets, garbage, etc. Also, if there is a undertow caused by another ship nearby, care must be taken when navigating the ship so that it does not become too rocky due to the influence of the undertow. Therefore, the obstacle / undertow detection unit 16 uses the water surface height to detect the presence of an obstacle or undertow near the ship.

[0053] (4-1) Estimation of water surface position FIG. 5 is a diagram for explaining the water surface height as seen by the LIDAR 3. The draft position of a ship changes depending on the number of passengers and the amount of cargo. In other words, the height to the water surface as seen by 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 by 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 by the LIDAR 3 is high. Therefore, by setting a search range having 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 for explaining the water surface reflection of light emitted from the LIDAR 3. Part of the light emitted from the LIDAR 3 facing downward may be reflected by the water surface and return to the LIDAR 3. Now, as shown in FIG. 6(A), it is assumed 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 on 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, returned directly to the LIDAR 3 and received, and is one type of water surface reflected light (hereinafter also referred to as "direct water surface reflected light"). Beam 103 is light emitted from the LIDAR 3, which is reflected by the water surface and hits a cliff or the like, causing part of the scattered light to be reflected again by the water surface, which returns to the LIDAR 3 and is received; it is one type of water surface reflected light (hereinafter also referred to as "indirect water surface reflected light"). Since the 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 the 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 for explaining point cloud data used to estimate the water surface height (hereinafter also referred to as the "water surface position"). FIG. 7(A) is a diagram of the ship seen from the rear, and FIG. 7(B) is a diagram of the ship seen from above. Around the ship, due to the fluctuation of the water surface, the beam from the LIDAR 3 sometimes becomes almost perpendicular to the water surface, and direct water surface reflection light such as the beam 102 described above occurs. 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, and indirect water surface reflection light such as the beam 103 described above occurs. Therefore, the obstacle / wake detection unit 16 obtains multiple point cloud data of direct water surface reflection light near the ship and averages the z coordinate values ​​to estimate the water surface position. Since the ship floats on the water, the amount of sinking into the water changes depending on the number of passengers and the amount of cargo, and the height from the LIDAR 3 to the water surface changes. Therefore, by the above method, it is possible to always 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 ship 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 and a pier) stored in the map DB 10. The shore may also be a ground location or a structure other than a docking location. By using the point cloud data measured at a position far from the shore, the point cloud data of indirect water surface reflection light can be excluded.

[0057] In addition, 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, a method for detecting an obstacle will be described. FIG. 8 is a diagram for explaining the method for detecting an obstacle. 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 the height near the water surface position. As shown in FIG. 8(A), when 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 the cluster is an obstacle candidate. The obstacle / undertow detection unit 16 similarly detects clusters in frames at multiple times, and when it detects clusters of similar size at each time, it determines that the cluster is some kind of obstacle.

[0059] In addition, 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 the LIDAR 3, reflected by the buoy and returned to the LIDAR 3. On the other hand, beam 112 is emitted from the LIDAR 3, reflected by the water surface and hits the buoy, and part of the scattered light is reflected again by the water surface, returns to the LIDAR 3 and is received. In the case of a small obstacle such as a buoy, the amount of data reflected directly from the buoy as beam 111 is small, so by including data of the component reflected by the water surface like beam 112, the amount of data used for analysis is increased and utilized for clustering. This increases the amount of data to be subjected to clustering processing, improving the performance of clustering.

[0060] When the obstacle / undertow detection unit 16 determines that a 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 for explaining 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 the height near the water surface position as a point cloud on a two-dimensional plane, ignoring the z coordinate. As shown in FIG. 9(A), when a "straight line" is detected by the Hough transform process, the obstacle / undertow detection unit 16 provisionally determines that the straight line is a undertow candidate. The obstacle / undertow detection unit 16 similarly detects straight lines in frames at multiple times, and when 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 water surface reflection component can also be valuable information from the viewpoint of detection. In FIG. 9(B), beam 113 is emitted from the lidar 3, reflected by the backwash, and returned to the lidar 3. On the other hand, beam 114 is emitted from the 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 the 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 by the water surface like beam 114, the amount of data used for analysis is increased and used for the Hough transform. This increases the amount of data to be subjected to the Hough transform process, improving the performance of the Hough transform.

[0063] After determining whether or not there is an undertow using the two-dimensional data as described above, the obstacle / undertow detection unit 16 re-evaluates the z coordinates of the points determined to be part of the 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 calculates the height Hw of the undertow from the water surface by subtracting the water surface position from the average z coordinate.

[0064] (4-4) Example of Obstacle / Rush Wave 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, making it easier to perform 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 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 straight 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, a water surface position estimation block 132, and a display control unit 133.

[0066] The search range setting block 121 extracts point cloud data of the direct water surface reflection light from the input point cloud data, and sets the 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 the search range set with the water surface position as the center as shown in Fig. 5(C), but if the ship is rocking heavily or the waves are large, obstacles and backwash floating on the water surface may fall outside the search range and may not be detected. On the other hand, if the search range is enlarged to avoid this, irrelevant data may enter when the waves are small, reducing the detection accuracy.

[0067] Therefore, the search range setting block 121 obtains 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 the 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. When 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 FIG. 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 makes it possible to reduce the inclusion of noise, thereby improving the detection accuracy of obstacles and backwash.

[0068] On the other hand, when 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 Fig. 11(B). Therefore, the search range setting block 121 widens the search range. In other words, the search range setting block 121 sets a search range that is wider 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, 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, as shown in Fig. 11(C). This makes it possible to widen the search range even when the waves are high, thereby preventing obstacles and backwash from being overlooked. 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 straight 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 straight line to the wake detection block 123. Since a discretized two-dimensional array is used to detect a straight line using the Hough transform, the straight line obtained as a result is an approximation. Therefore, the line extraction block 122 and the wake detection block 123 obtain a more accurate straight line using the following procedure. (Step 1) Calculate an approximate straight line using the Hough transform. (Step 2) Extract data whose distance to the approximation line is within a predetermined threshold (straight-line distance threshold). (Step 3) Principal component analysis is performed using the multiple 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 so 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 straight 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, first, the distance between every target point and every other point is calculated, and points whose inter-point distance is shorter than a predetermined value (hereinafter referred to as the "grouping threshold") are placed in the same group. Next, of each group, groups that contain points with a predetermined score or more (hereinafter referred to as the "score threshold") are regarded as clusters. Note that groups with a low score are not regarded as clusters, since they are likely to be noise.

[0075] FIG. 13 shows an example of Euclidean clustering. FIG. 13(A) shows a number of points to be subjected to Euclidean clustering. The inter-point distances shown in FIG. 13(A) were calculated and grouped by comparing with a grouping threshold. Since the distances shown by the arrows in FIG. 13(B) are greater than the grouping threshold, five groups A to E shown 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" here), and only groups A and C containing more points than the score threshold were ultimately determined to be clusters, as shown in FIG. 13(C).

[0076] Fig. 14 shows the simulation results of Euclidean clustering. Fig. 14(A) shows the simulation results when backwash data remains during Euclidean clustering. In Euclidean clustering, when performing group discrimination 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 Fig. 14(A), the backwash and the obstacle are close to each other, so the data for the obstacle and backwash are in the same group, 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. In order 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, since the light beam of a lidar is output radially, the further away the data is, the greater the distance between the positions of the data. Therefore, as shown in FIG. 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 detection points will be greater if they are close, and the number of detection points will be smaller if they are far away. Therefore, in the Euclidean clustering process, by setting a grouping threshold and a score 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 the lidar and objects far away from it.

[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] FIG. 16(A) shows Grouping threshold = 2.0m Score threshold = 6 points The simulation results are shown below.

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

[0082] As can be seen by comparing Fig. 16(A) and Fig. 16(B), in addition to cluster 1 located near the ship, cluster 2 located far from the ship can also be detected in Fig. 16(B). In cluster 2, the distance between each data is about 3 m, but the grouping threshold calculated using the distance from the ship to the data is about 4.5 m, so the distance is closer than the threshold and the cluster is grouped together. Also, the number of data points is 4, but the score threshold calculated using the distance to the center of gravity of the group is about 3.2, so the number of points is higher than the threshold and the cluster is determined to be a cluster. Note that, using the above formula, the grouping threshold calculated using the distance from the ship to the data is about 2.5 m, and the score threshold is about 7.1, so it can be seen that there is no significant change from the fixed value in Fig. 16(A). In this way, by setting the adaptive threshold, it is possible to prevent clusters from being undetected or erroneously detected as much as possible, and the performance of obstacle detection can be improved.

[0083] The obstacle detection block 127 outputs point cloud data (hereinafter referred to as "obstacle data") indicating obstacles detected by Euclidean clustering to an obstacle information calculation block 128 and an obstacle data removal block 129. The obstacle information calculation block 128 calculates the position, distance, angle, size and height of an obstacle based on the vessel's own position based on the obstacle data, and outputs it 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 waves or obstacles, the water surface position cannot be estimated correctly.

[0085] FIG. 17(A) shows direct water surface reflection data obtained when there is a wake or an obstacle around the ship. In this case, data at a position higher than the water surface and indirect water surface reflection light due to obstacles or wake (beam 112 in FIG. 8(B), beam 114 in FIG. 9(B), etc.) are factors of error in estimating the water surface position. Therefore, the wake data removal block 125 and the obstacle data removal block 129 use the search data after removing the wake and obstacles as shown in FIG. 17(B) to estimate the water surface position. Specifically, as shown in FIG. 18, from state 1 where there are wakes and obstacles around the ship, the wake is detected and removed as shown in state 2, resulting in state 3. Next, the obstacle is detected and removed as shown in state 4, and direct water surface reflection data not including the wake or obstacles is obtained as shown in state 5.

[0086] Specifically, the mean / variance calculation block 130 calculates the mean value and variance value of the z coordinate values ​​of 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 the past water surface position. The water surface position estimation block 132 estimates the water surface position using the mean value of the z coordinate values ​​after the averaging or filtering process, and the variance of the z coordinate values ​​of the search data.

[0087] When estimating the water surface position, if the variance value of the direct water surface reflection data around the ship is large, it can be expected that the waves are high due to the passage of other ships, or that there is a floating object that was not detected as an obstacle. Therefore, if the variance value 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 value is equal to or greater than 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, for example, twice the average value of the variance values. Then, the water surface position estimation block 132 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, while backwash and obstacles are detected.

[0088] The display control unit 133 is composed of, for example, a liquid crystal display device, and displays information about the surroundings of the ship on the display device 17 based on the backwash information calculated by the backwash information calculation block 124 and the obstacle information calculated by the obstacle information calculation block 128.

[0089] FIG. 19(A) shows an example of the display of the surrounding information when there is an obstacle around the ship. The surrounding information is displayed on the display screen of the display control unit 133. The surrounding information basically shows a schematic state of a range of a predetermined distance from the ship as seen from the sky. The display control unit 133 first displays the ship 80 near the center of the display screen of the display device 17. The display control unit 133 also specifies the positional relationship between the ship and the obstacle based on the position, moving speed, moving direction, etc. of the obstacle detected by the obstacle / wake detection unit 16, and displays the obstacle 82 on the display screen so as to show the specified positional relationship. In the example of FIG. 19(A), the display control unit 133 displays the point cloud (measurement points) 81 constituting the obstacle, and also displays the obstacle 82 as a figure surrounding the point cloud 81. At this time, the figure showing the obstacle 82 may be highlighted to emphasize the presence of the obstacle 82 by applying a conspicuous color or blinking the display. It is also possible to display only the detected obstacle 82 without displaying the point cloud 81.

[0090] Furthermore, the display control unit 133 displays, as the surrounding information, information indicating the relative positional relationship between the ship and the obstacle (hereinafter also referred to as "positional relationship information"). Specifically, an arrow 84 indicating the traveling direction of the obstacle 82 is displayed, and the moving speed (v = 0.13 [m / s]) of the obstacle 82 is displayed near the arrow 84. Furthermore, a straight line 85 indicating the direction of the obstacle 82 based on the ship 80 is displayed, and the distance (d = 2.12 [m]) between the ship 80 and the obstacle 82 is displayed near the straight line 85. Furthermore, the width (w = 0.21 [m]) of the obstacle 82 and the height (h = 0.15 [m]) of the obstacle 82 are displayed near the obstacle 82.

[0091] Here, the display control unit 133 changes the display mode of the positional relationship information according to the danger level of the obstacle to the ship. Basically, the higher the danger level, the higher the display mode in which the display control unit 133 displays the positional relationship information, that is, in a display mode that attracts the pilot's attention. Specifically, the closer the obstacle 82 is and the faster the moving speed of the obstacle 82 is, the more emphasized the arrow 84 and the numbers indicating the moving speed are displayed by the display control unit 133. For example, the arrow 84 is made thicker and the size of the numbers indicating the moving speed is increased. In addition, the color of the arrow 84 and the numbers indicating the moving speed may be made conspicuous or may be made to blink. In this case, the moving directions of the ship 80 and the obstacle 82 may be taken into consideration, and when the obstacle 82 is moving in a direction approaching the ship 80, the arrow 84 and the numbers indicating the moving speed may be emphasized as described above, and when the obstacle 82 is moving in a direction away from the ship 80, the arrow 84 and the numbers indicating the moving speed may not be emphasized. Furthermore, the display control unit 133 emphasizes the straight line 85 and the numbers indicating the distance to the obstacle as the distance between the ship 80 and the obstacle 82 becomes closer. For example, the display control unit 133 makes the straight line 85 thicker and increases the size of the numbers indicating the distance to the obstacle. Furthermore, the straight line 85 and the numbers indicating the distance to the obstacle 82 may be displayed in a conspicuous color or may be made to flash. This makes it possible to intuitively convey the degree of danger posed by the obstacle 82 to the pilot.

[0092] In the above example, the display control unit 133 displays the positional relationship information in a display mode with a higher emphasis level as the risk level increases, but instead, the risk level may be classified into a plurality of stages using a threshold value. For example, the display control unit 133 may classify the risk level into two stages using one threshold value. In that case, the display control unit 133 displays the positional relationship information in two display modes with different emphasis levels. Alternatively, the display control unit 133 may classify the risk level into three or more stages and display the positional relationship information in a display mode with an emphasis level corresponding to each stage.

[0093] Fig. 19(B) shows an example of the display of surrounding information when there is an undertow around the ship. In the example of Fig. 19(B) as well, the display control unit 133 displays a point cloud (measurement points) 81 that constitutes an obstacle, and highlights an undertow 86 as a figure surrounding the point cloud 81. It is also possible to display only the detected undertow 86 without displaying the point cloud 81.

[0094] In the example of FIG. 19(B), an arrow 84 indicating the direction of travel of the undertow 86 is displayed as the surrounding information, and the movement speed (v=0.41 [m / s]) of the undertow 86 is displayed near the arrow 84. Furthermore, a straight line 85 indicating the direction of the obstacle 82 relative to the ship 80 is displayed, and the distance (d=4.45 [m]) between the ship 80 and the obstacle 82 is displayed near the straight line 85. Note that in the example of FIG. 19(B) as well, the display control unit 133 displays the positional relationship information in a display mode with a higher emphasis level as the level of danger increases. Here, since the movement direction of the undertow 86 is toward the ship 80, the arrow 84 is made thicker and the size of the number indicating the movement speed is made larger.

[0095] Furthermore, in the case of backwash, the display control unit 133 displays the angle (θ=42.5[deg]) of the backwash 86 as seen from the ship itself. The angle of the backwash 86 is the angle between the traveling direction of the ship 80 and the direction in which the backwash 86 extends. In general, it is said that if the ship enters the backwash at an angle of about 45 degrees, the impact and rocking caused to the ship will be reduced. Therefore, the angle of the backwash 86 may be displayed to guide the pilot so that the ship can overcome the backwash at an angle that reduces the impact and rocking. Also, instead of displaying the angle of the backwash 86 relative to the ship 80, a sector showing a range of about 45 degrees relative to the backwash may be displayed to guide the pilot so that the ship enters the backwash within that angle range.

[0096] Furthermore, in the case of an undertow, the display control unit 133 displays the height of the undertow 86 (h=0.23 [m]) near the undertow 86. In this case, the higher the undertow, the larger the size of the number indicating the height of the undertow. Also, the height of the undertow may be indicated by the color of the displayed undertow 86, for example by changing the color of the undertow 86 (i.e., the shape indicating the undertow) to a color closer to red depending on the height of the undertow.

[0097] (4-5) Obstacle / undertow detection processing Next, a description will be given of the obstacle / wake detection process executed by the obstacle / wake detection unit 16. Fig. 20 is a flowchart of the obstacle / wake detection process. This process is realized by the controller shown in Fig. 2 executing a program prepared in advance and operating as each element shown in Fig. 10.

[0098] 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 σ, the search range setting block 121 determines Search range = Estimated water surface position ±3σ Let us assume that. 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).

[0099] Next, the obstacle / backwash detection unit 16 executes backwash detection processing (step S14). FIG. 21 is a flowchart of the backwash detection processing. First, the line extraction block 122 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 formula (7) (step S102). In order to create a discretized two-dimensional array with (θ,ρ) as elements, "θ" and "ρ" are expressed as integers.

[0100]

number

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

[0102] Next, the backwash detection block 123 again detects the n straight lines L1 to L2 generated for all the search points. n The distance to the target point is calculated, and any distance less than a predetermined distance is regarded as backwash data (step S105). Next, the backwash detection block 123 regards the above backwash data as three-dimensional data including z-values ​​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. 20.

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

[0104] Next, the obstacle / undertow detection unit 16 executes the obstacle detection process (step S16). FIG. 23 is a flowchart of the obstacle detection process. First, the Euclidean clustering block 126 calculates the inter-point distances between all search data and all other search data (step S111). If the number of search data is n, 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 the grouping threshold T1 using a predetermined coefficient a (step S113). For example, T1=a·r1. That is, the grouping threshold T1 is a different value for each target data.

[0105] Next, the Euclidean clustering block 126 groups data whose inter-point distance with the target data is smaller than the grouping threshold T1 (step S114). Next, the Euclidean clustering block 126 determines whether or not 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.

[0106] 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 score threshold T2 is set using a predetermined coefficient b (step S117). For example, T2=b / r2. That is, the score threshold T2 is a different value for each group.

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

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

[0109] Next, the obstacle / wake detection unit 16 executes a water surface position estimation process (step S18). FIG. 24 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 reached, obtains the average value and variance value in the z direction of the data (step S122).

[0110] Next, the average / variance calculation block 130 judges 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 position, 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. 20.

[0111] Next, the obstacle / backwash detection unit 16 executes backwash information calculation processing (step S19). FIG. 25(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 straight 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 straight line, and sets this as the angle of the backwash (step S131).

[0112] As shown in Figure 25(B), the shortest distance from the ship's own position to the straight line is the distance to the foot of the perpendicular line dropped to the straight line. However, since the straight line detected as the undertow is a line segment, the end point of the data detected as the undertow may be the shortest distance, as shown in Figure 25(C). Therefore, the undertow 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 end point of the line segment is the shortest distance.

[0113] Next, the backwash information calculation block 124 calculates the average of the z coordinate values ​​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 value of the z coordinate values, the maximum value may be used as the height of the backwash. Then, the process returns to the main routine in FIG. 20.

[0114] Next, the obstacle / undertow detection unit 16 executes an obstacle information calculation process (step S20). FIG. 26 is a flowchart of the obstacle information calculation process. First, the obstacle information calculation block 128 uses the ship's own position as a reference, 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).

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

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

[0117] Next, the display control unit 133 performs a process of displaying the backwash information on a screen (step S23). Fig. 27 is a flowchart of the process of displaying the backwash information on a screen. The display control unit 133 performs this process every time it acquires backwash information.

[0118] First, the display control unit 133 acquires backwash information from the backwash information calculation block 124, and acquires the position p, distance d, angle θ, and height h. The display control unit 133 also obtains the difference from the previously acquired backwash position, and calculates the relative velocity v and its vector (step S151).

[0119] Next, the display control unit 133 judges whether the speed vector is in the direction of the ship (step S152). If the speed vector is not in the direction of the ship (step S152: No), the display control unit 133 changes all font sizes and line widths of lines and frames to the normal size s min The positional relationship information is then displayed on the display screen of the display device 17 (step S156). Then, the process of displaying the backwash information on the screen is completed.

[0120] On the other hand, if the speed vector is in the direction of the ship itself (step S152: Yes), the display control unit 133 increases the emphasis parameters s1 to s4 and S for each value of the positional relationship information. Specifically, the larger the relative speed v, the larger s1 is made, the smaller the distance d, the larger s2 is made, the larger the height h, the larger s3 is made, and the larger the angle θ' (=|θ-45°|) is, the larger s4 is made. Also, S = s1 + s2 + s3 + s4 is set (step S153).

[0121] 28(A) is a diagram for explaining the emphasis parameter s. The emphasis parameter s is calculated according to the values ​​of the variables (v, d, h, θ') on the horizontal axis within a range between a preset lower limit value (normal size) and an upper limit value (maximum size). Note that a and b are set for the variables v, d, h, and θ', respectively.

[0122] Next, the display control unit 133 displays the numerical values ​​of the variables v, d, h, and θ' on the display screen, using the values ​​of the emphasis parameters s1 to s4 as the font size. The display control unit 133 also draws an arrow 84 of the relative speed v on the screen, using the emphasis parameter s1 as the line width. At this time, the length of the arrow 84 corresponds to the value of the relative speed v. The display control unit 133 also draws a straight line 85 from the ship to the position of the backwash, using the emphasis parameter s2 as the line width. The display control unit 133 also draws a frame 86 surrounding the backwash data, using the emphasis parameter S as the line width (step S154).

[0123] Next, when the values ​​of the emphasis parameters s1 to s4 exceed the respective predetermined thresholds, the display control unit 133 further blinks the font, straight line, or frame line (step S155). Then, the screen display process of the backwash information ends, and the process returns to the main routine of FIG.

[0124] Next, the obstacle / wake detection unit 16 determines whether a similar obstacle has been detected in multiple frames (step S24). 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 that the obstacle is the same. If a similar obstacle is not detected (step S24: No), the process ends. On the other hand, if a similar obstacle is detected (step S24: Yes), the obstacle information calculation block 128 determines that the data is an obstacle and outputs the obstacle information to the hull system (step S25).

[0125] Next, the display control unit 133 performs a process of displaying the obstacle information on the screen (step S26). Fig. 29 is a flowchart of the process of displaying the obstacle information on the screen. The display control unit 133 performs this process every time the obstacle information is acquired.

[0126] First, the display control unit 133 acquires obstacle information from the obstacle information calculation block 128, and acquires the position p, distance d, size w, and height h. In addition, the display control unit 133 obtains the difference from the previously acquired obstacle position, and calculates the relative speed v and its vector (step S161).

[0127] Next, the display control unit 133 judges whether the speed vector is in the direction of the ship (step S162). If the speed vector is not in the direction of the ship (step S162: No), the display control unit 133 changes all font sizes and line widths of lines and frames to the normal size s min The positional relationship information is then displayed on the display screen (step S166), and the process of displaying obstacle information on the screen is then completed.

[0128] On the other hand, if the speed vector is in the direction of the ship itself (step S162: Yes), the display control unit 133 increases the emphasis parameters s1 to s4 and S for each value of the positional relationship information. Specifically, the larger the relative speed v, the larger s1 is made, the smaller the distance d, the larger s2 is made, the larger the height h, the larger s3 is made, and the larger the size w, the larger s4 is made. Also, S = s1 + s2 + s3 + s4 is set (step S163).

[0129] 28B is a diagram explaining the emphasis parameter s. The emphasis parameter s is calculated according to the values ​​of the variables (v, d, h, w) on the horizontal axis within a range between a preset lower limit value (normal size) and an upper limit value (maximum size). Note that a and b are set for the variables v, d, h, and w, respectively.

[0130] Next, the display control unit 133 displays the numerical values ​​of the variables v, d, h, and w on the display screen, using the values ​​of the emphasis parameters s1 to s4 as the font size. The display control unit 133 also draws an arrow 84 of the relative speed v on the screen, using the emphasis parameter s1 as the line width. At this time, the length of the arrow 84 corresponds to the value of the relative speed v. The display control unit 133 also draws a straight line 85 from the ship to the position of the obstacle, using the emphasis parameter s2 as the line width. The display control unit 133 also draws a frame 82 surrounding the obstacle data, using the emphasis parameter S as the line width (step S164).

[0131] Next, when the values ​​of the emphasis parameters s1 to s4 exceed the respective predetermined thresholds, the display control unit 133 further blinks the font, the straight line, or the frame line (step S165). Then, the process of displaying the obstacle information on the screen ends, and the obstacle / undertow detection process in FIG. 20 also ends.

[0132] (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 Fig. 30, 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 also becomes 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, and obtain the starboard side water surface position and the port side water surface position 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 is reduced, thereby making it possible to estimate the water surface position without separating the starboard and port sides.

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

[0134] 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, go back to step 2. When the equation of the line stops changing, use it as the line of the undertow.

[0135] The graph on the left side of Figure 31 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 that has converged after performing step 4. By adding step 4, there is no omission of backwash data from extraction, which results in improved accuracy of the straight line.

[0136] Although the present invention has been described above with reference to the embodiments, the present invention is not limited to the above embodiments. Various modifications that can be understood by those skilled in the art can be made to the configuration and details of the present invention within the scope of the present invention. In other words, the present invention naturally includes various modifications and corrections that a person skilled in the art could make in accordance with the entire disclosure, including the claims, and the technical ideas. In addition, the disclosures of the above cited patent documents and the like are incorporated herein by reference. [Explanation of symbols]

[0137] 1. Information processing device 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 133 Display control section

Claims

[Claim 1] an object detection means for detecting an object based on point cloud data generated by a measuring device installed on the ship; a positional relationship acquisition means for acquiring a relative positional relationship between the object and the ship; a display control means for displaying information regarding the positional relationship on a display means in a display mode corresponding to the positional relationship; An information processing device comprising:

Images