Automatic docking device, automatic docking system, automatic docking method, and automatic docking program

Abstract

This automatic landing device (100) is provided with a point cloud generation unit (120) and a navigation scheme generation unit (140). A point cloud generation unit (120) converts relative coordinates corresponding to each point of a point cloud corresponding to a result obtained by distance measurement of a target berth by a target radar into absolute coordinates on the basis of a result obtained by positioning the position and attitude of the target ship by the GNSS / INS composite navigation device (200), and generates a target ship position information on the basis of the absolute coordinates corresponding to each point after conversion. An object map representing a navigable area at the object berth and representing absolute coordinates is generated. When a target position and a target attitude of a target ship at a target berth are specified on the basis of a target map, a navigation scheme generation unit (140) generates a plan for guiding the target ship to reach the target position in the target attitude on the basis of the target map and the position and attitude of the positioned target ship.

Description

Technical Field

[0001] This disclosure relates to automatic berthing devices, automatic berthing systems, automatic berthing methods, and automatic berthing procedures. Background Technology

[0002] In existing technologies related to automated ship berthing, GNSS (Global Navigation Satellite System) is used for positioning when the ship is far from the docking location. However, GNSS positioning is susceptible to multipath interference. Furthermore, there are often many buildings around the docking location. Therefore, as the ship approaches the docking location, it switches to LiDAR (Light Detection and Ranging) based automated navigation. Patent Document 1 discloses a LiDAR-based automated berthing device. Existing technical documents Patent documents

[0003] Patent Document 1: Japanese Patent Application Publication No. 2022-106753 Summary of the Invention The technical problem that the invention aims to solve

[0004] According to the technology disclosed in Patent Document 1, relative coordinates are used after switching to LiDAR-based autonomous navigation. Therefore, this technology has the following problem: it is impossible to generate an automatic docking navigation plan unless the location is moved to a place where a map can be generated using LiDAR. Furthermore, this technology requires the user to input the docking location. Therefore, this technology has the following problem: especially in cases like cruise ships where the docking location is fixed, setting the docking location every time is cumbersome.

[0005] The purpose of this disclosure is to generate a map representing absolute coordinates in automatic berthing technology for ships, and to automatically guide berthing based on the generated map representing absolute coordinates. Technical means for solving technical problems

[0006] The automatic berthing device disclosed herein controls the automatic berthing of a target vessel at a target berth. The target vessel includes a GNSS (Global Navigation Satellite System) / INS (Light Detection and Ranging) composite navigation device and a target radar that uses electromagnetic waves to measure the relative positions of surrounding objects. The automatic berthing device includes: A point cloud generation unit, based on the positioning results of the target vessel's position and attitude obtained by the GNSS / INS composite navigation device, converts the relative coordinates of each point in the point cloud corresponding to the ranging result obtained by the target radar at the target berth into absolute coordinates. Based on the converted absolute coordinates of each point, it generates an object map representing both the navigable area at the target berth and the absolute coordinates of the point cloud. The navigation scheme generation unit, when the target position and target attitude of the target vessel at the target berth are specified based on the target map, generates a plan to guide the target vessel to the target position in the target attitude based on the target map and the position and attitude of the target vessel located by the GNSS / INS composite navigation device. Invention Effects

[0007] According to this disclosure, the point cloud generation unit generates an object map based on the observation results of the object radar, which serves as a map representing the navigable area at the object berth and also as a map representing absolute coordinates. The navigation scheme generation unit generates a plan based on the object map to guide the object vessel to the target position in the target attitude. The generated plan can also be used to automatically guide the object vessel to shore. Therefore, according to this disclosure, in the automatic berthing technology for ships, it is possible to generate a map representing absolute coordinates and automatically guide berthing based on the generated map representing absolute coordinates. Attached Figure Description

[0008] Figure 1 This is a diagram illustrating a structural example of the automatic docking system 90 according to Embodiment 1. Figure 2 This is a diagram illustrating an example of the functional structure of the automatic docking system 90 according to Embodiment 1. Figure 3 This is a diagram illustrating an example of the functional structure of the automatic docking system 90 according to Embodiment 1. Figure 4 A diagram illustrating the processing of the velocity distribution unit 131 according to Embodiment 1. Figure 5 This diagram illustrates the processing of the radar / hull coordinate transformation unit 132 according to Embodiment 1. Figure 6 This is a diagram illustrating an example of the functional structure of the position and attitude calibration device 300 according to Embodiment 1. Figure 7 A diagram illustrating the coordinate system. Figure 8 This is a diagram illustrating an example of the hardware structure of the automatic docking device 100 according to Embodiment 1. Figure 9This is a flowchart illustrating the operation of the position and attitude calibration device 300 according to Embodiment 1. Figure 10 This is a diagram illustrating the algorithm for 3-DOF two-dimensional navigation involved in Implementation Method 1. Figure 11 This is a diagram illustrating the algorithm for 6-DOF three-dimensional navigation involved in Implementation Method 1. Figure 12 This is a flowchart illustrating the operation of the automatic docking system 90 according to Embodiment 1. Figure 13 This is a diagram illustrating use case 1 related to implementation method 1. Figure 14 This is a diagram illustrating use case 1 related to implementation method 1. Figure 15 This is a diagram illustrating use case 2 related to implementation method 1. Figure 16 This is a diagram illustrating use case 2 related to implementation method 1. Figure 17 This is a diagram illustrating use case 2 related to implementation method 1. Figure 18 This is a diagram illustrating use case 2 related to implementation method 1. Figure 19 This is a diagram illustrating use case 2 related to implementation method 1. Figure 20 This is a diagram illustrating use case 2 related to implementation method 1. Figure 21 This is a diagram illustrating an example of the hardware structure of the automatic docking device 100 according to a variation of Embodiment 1. Detailed Implementation

[0009] In the description and drawings of the embodiments, the same elements and corresponding elements are labeled with the same reference numerals. The descriptions of elements labeled with the same reference numerals are appropriately omitted or simplified. The arrows in the figures mainly indicate the flow of data or the process flow. In addition, "part" may be appropriately replaced with "device", "loop", "process", "step", "processing" or "circuit".

[0010] Implementation method 1. Hereinafter, this embodiment will be described in detail with reference to the accompanying drawings. The purpose of this implementation is to automate the docking of large leisure cruise ships or sightseeing boats to the pier when they return to port. An automatic navigation system is installed on the ship that is the subject of this embodiment. Therefore, the ship can automatically navigate to any location by indicating waypoints. In addition, the ship is equipped with various sensors such as millimeter-wave radar, GNSS (Global Navigation Satellite System) receiver, and IMU (Inertial Measurement Unit).

[0011] ***Structure Explanation*** Figure 1 This illustrates a structural example of the automated docking system 90 according to this embodiment. For example... Figure 1 As shown, the automatic berthing system 90 consists of a target vessel 20. The target vessel 20 is equivalent to a mobile body. The target vessel 20 is equipped with a millimeter-wave radar 50, an automatic berthing device 100, and a GNSS / INS (Inertial Navigation System) composite navigation device 200. As a specific example, the target vessel 20 can be a small recreational boat, a regular cruise ship, or a high-end cruise ship equipped with twin engines. The target vessel 20 can be equipped with any number of millimeter-wave radars 50. The Automated Landing System 90 is equipped with a millimeter-wave radar whose detection capability is not reduced in adverse weather conditions, a GNSS / INS composite navigation device that can reduce the effects of multipath propagation, and a navigation scheme generation device for automatic navigation control. As a specific example, the millimeter-wave radar 50 is a radar that uses the 76 to 77 GHz frequency band or the 77 to 81 GHz FMCW (Frequency Continuous Modulation) method, and is a radar capable of measuring distance, velocity, and angular velocity separately. The millimeter-wave radar 50 is also referred to as a millimeter-wave sensor.

[0012] In this embodiment, automatic docking is achieved through two stages: a map generation stage using millimeter-wave radar 50 and a guidance stage using the positioning results from the GNSS / INS composite navigation device 200 to determine the position and attitude. Specific examples of guidance control for automatic docking are described in References 1 and 2.

[0013] [Reference 1] International Publication No. 2018-100751 [Reference 2] Japanese Patent Application Publication No. 2020-059403

