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

Abstract

To provide an information processing device capable of suitably obtaining accurate information on berthing.SOLUTION: A controller 13 of an information processing device 1 obtains measurement data of a berthing location that is generated by a lidar 3 that is a measurement device provided on a target ship. In addition, the controller 13 calculates a normal vector with respect to the berthing location, based on the measurement data; and calculates a distance to a shore between the target ship and the berthing location, based on the normal vector.SELECTED DRAWING: Figure 3

Description

[Technical Field]

[0001] The present disclosure relates to ship docking procedures. [Background technology]

[0002] Conventionally, there have been known technologies for providing support for docking (berthing) of ships. For example, Patent Document 1 describes a method for controlling an automatic docking device that automatically docks a ship by changing the attitude of the ship so that light emitted from a lidar is reflected by objects around the docking position and can be received by the lidar. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2020-59403 Summary of the Invention [Problem to be solved by the invention]

[0004] When providing advanced berthing assistance such as automatic berthing of a ship at a berthing location, it is necessary to accurately grasp the distance to the berthing location, etc. However, cited document 1 does not disclose a method for calculating the accurate distance to the berthing location, etc.

[0005] The present disclosure has been made to solve the above-mentioned problems, and a main object of the present disclosure is to provide an information processing device that can suitably acquire accurate information regarding docking. [Means for solving the problem]

[0006] The claimed invention is an acquisition means for acquiring measurement data of a docking location generated by a measurement device provided on the ship; a first calculation means for calculating a normal vector relative to the docking location based on the measurement data; a second calculation means for calculating a distance from the ship to the docking location based on the normal vector; The information processing device has the following.

[0007] The claimed invention also includes: A computer-implemented control method comprising: Acquire measurement data of the docking location generated by a measuring device installed on the ship, Calculating a normal vector to the docking location based on the measurement data; calculating a distance from the ship to the docking location based on the normal vector; It is a control method.

[0008] The claimed invention also includes: Acquire measurement data of the docking location generated by a measuring device installed on the ship, Calculating a normal vector to the docking location based on the measurement data; The program causes a computer to execute a process of calculating the distance from the ship to the docking location based on the normal vector. [Brief explanation of the drawings]

[0009] [Figure 1] FIG. 1 is a schematic configuration diagram of a driving assistance system. [Figure 2] FIG. 2 is a block diagram showing a hardware configuration of the information processing device. [Figure 3] FIG. 2 is a functional block diagram relating to a docking assistance process. [Figure 4] (A) An example of a hull coordinate system based on the hull of the target ship is shown. (B) A perspective view of a structure with the normal vector clearly indicated. [Figure 5] This is a diagram showing a docking situation in which both the top and side surfaces of the structure at the docking location are included as the inner field of view and detection surfaces. [Figure 6](A) A diagram showing a docking situation where the inner field of view is the top and side surfaces, but the detection surface is only the side surfaces. (B) An example of the data array generated by the lidar in one scanning cycle. (C) A diagram showing a docking situation where the inner field of view is the top and side surfaces, but the detection surface is only the top surface. [Figure 7] (A) A diagram showing a docking situation where the inner field of view and the detection surface are both only the side, and (B) A diagram showing a docking situation where the inner field of view and the detection surface are both only the top. [Figure 8] FIG. 1 is a perspective view of a structure showing neighboring points and nearest neighboring points. [Figure 9] FIG. 10 is a diagram illustrating an outline of a method for calculating the distance to the opposite shore. [Figure 10] FIG. 10 is a diagram illustrating an outline of a method for calculating an average normal vector. [Figure 11] FIG. 10 is a diagram showing an outline of a method for calculating a docking speed. [Figure 12] FIG. 10 is a diagram illustrating an outline of a method for calculating an approach angle. [Figure 13] 10 shows an example of a data structure of reliability information. [Figure 14] 10 is an example of a flowchart illustrating an outline of a docking assistance process. [Figure 15] FIG. 10 is a block diagram illustrating an example of the configuration of an information processing device according to a second embodiment. [Figure 16] 10 is an example of a functional block of a self-position estimation unit in the second embodiment. [Figure 17] FIG. 11 is a diagram illustrating an outline of how a normal vector is obtained in the third embodiment. [Figure 18] 13 is an example of a functional block of a self-position estimation unit in the fourth embodiment. [Figure 19] FIG. 13 is a diagram illustrating an outline of how a normal vector is obtained in the fourth embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0010] According to a preferred embodiment of the present disclosure, an information processing device includes an acquisition means for acquiring measurement data of a docking location generated by a measurement device provided on a ship, a first calculation means for calculating a normal vector to the docking location based on the measurement data, and a second calculation means for calculating a distance from the ship to the docking location based on the normal vector. According to this aspect, the information processing device can accurately calculate the distance to the docking location based on the normal vector of the docking location calculated based on the measurement data.

[0011] In one aspect of the information processing device, the second calculation means calculates the distance to the shore based on a normal vector of a side surface of the docking location. This aspect enables the information processing device to suitably calculate the distance to the shore.

[0012] In another aspect of the information processing device, the measurement devices include at least a first measurement device and a second measurement device, the first calculation means calculates a first normal vector that is the normal vector based on the measurement data generated by the first measurement device and a second normal vector that is the normal vector based on the measurement data generated by the second measurement device, and the second calculation means calculates the opposite bank distance based on the first normal vector and the second normal vector. This aspect allows the information processing device to preferably calculate the opposite bank distance based on the normal vectors based on the measurement data of multiple measurement devices.

[0013] In another aspect of the information processing device, when a difference between the first normal vector and the second normal vector is less than a threshold, the second calculation means calculates the opposite-shore distance based on an integrated normal vector obtained by integrating the first normal vector and the second normal vector, and when a difference between the first normal vector and the second normal vector is equal to or greater than the threshold, calculates the opposite-shore distance for the first normal vector and the second normal vector, respectively. This aspect allows the information processing device to appropriately calculate the opposite-shore distance in both cases where it is preferable to calculate the opposite-shore distance for each measurement device and where it is preferable to calculate one common opposite-shore distance.

[0014] In another aspect of the information processing device, the second calculation means calculates the integrated normal vector by weighting the average of the first normal vectors and the average of the second normal vectors based on the number or variance of each of the first normal vectors and the second normal vectors. This aspect allows the information processing device to suitably calculate a normal vector obtained by integrating the normal vectors calculated for each measurement device.

[0015] In another aspect of the information processing device, the first calculation means calculates the normal vector based on data that is downsampled after removing water surface reflection data from the measurement data using the water surface position as a reference. This aspect allows the information processing device to suitably calculate the normal vector to the docking location.

[0016] In another aspect of the information processing device, the second calculation means calculates, as the opposite shore distance, the shortest distance from the measurement device to the docking location, or the shortest distance from a reference position of the ship to the docking location.

[0017] In another aspect of the information processing device, the second calculation means calculates the approach angle of the ship with respect to the docking location based on at least one of the distance to the shore or the normal vector. In another aspect of the information processing device, the second calculation means calculates the docking speed of the ship to the docking location based on a change in the distance to the shore over time. According to these aspects, the information processing device can preferably obtain information other than the distance to the shore required for the ship to dock.

[0018] In another aspect of the information processing device, the device further includes a self-position estimation means for estimating the position of the ship based on the measurement data, and the first calculation means calculates the normal vector based on the result of the position estimation and map data related to the docking location. This aspect enables the information processing device to suitably obtain an accurate normal vector for the docking location based on the map data. In a preferred example, the map data includes three-dimensional data of the docking location. In another preferred example, the map data includes information related to the position and orientation of a reference object provided at the docking location.

[0019] According to another preferred embodiment of the present disclosure, there is provided a control method executed by a computer, which includes acquiring measurement data of a docking location generated by a measurement device provided on a ship, calculating a normal vector to the docking location based on the measurement data, and calculating a distance from the ship to the docking location based on the normal vector. By executing this control method, the computer can accurately calculate the distance to the docking location based on the normal vector of the docking location.