[0014] In this embodiment, to mitigate wave sway, the millimeter-wave radar 50 is spatially stabilized using the azimuth and attitude angles represented by the positioning results of the GNSS / INS composite navigation device 200. Spatial stabilization of the millimeter-wave radar 50 involves assigning the attitude angles represented by the positioning results of the GNSS / INS composite navigation device 200 to the millimeter-wave radar 50 and measuring the horizontal direction within the millimeter-wave radar 50. The horizontal direction is represented by the X-axis and Y-axis. The map is generated in real time during the execution of the berthing sequence. The berthing sequence consists of an approach step, a map generation step, a path planning step, and a guidance and control step. Furthermore, in the approach step, the target vessel 20 travels at a low speed, and it is assumed that the relative angle of the target vessel 20 with respect to the shore is 45° to 60°. As a specific example, the map is a linear map that may include obstacles such as other moored vessels. As a specific example, the map's coordinate system is an absolute coordinate system (UTM (Universal Transverse Mercator) coordinate system that includes obstacles such as other ships). To ensure map accuracy and reliability, as a specific example, when generating the map, the following conditions must be met: the measurement distance between the millimeter-wave radar 50 and the berth must be less than 21 meters, and the berth must exist within an angle range of -45° to +45° from the antenna center of the millimeter-wave radar 50. The conditions related to this measurement distance are equivalent to the ranging reference distance conditions. The conditions related to this angle range are equivalent to the ranging reference angle conditions. To automate route planning, as a concrete example, the map format represents berths and approximate polygonal points of vessels moored at those berths, which are approximate polygonal points on a horizontal plane as observed from the target vessel 20. The map format consists of node numbers and XY coordinates. In this embodiment, as a countermeasure against satellites being invisible in places such as rivers or under bridges, positioning accuracy is ensured by combining millimeter-wave radar 50 with GNSS / INS composite navigation device 200. In this embodiment, in the GNSS / INS composite navigation device 200, a millimeter-wave radar 50 is used instead of GNSS to measure the distance and bearing from the target vessel 20 to the shore, thereby ensuring positioning accuracy even when the satellite is not visible. In this embodiment, a millimeter-wave radar 50 is used for stopping determination to prevent the accumulation of internal errors and ensure positioning accuracy. In this embodiment, a magnetic azimuth meter can also be used to measure the absolute orientation to reduce attitude error.

[0015] Figure 2 This illustrates a functional structure example of the automatic docking system 90 described in this embodiment. like Figure 2 As shown, the automatic berthing device 100 includes a target vessel 20. It includes a data collection unit 110, a point cloud generation unit 120, a speed and bearing detection unit 130, and a navigation scheme generation unit 140. Furthermore, the automatic berthing device 100 stores map data 190. The automatic berthing device 100 controls the automatic berthing of a target vessel 20 equipped with a GNSS / INS composite navigation device 200 and target radar at a target berth. The target radar is a radar that uses electromagnetic waves to measure the relative positions of surrounding objects. A specific example of target radar is LiDAR (Light Detection and Ranging) or millimeter-wave radar 50. The target berth is also called a dock berth. A target berth can be a dock berth or a service berth. Furthermore, LiDAR's detection capabilities decrease in adverse weather conditions, posing risks such as failure to navigate autonomously or detection errors during autonomous navigation, potentially leading to collisions. Additionally, LiDAR's positioning relies on detecting objects to calculate its own position relative to them, thus requiring the presence of detectable objects in the surrounding environment. On the other hand, millimeter-wave radar 50 offers a simpler approach compared to LiDAR. For example, when using millimeter-wave radar 50, there is no need to distinguish between shorelines and ships.

[0016] The data collection unit 110 collects data representing the observation results from the millimeter-wave radar 50.

[0017] The point cloud generation unit 120 generates a point cloud based on the data collected by the data collection unit 110. Specifically, the point cloud corresponds to a bank, a pier, and obstacles. The point cloud generation unit 120 includes a point cloud extraction unit 121, a coordinate transformation unit 122, and a map generation unit 123. The point cloud extraction unit 121 extracts point clouds from the data collected by the data collection unit 110. The coordinates of the extracted point clouds are relative coordinates. The coordinate transformation unit 122 converts the point cloud coordinates extracted by the point cloud extraction unit 121 into absolute coordinates. The map generation unit 123 generates a map representing the absolute coordinates obtained after transformation by the coordinate transformation unit 122, and adds the data representing the generated map to the map data 190. In other words, the point cloud generation unit 120, based on the positioning results of the target vessel 20 obtained by the GNSS / INS composite navigation device 200 in determining its position and attitude, converts the relative coordinates of each point in the point cloud corresponding to the result obtained by the target radar in ranging at the target berth into absolute coordinates. Based on the converted absolute coordinates of each point, it generates an object map representing both the navigable area at the target berth and the absolute coordinates. The point cloud extracted by the point cloud extraction unit 121 corresponds to the result obtained by the target radar in ranging at the target berth. When the target radar performs ranging at the target berth, the distance between the target vessel 20 and the berth wall can also be a distance corresponding to the shape of the target berth. When each point in the point cloud is set as the target point, typically, at the moment the millimeter-wave radar 50 measures the target point, the distance between the millimeter-wave radar 50 and the target point satisfies the ranging reference distance condition, and the angle formed by the center of the millimeter-wave radar 50 and the target point satisfies the ranging reference angle condition.

[0018] The velocity and bearing detection unit 130 includes a velocity distribution unit 131 and a radar / hull coordinate conversion unit 132. The hull coordinates are also referred to as vehicle coordinates.

[0019] The navigation scheme generation unit 140 is equivalent to a navigation scheme generation device, which includes a target location input unit 141, a path planning unit 142, and a guidance control unit 143. After generating map data 190, the target position input unit 141 receives input of the target position of the target vessel 20. The target position input unit 141 may also receive input of the target attitude of the target vessel 20 at the target position. The target position input unit 141 may also automatically calculate the target attitude at the target position based on the received target position and map data 190. The path planning unit 142 generates a plan representing a path that guides the object vessel 20 from its current position to the target position received by the target position input unit 141. This plan ensures that the object vessel 20 reaches the target position in the target attitude. At this time, the path planning unit 142 considers the size of the object vessel 20 and its possible directions of movement. Furthermore, the path planning unit 142 utilizes the common MAKLINK method as a specific example. The MAKLINK method is a graph theory algorithm that automatically generates paths connecting obstacles that are approximately convex polygons to a start and end point. The path planning unit 142 can also generate multiple waypoints based on a real-time generated map. Each waypoint represents the position and attitude of the object vessel 20. The guidance control unit 143 controls the target vessel 20 according to the plan generated by the path planning unit 142. As a specific example, the guidance control unit 143 controls the position and attitude of the target vessel 20 so that the position and attitude of the target vessel 20 follow the waypoints shown in the generated plan. In other words, when the target position and attitude of the vessel 20 at the target berth are specified based on the object map, the navigation scheme generation unit 140 generates a plan to guide the vessel 20 to the target position in the target attitude, based on the object map and the position and attitude of the vessel 20 measured by the GNSS / INS composite navigation device 200. Furthermore, the navigation scheme generation unit 140 guides the vessel 20 according to the generated plan.

[0020] Map data 190 is map data representing the area around object vessel 20 using absolute coordinates, and it represents the location where object vessel 20 is moored.

[0021] The GNSS / INS composite navigation device 200 transmits positioning data indicating the position and attitude of the target vessel 20 to the automatic berthing device 100. In addition, the GNSS / INS composite navigation device 200 receives information indicating the speed and bearing of the target vessel 20 as feedback. The GNSS / INS composite navigation device 200 corrects the positioning results based on the velocity and azimuth observed by the millimeter-wave radar 50.

[0022] Figure 3 This illustrates a functional structure example of the automatic docking system 90 described in this embodiment. like Figure 3 As shown, the millimeter-wave radar 50 includes a synthesizer 51, an oscillator 52, a millimeter-wave transceiver antenna 53, a mixer 54, a signal processing unit 55, and a symbol acquisition and tracking unit 56.

[0023] Synthesizer 51 generates a chirped signal that represents the amplitude of the signal from oscillator 52 as a function of time.

[0024] The millimeter-wave transceiver antenna 53 has both a transmitting antenna and a receiving antenna. The transmitting antenna sends a chirp signal. If an object reflects the chirp signal, a reflected chirp signal will be generated, which will then be captured by the receiving antenna.

[0025] Mixer 54 generates an intermediate frequency signal by appropriately combining the transmitted and received signals.

[0026] The signal processing unit 55 performs Fourier transform processing on the intermediate frequency signal to detect the beat frequency. Here, since the beat frequency is related to distance, the distance to the object is calculated based on the beat frequency. In addition, phase information can also be obtained simultaneously by performing Fourier transform processing. Based on the obtained phase information, the velocity Doppler can be calculated. Therefore, based on the result of Fourier transform processing, the relative velocity with the object can also be calculated.

[0027] The symbol acquisition and tracking unit 56 calculates the correlation between the encoded object and the symbol candidates represented by the scan results based on the intensity of the radar echo (reflecting object). When a symbol candidate with a high correlation to the encoded object is found, the symbol acquisition and tracking unit 56 sets a prediction gate for the next scan based on the velocity information of the found symbol candidate, thereby maintaining object tracking. In addition, each radar echo is assigned information representing each of the following: symbol ID, Doppler angle of arrival, Doppler velocity, echo intensity, and distance calculated based on the velocity integral of the previous frame.

[0028] The velocity distribution unit 131 randomly selects two points from the symbols output by the symbol acquisition and tracking unit 56, and calculates the coefficients of y = Vs × cos(x - α) using the least squares method, which represent each of the Doppler velocity Vs and the Doppler angle of arrival α. Specifically, the velocity distribution unit 131 selects the coefficients that maximize the number of points where the residual relative to the fitted curve is within a threshold. As a fitting method, the Random Sample Consensus (RANSAC) algorithm shown in Reference 3 is known as a specific example. By fitting the velocity distribution in this way, the roughness of both the angular resolution and velocity resolution of the millimeter-wave radar 50 can be compensated for, thus improving the accuracy of both velocity and angle.

[0029] [Reference 3] M. Fischler, R. Bolles. “Random sample consensus: a paradigm for model fitting with applications to image analysis and automated cartography.” Communications of the ACM, 1981.

[0030] Figure 4 This shows a specific example of the actual result of fitting the code symbols. Figure 4In the diagram, each point represents a symbol, and the curve represents the fitting result. Although there are relatively large deviations in the symbols, the X-axis value at the extreme value of the fitting result can be obtained as the Doppler angle of arrival, and the Y-axis value at that extreme value can be obtained as the Doppler velocity. Therefore, although the angular resolution and velocity resolution of the millimeter-wave radar 50 are both relatively coarse, each of the angular deviation and velocity deviation can be reduced.

[0031] Radar / hull coordinate transformation unit 132 performs as follows Figure 5 The coordinate transformation is shown. In Figure 5 In this context, let the X-axis and Y-axis be the X-axis and Y-axis of the ship's coordinate system, and let the center of IMU40 be located at the position where the X-axis and Y-axis are orthogonal. The radar / hull coordinate transformation unit 132 transforms the velocity Vs and azimuth α coordinates of the phase shift center of the radar antenna into the hull coordinate system of the moving body, and as shown in [Mathematical Equation 1], converts them from polar coordinates to Vb (X-axis velocity) and ω (Z-axis angular velocity) on the XY axes. Here, β represents the angle formed by the nose and the antenna centerline. ly represents the Y-axis lever arm between the nose and the antenna center. lx represents the X-axis lever arm between the antenna center and the center of IMU40. The center of the XY axes is the center of IMU40. The model of the object ship 20 is, as a specific example, the Ackerman model. The radar / hull coordinate transformation unit 132 outputs data representing the velocity and angular velocity after coordinate transformation to the GNSS / INS composite navigation device 200.

[0032] [Mathematical Expression 1]

[0033] Alternatively, the automatic berthing device 100 may also include a moving average unit. When the automatic berthing device 100 includes a moving average unit, the radar / hull coordinate transformation unit 132 uses the result calculated by the moving average unit. The moving average section reduces random angle measurement errors by using moving averages over multiple periods. The following is a specific example of the moving average unit, as shown in [Mathematical Formula 2], using a five-period moving average for each of the Doppler velocity Vs and the Doppler angle of arrival α. As a specific example, when the millimeter-wave symbol is updated in 10Hz increments, the moving average unit maintains the data corresponding to the past four periods and calculates the moving average of the current period and the past four periods (a total of five data points) using the operation shown in [Mathematical Formula 2], outputting the calculated value in 10Hz increments.

[0034] [Mathematical Expression 2]

[0035] The point cloud generation unit 120 converts the point cloud with relative coordinates into a point cloud with absolute coordinates, and appends data representing the map formed by the converted point cloud to the map data 190. The point cloud generation unit 120 may also update at least a portion of the map data 190 based on the map formed by the converted point cloud. The target location input unit 141 receives the target location input from the host system or the user. The route planning unit 142 sends the data of each waypoint represented by the planned route to the guidance and control unit 143. The guidance and control unit 143 guides and controls the target vessel 20 so that the target vessel 20 passes through each waypoint represented by the data received from the route planning unit 142 in sequence. Alternatively, the areas occupied by other ships can be considered in the map generation and the setting of each waypoint.

[0036] like Figure 3 As shown, the GNSS / INS integrated navigation device 200 includes a GNSS antenna 29, a GNSS receiver 30, an IMU 40, a strapdown computing unit 220, a satellite observation data estimation unit 230, and an error estimation unit 240. The GNSS / INS integrated navigation device 200 appropriately utilizes the technology disclosed in Reference 4. The GNSS / INS integrated navigation device 200 is also referred to as a GNSS / INS combined positioning device.

[0037] [Reference 4] Japanese Patent Application Publication No. 2012-193965

[0038] The IMU40, also known as an inertial sensor, measures acceleration and angular velocity in three axes. The three axes refer to the length, width, and height of the moving body. Hereinafter, the acceleration in the three axes will be referred to as "three-dimensional acceleration," and the angular velocity in the three axes will be referred to as "three-dimensional angular velocity." The IMU40 includes an acceleration sensor 41 and an angular velocity sensor 42. The object, the ship 20, and the IMU40 are integrally formed as a rigid body. Accelerometer 41 measures acceleration. The angular velocity sensor 42 measures the angular velocity of the azimuth angle. GNSS receiver 30 receives the signal received by GNSS antenna 29. The strapdown computing unit 220 calculates velocity, position, attitude, and orientation based on data acquired by the accelerometer 41 and the angular velocity sensor 42. A specific example of the strapdown computing unit 220 is described in reference 4. Satellite observation data speculation department 230 speculates on GNSS observation data. The error estimation unit 240 estimates the errors contained in the GNSS positioning data based on data received from the automatic docking device 100 or the magnetic orientation sensor 60, and the position, velocity, and attitude calculated by the strapdown calculation unit 220. The error estimation unit 240 includes a high-precision positioning calculation unit 241. As a specific example, the high-precision positioning calculation unit 241 uses a Kalman filter to infer errors and corrects the position, speed and attitude of the target ship 20 based on the inference results. The GNSS receiver 30, IMU 40, and positioning processor constitute the GNSS / INS composite navigation device 200. The positioning processor is a processor that performs calculations for positioning. The GNSS receiver 30 receives positioning enhancement information from quasi-zenith satellites or wireless communication. Based on the positioning enhancement information, the GNSS receiver 30 performs correction processing on the positioning signals received from the GNSS satellites, achieving centimeter-level high-precision position calibration.

[0039] The magnetic azimuth sensor 60, as a specific example, is a relatively simple sensor that uses the Earth's geomagnetism to determine azimuth. The GNSS / INS composite navigation device 200 can correct its positioning results based on the azimuth observed by the magnetic azimuth sensor 60. Switch 61 is used to switch the output of the magnetic orientation sensor 60 and the output of the radar / ship coordinate conversion unit 132. Switch 61 is switched by the host system or the user. A magnetic azimuth sensor 60 for measuring bearing can also be installed on the target vessel 20 to replace the millimeter-wave radar 50. The following explains the use of the magnetic azimuth sensor 60's output in the GNSS / INS composite navigation device 200. The GNSS / INS composite navigation device 200 uses angular velocity calculated based on the time-varying azimuth measured by the magnetic azimuth sensor 60 instead of the velocity and angular velocity measured by the millimeter-wave radar 50. The azimuth data output by the magnetic azimuth sensor 60 is converted into angular velocity data by performing the operation shown in [Mathematical Formula 3]. Here, ΔΨ represents the angular velocity data at time t+1, Ψt represents the azimuth data at time t, and Ψt+1 represents the azimuth data at time t+1. ΔΨ is equivalent to the angular velocity calculated based on the time-varying azimuth measured by the magnetic azimuth sensor 60.