[0020] According to another preferred embodiment of the present disclosure, there is provided a program for causing a computer to execute a process of acquiring measurement data of a docking location generated by a measurement device provided on a ship, calculating a normal vector to the docking location based on the measurement data, and calculating a distance from the ship to the docking location based on the normal vector. By executing this program, the computer can accurately calculate the distance to the docking location based on the normal vector of the docking location. Preferably, the program is stored in a storage medium. [Example]

[0021] 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).

[0022] <First Example> (1) Overview of the driving assistance system Figure 1 shows a schematic configuration of a driving assistance system according to a first 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."

[0023] The information processing device 1 is electrically connected to the sensor group 2, and performs operational support such as automatic operation control of the target ship on which the information processing device 1 is installed, based on the outputs of various sensors included in the sensor group 2. The operational support also includes berthing support such as automatic berthing (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. In addition, hereinafter, "docking location" is a general term for structures such as quays and piers that are the target for docking. The information processing device 1 may be a navigation device installed on the target ship, or an electronic control device built into the ship.

[0024] The sensor group 2 includes various external and internal sensors provided on the target ship. In this embodiment, the sensor group 2 includes at least a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 3.

[0025] 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 based on the light receiving signal output by the light receiving unit. Data measured for each direction (scanning position) of laser light irradiation (also referred to as a "measurement point" or "measurement point data") is 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 the LIDAR 3 is not limited to the above-mentioned scan-type LIDAR, but may also be a flash-type LIDAR that generates three-dimensional data by irradiating a diffused laser beam within the field of view of a two-dimensional array sensor. The LIDAR 3 is an example of a "measurement device" in the present invention.

[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 and supplies it 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 may include a drive source such as an engine or an 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 a hardware interface for connecting to an external device via a cable or the like. 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 information necessary for the processing executed by the information processing device 1 in this embodiment. For example, the memory 12 may store map data including information about the position of a docking location. In another example, the memory 12 stores information about the 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 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, etc., to perform processing related to operational support for the target ship, etc.

[0031] The controller 13 also functionally includes a berthing location detection unit 15 and a berthing parameter calculation unit 16. The berthing location detection unit 15 performs processing related to the detection of a berthing location based on the point cloud data output by the LIDAR 3. The berthing parameter calculation unit 16 calculates parameters (also referred to as "berthing parameters") required for berthing at the berthing location. The berthing parameters include the distance to the berthing location (distance from the berthing location), the approach angle to the berthing location, and the speed at which the vehicle approaches the berthing location (berthing speed). The berthing parameter calculation unit 16 also calculates information (also referred to as "reliability information") indicating the reliability of berthing at the berthing location based on the processing result of the berthing location detection unit 15 and the berthing parameters. The controller 13 functions as an "acquisition means," a "first calculation means," a "second calculation means," a "self-position estimation means," a computer that executes programs, etc.

[0032] The processes executed by the controller 13 are not limited to being realized by software programs, but may be realized by any combination of hardware, firmware, and software. Furthermore, the processes executed by the controller 13 may be realized by using a user-programmable integrated circuit, such as an FPGA (Field-Programmable Gate Array) or a microcomputer. In this case, the programs executed by the controller 13 in this embodiment may be realized by using this integrated circuit.

[0033] (3) Berthing support processing Next, we will explain the docking assistance process executed by the information processing device 1. In summary, the information processing device 1 calculates a normal vector of the side of the docking location based on point cloud data of the lidar 3 measured in the direction in which the docking location exists, and calculates docking parameters such as the distance to the opposite shore based on the calculated normal vector.

[0034] (3-1) Functional Blocks 3 is a functional block diagram of the docking location detection unit 15 and the docking parameter calculation unit 16 related to the docking assistance process. The docking location detection unit 15 functionally comprises a normal vector calculation block 20, a field of view / detection plane specification block 21, a normal number specification block 22, a mean / variance calculation block 23, and a docking situation determination block 24. The docking parameter calculation unit 16 functionally comprises a neighboring point search block 25, a nearest neighbor determination block 26, a docking distance calculation block 27, an approach angle calculation block 28, a docking speed calculation block 29, and a reliability information generation block 30.

[0035] The normal vector calculation block 20 calculates the normal vector of the plane formed by the docking location (also called the "docking surface") based on point cloud data generated by the LIDAR 3 in the direction in which the docking location is located. In this case, the normal vector calculation block 20 calculates the above-mentioned normal vector based on point cloud data generated by the LIDAR 3, for example, whose measurement range includes the docking side of the target ship. Information regarding the measurement range of the LIDAR 3 and the direction in which the docking location is located may be registered in advance in the memory 12, for example.

[0036] In this case, the normal vector calculation block 20 preferably downsamples the point cloud data and removes data obtained by the laser light reflecting off the water surface (also called "water surface reflection data").

[0037] In this case, the normal vector calculation block 20 first removes data that exists below the water surface position from the point cloud data generated by the LIDAR 3 as water surface reflection data (i.e., false detection data). Note that the normal vector calculation block 20 estimates the water surface position based on, for example, the average value in the height direction of the point cloud data generated by the LIDAR 3 when there are no objects other than the water surface in the vicinity. Then, the normal vector calculation block 20 performs downsampling on the point cloud data after the water surface reflection data has been removed, which is a process of integrating measurement points for each grid space of a predetermined size. Then, for each measurement point indicated by the point cloud data after downsampling, the normal vector calculation block 20 calculates a normal vector using multiple surrounding measurement points. Note that downsampling may be performed before removing the data reflected by the water surface.

[0038] The field of view / detection surface identification block 21 identifies the surface of the docking location that exists within the field of view angle of the lidar 3 (also referred to as the "inner field of view") and the surface of the docking location that is detected based on the normal vector calculated by the normal vector calculation block 20 (also referred to as the "detection surface"). In this case, the field of view / detection surface identification block 21 identifies whether the top surface and / or side surface of the docking location are included as the inner field of view and the detection surface. A specific identification method will be described with reference to Figures 5 to 7.

[0039] The normal number specification block 22 extracts the vertical normal vectors and the normal vectors in the direction perpendicular to the vertical normal vectors (i.e., the horizontal direction) from the normal vectors calculated by the normal vector calculation block 20, and calculates the number of vertical normal vectors and the number of horizontal normal vectors. Here, the normal number specification block 22 regards the vertical normal vectors as normals to the measurement points on the top surface of the docking location, and the horizontal normal vectors as normals to the measurement points on the side surfaces of the docking location, and calculates the respective numbers as an index of the reliability of the docking location.

[0040] The mean / variance calculation block 23 extracts the vertical normal vectors and the normal vectors perpendicular to the vertical normal vectors (i.e., the horizontal direction) from the normal vectors calculated by the normal vector calculation block 20, and calculates the mean and variance of the vertical normal vectors and the mean and variance of the horizontal normal vectors.

[0041] The docking situation determination block 24 acquires the processing results of the field of view / detection plane determination block 21, the number of normals determination block 22, and the mean / variance calculation block 23, which are specified or calculated based on the same point cloud data, as a determination result representing the detection situation of the docking location at the time the point cloud data was generated.The docking situation determination block 24 then supplies the processing results of the field of view / detection plane determination block 21, the number of normals determination block 22, and the mean / variance calculation block 23 to the reliability information generation block 30 as the determination result of the detection situation of the docking location.

[0042] The neighboring point search block 25 detects a predetermined number of neighboring points of the docking location for the target ship from the measurement points that make up the point cloud data by searching for multiple points with short distances. The nearest neighbor determination block 26 performs processing to determine the nearest point from the predetermined number of neighboring points detected by the neighboring point search block 25.

[0043] The shore distance calculation block 27 calculates the shore distance, which corresponds to the shortest distance between the target ship and the berthing location, based on the nearest point determined by the nearest neighbor determination block 26. The approach angle calculation block 28 calculates the approach angle of the target ship with respect to the berthing location, based on the shore distance calculated by the shore distance calculation block 27 or based on the average value of the normal vectors calculated by the mean / variance calculation block 23. The berthing speed calculation block 29 calculates the berthing speed, which is the speed at which the target ship approaches the berthing location, based on the shore distance calculated by the shore distance calculation block 27.

[0044] The reliability information generation block 30 generates reliability information based on the processing results of the docking situation determination block 24, the neighboring point search block 25, the nearest neighbor determination block 26, the distance to the shore calculation block 27, and the approach angle calculation block 28. The reliability information will be described in detail later.

[0045] (3-2) Normal vector operations Next, a specific example of the processing of the normal vector calculation block 20, the normal number specification block 22, and the mean / variance calculation block 23 will be described with reference to FIG.

[0046] FIG. 4(A) shows an example of a hull coordinate system based on the hull of the target ship. As shown in FIG. 4(A), the front (forward) direction of the target ship is the "x" coordinate, the lateral direction of the target ship is the "y" coordinate, and the height direction of the target ship is the "z" coordinate. The measurement data measured by the LIDAR 3 in the coordinate system based on the LIDAR 3 is converted into the hull coordinate system shown in FIG. 4(A). Note that the process of converting point cloud data in a coordinate system based on a LIDAR installed on a moving body into the coordinate system of the moving body is disclosed, for example, in International Publication WO2019 / 188745.

[0047] 4(B) is a perspective view of the structure 50, which is the docking location, clearly showing measurement points that indicate measurement positions measured by the lidar 3 and normal vectors calculated based on the measurement points. In FIG. 4(B), the measurement points are indicated by circles, and the normal vectors are indicated by arrows. This shows an example in which both the top and side surfaces of the structure 50 were measured by the lidar 3.

[0048] As shown in FIG. 4B, the normal vector calculation block 20 calculates normal vectors for the measurement points on the side and top surfaces of the structure 50. Because normal vectors are vectors perpendicular to the target plane or curved surface, they are calculated using multiple measurement points that can be configured as a surface. Therefore, a grid of predetermined length and width or a circle of predetermined radius is set, and calculations are performed using measurement points within the grid. In this case, the normal vector calculation block 20 may calculate normal vectors for each measurement point or at predetermined intervals. The normal number determination block 22 then determines that a normal vector whose z-component is greater than a predetermined threshold is a normal vector pointing in the vertical direction. Note that the normal vectors are assumed to be unit vectors. Furthermore, a normal vector whose z-component is less than a predetermined threshold is a normal vector pointing in the horizontal direction. The normal number determination block 22 then determines the number of vertical normal vectors (here, five) and the number of horizontal normal vectors (here, four). Furthermore, the mean / variance calculation block 23 calculates the mean and variance of the normal vectors in the vertical direction and the mean and variance of the normal vectors in the horizontal direction. Note that the measurement points around the edge portion are on the top surface or the side surface, so the direction is oblique.

[0049] (3-3) Identifying the inner field of view and detection surface The process of specifying the inner field of view and the detection surface will be explained below by dividing the process into docking situations A to E, which represent specific docking situations.

[0050] (3-3-1) Berthing situation A: Both the side and top of the berthing location are within the field of view and detection surface Fig. 5 is a diagram showing a docking situation in which both the top and side surfaces of a structure 50, which is the docking location, are included as the inner field of view and detection surface. In Fig. 5, a dashed line 51 indicates the estimated water surface position. Circles below the dashed line 51 indicate measurement points that have been removed as measurement points below the water surface position. Circles on the top and side surfaces of the structure 50 indicate measurement points corresponding to the normal vectors calculated by the normal vector calculation block 20.

[0051] In this case, the field of view and detection surface identification block 21 detects both the top and side surfaces of the structure 50 as detection surfaces based on the normal vectors calculated by the normal vector calculation block 20. For example, the field of view and detection surface identification block 21 determines that the top surface of the structure 50 has been detected if the number of vertical normal vectors is equal to or greater than a predetermined number and the variance calculated by the mean and variance calculation block 23 is within a threshold value. Also, the field of view and detection surface identification block 21 determines that the side surface of the structure 50 has been detected if the number of horizontal normal vectors is equal to or greater than a predetermined number and the variance calculated by the mean and variance calculation block 23 is within a threshold value. The predetermined number and threshold value are stored in advance in, for example, the memory 12.

[0052] In addition, in the example of Figure 5, the field of view / detection surface identification block 21 determines that both the top and side surfaces of the structure 50 are within the field of view angle of the lidar 3, since the top and side surfaces of the structure 50 are detected as detection surfaces.

[0053] In docking situation A shown in Figure 5, the inner field of view and the detection surface include both the top and side surfaces of structure 50, which is the docking location, so it is estimated that the docking location has been detected accurately and that the reliability of the detection of the docking location is high.

[0054] (3-3-2) Berthing situation B: Both sides of the berthing location are inside the field of view and only the sides are detected FIG. 6(A) is a diagram showing a docking situation in which the inner surface of the field of view is the top surface and the side surface, while the detection surface is only the side surface.

[0055] In the case of the docking situation shown in FIG. 6(A), the field of view / detection plane identification block 21 detects the side of the structure 50 as the detection plane based on the normal vectors calculated by the normal vector calculation block 20. For example, the field of view / detection plane identification block 21 detects the side of the structure 50 as the detection plane because the number of horizontal normal vectors is equal to or greater than a predetermined number and the number of vertical normal vectors is less than a predetermined number. On the other hand, the field of view / detection plane identification block 21 determines that the inner field of view includes both the top and side surfaces because there is a scanning position that can measure an area above the scanning position that detected the side of the structure 50 (in other words, the highest vertical number (described later in FIG. 6(B)) of the data that detected the side of the structure 50 is not the number at the top of the vertical field of view). FIG. 6(B) shows an example of an array of data generated by the lidar 3 in one scanning cycle. Each piece of data generated by the lidar 3 is identified by a combination of a vertical number (here, 1 to m) and a horizontal number (here, 1 to n) according to the emission direction of the laser light. In the example of FIG. 6(B), the data with vertical number 1 is the number at the top of the vertical field of view. In this case, in the example of FIG. 6(A), if the highest vertical number in the data detecting the side of the structure 50 is not 1, the field of view / detection surface identification block 21 determines that the inner field of view includes both the top surface and the side surface.

[0056] In the docking situation shown in Figure 6(A), although the inner field of view includes both the top and side surfaces, only the side surfaces of the docking location can be detected, so it is estimated that the reliability of detecting the docking location is lower than in the case of Figure 5.

[0057] (3-3-3) Berthing situation C: Both sides of the berthing location are inside the field of view and only the top surface is detected FIG. 6(C) is a diagram showing a docking situation in which the inner surface of the field of view is the top and side surfaces, while the detection surface is only the top surface.

[0058] In the case of the docking situation shown in Fig. 6(C), the field of view and detection plane identification block 21 detects the side of the structure 50 as the top surface based on the normal vector calculated by the normal vector calculation block 20. On the other hand, the field of view and detection plane identification block 21 determines that the inner surface of the field of view includes both the top surface and the side surface because there is a scanning position that can measure below the scanning position that detected the top surface of the structure 50 (in other words, the lowest vertical number of the data that detected the top surface of the structure 50 is not the number at the bottom of the vertical field of view (number m in Fig. 6(B))).

[0059] In the docking situation shown in Figure 6(C), although the inner field of view includes both the top and side surfaces, only the top surface of the docking location can be detected, so it is estimated that the reliability of detecting the docking location is lower than in the cases of Figures 5 and 6(A).

[0060] (3-3-4) Berthing situation D: Both the inner field of view and the detection surface are only on the side FIG. 7(A) is a diagram showing a docking situation in which the inner field of view and the detection surface are both side surfaces.

[0061] In the case of the docking situation shown in FIG. 7(A), the field of view and detection plane identification block 21 detects part of the side of the structure 50 as the detection plane based on the normal vector calculated by the normal vector calculation block 20. Furthermore, the field of view and detection plane identification block 21 determines that the inner field of view includes only part of the side because there is no scanning position capable of measuring above the scanning position where the side of the structure 50 was detected (in other words, the highest vertical number of the data detecting the side of the structure 50 is the number at the top of the vertical field of view (number 1 in FIG. 6(B))). Furthermore, in the docking situation shown in FIG. 7(A), only part of the side of the docking location has been detected, so it is estimated that the reliability of the detection of the docking location is lower than in the case of FIG. 5, etc.

[0062] (3-3-5) Berthing situation E: The inner field of view and the detection surface are both on the upper surface only FIG. 7(B) is a diagram showing a docking situation in which the inner field of view and the detection surface are both only the upper surface.

[0063] In the docking situation shown in FIG. 7(B), the field of view / detection plane identification block 21 detects only the top surface of the structure 50 as the detection plane based on the normal vector calculated by the normal vector calculation block 20. Furthermore, the field of view / detection plane identification block 21 determines that the inner field of view includes only the top surface because there is no scanning position capable of measuring below the scanning position where the side of the structure 50 was detected (in other words, the lowest vertical number of the data detecting the top surface of the structure 50 is the number at the bottom of the vertical field of view (number m in FIG. 6(B))). Furthermore, in the docking situation shown in FIG. 7(B), only a portion of the top surface of the structure 50 can be detected, and it is estimated that the reliability of detecting the docking location is lower than in the case of FIG. 5, etc. Furthermore, in this case, the nearest point does not become the reference position of the structure 50 when calculating the distance to the opposite bank, and therefore the distance to the opposite bank cannot be calculated accurately.

[0064] (3-4) Nearest neighbor search Next, a specific example of the processing of the neighbor point search block 25 and the nearest neighbor determination block 26 will be described with reference to Fig. 8(A) and Fig. 8(B). Fig. 8(A) is a perspective view of a structure 50, clearly indicating the measurement points of the structure 50 measured by the LIDAR 3 and the nearest points therein. Fig. 8(B) is a perspective view of a structure 50, clearly indicating the measurement points of the structure 50 measured by the LIDAR 3 and the nearest points therein.

[0065] The neighboring point search block 25 searches for a predetermined number of neighboring points based on distance information of the point cloud data acquired from the LIDAR 3. In the example of Fig. 8(A), the neighboring point search block 25 finds seven neighboring points. In this case, it is preferable that the neighboring point search block 25 removes data from the point cloud data that represents a position below a height that can be estimated as the water surface position, as water surface reflection data (i.e., false detection data) obtained by the laser light reflecting off the water surface.

[0066] The nearest neighbor determination block 26 determines the nearest neighbor among the neighboring points found by the neighboring point search block 25 as a nearest neighbor candidate and calculates the coordinate difference between the candidate and the other neighboring points (e.g., the average distance between the candidate and the other neighboring points). If the difference is less than a predetermined threshold, the nearest neighbor determination block 26 determines that the candidate is the nearest neighbor. Strong waves or winds near a quay can accidentally capture sea spray or floating objects. Therefore, the threshold is used to determine whether the measurement point is noise or an object other than a docked location, and is stored in advance in the memory 12, for example. On the other hand, if the difference is greater than or equal to the predetermined threshold, the nearest neighbor determination block 26 determines that the candidate is likely to be noise or a measurement point of another object, and does not determine that the candidate is the nearest neighbor. In this case, the nearest neighbor determination block 26 determines the next nearest neighbor after the candidate as a new candidate, calculates the difference, compares it with the threshold, and determines whether the candidate is the nearest neighbor.

[0067] For example, in the example of FIG. 8(A), nearest neighbor determination block 26 first extracts neighbor point 59 as a candidate for the nearest neighbor point. Then, because the difference between neighbor point 59 and the other neighbor points is equal to or greater than a predetermined threshold, nearest neighbor determination block 26 considers neighbor point 59 to be noise or the like and removes it. Next, nearest neighbor determination block 26 extracts neighbor point 60, which is the next closest to neighbor point 59, as a candidate for the next nearest neighbor point, and because the difference between neighbor point 60 and the other neighbor points is less than a predetermined threshold, nearest neighbor determination block 26 determines that neighbor point 60 is the nearest neighbor point, as shown in FIG. 8(B).

[0068] (3-5) Calculating the distance to the opposite shore Next, calculation of the docking distance will be explained. Hereinafter, as a representative example, a case where the LIDAR 3 is installed at two locations, one in front and one in rear of the target ship, will be explained.

[0069] 9(A) is a diagram showing the target ship and the structure 50, which is the docking location, viewed from above. In this example, the target ship is not parallel to the structure 50, which is the docking location (there is an angle), so the actual nearest position of the structure 50 is not included in the measurement range of the LIDAR 3, and the distance to the nearest point calculated by the nearest neighbor determination block 26 is not the shortest distance.

[0070] FIG. 9(B) is a diagram showing an outline of the method for calculating the distance to the opposite bank. Here, the shortest distance (i.e., the distance to the opposite bank) based on the measurement results of the front lidar 3 is expressed as "d1(k)" ("k" is a time index), the shortest distance (i.e., the distance to the opposite bank) based on the measurement results of the rear lidar 3 is expressed as "d2(k)", the vector representing the nearest point based on the measurement results of the front lidar 3 is expressed as "p1", and the vector representing the nearest point based on the measurement results of the rear lidar 3 is expressed as "p2". In addition, the average (also called the "average normal vector") of the normal vectors (i.e., normal vectors in the horizontal direction) to the side of the docking location based on the measurement results of the front lidar 3 is expressed as "n1 - ", and the average normal vector based on the measurement results of the rear rider 3 is "n2 - " Here, the average normal vector n1 - , n2 - is a vector converted into a unit vector.

[0071] In the opposite shore distance calculation method, the opposite shore distance calculation block 27 calculates the dot product of the vector from the rider 3 to the nearest point and the average normal vector, which is the average of the normal vectors to the side of the docking location, for each of the front rider 3 and the rear rider 3. Note that the dot product may be calculated using the normal vector at the nearest point instead of the average normal vector. The opposite shore distance calculation block 27 then regards the calculation result of the dot product for each rider 3 as the shortest distance to each rider 3, and determines the opposite shore distance from these shortest distances.

[0072] Specifically, if the vector length of the nearest point vector p1 (i.e., the distance from the forward rider 3 to the nearest point) is "L1" and the angle with the direction of the shortest distance to the structure 50 is "θ", the shortest distance d1(k) can be calculated as follows: d1(k)=L1COSθ

[0073] Also, the nearest point vector p1 and the average normal vector n1 - The angle between the two is calculated using the following formula: COSθ=(p1·n1 - ) / (|p1||n1 - |) Here, the average normal vector n1 - is a unit vector, so |n1 - |=1, and the length of the nearest point vector p1 is L1, so |p1|=L1, and the shortest distance d1(k) is determined as follows:

[0074]

number

[0075] Generally, the distance from the ship to the docking location needs to be calculated on a horizontal plane, so hereafter the z component will be ignored. p1=[p x1 ,p y1 , p z1 ] T , p2=[p x2 ,p y2 , p z2 ] T , n1 - =[n x1 - ,n y1 - ,n z1 - ] T , n2 - =[n x2 - ,n y2 - ,n z2 - ] T When calculation is performed only on the x and y components, the opposite bank distance calculation block 27 calculates the shortest distances d1(k) and d2(k) as follows.

[0076]

number

[0077] The shore distance calculation method allows the shore distance calculation block 27 to suitably calculate the shore distance regardless of the orientation of the target ship relative to the berthing location.

[0078] Instead of calculating the distance from the LIDAR 3 to the docking location as the docking distance (shortest distance), the docking distance calculation block 27 may calculate the distance from a predetermined reference position (for example, the center position of the ship) of the target ship to the docking location as the docking distance (shortest distance). In this case, information about the reference position is stored in advance in the memory 12 as coordinate values ​​in the hull coordinate system or a coordinate system based on the LIDAR 3. The docking distance calculation block 27 sets the starting point of the nearest point vector p1 to the above-mentioned reference position. The docking distance calculation block 27 may also set multiple positions of the outer plating (outer edge) of the target ship as multiple candidates for the reference position, calculate the shortest distance between each of these positions of the outer plating and the docking location, and calculate the shortest shortest distance as the shortest distance (docking distance) for the target ship.

[0079] Next, a specific embodiment of the method for calculating the average normal vector used in the method for calculating the opposite bank distance will be described in detail.

[0080] In the first mode, the opposite shore distance calculation block 27 calculates the average normal vector n1 based on the measurement results of the forward rider 3 as described above. - and the average normal vector n2 based on the measurement results of the rear lidar 3 - In the second mode, the opposite bank distance calculation block 27 calculates a single average normal vector (also called an "integrated normal vector") "n - In the third embodiment, the opposite shore distance calculation block 27 calculates the average normal vector n1 based on the measurement results of the front rider 3. - and the average normal vector n2 based on the measurement results of the rear lidar 3 -and the integrated normal vector n - Calculate.

[0081] Here, the first to third modes will be described. When the side of the docking location measured by the front rider 3 and the side of the docking location measured by the rear rider 3 face different directions, the shore distance calculation block 27 calculates the average normal vector n1 based on the first mode. - and the average normal vector n2 - and calculate.

[0082] FIG. 10(A) is a diagram clearly showing normal vectors calculated for a structure 50 whose side surfaces measured by the front lidar 3 and the rear lidar 3 have different surface orientations. Here, the normal vector of the side surface based on the measurement result of the front lidar 3 is expressed as "n1(i)=[n x1 (i),n y1 (i),n z1 (i)] T ” (i=1 to N1), and the normal vector of the side based on the measurement results of the rear lidar 3 is “n2(j)=[n x2 (j),n y2 (j),n z2 (j)] T " (j=1~N2).

[0083] In this case, the opposite shore distance calculation block 27 calculates the average normal vector n1 based on the measurement results of the front lidar 3. - and the average normal vector n2 based on the measurement results of the rear lidar 3 - The opposite bank distance calculation block 27 calculates the average normal vector n1 - and the average normal vector n2 - If the difference (magnitude of vector difference) is greater than a predetermined threshold, it is determined that the orientation of the docking location measured by the front rider 3 is different from the orientation of the docking location measured by the rear rider 3. Here, since the difference is greater than the threshold, the shore distance calculation block 27 calculates the average normal vector n1 - and the average normal vector n2 - Both will be adopted.

[0084] On the other hand, when the orientation of the side measured by the front lidar 3 and the side measured by the rear lidar 3 is the same, the opposite bank distance calculation block 27 calculates the integrated normal vector n by weighted averaging using the number of normals based on the second mode. - FIG. 10(B) is a diagram clearly showing the normal vectors calculated for the structure 50 whose side measured by the front lidar 3 and the side measured by the rear lidar 3 have the same surface orientation. In this case, the opposite bank distance calculation block 27 calculates the average normal vector n1 - and the average normal vector n1 - Since the difference between the normal vectors n1 and n2 is less than the predetermined threshold, it is determined that the orientation of the docking location measured by the front lidar 3 is the same as that of the docking location measured by the rear lidar 3. Therefore, in this case, the shore distance calculation block 27 calculates the integrated normal vector n1 using the following formula based on the number N1 of normal vectors n1 and the number N2 of average normal vectors n2: - Calculate.

[0085]

number

[0086] Furthermore, when the orientation of the side measured by the front lidar 3 and the side measured by the rear lidar 3 are the same but the planarity of these surfaces is different (i.e., the variation in the normal vectors calculated for each lidar 3 is different), the opposite bank distance calculation block 27 calculates the integrated normal vector n - Calculate.

[0087] 10(C) is a diagram clearly showing the normal vectors calculated for the structure 50 whose side measured by the front lidar 3 and the side measured by the rear lidar 3 have different flatness. In this case, the opposite bank distance calculation block 27 calculates the average normal vector n1 - and the average normal vector n1 - The difference between the normal vector n1 and the normal vector n2 is less than a predetermined threshold (for example, the inner product is greater than or equal to a threshold). - The variance of the normal vectors used to calculate the mean normal vector n2 -Therefore, in this case, the opposite bank distance calculation block 27 determines that the orientation of the side measured by the front rider 3 and the side measured by the rear rider 3 are the same, but the planarity of these surfaces is different. Therefore, in this case, the average normal vector n1 based on the variance - The weight vector for w1 = [w x1 ,w y1 ,w z1 ] T ", the average normal vector n2 based on the variance - The weight vector for w2 = [w x2 ,w y2 ,w z2 ] T ", then the integrated normal vector n - =[n x - ,n y - ,n z - ] T is calculated by the following formula:

[0088]

number

[0089] Since the normal vector is three-dimensional, the variance is expressed as a 3-row, 3-column covariance matrix. - The covariance matrix for V n1 ", mean normal vector n2 - The covariance matrix for V n2 " is expressed as follows:

[0090]

number

[0091] In this case, each weight vector is expressed by the following equation: Each weight vector is the reciprocal of the variance value, which is the diagonal component of the covariance matrix, and the smaller the variance value, the larger the weight.

[0092]

number

[0093] Even when there are three or more riders 3, the opposite bank distance calculation block 27 can calculate the average normal vector based on any one of the first to third modes. For example, when there are three riders 3 (first rider, second rider, third rider), the number of normal vectors "n1" of the first rider is "N1", the number of normal vectors "n2" of the second rider is "N2", the number of normal vectors "n3" of the third rider is "N3", and the average normal vector "n1" based on the variance of the first rider is "N1". - The weight vector for " is [w x1 ,w y1 ,w z1 ] T , the average normal vector based on the variance of the second lidar, "n2 - The weight vector for " is [w x2 ,w y2 ,w z2 ] T , the average normal vector based on the variance of the third lidar, "n3 - The weight vector for " is [w x3 ,w y3 ,w z3 ] T In this case, in the calculation method based on the second aspect, the opposite bank distance calculation block 27 calculates the integrated normal vector n - Calculate.

[0094]

number

[0095]

number

[0096] (3-6) Calculating berthing speed Next, a method for calculating the docking speed will be described. The docking speed calculation block 29 calculates the change over time in the distance to the shore (shortest distance) obtained for each of the front rider 3 and the rear rider 3 as the docking speed.

[0097] 11 is a diagram showing an outline of a method for calculating the docking speed, where "d1" represents the distance to the shore (shortest distance) of the front rider 3, "d2" represents the distance to the shore (shortest distance) of the rear rider 3, "k" represents the current processing time, "k-1" represents the immediately preceding processing time, and "Δt" represents the time interval from the immediately preceding calculation time of the distance to the shore.

[0098] Here, the docking speed calculation block 29 inserts a filter to deal with an increase in noise when dividing the difference in the distance to the shore between the previous time and the current time by the time interval. Specifically, the docking speed calculation block 29 uses a time constant "τ" and a Laplace operator "s" to calculate a docking speed "v1" for the front rider 3 and a docking speed "v2" for the rear rider 3 according to the following equations.

[0099]

number

[0100] This allows the docking speed calculation block 29 to appropriately calculate the docking speed taking into account the influence of noise.

[0101] (3-7) Calculating approach angle Next, the approach angle calculation methods (first calculation method and second calculation method) will be described. In the first approach angle calculation method, the approach angle calculation block 28 calculates the approach angle using "atan2," a function that determines the arc tangent from two arguments that define the tangent. Specifically, the approach angle calculation block 28 calculates the approach angle from the average normal vector by calculating the function atan2.

[0102] 12(A) is a diagram showing an overview of the first approach angle calculation method. In this case, the approach angle calculation block 28 calculates the approach angle "Ψ1" calculated based on the point cloud data of the front rider 3 and the approach angle "Ψ2" calculated based on the point cloud data of the rear rider 3 based on the following equations.

[0103]

number

[0104] In the second approach angle calculation method, the approach angle calculation block 28 calculates the angle from the difference "Δd" between the shortest distances d1(k) and d2(k). As shown in Figure 12(B), by knowing the installation interval "ΔS" between the front rider 3 and the rear rider 3, the approach angle "Ψ" can be calculated based on the following formula.

[0105]

number

[0106] (3-8) Generating reliability information The reliability information generation block 30 generates a flag for each element such as the field of view when detecting the docking location, the surface detection of the docking location, the number and variance of normal vectors, etc., and generates a vector of the generated flags as reliability information. Hereinafter, a flag of "1" indicates that the reliability of the corresponding element is high, and a flag of "0" indicates that the reliability of the corresponding element is low.

[0107] 13 shows an example of the data structure of reliability information generated by the reliability information generation block 30. As shown in FIG. 13, the reliability information has the following items: "Top surface," "Side surface," "Neighboring points," "Distance," and "Angle." The item "Top surface" has the sub-items "Viewing angle," "Detection," "Number of normals," and "Variance," and the item "Side surface" has the sub-items "Viewing angle," "Detection," "Number of normals," and "Variance." The item "Neighboring points" has the sub-item "Variance," the item "Distance" has the sub-items "Amount of change" and "Rate of change," and the item "Angle" has the sub-item "Amount of change."

[0108] Here, the reliability information generation block 30 registers a flag in the sub-item "viewing angle" of the item "upper surface" that is "1" if the upper surface of the docking location is within the range of the viewing angle, and "0" if the upper surface is outside the viewing angle. Also, the reliability information generation block 30 registers a flag in the sub-item "viewing angle" of the item "upper surface" that is "1" if the upper surface of the docking location is the detection surface, and "0" if the upper surface is not the detection surface. Also, the reliability information generation block 30 registers a flag in the sub-item "number of normals" of the item "upper surface" that is "1" if the number of normal vectors to the upper surface of the docking location is equal to or greater than a predetermined threshold (e.g., 10), and "0" if the number is less than the threshold. Furthermore, the reliability information generation block 30 registers a flag in the "variance" sub-item of the "top surface" item that is set to "1" if the variances of the x, y, and z components of the normal vector relative to the top surface of the docking location are all less than a predetermined threshold (e.g., 1.0), and is set to "0" if any of the variances is equal to or greater than the threshold. The reliability information generation block 30 also registers flags in each sub-item of the "side surface" item that are determined according to the same rules as for each sub-item of the "top surface" item.

[0109] Furthermore, the reliability information generation block 30 registers a flag in the sub-item "variance" of the item "neighborhood point" that is "1" if the variances of the x, y, and z components of the neighboring points searched by the neighboring point search block 25 are all less than a threshold (e.g., 1.0), and "0" if any of the variances is equal to or greater than the threshold. Furthermore, the reliability information generation block 30 registers a flag in the sub-item "change amount" of the item "distance" that is "1" if the amount of change in the opposite bank distance calculated by the opposite bank distance calculation block 27 from one time before is less than a predetermined threshold (e.g., 1.0 m), and "0" if the amount of change is equal to or greater than the threshold. Furthermore, the reliability information generation block 30 registers a flag in the sub-item "change rate" of the item "distance" that is "1" if the rate of change in the opposite bank distance calculated by the opposite bank distance calculation block 27 from one time before is less than a predetermined threshold (e.g., ±10%), and "0" if the rate of change is equal to or greater than the threshold. In addition, the reliability information generation block 30 registers a flag in the sub-item "change amount" of the item "angle" that is set to "1" if the change amount in the approach angle calculated by the approach angle calculation block 28 from one time before is less than a predetermined threshold (e.g., 1.0 degree), and to "0" if the change amount is equal to or greater than the threshold.

[0110] The above-mentioned threshold values ​​are set to suitable values ​​stored in advance in the memory 12, for example.

[0111] Using reliability information with such a data structure, it is possible to grasp the reliability of the calculated distance to berth, berthing speed, and approach angle. Note that when each sub-item of the reliability information is "1," the reliability is highest. The information processing device 1 then adjusts the output of the drive source when berthing based on this reliability information. For example, the information processing device 1 may determine the upper limit of the target ship's speed when berthing according to the total value of each sub-item indicated by the reliability information. In this case, the information processing device 1 determines that the smaller the total value, the lower the reliability of the information regarding the berthing location and the more careful berthing is required, and reduces the upper limit of the target ship's speed when berthing.

[0112] (3-9) Processing Flow 14 is an example of a flowchart showing an outline of the docking support process in this embodiment. The information processing device 1 repeatedly executes the process of the flowchart in FIG.

[0113] First, the information processing device 1 acquires point cloud data in the direction of the docking location (step S11). In this case, the information processing device 1 acquires point cloud data generated by, for example, a lidar 3 of the target ship whose measurement range includes the docking side. The information processing device 1 may further downsample the acquired point cloud data and remove data reflected on the water surface.

[0114] Next, the docking location detection unit 15 of the information processing device 1 calculates normal vectors based on the point cloud data acquired in step S11 (step S12). Furthermore, in step S12, the docking location detection unit 15 calculates the number of normal vectors, the variance of the normal vectors, etc. Furthermore, based on the processing result of step S12, the docking location detection unit 15 identifies the inner field of view and the detection plane (step S13).

[0115] Next, the berthing parameter calculation unit 16 calculates berthing parameters based on the point cloud data acquired in step S11 and the information on the normal vectors calculated in step S12 (step S14). In this case, the berthing parameter calculation unit 16 calculates the neighboring points and the nearest points of the berthing location, and further calculates the distance to the berthing location, the approach angle, the berthing speed, etc. using the calculation results.

[0116] Then, the berthing parameter calculation unit 16 generates reliability information based on the results of identifying the field of view inner surface and the detection surface in step S13 and the results of calculating the berthing parameters in step S14 (step S15). Thereafter, the information processing device 1 controls the ship based on the reliability information (step S16). This allows the information processing device 1 to accurately control the ship regarding berthing based on the reliability that accurately reflects the berthing situation.

[0117] The information processing device 1 then determines whether the target ship has come alongside (docking) (step S17). In this case, the information processing device 1 determines whether the target ship has come alongside (docking) based on, for example, the output signal of the sensor group 2 or user input via the interface 11. If the information processing device 1 determines that the target ship has come alongside (step S17; Yes), it terminates the processing of the flowchart. On the other hand, if the target ship has not come alongside (step S17; No), the information processing device 1 returns the processing to step S11.

[0118] As explained above, the controller 13 of the information processing device 1 according to the first embodiment acquires measurement data of the docking location generated by the lidar 3, which is a measurement device provided on the target ship. Then, the controller 13 calculates a normal vector to the docking location based on the measurement data, and calculates the distance from the target ship to the docking location based on the normal vector. According to this aspect, the information processing device 1 can accurately calculate the distance from the docking location, which is one of the important parameters when docking at the docking location.

[0119] <Second Example> 15 shows an example of a block diagram of an information processing device 1A in the second embodiment. The information processing device 1A in the second embodiment differs from the information processing device 1 in the first embodiment in that it processes the self-position of the target vessel. Hereinafter, the same components as those in the first embodiment will be appropriately designated by the same reference numerals, and their description will be omitted.

[0120] As shown in FIG. 15, the information processing device 1A includes an interface 11, a memory 12, and a controller 13.

[0121] The memory 12 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, 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 in each voxel using a normal distribution, and is used for scan matching using NDT (Normal Distribution Transform), as described below. In addition to the voxel data VD, the map DB 10 may also include, for example, information on docking locations (including shores and piers) and information on waterways that ships can navigate.

[0122] 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 related to the 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.

[0123] The controller 13 functionally includes a docking location detection unit 15, a docking parameter calculation unit 16, and a self-position estimation unit 17. The docking location detection unit 15 and the docking parameter calculation unit 16 have the functional blocks shown in Fig. 3 described in the first embodiment.

[0124] The self-position estimation unit 17 acquires the outputs of the various sensors included in the sensor group 2 via the interface 11 and estimates the self-position of the target ship on which the information processing device 1 is installed. Here, the self-position estimation unit 17 estimates, for example, the planar position, height position, yaw angle, pitch angle, and roll angle of the target ship using NDT scan matching. Unless otherwise specified, the self-position also includes the attitude angle of the target ship, such as the yaw angle. Note that NDT scan matching for the purpose of estimating the position of a moving body is described, for example, in JP 2019-174675 A. The controller 13 performs driving assistance, such as automatic driving control of the target ship, based on the self-position estimation result by the self-position estimation unit 17.

[0125] 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 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 moving body (target ship) in three axial directions. The speed sensor 4 may be, for example, a speedometer that uses Doppler or a speedometer that uses GNSS. Note that the sensor group 2 may include a receiver that generates positioning results using GNSS other than GPS, instead of the GPS receiver 5.

[0126] Fig. 16 shows an example of functional blocks of the self-position estimation unit 17. As shown in Fig. 16, the self-position estimation unit 17 has a dead reckoning block 31, a coordinate transformation block 32, a data extraction block 33, a water surface position calculation block 34, a water surface reflection data removal block 35, and an NDT position calculation block 36.

[0127] The dead reckoning block 31 calculates the DR position based on the signal output by the sensor group 2. Specifically, the dead reckoning block 31 uses the moving speed and angular velocity of the target ship based on the outputs of the speed sensor 4, the IMU 6, etc. to determine the moving distance and change in heading from the previous time. The dead reckoning block 31 then calculates a provisional self-position at time k (also referred to as the "predicted self-position") by adding the moving distance and change in heading from the previous time to the self-position (also referred to as the "estimated self-position") finally estimated at time k-1, which is the processing time immediately before the current processing time k. This predicted self-position is the self-position calculated at time k based on dead reckoning and will hereinafter be referred to as the "DR position." Note that if there is no estimated self-position at time k-1 immediately after the start of self-position estimation, the dead reckoning block 31 determines the DR position based on, for example, the signal output by the GPS receiver 5.

[0128] The coordinate transformation block 32 transforms the point cloud data based on the output of the LIDAR 3 into a world coordinate system, which is the same coordinate system as the map DB 10. In this case, the coordinate transformation block 32 performs coordinate transformation of the point cloud data at time k, for example, based on the predicted self-position output by the dead reckoning block 31 at time k. Note that the process of transforming point cloud data in a coordinate system based on a LIDAR installed on a moving body (a ship in this embodiment) into the coordinate system of the moving body, and the process of transforming from the coordinate system of the moving body to the world coordinate system are disclosed, for example, in International Publication WO2019 / 188745.

[0129] The data extraction block 33 extracts water surface reflection data, which is a sample used to calculate the water surface height, from the point cloud data output by the coordinate transformation block 32. In this case, the data extraction block 33 extracts, for example, data of measurement points indicating positions around the subject's position (within a predetermined distance from the subject's position) and below the subject's position, where no voxel data exists, as the water surface reflection data.

[0130] The water surface position calculation block 34 calculates the water surface position as the average value of the z-direction positions (z coordinate values) represented by each measurement point extracted by the data extraction block 33. In this case, the water surface position calculation block 34 may determine whether or not to adopt the calculated water surface position as the current water surface position based on the variance value of the z-direction positions of each measurement point extracted by the data extraction block 33. The water surface position calculation block 34 may also determine the current water surface position by performing an averaging process or a filtering process on the calculated provisional water surface position with a water surface position calculated in the past. In addition, the water surface position calculation block 34 or the water surface position calculation block 34 may set an offset so that the set water surface position is a predetermined distance above the set water surface position in order to reliably remove erroneous detection data.

[0131] The water surface reflection data removal block 35 determines each measurement point of the point cloud data that represents a position (i.e., a position with the same or lower z coordinate value) below the water surface position (including the same height, the same applies below) supplied from the water surface position calculation block 34 as water surface reflection data, and removes the determined water surface reflection data from the point cloud data.

[0132] The NDT position calculation block 36 calculates the NDT position based on the point cloud data after water surface reflection data removal, which is supplied from the water surface reflection data removal block 35. In this case, the NDT position calculation block 36 matches the point cloud data in the world coordinate system supplied from the water surface reflection data removal block 35 with the voxel data VD expressed in the same world coordinate system, thereby associating the point cloud data with the voxels. The NDT position calculation block 36 then performs matching on each voxel associated with the point cloud data, thereby calculating estimated parameters related to the position and attitude of the target ship at time k. The NDT position calculation block 36 then outputs the position at time k determined by applying the estimated parameters calculated at time k to the DR position output by the dead reckoning block 31 as the estimated self-position at time k.

[0133] The self-location estimation unit 17 may perform position estimation based on any mobile object position estimation method, not limited to position estimation based on NDT scan matching. For example, the self-location estimation unit 17 may perform self-location estimation based on matching landmark information on a map with point cloud data of the LIDAR 3, as described in JP 2017-72422 A.

[0134] Next, a supplementary explanation will be given of the processing of the berthing location detection unit 15 and the berthing parameter calculation unit 16 in the second embodiment. The berthing location detection unit 15 and the berthing parameter calculation unit 16 each have the functional blocks shown in Fig. 3, and calculate normal vectors, search for the nearest point, and calculate the distance to the berth and approach angle, etc., based on the flowchart in Fig. 14. In this case, the berthing location detection unit 15 and the berthing parameter calculation unit 16 determine whether the berthing location is included in the measurement range of the LIDAR 3, based on the map DB 10 and the estimated self-position output by the self-position estimation unit 17. Then, if the berthing location is included in the measurement range of the LIDAR 3, the berthing location detection unit 15 and the berthing parameter calculation unit 16 determine that the target ship has approached the berthing location, and start the processing of the flowchart in Fig. 14.

[0135] In this case, the approach angle calculation block 28 of the berthing parameter calculation unit 16 may calculate the velocity vector of the target ship from the time change in the estimated self-position output by the self-position estimation unit 17, and calculate the approach angle based on the velocity vector. In this case, the approach angle calculation block 28 calculates the approach angle based on the dock product of the velocity vector of the target ship and the average normal vector of the side of the berthing location calculated based on the point cloud data of the LIDAR 3 converted into the world coordinate system. Also, the berthing speed calculation block 29 may calculate the component of the normal vector direction of the velocity vector of the target ship as the berthing speed, instead of calculating the berthing speed based on the time change in the distance to the berth.

[0136] In this way, the information processing device 1A according to the second embodiment can preferably acquire information about the docking situation while performing self-position estimation.

[0137] <Third Example> The information processing device 1A according to the third embodiment differs from the information processing device 1A according to the second embodiment in that the information processing device 1A calculates the normal vector of the side surface of the docking location based on the map DB 10, instead of calculating the normal vector of the side surface of the docking location based on the point cloud data of the LIDAR 3. Hereinafter, the same components as those in the second embodiment will be appropriately designated by the same reference numerals, and the description thereof will be omitted.

[0138] FIG. 17(A) is an overhead view of the area around a target ship, clearly showing the normal vectors (e.g., average normal vectors) of the side surfaces of a structure 50 that has fenders installed on its docking side. The normal vectors are calculated based on point cloud data measured by a lidar 3 when the target ship approaches the structure 50. In the example of FIG. 17(A), another ship 65 is present near the target ship, and the other ship 65 is included within the measurement range of the lidar 3, so point cloud data is generated for measurement points that measure the paddle wheels of the other ship 65. In addition, the presence of the fenders prevents the lidar 3 from measuring the side surfaces of the structure 50 where the target ship is docking, so there are relatively few measurement points on the side surfaces of the structure 50. Note that the fenders are black and hardly reflect the laser light of the lidar 3, making it difficult for the lidar 3 to measure the position of the fenders.

[0139] In this case, if the normal vector (for example, the average normal vector) of the side surface of the structure 50 is calculated based on the point cloud data generated by the lidar 3, an inaccurate normal vector will be calculated.

[0140] Fig. 17(B) is an overhead view of the area around the target ship, clearly showing the normal vectors (also called "map normal vectors") of the side surfaces of the structure 50 calculated based on the map DB 10. In Fig. 17(B), the map DB 10 includes three-dimensional data for the side surfaces of the structure 50, and the position of the side surfaces of the structure 50 represented by the three-dimensional data based on the map DB 10 is indicated by a rectangle. Note that this three-dimensional data may be included in the map DB 10 as voxel data, or may be included in the map DB 10 in any data format other than voxel data as data representing the position of the side surfaces of the berthing location.

[0141] In this case, the information processing device 1A calculates the normal vector (for example, the average normal vector) of the side surface of the structure 50 based on the three-dimensional data of the side surface of the structure 50 that is based on the map DB 10. In this case, the information processing device 1A can accurately calculate the normal vector of the side surface of the structure 50 without being affected by other ships 65 or fenders that are present within the measurement range of the LIDAR 3. Then, the information processing device 1A calculates the distance to the shore, the docking speed, the approach angle, etc., based on the accurately calculated normal vector of the side surface of the structure 50, as in the first or second embodiment. This allows the information processing device 1A to accurately acquire information required when the target ship comes docking.

[0142] In this embodiment, the information processing device 1A does not need to perform the processing of the field of view / detection plane specifying block 21, the normal number specifying block 22, the mean / variance calculation block 23, and the docking situation determination block 24, and to generate reliability information based on the results of these processing. Also, the information processing device 1A may perform a search for the nearest point based on the estimated self-position and three-dimensional data of the docking location, instead of searching for a nearest point based on point cloud data, as the processing performed by the neighboring point search block 25 and the nearest neighbor determination block 26.

[0143] <Fourth Example> The fourth embodiment differs from the second or third embodiment in that the map DB 10 stores information about guide boards provided at docking locations, and the information processing device 1A calculates normal vectors of the side surfaces of the docking locations based on the information about the guide boards. Hereinafter, the same components as those in the second or third embodiment will be appropriately designated by the same reference numerals, and their description will be omitted.

[0144] The self-position estimation unit 17 according to the fourth embodiment estimates the self-position of the target ship based on matching the position information of features registered in the map DB 10 with the measurement results of the LIDAR 3 for the features. The map DB 10 includes landmark data. The landmark data includes attribute information such as the position, size, and orientation of landmarks (features) that are used as references for self-position estimation. The landmark data includes at least information on guide boards (reference objects) installed at the berthing location. The guide boards are fixed objects made of retroreflective material and function as landmarks that indicate the berthing location.

[0145] 18 shows an example of functional blocks of the self-location estimation unit 17 according to the fourth embodiment. Functionally, the self-location estimation unit 17 includes a dead reckoning block 41, a position prediction block 42, a coordinate transformation block 43, a landmark search / extraction block 44, and a position correction block 45.

[0146] Similar to the dead reckoning block 31, the dead reckoning block 41 calculates a DR position, which is a predicted self-position, based on signals output by the speed sensor 4 and the IMU 6 and the estimated self-position at the immediately preceding time. At the same time, a covariance matrix corresponding to the error distribution of the predicted self-position is calculated from the covariance matrix at the immediately preceding time. Furthermore, the coordinate transformation block 43 transforms the point cloud data output from the LIDAR 3 into a world coordinate system, which is the same coordinate system as the map DB 10. Note that the self-position estimation unit 17 has a data extraction block 33, a water surface position calculation block 34, and a water surface reflection data removal block 35, similar to the functional blocks shown in FIG. 16, and may further perform processing such as removing water surface reflection data from the point cloud data.

[0147] The landmark search and extraction block 44 associates the position vector of the landmark to be measured, registered in the map DB 10, with the point cloud data of the LIDAR 3, which has been converted into the world coordinate system by the coordinate transformation block 43. In this case, the landmark search and extraction block 44 determines whether or not a landmark exists within the measurement range of the LIDAR 3, based on the landmark data LD and the predicted self-position output by the position prediction block 42. If a measurable landmark is registered in the landmark data LD, the landmark search and extraction block 44 determines whether or not there is a measurement point of point cloud data with a high reflectance, with a reflectance equal to or greater than a predetermined threshold, within the measurement range of the landmark. If such a measurement point exists, the landmark search and extraction block 44 determines that the above-mentioned association has been established, and combines the measurement value "Z(t)" by the LIDAR 3 of the associated landmark with the predicted self-position and the measurement predicted value "Z(t)", which is a vector value indicating the position of the landmark based on the predicted self-position and the landmark data LD. - (t)"

[0148] Then, the position correction block 45 of the self-position estimation unit 17 calculates the measured value Z(t) and the predicted measured value Z(t) as shown in the following equation (1): - (t), multiply the difference value by the Kalman gain "K(t)" and use this as the predicted self-position "X - By adding this to (t), the estimated self-position "X ^ (t)" is calculated.

[0149]

number

[0150] Parameters such as the Kalman gain K(t) can be calculated in the same manner as in a known self-location estimation technique using, for example, an extended Kalman filter. Details of this self-location estimation are described in, for example, JP 2019-174675 A.

[0151] FIG. 19 shows an overhead view of a target ship docking at a structure 50 on which guide boards 66 and 67 are provided. In the example of FIG. 19, the self-position estimation unit 17 of the information processing device 1A refers to the landmark data LD and reads the position information of each of the guide boards 66 and 67 installed at two locations on the structure 50, which is the docking location. Then, the self-position estimation unit 17 performs matching using a Kalman filter based on equation (1) using the measurement results of the distance and angle to the guide boards 66 and 67 by the LIDAR 3 and the read position information of the guide boards 66 and 67, to calculate an estimated self-position of the target ship. In addition, the normal vector calculation block 20 of the docking location detection unit 15 recognizes the normal vector of the side of the structure 50 based on the orientation information of the guide boards 66 and 67 included in the landmark data LD. In this case, since the orientation of the side of the structure 50 and the orientation of the guide boards 66, 67 coincide, the normal vector calculation block 20 regards the normal vector of the guide boards 66, 67 specified by the orientation information of the guide boards 66, 67 as the normal vector of the side of the structure 50. Thereafter, the information processing device 1A calculates the distance to the shore, the docking speed, the approach angle, etc. based on the normal vector, as in the second or third embodiment.

[0152] In this way, the information processing device 1A according to the fourth embodiment can accurately determine its own position when docking at a docking location, and can accurately acquire information necessary for the target ship to dock. Note that the normal vectors of the guide boards 66 and 67 may be obtained from the orientation information stored in the landmark data LD, or may be calculated from the detection data of the guide boards 66 and 67.

[0153] In the above-described embodiments, the program can be stored using various types of non-transitory computer-readable media and supplied to a controller or the like that is a computer. Non-transitory computer-readable media include various types of tangible storage media. Examples of non-transitory computer-readable media include magnetic storage media (e.g., flexible disks, magnetic tapes, hard disk drives), magneto-optical storage media (e.g., magneto-optical disks), CD-ROMs (Read Only Memory), CD-Rs, CD-R / Ws, and semiconductor memories (e.g., mask ROMs, PROMs (Programmable ROMs), EPROMs (Erasable PROMs), flash ROMs, and RAMs (Random Access Memory)).

[0154] 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]

[0155] 1. Information processing equipment 2 Sensor group 3 Rider

Claims

[Claim 1] an acquisition means for acquiring measurement data of a docking location generated by a measurement device provided on the ship; a first calculation means for calculating a normal vector relative to the docking location based on the measurement data; a second calculation means for calculating a distance from the vessel to the docking location based on the normal vector; An information processing device having the above.

Images