[0040] [Mathematical Expression 3]

[0041] When using the magnetic azimuth sensor 60, it is impossible to correct for velocity errors compared to using the millimeter-wave radar 50. However, the high-precision positioning calculation unit 241 corrects for the angular velocity error of the strapdown calculation unit 220. Therefore, under these circumstances, azimuth value drift caused by gyroscope errors can be avoided, and the position of the target ship 20 can be calibrated with relatively high accuracy.

[0042] Around the target vessel 20, there are relatively few magnetic objects such as iron towers, buildings, and cars. Therefore, by using a magnetic orientation sensor 60 instead of millimeter-wave radar 50, the same effect can be achieved as when using millimeter-wave radar 50. In addition, the automatic docking system 90 can also use millimeter-wave radar 50 and magnetic orientation sensor 60 together.

[0043] Furthermore, the millimeter-wave radar 50 cannot consistently capture the pier during the approach to and berthing process. Therefore, when the positional relationship between the millimeter-wave radar 50 and the berth meets certain conditions, a map is generated, and a path plan is created based on the generated map and the ship's position. Subsequently, based on the waypoints shown in the path plan, and the ship's own position calibration and attitude calculation performed by the GNSS / INS composite navigation device 200, the ship berths. As a specific example, the certain conditions refer to the measurement distance between the millimeter-wave radar 50 and the berth being less than 21 meters, and the berth being located within an angle of -45° to +45° from the antenna center of the millimeter-wave radar 50. By inputting the velocities of the X and Y axes in the ship's coordinate system, which can be detected by the millimeter-wave radar 50, into the GNSS / INS composite navigation device 200, the accuracy of the velocity and attitude calculations performed by the GNSS / INS composite navigation device 200 can be improved. By using the position, velocity, and attitude shown by the positioning results of the GNSS / INS composite navigation device 200, absolute coordinates are assigned to the ranging points of the millimeter-wave radar 50, and a three-dimensional map representing the absolute coordinates can be generated in real time. The navigation scheme generation unit 140 generates a navigation scheme based on a real-time generated 3D map, which shows a path that avoids obstacles and has a relatively short travel distance. In addition, the navigation scheme generation unit 140 updates and corrects the navigation scheme each time based on the position and attitude shown by the positioning results of the GNSS / INS composite navigation device 200.

[0044] The GNSS / INS composite navigation device 200 functions as a position and attitude calibration device 300. The position and attitude calibration device 300 calibrates the position of a moving body equipped with a GNSS receiver 30, a millimeter-wave radar 50, an accelerometer 41, and an angular velocity sensor 42. The position and attitude calibration device 300 uses data acquired from the GNSS receiver 30, data acquired from the IMU 40, and data acquired from the millimeter-wave radar 50 to calibrate each of the moving body's position, attitude, and azimuth. The position and attitude calibration device 300 is also referred to as a self-position and attitude calibration device.

[0045] Figure 6 This illustrates an example of the functional structure of the position and attitude calibration device 300. For example... Figure 6 As shown, the position and attitude calibration device 300 includes an initial value calculation unit 310, a calibration unit 320, a Kalman filter unit 330, a strapdown arithmetic unit 220, and a storage unit 390.

[0046] The initial value calculation unit 310 controls the Kalman filter unit 330 to calculate the initial value of the azimuth angle. Specifically, firstly, the initial value calculation unit 310 calculates the azimuth of the target vessel 20 as the first navigation azimuth based on the angular velocity of the azimuth measured by the angular velocity sensor 42. Next, based on the calculated first navigation azimuth, the initial value calculation unit 310 integrates the velocity and angular velocity measured by the millimeter-wave radar 50 to calculate the change in the two-dimensional coordinates. Then, the initial value calculation unit 310 adds the calculated change to the two-dimensional initial position value, thereby calculating the two-dimensional coordinate value of the target vessel 20 as the first navigation coordinate value. The two-dimensional initial position value is the initial position of the navigation coordinate system. Next, based on the calculated first navigation coordinate value and the two-dimensional coordinate value contained in the coordinate value located by the GNSS receiver 30, the initial value calculation unit 310 uses a first Kalman filter to calculate a correction value for correcting the first navigation azimuth. Finally, the initial value calculation unit 310 corrects the first navigation azimuth using the calculated correction value and outputs the corrected first navigation azimuth as the initial azimuth value. In other words, the initial value calculation unit 310 calculates the initial value of the azimuth angle of the target vessel 20, i.e., the initial azimuth angle value, based on the measurement values ​​of the angular velocity sensor 42, the measurement values ​​of the millimeter-wave radar 50, and the positioning results of the GNSS receiver 30. More specifically, the initial value calculation unit 310 calculates the first navigation azimuth angle based on the angular velocity measured by the angular velocity sensor 42, calculates the first two-dimensional navigation coordinate value based on the calculated first navigation azimuth angle, the velocity and angular velocity measured by the millimeter-wave radar 50, and the two-dimensional initial position value, and corrects the first navigation azimuth angle based on the calculated two-dimensional first navigation coordinate value and the positioning results of the GNSS receiver 30, thereby calculating the initial azimuth angle value.

[0047] Here, using Figure 7 Explain the coordinate system. The initial position of the navigation coordinate system is the center position of the IMU40 when the power is turned on. Normally, (0,0) is set as the "two-dimensional initial position value" and (0,0,0) is set as the "three-dimensional initial position value". In addition, such as Figure 7 As shown, the X, Y, and Z axes of the hull coordinate system of the object vessel 20 at the initial attitude angle are used as the X, Y, and Z axes of the navigation coordinate system, respectively. At this time, the position of the object vessel 20 in the two-dimensional navigation coordinate system is the amount of movement of the object vessel 20, calculated by adding the amounts of movement along the X and Y axes in the hull coordinate system. Furthermore, the position of the object vessel 20 in the three-dimensional navigation coordinate system is also the amount of movement of the object vessel 20, calculated by adding the amounts of movement along the X, Y, and Z axes in the hull coordinate system. like Figure 7 As shown, the ship coordinate system is a coordinate system with the center of IMU40 as the origin, the forward direction of the object ship 20 as the X-axis, the rightward direction of the object ship 20 relative to the forward direction as the Y-axis, and the vertical downward direction of the object ship 20 as the Z-axis.

[0048] The calibration unit 320 includes a calibration unit 321 for movement, a calibration unit 322 for stopping, and a stopping determination unit 323. The calibration unit 320 calibrates the position of the target vessel 20 based on the measurements from the angular velocity sensor 42, the initial azimuth angle, the measurements from the acceleration sensor 41, and the composite navigation positioning result from the GNSS receiver 30. More specifically, the calibration unit 320 calculates the change in azimuth angle based on the angular velocity measured by the angular velocity sensor 42, calculates a second navigation azimuth angle based on the calculated change in azimuth angle and the initial azimuth angle, calculates a three-dimensional second navigation coordinate value based on the calculated second navigation azimuth angle, the acceleration measured by the acceleration sensor 41, and the three-dimensional initial position value, and corrects the second navigation coordinate value based on the calculated three-dimensional second navigation coordinate value and the positioning result from the GNSS receiver 30, thereby calibrating the position of the target vessel 20. The three-dimensional initial position value is the initial position of the navigation coordinate system. During navigation, the calibration unit 321 controls the Kalman filter unit 330 and the strapdown calculation unit 220 to calibrate (calculate) the position, attitude, and bearing of the target vessel 20. The navigation calibration unit 321 is also referred to as the calibration value calculation unit. When the target vessel 20 is stationary, the calibration unit 322 controls the Kalman filter unit 330 and the strapdown arithmetic unit 220 to calibrate the position, attitude, and bearing of the target vessel 20. The calibration unit 322 is also known as the bearing correction unit. The determination department 323 determines whether vessel 20 has stopped or started sailing.

[0049] Specifically, firstly, the calibration unit 320 calculates the change in azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor 42. Based on the calculated change in azimuth angle and the output initial azimuth angle value, the azimuth angle of the target vessel 20 is used as the second navigation azimuth angle for calculation. Next, based on the calculated second navigation azimuth angle, the calibration unit 320 converts the three-dimensional acceleration measured by the acceleration sensor 41 into three-dimensional acceleration in the navigation coordinate system. Then, the calibration unit 320 integrates the converted three-dimensional acceleration, calculates the change in the three-dimensional coordinates, and adds the calculated change to the initial three-dimensional position value, thereby using the three-dimensional coordinate value of the target vessel 20 as the second navigation coordinate value for calculation. Next, based on the calculated second navigation coordinate value and the three-dimensional coordinate value measured by the GNSS receiver 30, the calibration unit 320 uses a second Kalman filter to calculate a correction value for correcting the second navigation coordinate value. Next, the calibration unit 320 uses the calculated correction value to correct the second navigation coordinate value, and outputs the corrected second navigation coordinate value as the position calibration value obtained after calibrating the position of the target vessel 20. Furthermore, when the moving vessel 20 stops moving, the calibration unit 320, based on the previously calculated second navigation azimuth and the currently calculated second navigation azimuth, uses the third Kalman filter to calculate an azimuth correction value to correct the currently calculated second navigation azimuth, and uses the calculated azimuth correction value to correct the currently calculated second navigation azimuth, and then calculates the corrected second navigation azimuth. Here, the third Kalman filter uses the correction value that makes the previously calculated second navigation azimuth and the currently calculated second navigation azimuth consistent as the azimuth correction value for calculation. The third Kalman filter generates an observation equation representing the difference between the previously calculated second navigation azimuth and the currently calculated second navigation azimuth, and uses the generated observation equation to calculate the azimuth correction value. Furthermore, when the stationary vessel 20 resumes movement, the calibration unit 320 first calculates a new second navigation azimuth angle based on the corrected second navigation azimuth angle and the change in the new azimuth angle. Based on the calculated new second navigation azimuth angle, the new three-dimensional acceleration measured by the accelerometer 41 is converted into three-dimensional acceleration in the navigation coordinate system. Next, the calibration unit 320 integrates the converted three-dimensional acceleration, calculates the change in the three-dimensional coordinates, and adds the calculated change to the previously calculated second navigation coordinate value to calculate a new second navigation coordinate value. Then, based on the calculated new second navigation coordinate value and the new three-dimensional coordinate value measured by the GNSS receiver 30, the calibration unit 320 uses a second Kalman filter to calculate a position correction value for correcting the new second navigation coordinate value. The calculated position correction value is used to correct the new second navigation coordinate value, and the corrected new second navigation coordinate value is output as a new position calibration value. When the speed measured by the millimeter-wave radar 50 is less than a predetermined speed threshold, the calibration unit 320 determines that the target vessel 20 should stop moving. After the target vessel 20 stops moving, if the speed measured by the millimeter-wave radar 50 is above the speed threshold, the calibration unit 320 determines that the target vessel 20 should resume moving.

[0050] The Kalman filter unit 330 generates at least one of the observation equation and the state equation, and uses at least one of the generated observation equation and the state equation to calculate the correction values ​​for various data. The Kalman filter unit 330 includes a first Kalman filter 331, a second Kalman filter 332, and a third Kalman filter 333 as multiple Kalman filters with different observation equations. The satellite observation data inference unit 230 performs the processing included in the Kalman filter unit 330. The error prediction unit 240 is equivalent to the first Kalman filter 331, the second Kalman filter 332, and the third Kalman filter 333.

[0051] The strapdown computing unit 220 calculates velocity, position, attitude, and orientation based on acceleration data 393 and angular velocity data 394. Specific examples of the strapdown computing unit 220 are described in Patent Document 1.

[0052] The storage unit 390 stores GNSS data 391, velocity / angular velocity data 392, acceleration data 393, and angular velocity data 394. GNSS data 391 consists of data observed by GNSS receiver 30. The velocity / angular velocity data 392 consists of data representing the velocity and angular velocity observed by the millimeter-wave radar 50. Acceleration data 393 consists of data observed by acceleration sensor 41. Angular velocity data 394 is composed of data observed by angular velocity sensor 42.

[0053] When the position and attitude calibration device 300 uses the output of the magnetic azimuth sensor 60 instead of the output of the millimeter-wave radar 50, the position and attitude calibration device 300 uses azimuth data instead of velocity / angular velocity data 392. That is, in this case, the position and attitude calibration device 300 uses the angular velocity calculated based on the time change of azimuth measured by the magnetic azimuth sensor 60 instead of the velocity and angular velocity measured by the millimeter-wave radar 50.

[0054] Figure 8 This illustrates an example of the hardware structure of the automatic docking device 100 according to this embodiment. The automatic docking device 100 is configured with a computer. The automatic docking device 100 may also be configured with multiple computers.

[0055] As shown in the figure, the automatic docking device 100 is a computer that includes hardware such as a processor 11, a memory 12, an auxiliary storage device 13, an input / output interface 14, and a communication device 15. These hardware components are appropriately connected via signal lines 19.

[0056] Processor 11 is an IC (Integrated Circuit) that performs computational processing and controls the hardware included in the computer. As a specific example, processor 11 is a CPU (Central Processing Unit), a DSP (Digital Signal Processor), or a GPU (Graphics Processing Unit). The automated docking device 100 may also include multiple processors instead of processor 11. The multiple processors share the functions of processor 11.

[0057] Memory 12 is typically a volatile storage device, a specific example being RAM (Random Access Memory). Memory 12 is also referred to as main storage or main memory. Data stored in memory 12 is saved to secondary storage device 13 as needed.

[0058] Auxiliary storage device 13 is typically a non-volatile storage device, such as ROM (Read-Only Memory), HDD (Hard Disk Drive), or flash memory. Data stored in auxiliary storage device 13 is loaded into memory 12 as needed. The memory 12 and the auxiliary storage device 13 can also be integrated into one unit.

[0059] Input / output IF14 is a port that connects input and output devices. As a specific example, input / output IF14 can be a USB (Universal Serial Bus) terminal. As a specific example, input devices are a keyboard and mouse. As a specific example, output devices are displays.

[0060] Communication device 15 is a receiver and a transmitter. As a specific example, communication device 15 is a communication chip or NIC (Network Interface Card).

[0061] When the various parts of the automatic docking device 100 communicate with other devices, the input / output IF14 and the communication device 15 can be used appropriately.

[0062] Auxiliary storage device 13 stores the automatic docking program. The automatic docking program is a program that enables the computer to implement the various functions of the automatic docking device 100. The automatic docking program is loaded into memory 12 and executed by processor 11. The functions of the various parts of the automatic docking device 100 are implemented through software.

[0063] Data used during the execution of the automatic docking procedure, as well as data obtained through the execution of the automatic docking procedure, are appropriately stored in a storage device. Various parts of the automatic docking device 100 appropriately utilize storage devices. As a specific example, the storage device comprises at least one of a memory 12, an auxiliary storage device 13, a register within the processor 11, and a cache memory within the processor 11. Furthermore, the terms "data" and "information" sometimes have the same meaning. The storage device may also be a device independent of the computer. The functions of memory 12 and auxiliary storage device 13 can also be implemented by other storage devices.

[0064] The automated docking program can also be recorded on a computer-readable, non-volatile recording medium. As specific examples, the non-volatile recording medium could be an optical disc or flash memory. The automated docking program can also be provided as a program product.

[0065] ***Instructions for the Actions*** Figure 9 This is a flowchart illustrating an example of the operation of the position and attitude calibration device 300. Figure 9 To illustrate the operation of the position and attitude calibration device 300.

[0066] (Step S11) When the target vessel 20 departs, the initial value calculation unit 310 calculates the initial value of the azimuth angle using 3-DOF two-dimensional navigation.

[0067] (Step S12) Before the target vessel 20 stops, the calibration unit 321 uses the initial value of the azimuth angle or the most recently calibrated azimuth angle to calibrate the position, attitude and bearing of the target vessel 20 through 6-DOF three-dimensional navigation.

[0068] (Step S13) If the target vessel 20 is stationary, the position and attitude calibration device 300 proceeds to step S14. Otherwise, the position and attitude calibration device 300 proceeds to step S12.

[0069] (Step S14) During the period when the target vessel 20 is stationary, the calibration unit 322 calibrates the position, attitude, and bearing of the target vessel 20 using a 6-DOF three-dimensional navigation method based on the extended ZUPT (Zero Velocity Update) algorithm. A summary of the extended ZUPT algorithm is disclosed in Patent Document 1.

[0070] (Step S15) If the target vessel 20 is stationary, the position and attitude calibration device 300 proceeds to step S14. Otherwise, the position and attitude calibration device 300 proceeds to step S12.

[0071] Figure 10 This illustrates the 3-DOF two-dimensional navigation algorithm described in Implementation 1. This algorithm is similar to that in Patent Document 1. Figure 7 The algorithms shown are the same. The following are... Figure 10 The following explanation is provided. Additionally, the initial value calculation unit 310 includes an input correction unit 311, an orientation update unit 312, a coordinate transformation unit 313, a latitude and longitude update unit 314, and an output correction unit 315.

[0072] The input correction unit 311 corrects the velocity (velocity vector) represented by the velocity / angular velocity data 392 by adding the correction value calculated by the Kalman filter unit 330 to the velocity represented by the velocity / angular velocity data 392. In addition, the input correction unit 311 corrects the angular velocity of the azimuth angle by adding the correction value calculated by the Kalman filter unit 330 to the angular velocity of the "azimuth angle" represented by the angular velocity data 394.

[0073] The azimuth update unit 312 calculates the change in azimuth angle by integrating the angular velocity of the azimuth angle corrected by the input correction unit 311. The azimuth update unit 312 adds the calculated change in azimuth angle to the initial value of the azimuth angle or the azimuth angle previously calculated by the output correction unit 315, thereby calculating a new azimuth angle. Here, the initial value of the azimuth angle can be an appropriate value.

[0074] Based on the attitude and orientation calculated by the orientation update unit 312, the coordinate transformation unit 313 uses a prescribed transformation formula to convert the velocity, corrected by the input correction unit 311, from a value in the sensor coordinate system to a value in the navigation coordinate system. Thus, a two-dimensional velocity in the navigation coordinate system can be obtained. The two-dimensional velocity consists of the velocity in the latitude direction and the velocity in the longitude direction.

[0075] The latitude and longitude update unit 314 integrates the two-dimensional velocity obtained by the coordinate transformation unit 313 to calculate the change in the two-dimensional coordinates. The two-dimensional coordinates consist of latitude and longitude components. The latitude and longitude update unit 314 adds the calculated change in the two-dimensional coordinates to the initial value of the two-dimensional coordinates or the two-dimensional coordinates previously calculated by the output correction unit 315, thereby calculating the new two-dimensional coordinates. As a specific example, the latitude and longitude update unit 314 uses the two-dimensional coordinate values ​​located by the GNSS receiver 30 as the initial values ​​of the two-dimensional coordinates before the target vessel 20 departs, that is, when the target vessel 20 stops.

[0076] First, the Kalman filter unit 330 calculates the difference between the two-dimensional coordinates represented by the GNSS data 391 and the two-dimensional coordinates calculated by the latitude and longitude update unit 314 as the "coordinate residual". Next, the Kalman filter unit 330 calculates the difference between the two-dimensional velocity represented by the GNSS data 391 and the two-dimensional velocity obtained by the coordinate transformation unit 313 as the "velocity residual". Next, the Kalman filter unit 330 takes the calculated coordinate residual and velocity residual as input and executes the first Kalman filter 331 to calculate the correction values ​​for velocity, angular velocity, two-dimensional coordinates and azimuth angle respectively.

[0077] The output correction unit 315 corrects the two-dimensional coordinate values ​​by adding the correction value calculated by the Kalman filter unit 330 to the two-dimensional coordinate values ​​calculated by the latitude and longitude update unit 314, and outputs the corrected two-dimensional coordinate values. Furthermore, the output correction unit 315 corrects the azimuth angle by adding the correction value calculated by the Kalman filter unit 330 to the azimuth angle calculated by the azimuth update unit 312, and outputs the corrected azimuth angle.

[0078] As described above, in 3-DOF two-dimensional navigation, since the velocity measured by the millimeter-wave radar 50 is used instead of the acceleration measured by the accelerometer 41, no velocity integration error occurs. Furthermore, even if the initial azimuth angle is unsuitable, a suitable value can be immediately calculated as the azimuth angle. This is because the angular velocity measured by the millimeter-wave radar 50 is used instead of the angular velocity measured by the angular velocity sensor 42. Therefore, according to the 3-DOF two-dimensional navigation, the integral calculation for obtaining the two-dimensional coordinates only needs to be performed once, the influence of the error of the initial value of the azimuth angle for the coordinate transformation of velocity is relatively small, and the two-dimensional coordinate values ​​and azimuth angle can be appropriately corrected by using a Kalman filter.

[0079] Figure 11 This illustrates the 6-DOF three-dimensional navigation algorithm described in Implementation 1. This algorithm is similar to that in Patent Document 1. Figure 9 The algorithms shown are the same. The following are... Figure 11 The following explanation will be provided. Additionally, the calibration unit 320 includes an input calibration unit 324 and an output calibration unit 325.

[0080] The input correction unit 324 corrects the three-dimensional acceleration by adding the correction value calculated by the Kalman filter unit 330 to the three-dimensional acceleration represented by the acceleration data 393. In addition, the input correction unit 324 corrects the three-dimensional angular velocity by adding the correction value calculated by the Kalman filter unit 330 to the three-dimensional angular velocity represented by the angular velocity data 394.

[0081] The strapdown computing unit 220 uses the three-dimensional acceleration and the three-dimensional angular velocity corrected by the input correction unit 324 to calculate the position, attitude and bearing of the target ship 20.

[0082] The output correction unit 325 corrects the position by adding the correction value calculated by the Kalman filter unit 330 to the position calculated by the strapdown calculation unit 220, and outputs the corrected position as the calibration result position. Furthermore, the output correction unit 325 corrects the attitude by adding the correction value calculated by the Kalman filter unit 330 to the attitude calculated by the strapdown calculation unit 220, and outputs the corrected attitude as the attitude angle of the calibration result. Furthermore, the output correction unit 325 corrects the azimuth by adding the correction value calculated by the Kalman filter unit 330 to the azimuth calculated by the strapdown calculation unit 220, and outputs the corrected azimuth as the azimuth angle of the calibration result.

[0083] The stopping determination unit 323 determines whether the target vessel 20 has stopped based on the speed calculated from the observation results of the millimeter-wave radar 50.

[0084] If the target vessel 20 does not stop, the Kalman filter unit 330 calculates the difference between the position shown in the GNSS data 391 and the position calculated by the strapdown calculation unit 220 as the "position residual". Next, the Kalman filter unit 330 calculates the difference between the velocity represented by the velocity / angular velocity data 392 and the velocity calculated by the strapdown calculation unit 220 as the "velocity residual", and calculates the difference between the angular velocity represented by the velocity / angular velocity data 392 and the angular velocity calculated by the strapdown calculation unit 220 as the "angular velocity residual". Next, the Kalman filter unit 330 uses the calculated coordinate residual and velocity residual as input values ​​and executes the second Kalman filter 332 to calculate the correction values ​​for three-dimensional acceleration, three-dimensional angular velocity, position, and attitude, respectively.

[0085] In step S14, during 6-DOF 3D navigation, a third Kalman filter 333 with an extended ZUPT algorithm is executed.

[0086] Figure 12 This is a flowchart illustrating an example of the process for generating a navigation scheme in an automated docking system 90. Figure 12 The process for generating the navigation scheme is explained. The operational steps of each device included in the automatic berthing system 90 are equivalent to the automatic berthing method. Furthermore, the program for implementing the operations of each device included in the automatic berthing system 90 is equivalent to the automatic berthing program.

[0087] (Step S101) Each millimeter-wave radar 50 measures the distance around itself and measures the Doppler velocity along the X and Y axes.

[0088] (Step S102) Each millimeter-wave radar 50 inputs the ranging results of its surrounding area into the automatic docking device 100.

[0089] (Step S103) The automatic docking device 100 inputs the Doppler velocity measurement results of each millimeter-wave radar 50 into the GNSS / INS composite navigation device 200.

[0090] (Step S104) The GNSS / INS composite navigation device 200 calculates the position, velocity, and attitude of the target ship 20 at time t.

[0091] (Step S105) The GNSS / INS composite navigation device 200 inputs the calculated position, speed and attitude of the target vessel 20 into the automatic docking device 100.

[0092] (Step S106) The point cloud generation unit 120 generates a three-dimensional map as map data 190, which represents absolute coordinates, based on data input from each millimeter-wave radar 50 and data input from the GNSS / INS composite navigation device 200.

[0093] (Step S107) If the target coordinates and target attitude have been set, the automatic docking system 90 proceeds to step S108. Otherwise, the automatic docking system 90 returns to step S101.

[0094] (Step S108) Based on the generated 3D map and the current position, speed and attitude of the object ship 20, the path planning unit 142 generates a reasonable path plan and attitude change plan that avoids obstacles as a navigation scheme to reach the target coordinates in the target attitude.

[0095] (Step S109) The path planning unit 142 compares the predetermined position and predetermined attitude in the generated navigation plan with the currently measured position and attitude of the target vessel 20.

[0096] (Step S110) The route planning unit 142 corrects the navigation scheme based on the comparison results of step S109.

[0097] (Step S111) If there is an obstacle at the target coordinates, the automatic docking system 90 proceeds to step S112. Otherwise, the automatic docking system 90 returns to step S113.

[0098] (Step S112) The path planning unit 142 will reset the coordinates and attitude that can avoid obstacles near the target coordinates to the target coordinates and target attitude, respectively.

[0099] (Step S113) If the target vessel 20 reaches the target coordinates and attitude in the navigation scheme, the automatic berthing system 90 proceeds to step S114. Otherwise, the automatic berthing system 90 returns to step S101.

[0100] (Step S114) The automatic docking system has completed the generation of the navigation scheme at 90.

[0101] Figure 13 This is a diagram illustrating the operational sequence of the automated berthing system 90 in Use Case 1. As a concrete example, Use Case 1 is the case of a high-end cruise ship with two engines berthing at the service terminal. Use Case 1 represents the most easily implemented basic use case. Figure 13 Let's illustrate a specific example of use case 1. In this example, the general MAKLINK method is used. exist Figure 13 The diagram shows the symbol map area corresponding to the antenna center, the symbol map area corresponding to the area within ±45 degrees of the antenna center, the LOS of the antenna center, and the range within ±45 degrees of the antenna coverage area. LOS is an abbreviation for Line of Sight.

[0102] (1) Begin to approach The target vessel 20 begins its approach to the berth at the dock.

[0103] (2) Start map generation Map generation begins when vessel 20 is 15 meters away from the berth. Map generation continues as vessel 20 travels parallel to the shoreline of the berth.

[0104] (3) Complete map generation While generating the map, the object ship 20 moves forward in a straight line for a distance of about 1 to 2 ship lengths to complete the map generation.

[0105] (4) Implement path planning The target vessel 20 performs path planning based on the generated map and its position and attitude. Additionally, it is assumed that the target position and target attitude of the target vessel 20 have already been set.

[0106] (5) Automatic docking The target vessel 20 automatically berths based on the implemented route planning.

[0107] Figure 14 This is a diagram illustrating the operational sequence of the automated berthing system 90 in Use Case 1, and it includes waypoints and specific map examples. In this example, the target coordinates and attitude are shown, and other vessels are present as obstacles at the berth. The map shows multiple nodes in this example. Lines connecting adjacent nodes represent inaccessible lines. Inaccessible lines indicate the end of an area inaccessible to the target vessel 20, corresponding to the boundary between the navigable area and the inaccessible line. Each node represents an observation from the millimeter-wave radar 50, and as a specific example, represents part of a quay, pier, or obstacle. Figure 14 In the middle, the area closer to the target vessel 20 is the navigable area, relative to the unnavigable line.

[0108] Furthermore, if the target vessel 20 detects a difference in map shape during the automatic berthing process, it immediately stops the automatic berthing. The difference in map shape refers to the difference between the unnavigable line shown on the map generated before the automatic berthing is implemented and the unnavigable line observed by the millimeter-wave radar 50 during the automatic berthing process. As a specific example, the difference in map shape may arise because other vessels that were moored before the automatic berthing began to move. Here, in this embodiment, since the map uses an absolute coordinate system, the similarity between past and current maps can always be verified, and differences in map shape can be detected. On the other hand, in the prior art, since the map uses a relative coordinate system, it is impossible to verify the similarity between past and current maps. Additionally, the unnavigable line can also be a three-dimensional surface. After the host system or user confirms the safety of the area around the target vessel 20, the automatic docking system 90 updates the map based on the observation results of the millimeter-wave radar 50 and restarts from the running sequence (4). In other words, during the guidance of the target vessel 20, the target radar performs ranging at the target berth, and the GNSS / INS composite navigation device 200 determines the position and attitude of the target vessel 20. During the guidance of the target vessel 20, the point cloud generation unit 120 generates an updated map based on the ranging results from the target radar at the target berth and the position and attitude determination results from the GNSS / INS composite navigation device 200. The updated map represents the navigable area at the target berth and is a map showing absolute coordinates. When there is a difference between the navigable area shown in the updated map and the navigable area shown in the target map, the navigation scheme generation unit 140 stops guiding the target vessel 20.

[0109] Figure 15 This diagram illustrates the operational sequence of the automated berthing system 90 in Use Case 2. As a concrete example, Use Case 2 depicts a high-end cruise ship equipped with two engines berthing at a home port berth. Use Case 2 corresponds to the standard scenario during actual operation. The processing flow in Use Case 2 is the same as that in Use Case 1. Furthermore, in use case 1, the target vessel 20 can approach the shore quite closely to generate a map. However, in use case 2, if the target vessel 20 attempts to approach the shore, it may collide with the pier. Therefore, the user or the host system needs to change the docking mode according to the use case. The user can use input / output devices to set the use cases. Alternatively, based on the target location, a pre-defined table of use cases based on the target location can be used to automatically set use cases near the target location.

[0110] Figures 16 to 20This is a diagram illustrating the chronological sequence of operations of the automated docking system 90 in Use Case 2. Figures 16 to 20 Here is a specific example illustrating this running sequence. In this example, the general MAKLINK method is used.

[0111] (1) Begin to approach like Figure 16 As shown, the target vessel 20 moves towards the direction where the shore wall exists in order to search for the edge of the pier. Furthermore, the docking mode of the target vessel 20 has been set to the mode corresponding to use case 2.

[0112] (2) Start map generation like Figure 17 As shown, after observing the edge of the pier, the target vessel 20 moves in the direction where it believes there is a berth edge in order to search for the berth edge. like Figure 18 As shown, after determining the width of the home port berth, the target vessel 20 proceeds to search within the detected home port berth. The width of the home port berth is calculated based on the observed edges of the pier and the observed edges of the berth. like Figure 19 As shown, the target vessel 20 enters the detected home port berth and the shape of the home port berth is drawn into a map. The subsequent processing is the same as in use case 1. Additionally, Figure 20 The generated map, target coordinates and attitude, and waypoints shown on the planned route are displayed.

[0113] ***Explanation of the effects of Implementation Method 1*** According to this embodiment, since a map showing the docking position can be automatically generated in real time, there is no need to register the docking position in advance. Furthermore, according to this embodiment, even if other vessels are docked at the berth, it is possible to dock at any berth while automatically avoiding other vessels. According to this embodiment, since the navigation scene can be generated using absolute coordinates, in applications where the docking position is fixed, such as cruise ship operations, the docking coordinates can be pre-specified from the control system. Therefore, the user does not need to manually set the docking coordinates. Furthermore, the user can manually set the docking coordinates when the docking position changes. According to this embodiment, the docking position can be specified using absolute coordinates. Therefore, according to this embodiment, navigation plans can also be generated in areas outside the ranging range of the millimeter-wave radar 50 and in areas where no map has been generated.

[0114] ***Other Structures*** <Variation Example 1> Figure 21This illustrates a hardware structure example of the automatic docking device 100 involved in this variation. The automatic docking device 100 includes a processing circuit 18 in place of the processor 11, the processor 11 and memory 12, the processor 11 and auxiliary storage device 13, or the processor 11, memory 12 and auxiliary storage device 13. The processing circuit 18 is the hardware that implements at least a portion of the components included in the automatic docking device 100. The processing circuit 18 can be dedicated hardware, or it can be a processor that executes a program stored in the memory 12.

[0115] In the case where the processing circuit 18 is dedicated hardware, the processing circuit 18, as a specific example, corresponds to a single circuit, a composite circuit, a programmed processor, a parallel programmed processor, an ASIC (Application Specific Integrated Circuit), an FPGA (Field Programmable Gate Array), or a combination thereof. The automatic docking device 100 may also include multiple processing circuits instead of processing circuit 18. The multiple processing circuits share the functions of processing circuit 18.

[0116] In the automatic docking device 100, some functions can be implemented by dedicated hardware, while the remaining functions can be implemented by software or firmware.

[0117] As a specific example, the processing circuit 18 can be implemented by hardware, software, firmware, or a combination thereof. The processor 11, memory 12, auxiliary storage device 13, and processing circuit 18 are collectively referred to as the "processing circuit group". In other words, the functions of each functional component of the automatic docking device 100 are implemented by the processing circuit group.

[0118] ***Other Implementation Methods*** Although Embodiment 1 has been described, multiple parts may be combined in this embodiment. Alternatively, this embodiment may be implemented in parts. Furthermore, this embodiment can be modified as needed, and may be implemented in whole or in part in any way. Furthermore, the above embodiments are essentially preferred examples and are not intended to limit the scope of this disclosure and its applicability and uses. The steps described using flowcharts and other similar methods can also be appropriately modified. Label Explanation

[0119] 11 Processor, 12 Memory, 13 Auxiliary Storage Device, 14 Input / Output (IF), 15 Communication Device, 18 Processing Circuit, 19 Signal Line, 20 Target Ship, 29 GNSS Antenna, 30 GNSS Receiver, 40 IMU, 41 Accelerometer, 42 Angular Velocity Sensor, 50 Millimeter-Wave Radar, 51 Synthesizer, 52 Oscillator, 53 Millimeter-Wave Transceiver Antenna, 54 Mixer, 55 Signal Processing Unit, 56 Symbol Acquisition and Tracking Unit, 60 Magnetic Azimuth Sensor, 61 Switch, 90 Automatic Berthing System, 100 Automatic Berthing Device, 110 Data Collection Unit, 120 Point Cloud Generation Unit, 121 Point Cloud Extraction Unit, 122 Coordinate Transformation Unit, 123 Map Generation Unit, 130 Velocity and Azimuth Detection Unit, 131 Velocity Distribution Unit, 132 Radar / hull coordinate transformation unit, 140 navigation scheme generation unit, 141 target position input unit, 142 path planning unit, 143 guidance and control unit, 190 map data, 200 GNSS / INS composite navigation device, 220 strapdown computing unit, 230 satellite observation data estimation unit, 240 error estimation unit, 241 high-precision positioning calculation unit, 300 position and attitude calibration device, 310 initial value calculation unit, 311 input correction unit, 312 bearing update unit, 313 coordinate transformation unit, 314 latitude and longitude update unit, 315 output correction unit, 320 calibration unit, 321 calibration unit during movement, 322 calibration unit when stopped, 323 stop determination unit, 324 input correction unit, 325 output correction unit, 330 Kalman filter unit, 331 first Kalman filter, 332 second Kalman filter, 333 Third Kalman filter, 390 storage unit, 391 GNSS data, 392 velocity / angular velocity data, 393 acceleration data, 394 angular velocity data.

Claims

1. An automatic berthing device for controlling the automatic berthing of a target vessel at a target berth, the target vessel including a GNSS / INS composite navigation device and a target radar that uses electromagnetic waves to measure the relative positions of surrounding objects, the automatic berthing device being characterized by comprising: The point cloud generation unit, based on the positioning results of the target ship by the GNSS / INS composite navigation device, converts the relative coordinates of each point in the point cloud corresponding to the result obtained by the target radar after ranging at the target berth into absolute coordinates. Based on the absolute coordinates of each point after conversion, it generates an object map as a map representing the navigable area at the target berth and as a map representing the absolute coordinates. as well as The navigation scheme generation unit, when the target position and target attitude of the target vessel at the target berth are specified based on the target map, generates a plan to guide the target vessel to the target position in the target attitude based on the target map and the position and attitude of the target vessel located by the GNSS / INS composite navigation device.

2. The automatic docking device as described in claim 1, characterized in that, When the target radar performs ranging at the target berth, the distance between the target vessel and the shore wall of the target berth is a distance corresponding to the shape of the target berth.

3. The automatic docking device as described in claim 1 or 2, characterized in that, The target radar is a millimeter-wave radar.

4. The automatic docking device as described in claim 3, characterized in that, When each point in the point cloud is taken as an object point, at the moment when the millimeter-wave radar measures the distance between the object point and the millimeter-wave radar, the distance between the millimeter-wave radar and the object point satisfies the ranging reference distance condition, and the angle formed by the center of the millimeter-wave radar and the object point satisfies the ranging reference angle condition.

5. The automatic docking device as described in claim 3 or 4, characterized in that, The GNSS / INS composite navigation device corrects the positioning results based on the velocity and azimuth observed by the millimeter-wave radar.

6. The automatic docking device as described in any one of claims 1 to 5, characterized in that, The target vessel includes a magnetic orientation sensor. The GNSS / INS composite navigation device corrects its positioning results based on the orientation observed by the magnetic orientation sensor.

7. The automatic docking device as described in any one of claims 1 to 6, characterized in that, The navigation scheme generation unit guides the target vessel according to the generated plan. During the guidance of the target vessel, the target radar performs ranging at the target berth, and the GNSS / INS composite navigation device determines the position and attitude of the target vessel. The point cloud generation unit, while the target vessel is being guided, generates an updated map, representing both a navigable area at the target berth and an absolute coordinate map, based on the ranging results from the target radar at the target berth and the positioning results from the GNSS / INS composite navigation device. When there is a difference between the navigable area represented by the updated map and the navigable area represented by the object map, the navigation scheme generation unit stops guiding the object vessel.

8. An automatic docking system, characterized in that, include: The GNSS / INS composite navigation device; Millimeter-wave radar as the target radar; as well as The automatic docking device as described in any one of claims 1 to 7.

9. An automatic berthing method, executed by an automatic berthing device, said automatic berthing device being a computer controlling the automatic berthing of a target vessel at a target berth, said target vessel including a GNSS / INS composite navigation device and a target radar using electromagnetic waves to measure the relative positions of surrounding objects, the automatic berthing method being characterized in that... The automatic berthing device, based on the position and attitude of the target vessel determined by the GNSS / INS composite navigation device, converts the relative coordinates of each point in the point cloud corresponding to the distance measured by the target radar at the target berth into absolute coordinates. Based on the converted absolute coordinates, it generates an object map representing both the navigable area at the target berth and the absolute coordinates. When the automatic berthing device specifies the target position and target attitude of the target vessel at the target berth based on the target map, it generates a plan to guide the target vessel to the target position in the target attitude based on the target map and the position and attitude of the target vessel located by the GNSS / INS composite navigation device.

10. An automatic berthing procedure executed by an automatic berthing device, the automatic berthing device being a computer controlling the automatic berthing of a target vessel at a target berth, the target vessel including a GNSS / INS composite navigation device and a target radar using electromagnetic waves to range the relative positions of surrounding objects, the automatic berthing procedure being characterized in that the automatic berthing device executes: Point cloud generation processing, based on the positioning results of the target vessel by the GNSS / INS composite navigation device, converts the relative coordinates of each point in the point cloud corresponding to the ranging result obtained by the target radar at the target berth into absolute coordinates. Based on the converted absolute coordinates of each point, an object map is generated, representing both the navigable area at the target berth and the absolute coordinates. The navigation scheme generation process generates a plan to guide the target vessel to the target position in the target attitude when the target position and target attitude of the target vessel at the target berth are specified based on the target map and the position and attitude of the target vessel located by the GNSS / INS composite navigation device.

Images