Automatic docking device, automatic docking system, automatic docking method, and automatic docking program
The automatic berthing system addresses the challenge of generating navigation scenarios for ships by using GNSS/INS and millimeter wave radar to create absolute coordinate maps, enabling accurate and user-independent ship guidance in fixed berthing scenarios.
Patent Information
- Application Number
- PCT/JP2023/023759
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- Filing Date
- 2023-06-27
- Publication Date
- 2025-01-02
AI Technical Summary
Conventional automatic berthing technologies face challenges in generating a navigation scenario for ships without pre-defined berthing positions and require manual input of coordinates, especially in fixed berthing scenarios, due to reliance on relative coordinates and LiDAR, which is affected by multipath and requires detectable objects.
An automatic berthing system using a composite navigation device combining GNSS/INS with millimeter wave radar to generate absolute coordinate maps and guide ships to berth, eliminating the need for manual input by creating navigable area maps and navigation plans based on absolute coordinates.
Enables automatic berthing without pre-defined positions, improves navigation accuracy, and reduces user intervention by generating real-time maps and navigation scenarios using absolute coordinates, ensuring accurate and reliable ship guidance.
Smart Images

Figure JP2023023759_02012025_PF_FP_ABST
Abstract
Description
Automatic docking device, automatic docking system, automatic docking method, and automatic docking program
[0001] The present disclosure relates to an automatic docking device, an automatic docking system, an automatic docking method, and an automatic docking program.
[0002] In conventional technology related to automatic docking of ships, GNSS (Global Navigation Satellite System) positioning is used when the ship's position is far from the docking position. However, GNSS positioning is susceptible to multipath interference. Furthermore, there may be many structures around the docking position. Therefore, when the ship approaches the docking position, automatic navigation is switched to LiDAR (Light Detection and Ranging, optical sensor)-based navigation. Patent Document 1 discloses an automatic docking device using LiDAR.
[0003] Japanese Patent Application Laid-Open No. 2022-106753
[0004] According to the technology disclosed in Patent Document 1, relative coordinates are used after switching to automatic navigation using LiDAR. Therefore, this technology has a problem in that it is not possible to generate a navigation scenario for automatic docking unless the vessel moves to a location where a map can be generated using LiDAR. Furthermore, this technology requires the user to input the docking location. Therefore, this technology has a problem in that it is time-consuming to set the docking location every time, especially for a vessel with a fixed docking location such as a cruise ship.
[0005] The present disclosure aims to provide an automatic docking technology for ships by generating a map showing absolute coordinates and automatically guiding the ship to dock based on the generated map showing the absolute coordinates.
[0006] The automatic docking device according to the present disclosure is an automatic docking device that controls automatic docking of a target ship at a target berth, the target ship being equipped with a GNSS (Global Navigation Satellite System) / INS (Inertial Navigation System) hybrid navigation device and a target radar that measures the relative positions of surrounding objects using electromagnetic waves, and includes a point cloud generation unit that converts relative coordinates corresponding to each point of a point cloud corresponding to the results of distance measurement at the target berth by the target radar into absolute coordinates based on the results of measurement of the position and attitude of the target ship by the GNSS / INS hybrid navigation device, and generates a target map that shows absolute coordinates, which is a map showing a navigable area at the target berth, based on the absolute coordinates corresponding to each converted point; and a navigation scenario generation unit that, when a target position and target attitude of the target ship at the target berth are specified based on the target map, generates a plan to guide the target ship to reach the target position at the target attitude based on the target map and the position and attitude of the target ship measured by the GNSS / INS integrated navigation device.
[0007] According to the present disclosure, the point cloud generation unit generates a target map, which is a map showing absolute coordinates and indicates a navigable area in a target berth, based on the observation results of the target radar. The navigation scenario generation unit generates a plan for guiding a target ship to reach a target position with a target attitude based on the target map. The generated plan may be used to automatically guide the target ship to dock. Therefore, according to the present disclosure, in an automatic docking technology for a ship, a map showing absolute coordinates is generated, and automatic docking guidance can be performed based on the generated map showing absolute coordinates.
[0008] 1 is a diagram showing an example of the configuration of an automatic docking system 90 according to the first embodiment. 2 is a diagram showing an example of the functional configuration of the automatic docking system 90 according to the first embodiment. 3 is a diagram showing an example of the functional configuration of the automatic docking system 90 according to the first embodiment. 4 is a diagram explaining the processing of a speed profile unit 131 according to the first embodiment. 5 is a diagram explaining the processing of a radar / body coordinate conversion unit 132 according to the first embodiment. 6 is a diagram showing an example of the functional configuration of a position and attitude localization device 300 according to the first embodiment. 7 is a diagram explaining a coordinate system. 8 is a diagram showing an example of the hardware configuration of an automatic docking device 100 according to the first embodiment. 9 is a flowchart showing the operation of the position and attitude localization device 300 according to the first embodiment. 10 is a diagram explaining an algorithm for three-degrees-of-freedom two-dimensional navigation according to the first embodiment. 11 is a diagram explaining an algorithm for six-degrees-of-freedom three-dimensional navigation according to the first embodiment. 12 is a flowchart showing the operation of the automatic docking system 90 according to the first embodiment. 13 is a diagram explaining use case 1 according to the first embodiment. 14 is a diagram explaining use case 2 according to the first embodiment. 15 is a diagram explaining use case 2 according to the first embodiment. Fig. 10 is a diagram for explaining a use case 2 according to the embodiment 1. Fig. 11 is a diagram for explaining a use case 2 according to the embodiment 1. Fig. 12 is a diagram showing an example of the hardware configuration of the automatic docking device 100 according to a modified example of the embodiment 1.
[0009] In the description of the embodiments and drawings, the same elements and corresponding elements are given the same reference numerals. The description of elements given the same reference numerals will be omitted or simplified as appropriate. Arrows in the drawings mainly indicate the flow of data or the flow of processing. Furthermore, "unit" may be read as "device," "circuit," "step," "procedure," "process," or "circuitry" as appropriate.
[0010] Embodiment 1. This embodiment will be described in detail below with reference to the drawings. This embodiment aims to automate the docking of a vessel, such as a large leisure cruiser or a tourist boat, at a marina pier when returning to port. The vessel targeted by this embodiment is equipped with an automatic navigation system. Therefore, the vessel can automatically navigate to any point by specifying a waypoint. The vessel is also equipped with various sensors, such as a millimeter-wave radar, a GNSS (Global Navigation Satellite System) receiver, and an IMU (Inertial Measurement Unit).
[0011] ***Description of Configuration*** Figure 1 shows an example of the configuration of an automatic docking system 90 according to this embodiment. As shown in Figure 1, the automatic docking system 90 comprises a target vessel 20. The target vessel 20 is a moving body. The target vessel 20 is equipped with a millimeter-wave radar 50, an automatic docking device 100, and a GNSS / INS (Inertial Navigation System) integrated navigation system 200. Specific examples of the target vessel 20 include a small leisure boat, a general cruiser, or a high-end cruiser with two engines. The target vessel 20 may be equipped with any number of millimeter-wave radars 50. The automatic docking system 90 is equipped with a "millimeter-wave radar" whose detection capability is not impaired even in bad weather, a "GNSS / INS integrated navigation system" that can reduce the effects of multipath, and a "navigation scenario generation device" for automatic navigation control. The millimeter-wave radar 50 is, for example, a radar that employs a frequency-continuous modulation wave (FMCW) system in the 76 to 77 GHz band or the 77 to 81 GHz band, and is capable of measuring distance, velocity, and angular velocity. The millimeter-wave radar 50 is also called a millimeter-wave sensor.
[0012] In this embodiment, automatic docking is realized in two stages: a map generation stage using the millimeter wave radar 50, and a guidance stage using the position and attitude indicated by the positioning results of the GNSS / INS integrated navigation system 200. Specific examples of guidance control for automatic docking are described in References 1 and 2.
[0013] [Reference 1] International Publication No. 2018-100751 Pamphlet [Reference 2] Japanese Patent Application Laid-Open No. 2020-059403
[0014] In this embodiment, to mitigate wave motions, the millimeter-wave radar 50 is spatially stabilized by using the azimuth and attitude angle indicated by the positioning results of the GNSS / INS integrated navigation system 200. Spatial stabilization of the millimeter-wave radar 50 involves assigning the attitude angle indicated by the positioning results of the GNSS / INS integrated navigation system 200 to the millimeter-wave radar 50 data and measuring the horizontal direction using 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 execution of the docking sequence. The docking sequence consists of an approach step, a map generation step, a route planning step, and a guidance control step. Note that in the approach step, the target vessel 20 navigates at a low speed, and the relative angle of the target vessel 20 with respect to the shore is assumed to be between 45° and 60°. The map is, for example, a linear map that may include obstacles such as other ships at anchor. The map coordinate system is, for example, an absolute coordinate system (UTM (Universal Transverse Mercator) coordinates) that includes obstacles such as other ships. To ensure the accuracy and reliability of the map, for example, when the map is generated, the measured distance between the millimeter-wave radar 50 and the docking berth must be 21 m or less, and the docking berth must be located within an angle range of -45° to +45° from the center of the antenna of the millimeter-wave radar 50. The condition regarding this measured distance corresponds to the distance measurement reference condition. The condition regarding this angle range corresponds to the angle measurement reference condition. To automate route planning, the map format is, for example, broken line approximation points indicating the docking berth and ships moored at the docking berth, which are broken line approximation points on a horizontal plane as seen from the target ship 20. The map format consists of node numbers and X and Y coordinates. In this embodiment, as a countermeasure for satellites not being visible under rivers or bridges, etc., positioning accuracy is ensured by combining the millimeter-wave radar 50 with the GNSS / INS integrated navigation system 200. In this embodiment, the GNSS / INS integrated navigation system 200 uses the millimeter-wave radar 50 instead of GNSS to measure the distance and direction from the target ship 20 to the shore, thereby ensuring positioning accuracy even when satellites are not visible.In this embodiment, the millimeter wave radar 50 is used to determine whether the vehicle has stopped, preventing the accumulation of internal errors and ensuring positioning accuracy. In this embodiment, a magnetic compass may be used to measure absolute orientation, thereby reducing attitude errors.
[0015] FIG. 2 shows an example of the functional configuration of an automatic docking system 90 according to this embodiment. As shown in FIG. 2 , the automatic docking device 100 includes a target vessel 20. The automatic docking device 100 includes a data collection unit 110, a point cloud generation unit 120, a speed and direction detection unit 130, and a navigation scenario generation unit 140. The automatic docking device 100 also stores map data 190. The automatic docking device 100 controls automatic docking of the target vessel 20, which includes a GNSS / INS integrated navigation system 200 and a target radar, at a target berth. The target radar is a radar that measures the relative position of surrounding objects using electromagnetic waves. Specific examples of the target radar include a LiDAR (Light Detection and Ranging) radar or a millimeter-wave radar 50. The target berth is also called a marina berth. The target berth may be a home berth or a service berth. It should be noted that the detection capability of LiDAR is reduced in bad weather, which may result in situations where automatic navigation is not possible or in a risk of collision due to detection errors during automatic navigation. Furthermore, since positioning using LiDAR involves detecting some kind of object and calculating the vehicle's relative position, there must be a detectable object in the vicinity. On the other hand, when using millimeter-wave radar 50, processing is simpler than when using LiDAR. As a specific example, when using millimeter-wave radar 50, there is no need to distinguish between a quay and a ship.
[0016] The data collection unit 110 collects data indicating the observation results obtained by 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. Specific examples of the point cloud correspond to a quay, a pier, and an obstacle. The point cloud generation unit 120 includes a point cloud extraction unit 121, a coordinate conversion unit 122, and a map generation unit 123. The point cloud extraction unit 121 extracts a point cloud from the data collected by the data collection unit 110. The coordinates of the extracted point cloud are relative coordinates. The coordinate conversion unit 122 converts the coordinates of the point cloud extracted by the point cloud extraction unit 121 into absolute coordinates. The map generation unit 123 generates a map indicating the absolute coordinates converted by the coordinate conversion unit 122, and adds data indicating the generated map to the map data 190. In other words, based on the results of the GNSS / INS integrated navigation system 200 measuring the position and attitude of the target ship 20, the point cloud generation unit 120 converts relative coordinates corresponding to each point of the point cloud corresponding to the results of distance measurement by the target radar at the target berth into absolute coordinates, and generates a target map, which is a map showing absolute coordinates and indicates the navigable area at the target berth, based on the absolute coordinates corresponding to each converted point. The point cloud extracted by the point cloud extraction unit 121 corresponds to the results of distance measurement by the target radar at the target berth. When the target radar measures distance at the target berth, the distance between the target ship 20 and the quay of the target berth may be a distance corresponding to the shape of the target berth. When each point of the point cloud is a target point, typically, at the time when the millimeter-wave radar 50 measures distance to the target point, the distance between the millimeter-wave radar 50 and the target point satisfies the distance measurement reference distance condition, and the angle formed by the center of the millimeter-wave radar 50 and the target point satisfies the distance measurement reference angle condition.
[0018] The speed and direction detection unit 130 includes a speed profile unit 131 and a radar / body coordinate conversion unit 132. Body coordinates are also called vehicle body coordinates.
[0019] The navigation scenario generation unit 140 corresponds to a navigation scenario generation device and includes a target position input unit 141, a route planning unit 142, and a guidance control unit 143. The target position input unit 141 accepts input of a target position of the target ship 20 after the map data 190 is generated. The target position input unit 141 may accept input of a target attitude of the target ship 20 at the target position. The target position input unit 141 may automatically calculate the target attitude at the target position based on the accepted target position and the map data 190. The route planning unit 142 generates a plan indicating a route for guiding the target ship 20 from the current position of the target ship 20 to the target position accepted by the target position input unit 141. The plan is a plan for the target ship 20 to arrive at the target position at the target attitude. At this time, the route planning unit 142 takes into consideration the size of the target ship 20 and the direction in which the target ship 20 can move. Furthermore, the route planning unit 142 uses the general MAKLINK method as a specific example. The MAKLINK method is a type of graph method that automatically generates a route connecting obstacles approximated by convex polygons and a start and goal. The route planning unit 142 may generate multiple waypoints based on a map generated in real time. Each waypoint indicates the position and attitude of the target ship 20. The guidance control unit 143 controls the target ship 20 according to the plan generated by the route planning unit 142. As a specific example, the guidance control unit 143 controls the position and attitude of the target ship 20 so that the position and attitude of the target ship 20 follow each waypoint indicated in the generated plan. In other words, when a target position and target attitude of the target ship 20 at the target berth are specified based on the target map, the navigation scenario generation unit 140 generates a plan to guide the target ship 20 to arrive at the target position at the target attitude based on the target map and the position and attitude of the target ship 20 measured by the GNSS / INS integrated navigation system 200. The navigation scenario generation unit 140 also guides the target ship 20 according to the generated plan.
[0020] The map data 190 is data of a map showing the surroundings of the target ship 20 using absolute coordinates, and is data showing the location where the target ship 20 is anchored.
[0021] The GNSS / INS combined navigation system 200 transmits positioning data indicating the position and attitude of the target ship 20 to the automatic docking device 100. The GNSS / INS combined navigation system 200 also receives, as feedback, information indicating the speed and direction of the target ship 20. The GNSS / INS combined navigation system 200 corrects the positioning results of the GNSS / INS combined navigation system 200 based on the speed and direction observed by the millimeter-wave radar 50.
[0022] 3 shows an example of the functional configuration of the automatic berthing system 90 according to this embodiment. As shown in FIG. 3, the millimeter-wave radar 50 includes a synthesizer 51, an oscillator 52, a millimeter-wave transmitting / receiving antenna 53, a mixer 54, a signal processing unit 55, and a symbol acquisition and tracking unit 56.
[0023] The synthesizer 51 generates a chirp signal that represents the amplitude of the signal from the oscillator 52 as a function of time.
[0024] The millimeter-wave transmitting / receiving antenna 53 includes a transmitting antenna and a receiving antenna. The transmitting antenna emits a chirp signal. When an object reflects the chirp signal, a reflected chirp signal is generated, and the generated chirp signal is captured by the receiving antenna.
[0025] The mixer 54 generates an intermediate frequency signal by appropriately combining the transmission signal and the reception signal.
[0026] The signal processor 55 detects the beat frequency by performing a Fourier transform on the intermediate frequency signal. Since the beat frequency correlates with distance, the distance to the target is calculated based on the beat frequency. Furthermore, the Fourier transform also provides phase information, and the velocity Doppler can be calculated based on the obtained phase information. Therefore, the relative velocity to the target can also be calculated based on the results of the Fourier transform.
[0027] The symbol acquisition and tracking unit 56 calculates the correlation between the symbolized target and the symbol candidate indicated by the scan result based on the intensity of the radar echo (reflecting object). If a symbol candidate highly correlated with the symbolized target is found, the symbol acquisition and tracking unit 56 maintains tracking of the target by setting a prediction gate for the next scan based on the velocity information of the found symbol candidate. Each radar echo is assigned information indicating the symbol ID, Doppler angle of arrival, Doppler velocity, echo intensity, and distance calculated from the integral of velocity from the previous frame.
[0028] The velocity profile unit 131 randomly selects two points from the symbols output by the symbol acquisition and tracking unit 56 and calculates the Doppler velocity Vs and the Doppler angle of arrival α, which are coefficients of y = Vs · cos(x - α), using the least squares method. Specifically, the velocity profile unit 131 selects the coefficient with the largest number of points whose residuals with respect to the fitted curve are within a threshold. A specific example of a known fitting method is the A Random Sample Consensus (RANSAC) algorithm described in Reference 3. By fitting the velocity profile in this manner, it is possible to compensate for the roughness of the angular resolution and the roughness of the velocity resolution of the millimeter-wave radar 50, thereby improving the accuracy of both the velocity and the angle.
[0029] [Reference 3] M. Fischler, R. Bolles. “Random sample consensus: a paradigm for model fitting with applications to image analysis and automation ”, Communications of the ACM, 1981.
[0030] Figure 4 shows a specific example of the results of actual symbol fitting. In Figure 4, each point represents a symbol, and the curve represents the fitting result. Although there is a relatively large variation in the symbols, the value on the X-axis at the extreme value of the fitting result is obtained as the Doppler angle of arrival, and the value on the Y-axis at that extreme value is obtained as the Doppler velocity. Therefore, it can be seen that, despite the fact that the angular resolution and velocity resolution of the millimeter-wave radar 50 are both coarse, it is possible to reduce the variation in angle and velocity.
[0031] The radar / body coordinate conversion unit 132 performs coordinate conversion as shown in FIG. 5 . In FIG. 5 , the X-axis and Y-axis are the X-axis and Y-axis of the body coordinate system, and the center of the IMU 40 is located at the position where the X-axis and Y-axis intersect at right angles. The radar / body coordinate conversion unit 132 converts the velocity Vs and azimuth angle α of the phase shift center of the radar antenna into the body coordinate system of the moving body, converting them from polar coordinates to Vb (X-axis velocity) and ω (Z-axis angular velocity) on the XY axes as shown in [Equation 1]. Here, β indicates the angle formed between the nose and the center line of the antenna. ly indicates the Y-axis lever arm between the nose and the center of the antenna. 1x indicates the X-axis lever arm between the center of the antenna and the center of the IMU 40. The center of the XY axes is the center of the IMU 40. A specific example of the model of the target ship 20 is the Ackermann model. The radar / body coordinate conversion unit 132 outputs data indicating the coordinate-converted velocity and angular velocity to the GNSS / INS hybrid navigation system 200 .
[0032]
[0033] The automatic docking device 100 may also include a moving average unit. When the automatic docking device 100 includes a moving average unit, the radar / body coordinate conversion unit 132 uses the results calculated by the moving average unit. The moving average unit reduces random angle measurement errors by using moving averages of multiple epochs. Below, a specific example is shown in which the moving average unit uses moving averages of five epochs as each of the Doppler velocity Vs and the Doppler angle of arrival α, as shown in [Equation 2]. As a specific example, when the millimeter-wave symbol is updated in 10 Hz increments, the moving average unit stores data for the past four epochs, calculates the moving average of a total of five data points, including the current epoch and the past four epochs, using the calculation shown in [Equation 2], and outputs the calculated value in 10 Hz increments.
[0034]
[0035] The point cloud generation unit 120 converts the point cloud in relative coordinates into a point cloud in absolute coordinates, and adds data indicating a map made up of the converted point cloud to the map data 190. The point cloud generation unit 120 may update at least a portion of the map data 190 based on the map made up of the converted point cloud. The target position input unit 141 accepts input of a target position from a higher-level system or a user. The route planning unit 142 sends data indicating each waypoint indicated by the planned route to the guidance control unit 143. The guidance control unit 143 controls the guidance of the target ship 20 so that the target ship 20 passes each waypoint indicated by the data received from the route planning unit 142 in order. Note that the occupied areas of other ships may be taken into consideration in both the map generation and the setting of each waypoint.
[0036] 3, the GNSS / INS hybrid navigation system 200 includes a GNSS antenna 29, a GNSS receiver 30, an IMU 40, a strapdown calculation unit 220, a satellite observation data estimation unit 230, and an error estimation unit 240. The GNSS / INS hybrid navigation system 200 appropriately utilizes the technology disclosed in Reference 4. The GNSS / INS hybrid navigation system 200 is also called a GNSS / INS hybrid positioning system.
[0037] [Reference 4] JP 2012-193965 A
[0038] The IMU 40, also known as an inertial sensor, measures acceleration in three axes and angular velocity in three axes. The three axes refer to the length, width, and height directions of the moving object. Hereinafter, the acceleration in the three axes is referred to as "three-dimensional acceleration," and the angular velocity in the three axes is referred to as "three-dimensional angular velocity." The IMU 40 includes an acceleration sensor 41 and an angular velocity sensor 42. The target vessel 20 and the IMU 40 form an integral rigid body. The acceleration sensor 41 measures acceleration. The angular velocity sensor 42 measures angular velocity of the azimuth angle. The GNSS receiver 30 receives signals from the GNSS antenna 29. The strapdown calculation unit 220 calculates velocity, position, attitude, and direction based on the data acquired by the acceleration sensor 41 and the data acquired by the angular velocity sensor 42. Specific examples of the strapdown calculation unit 220 are described in Reference 4. The satellite observation data estimation unit 230 estimates observation data obtained by the GNSS. The error estimation unit 240 estimates errors contained in the GNSS positioning data based on data received from the automatic docking device 100 or the magnetic direction sensor 60 and the position, speed, and attitude calculated by the strapdown calculation unit 220. The error estimation unit 240 includes a high-precision positioning calculation unit 241. The high-precision positioning calculation unit 241 estimates errors using, as a specific example, a Kalman filter and corrects the position, speed, and attitude of the target vessel 20 based on the estimation results. The GNSS receiver 30, the IMU 40, and the positioning processor constitute the GNSS / INS integrated navigation system 200. The positioning processor is a processor that performs calculations for positioning. The GNSS receiver 30 receives positioning augmentation information from quasi-zenith satellites or wireless communication. The GNSS receiver 30 performs correction processing on the positioning signals received from GNSS satellites based on the positioning augmentation information, thereby performing centimeter-level high-precision position location.
[0039] As a specific example, the magnetic direction sensor 60 is a relatively simple sensor that determines direction using the Earth's geomagnetism. The GNSS / INS hybrid navigation system 200 may correct its positioning results based on the direction observed by the magnetic direction sensor 60. The switch 61 switches between the output of the magnetic direction sensor 60 and the output of the radar / body coordinate conversion unit 132. The switch 61 is switched by a higher-level system or a user. The target ship 20 may be equipped with a magnetic direction sensor 60 that measures direction instead of the millimeter-wave radar 50. The following describes a case where the GNSS / INS hybrid navigation system 200 uses the output of the magnetic direction sensor 60. The GNSS / INS hybrid navigation system 200 uses an angular velocity calculated from the time change in direction measured by the magnetic direction sensor 60 instead of the velocity and angular velocity measured by the millimeter-wave radar 50. The direction data output from the magnetic direction sensor 60 is converted into angular velocity data by performing the calculation shown in [Equation 3]. Here, ΔΨ represents the angular velocity data at time t+1, Ψt represents the direction data at time t, and Ψt+1 represents the direction data at time t+1. ΔΨ corresponds to the angular velocity calculated from the change over time in the direction measured by the magnetic direction sensor 60.
[0040]
[0041] When the magnetic direction sensor 60 is used, it is not possible to correct the velocity error compared to when the millimeter-wave radar 50 is used. However, the high-precision positioning calculation unit 241 corrects the angular velocity error of the strapdown calculation unit 220. Therefore, in this case, it is possible to avoid the phenomenon in which the direction value drifts due to gyro error or the like, and it is possible to locate the position of the target ship 20 with a relatively high degree of accuracy.
[0042] There are few magnetic objects such as steel towers, buildings, and automobiles around the target vessel 20. Therefore, by using the magnetic direction sensor 60 instead of the millimeter-wave radar 50, it is possible to obtain the same effect as when the millimeter-wave radar 50 is used. Note that the automatic docking system 90 may use both the millimeter-wave radar 50 and the magnetic direction sensor 60.
[0043] Note that the millimeter-wave radar 50 cannot constantly capture the pier from the approach to docking. Therefore, when the positional relationship between the millimeter-wave radar 50 and the docking berth satisfies certain conditions, a map is generated, and a route plan is generated based on the generated map and the ship's position. The ship then docks using each waypoint indicated in the route plan and calculations of the ship's self-position and attitude by the GNSS / INS integrated navigation system 200. Specific examples of the certain conditions include: the measured distance between the millimeter-wave radar 50 and the docking berth is 21 m or less; and the docking berth is located within an angle range of −45° to +45° from the center of the millimeter-wave radar 50 antenna. By inputting the speeds on the X and Y axes of the body coordinate system, which can be detected by the millimeter-wave radar 50, into the GNSS / INS integrated navigation system 200, the accuracy of the speed and attitude calculations by the GNSS / INS integrated navigation system 200 can be improved. A three-dimensional map showing the absolute coordinates is generated in real time by assigning absolute coordinates to the ranging points measured by the millimeter-wave radar 50 based on the position, velocity, and attitude indicated by the positioning results of the GNSS / INS hybrid navigation system 200. The navigation scenario generation unit 140 generates a navigation scenario showing a route that avoids obstacles and has a relatively short navigation distance based on the three-dimensional map generated in real time. The navigation scenario generation unit 140 also updates and corrects the navigation scenario each time based on the position and attitude indicated by the positioning results of the GNSS / INS hybrid navigation system 200.
[0044] The GNSS / INS hybrid navigation system 200 functions as a position and attitude localization device 300. The position and attitude localization device 300 locates the position of a moving object to which a GNSS receiver 30, a millimeter-wave radar 50, an acceleration sensor 41, and an angular velocity sensor 42 are attached. The position and attitude localization device 300 locates the position, attitude, and direction of the moving object using data acquired from the GNSS receiver 30, data acquired from the IMU 40, and data acquired from the millimeter-wave radar 50. The position and attitude localization device 300 is also called a self-position and attitude localization device.
[0045] 6 shows an example of the functional configuration of the position and attitude localization device 300. As shown in FIG. 6, the position and attitude localization device 300 includes an initial value calculation unit 310, a localization unit 320, a Kalman filter unit 330, a strapdown calculation unit 220, and a storage unit 390.
[0046] The initial value calculation unit 310 controls the Kalman filter unit 330 to calculate an initial value of the azimuth angle. Specifically, the initial value calculation unit 310 first calculates the azimuth angle of the target ship 20 as a first navigation azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor 42. Next, the initial value calculation unit 310 calculates a change amount in two-dimensional coordinates by integrating the velocity and angular velocity measured by the millimeter-wave radar 50 based on the calculated first navigation azimuth angle. Next, the initial value calculation unit 310 calculates two-dimensional coordinate values of the target ship 20 as first navigation coordinate values by adding the calculated change amount to two-dimensional position initial values. The two-dimensional position initial values are the initial position of the navigation coordinate system. Next, the initial value calculation unit 310 calculates a correction value for correcting the first navigation azimuth angle using a first Kalman filter based on the calculated first navigation coordinate values and two-dimensional coordinate values included in the coordinate values measured by the GNSS receiver 30. Next, 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 angle. That is, the initial value calculation unit 310 calculates an initial azimuth angle, which is an initial value of the azimuth angle of the target ship 20, based on the measurement values of the angular velocity sensor 42, the measurement values of the millimeter-wave radar 50, and the positioning result of the GNSS receiver 30. More specifically, the initial value calculation unit 310 calculates the first navigation azimuth based on the angular velocity measured by the angular velocity sensor 42, calculates two-dimensional first navigation coordinate values based on the calculated first navigation azimuth angle, the velocity and angular velocity measured by the millimeter-wave radar 50, and the two-dimensional position initial value, and corrects the first navigation azimuth based on the calculated two-dimensional first navigation coordinate values and the positioning result of the GNSS receiver 30 to calculate the initial azimuth angle.
[0047] Here, the coordinate system will be described using FIG. 7 . The initial position of the navigation coordinate system is the center position of the IMU 40 when the power is turned on. Normally, (0,0) is set as the "two-dimensional position initial value," and (0,0,0) is set as the "three-dimensional position initial value." Also, as shown in FIG. 7 , the X-axis, Y-axis, and Z-axis of the body coordinate system at the initial attitude angle of the target ship 20 are set as the X-axis, Y-axis, and Z-axis of the navigation coordinate system, respectively. In this case, the position of the target ship 20 in the two-dimensional navigation coordinate system is the amount of movement of the target ship 20, and is calculated by adding up the amount of movement of the target ship 20 on the X-axis and Y-axis in the body coordinate system. Also, the position of the target ship 20 in the three-dimensional navigation coordinate system is the amount of movement of the target ship 20, and is calculated by adding up the amount of movement of the target ship 20 on the X-axis, Y-axis, and Z-axis in the body coordinate system. As shown in Figure 7, the Body coordinate system is a coordinate system with the center of the IMU 40 as the origin, the forward direction of the target vessel 20 as the X axis, the right direction relative to the forward direction of the target vessel 20 as the Y axis, and the vertical downward direction of the target vessel 20 as the Z axis.
[0048] The location unit 320 includes a traveling location unit 321, a stopped location unit 322, and a stop determination unit 323. The location unit 320 locates the position of the target ship 20 based on the measurement values of the angular velocity sensor 42, the initial azimuth angle value, the measurement values of the acceleration sensor 41, and the positioning results of the combined navigation by the GNSS receiver 30. More specifically, the location unit 320 calculates the amount of change in azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor 42, calculates a second navigation azimuth angle based on the calculated amount of change in azimuth angle and the initial azimuth angle value, calculates three-dimensional second navigation coordinate values based on the calculated second navigation azimuth angle, the acceleration measured by the acceleration sensor 41, and the three-dimensional position initial value, and corrects the second navigation coordinate values based on the calculated three-dimensional second navigation coordinate values and the positioning results of the GNSS receiver 30, thereby locating the position of the target ship 20. The three-dimensional position initial value is the initial position of the navigation coordinate system. The traveling orientation unit 321 controls the Kalman filter unit 330 and the strapdown calculation unit 220 to orient (calculate) the position, attitude, and direction of the target ship 20 when the target ship 20 is traveling. The traveling orientation unit 321 is also called an orientation value calculation unit. The stopped orientation unit 322 controls the Kalman filter unit 330 and the strapdown calculation unit 220 to orient the position, attitude, and direction of the target ship 20 when the target ship 20 is stopped. The stopped orientation unit 322 is also called an azimuth angle correction unit. The stop determination unit 323 determines whether the target ship 20 has stopped or started.
[0049] Specifically, the orientation unit 320 first calculates the amount of change in the azimuth angle based on the angular velocity of the azimuth angle measured by the angular velocity sensor 42, and then calculates the azimuth angle of the target ship 20 as a second navigation azimuth angle based on the calculated amount of change in the azimuth angle and the output initial azimuth angle value. Next, the orientation unit 320 converts the three-dimensional acceleration measured by the acceleration sensor 41 into a three-dimensional acceleration in the navigation coordinate system based on the calculated second navigation azimuth angle. Next, the orientation unit 320 integrates the converted three-dimensional acceleration to calculate the amount of change in the three-dimensional coordinates, and then calculates the three-dimensional coordinate values of the target ship 20 as second navigation coordinate values by adding the calculated amount of change to the three-dimensional position initial values. Next, the orientation unit 320 calculates correction values for correcting the second navigation coordinate values using a second Kalman filter based on the calculated second navigation coordinate values and the three-dimensional coordinate values measured by the GNSS receiver 30. Next, the orientation unit 320 corrects the second navigation coordinate values using the calculated correction values and outputs the corrected second navigation coordinate values as position orientation values that have positioned the position of the target ship 20. Furthermore, when the moving target ship 20 stops moving, the orientation unit 320 calculates an azimuth angle correction value for correcting the currently calculated second navigation azimuth using a third Kalman filter based on the previously calculated second navigation azimuth angle and the currently calculated second navigation azimuth angle, and corrects the currently calculated second navigation azimuth angle using the calculated azimuth angle correction value to calculate a corrected second navigation azimuth angle. Here, the third Kalman filter calculates a correction value that matches the previously calculated second navigation azimuth angle with the currently calculated second navigation azimuth angle. The third Kalman filter generates an observation equation that represents the difference between the previously calculated second navigation azimuth angle and the currently calculated second navigation azimuth angle, and calculates the azimuth angle correction value using the generated observation equation. In addition, when the stopped target ship 20 resumes movement, the positioning 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, and then converts the new three-dimensional acceleration measured by the acceleration sensor 41 into a three-dimensional acceleration in the navigation coordinate system based on the calculated new second navigation azimuth angle.Next, the location unit 320 integrates the converted three-dimensional acceleration to calculate a 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. Next, the location unit 320 uses a second Kalman filter to calculate a position correction value for correcting the new second navigation coordinate value based on the calculated new second navigation coordinate value and the new three-dimensional coordinate value measured by the GNSS receiver 30, corrects the new second navigation coordinate value using the calculated position correction value, and outputs the corrected new second navigation coordinate value as a new position location value. If the speed measured by the millimeter-wave radar 50 is less than a predetermined speed threshold, the location unit 320 determines that the target ship 20 has stopped moving. If the speed measured by the millimeter-wave radar 50 is equal to or greater than the speed threshold after determining that the target ship 20 has stopped moving, the location unit 320 determines that the target ship 20 has resumed movement.
[0050] The Kalman filter unit 330 generates at least one of an observation equation and a state equation, and calculates correction values for various data using the generated observation equation and / or the state equation. 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 estimation unit 230 executes the processing included in the Kalman filter unit 330. The error estimation unit 240 corresponds to the first Kalman filter 331, the second Kalman filter 332, and the third Kalman filter 333.
[0051] The strapdown calculation unit 220 calculates the velocity, position, attitude, and direction based on the acceleration data 393 and the angular velocity data 394. A specific example of the strapdown calculation unit 220 is 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. The GNSS data 391 consists of data observed by the GNSS receiver 30. The velocity / angular velocity data 392 consists of data indicating the velocity and angular velocity observed by the millimeter-wave radar 50. The acceleration data 393 consists of data observed by the acceleration sensor 41. The angular velocity data 394 consists of data observed by the angular velocity sensor 42.
[0053] When the position and attitude locating device 300 uses the output of the magnetic direction sensor 60 instead of the output of the millimeter-wave radar 50, the position and attitude locating device 300 uses the direction data instead of the velocity and angular velocity data 392. That is, in this case, the position and attitude locating device 300 uses the angular velocity calculated from the change in the direction over time measured by the magnetic direction sensor 60 instead of the velocity and angular velocity measured by the millimeter-wave radar 50.
[0054] 8 shows an example of the hardware configuration of the automatic docking device 100 according to this embodiment. The automatic docking device 100 is composed of a computer. The automatic docking device 100 may be composed of multiple computers.
[0055] As shown in the figure, the automatic docking device 100 is a computer including hardware such as a processor 11, a memory 12, an auxiliary storage device 13, an input / output IF (Interface) 14, and a communication device 15. These pieces of hardware are appropriately connected via signal lines 19.
[0056] The processor 11 is an integrated circuit (IC) that performs arithmetic processing and controls the hardware of the computer. Specific examples of the processor 11 include a central processing unit (CPU), a digital signal processor (DSP), or a graphics processing unit (GPU). The automatic docking device 100 may include multiple processors that replace the processor 11. The multiple processors share the role of the processor 11.
[0057] The memory 12 is typically a volatile storage device, specifically a random access memory (RAM). The memory 12 is also called a primary storage device or a main memory. Data stored in the memory 12 is saved in the secondary storage device 13 as needed.
[0058] The auxiliary storage device 13 is typically a non-volatile storage device, and specific examples thereof include a ROM (Read Only Memory), an HDD (Hard Disk Drive), or a flash memory. Data stored in the auxiliary storage device 13 is loaded into the memory 12 as needed. The memory 12 and the auxiliary storage device 13 may be configured integrally.
[0059] The input / output IF 14 is a port to which an input device and an output device are connected. Specific examples of the input / output IF 14 include a USB (Universal Serial Bus) terminal. Specific examples of the input device include a keyboard and a mouse. Specific examples of the output device include a display.
[0060] The communication device 15 is a receiver and a transmitter, and is specifically a communication chip or a NIC (Network Interface Card).
[0061] Each part of the automatic docking device 100 may use the input / output IF 14 and the communication device 15 as appropriate when communicating with other devices.
[0062] The auxiliary storage device 13 stores an automatic docking program. The automatic docking program is a program that causes a computer to realize the functions of each unit of the automatic docking device 100. The automatic docking program is loaded into the memory 12 and executed by the processor 11. The functions of each unit of the automatic docking device 100 are realized by software.
[0063] Data used when executing the automatic docking program and data obtained by executing the automatic docking program are stored in a storage device as appropriate. Each part of the automatic docking device 100 uses a storage device as appropriate. Specific examples of the storage device include at least one of the memory 12, the auxiliary storage device 13, a register in the processor 11, and a cache memory in the processor 11. Note that the terms "data" and "information" may have the same meaning. The storage device may be independent of the computer. The functions of the memory 12 and the auxiliary storage device 13 may be realized by other storage devices.
[0064] The automatic docking program may be recorded on a computer-readable nonvolatile recording medium. Specific examples of the nonvolatile recording medium include an optical disk and a flash memory. The automatic docking program may be provided as a program product.
[0065] ***Explanation of Operation*** Fig. 9 is a flowchart showing an example of the operation of the position and attitude localization device 300. The operation of the position and attitude localization device 300 will be described with reference to Fig. 9 .
[0066] (Step S11) The initial value calculation unit 310 calculates the initial value of the azimuth angle by three-degree-of-freedom two-dimensional navigation when the target ship 20 departs.
[0067] (Step S12) The traveling orientation unit 321 locates the position, attitude, and orientation of the target ship 20 using six-degree-of-freedom three-dimensional navigation with the initial azimuth angle or the most recently determined azimuth angle until the target ship 20 stops.
[0068] (Step S13) If the target vessel 20 is stationary, the position and attitude localizing device 300 proceeds to step S14. Otherwise, the position and attitude localizing device 300 proceeds to step S12.
[0069] (Step S14) While the target vessel 20 is stopped, the stationary positioning unit 322 locates the position, attitude, and direction of the target vessel 20 using six-degree-of-freedom three-dimensional navigation based on an extended ZUPT (Zero Velocity Update) algorithm. An overview 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 localizing device 300 proceeds to step S14. Otherwise, the position and attitude localizing device 300 proceeds to step S12.
[0071] Fig. 10 shows an algorithm for three-degree-of-freedom two-dimensional navigation according to the first embodiment. This algorithm is the same as the algorithm shown in Fig. 7 of Patent Document 1. Fig. 10 will be explained below. The initial value calculation unit 310 includes an input correction unit 311, an orientation update unit 312, a coordinate conversion 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) indicated by the velocity / angular velocity data 392 by adding the correction value calculated by the Kalman filter unit 330 to the velocity indicated by the velocity / angular velocity data 392. The input correction unit 311 also 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" indicated by the angular velocity data 394.
[0073] The azimuth update unit 312 calculates the amount of change in the azimuth angle by once integrating the angular velocity of the azimuth angle corrected by the input correction unit 311. The azimuth update unit 312 calculates a new azimuth angle by adding the calculated amount of change in the azimuth angle to the initial value of the azimuth angle or the azimuth angle previously calculated by the output correction unit 315. Here, the initial value of the azimuth angle may be any appropriate value.
[0074] The coordinate conversion unit 313 converts 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 using a predetermined conversion formula based on the attitude and orientation calculated by the orientation update unit 312. This results in a two-dimensional velocity in the navigation coordinate system. The two-dimensional velocity consists of a velocity in the latitudinal direction and a velocity in the longitudinal direction.
[0075] The latitude and longitude update unit 314 calculates the amount of change in the two-dimensional coordinates by integrating the two-dimensional velocity obtained by the coordinate conversion unit 313 once. The two-dimensional coordinates consist of a latitude component and a longitude component. The latitude and longitude update unit 314 calculates new two-dimensional coordinates by adding the calculated amount of change in the two-dimensional coordinates to the initial values of the two-dimensional coordinates or the two-dimensional coordinates previously calculated by the output correction unit 315. As a specific example, the latitude and longitude update unit 314 uses the two-dimensional coordinate values measured by the GNSS receiver 30 before the target ship 20 departs, that is, when the target ship 20 is stopped, as the initial values of the two-dimensional coordinates.
[0076] First, the Kalman filter unit 330 calculates, as a "coordinate residual," the difference between the two-dimensional coordinates indicated by the GNSS data 391 and the two-dimensional coordinates calculated by the latitude and longitude update unit 314. Next, the Kalman filter unit 330 calculates, as a "velocity residual," the difference between the two-dimensional velocity indicated by the GNSS data 391 and the two-dimensional velocity obtained by the coordinate conversion unit 313. Next, the Kalman filter unit 330 executes the first Kalman filter 331 using the calculated coordinate residual and velocity residual as input, thereby calculating a velocity correction value, an angular velocity correction value, a two-dimensional coordinate correction value, and an azimuth correction value.
[0077] The output correction unit 315 corrects the two-dimensional coordinate values calculated by the latitude and longitude update unit 314 by adding the correction value calculated by the Kalman filter unit 330 to the two-dimensional coordinate values, and outputs the corrected two-dimensional coordinate values. Furthermore, the output correction unit 315 corrects the azimuth angle calculated by the azimuth update unit 312 by adding the correction value calculated by the Kalman filter unit 330 to the azimuth angle, and outputs the corrected azimuth angle.
[0078] As described above, three-degree-of-freedom two-dimensional navigation uses the velocity measured by the millimeter-wave radar 50, rather than the acceleration measured by the acceleration sensor 41, so no integral error is carried over into the velocity. Furthermore, even if the initial value of the azimuth angle is inappropriate, an appropriate value for the azimuth angle can be quickly determined. This is because the angular velocity measured by the millimeter-wave radar 50, rather than the angular velocity measured by the angular velocity sensor 42, is used. Therefore, three-degree-of-freedom two-dimensional navigation requires only one integral calculation to determine the two-dimensional coordinate, is relatively less affected by errors in the initial value of the azimuth angle used in the coordinate conversion of the velocity, and can appropriately correct both the two-dimensional coordinate value and the azimuth angle using a Kalman filter.
[0079] Fig. 11 shows an algorithm for six-degree-of-freedom three-dimensional navigation according to the first embodiment. This algorithm is the same as the algorithm shown in Fig. 9 of Patent Document 1. Fig. 11 will be explained below. The orientation unit 320 includes an input correction unit 324 and an output correction unit 325.
[0080] The input corrector 324 corrects the three-dimensional acceleration by adding the correction value calculated by the Kalman filter unit 330 to the three-dimensional acceleration indicated by the acceleration data 393. The input corrector 324 also corrects the three-dimensional angular velocity by adding the correction value calculated by the Kalman filter unit 330 to the three-dimensional angular velocity indicated by the angular velocity data 394.
[0081] The strapdown calculation unit 220 calculates the position, attitude, and direction of the target vessel 20 using the three-dimensional acceleration corrected by the input correction unit 324 and the three-dimensional angular velocity corrected by the input correction unit 324.
[0082] The output correction unit 325 corrects the position calculated by the strapdown calculation unit 220 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 position of the location result. The output correction unit 325 also corrects the attitude calculated by the strapdown calculation unit 220 by adding the correction value calculated by the Kalman filter unit 330 to the attitude, and outputs the corrected attitude as the attitude angle of the location result. The output correction unit 325 also corrects the orientation calculated by the strapdown calculation unit 220 by adding the correction value calculated by the Kalman filter unit 330 to the orientation, and outputs the corrected orientation as the azimuth angle of the location result.
[0083] The stop determination unit 323 determines whether the target vessel 20 is stopped or not based on the speed calculated from the observation results of the millimeter wave radar 50.
[0084] When the target vessel 20 is not stationary, the Kalman filter unit 330 calculates the difference between the position indicated by the GNSS data 391 and the position calculated by the strap-down calculation unit 220 as a "position residual." Next, the Kalman filter unit 330 calculates the difference between the velocity indicated by the velocity / angular velocity data 392 and the velocity calculated by the strap-down calculation unit 220 as a "velocity residual," and calculates the difference between the angular velocity indicated by the velocity / angular velocity data 392 and the angular velocity calculated by the strap-down calculation unit 220 as an "angular velocity residual." Next, the Kalman filter unit 330 executes a second Kalman filter 332 using the calculated position residual and velocity residual as input values, thereby calculating a correction value for three-dimensional acceleration, a correction value for three-dimensional angular velocity, a correction value for position, and a correction value for attitude.
[0085] In step S14, the third Kalman filter 333 to which the extended ZUPT algorithm is applied is executed in the six-degree-of-freedom three-dimensional navigation.
[0086] Fig. 12 is a flowchart showing an example of a process for generating a navigation scenario in the automatic docking system 90. The process for generating a navigation scenario will be described with reference to Fig. 12. The operation procedures of each device provided in the automatic docking system 90 correspond to an automatic docking method. Furthermore, the program for realizing the operation of each device provided in the automatic docking system 90 corresponds to an automatic docking program.
[0087] (Step S101) Each millimeter wave radar 50 measures the distance around the millimeter wave radar 50 and measures the Doppler velocity in the X-axis and Y-axis.
[0088] (Step S102 ) Each millimeter wave radar 50 inputs the results of measuring the distance around each millimeter wave radar 50 to the automatic docking device 100 .
[0089] (Step S103 ) The automatic docking device 100 inputs the Doppler velocity measurement results obtained by each millimeter wave radar 50 to the GNSS / INS integrated navigation system 200 .
[0090] (Step S104) The GNSS / INS integrated navigation system 200 calculates the position, velocity, and attitude of the target ship 20 at time t.
[0091] (Step S105 ) The GNSS / INS integrated navigation system 200 inputs the calculated position, speed, and attitude of the target vessel 20 to the automatic docking device 100 .
[0092] (Step S106) The point cloud generation unit 120 generates a three-dimensional map indicating absolute coordinates as map data 190 based on the data input from each millimeter wave radar 50 and the data input from the GNSS / INS integrated navigation system 200.
[0093] (Step S107) If the target coordinates and the 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 three-dimensional map and the current position, speed, and attitude of the target ship 20, the route planning unit 142 generates a route plan and attitude change plan that avoids obstacles and is rational as a navigation scenario until the target coordinates are reached at the target attitude.
[0095] (Step S109) The route planning unit 142 compares the planned position and planned attitude in the generated navigation scenario with the currently measured position and attitude of the target ship 20.
[0096] (Step S110) The route planning unit 142 corrects the navigation scenario based on the comparison result in 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 proceeds to step S113.
[0098] (Step S112) The path planning unit 142 resets the coordinates and the attitude that can avoid the obstacle near the target coordinates as the target coordinates and the target attitude, respectively.
[0099] (Step S113) If the target vessel 20 reaches the target coordinates and the target attitude in the navigation scenario, the automatic docking system 90 proceeds to step S114. Otherwise, the automatic docking system 90 returns to step S101.
[0100] (Step S114) The automatic docking system 90 ends the generation of the navigation scenario.
[0101] FIG. 13 is a diagram illustrating the operation sequence of the automatic docking system 90 in use case 1. Use case 1 is, as a specific example, a case in which a high-end cruiser with two engines docks at a service berth. Use case 1 is the most easily realized basic use case. A specific example of use case 1 will be explained using FIG. 13. In this example, the general MAKLINK method is used. FIG. 13 shows the symbol map area corresponding to the center of the antenna, the symbol map area corresponding to the area within a range of ±45 degrees from the center of the antenna, the LOS of the antenna center, and the range of antenna coverage within ±45 degrees. LOS is an abbreviation for Line Of Sight.
[0102] (1) Start of Approach The target vessel 20 starts its approach to dock at the marina berth.
[0103] (2) Start of Map Generation The target vessel 20 starts map generation at a point where the distance from the marina berth is 15 m. The target vessel 20 generates a map while proceeding parallel to the shore of the marina berth.
[0104] (3) Completion of Map Generation The target vessel 20 travels straight for about one to two boat lengths while generating a map, and then the generation of the map is completed.
[0105] (4) Route Planning Implementation The target ship 20 implements route planning based on the generated map and the position and attitude of the target ship 20. It is assumed that the target position and target attitude of the target ship 20 have been set.
[0106] (5) Automatic Docking The target vessel 20 performs automatic docking based on the executed route plan.
[0107] FIG. 14 is a diagram illustrating the operation sequence of the automatic docking system 90 in Use Case 1, showing specific examples of waypoints and maps. In this example, target coordinates and target attitude are shown, and there are other ships in the marina berth as obstacles. In this example, the map shows multiple nodes. A line connecting two adjacent nodes indicates a no-navigation line. The no-navigation line indicates the edge of an area where the target ship 20 cannot enter, and corresponds to the boundary between the navigable area and the unnavigable area. Each node shows the observation results of the millimeter-wave radar 50, and as a specific example, shows a quay, a pier, or a part of an obstacle. In FIG. 14, the area on the target ship 20 side of the no-navigation line is the navigable area.
[0108] If the target vessel 20 detects a difference in the map shape during automatic docking, it immediately stops the automatic docking. The difference in the map shape is the difference between the no-navigation line shown on the map generated before the automatic docking was performed and the no-navigation line based on the observation results of the millimeter-wave radar 50 during the automatic docking. A specific example of a difference in the map shape occurs when another vessel that was anchored before the automatic docking was performed begins to move. In this embodiment, the map is an absolute coordinate system, so it is possible to constantly verify the similarity between the past map and the current map and to detect differences in the map shape. On the other hand, in existing technologies, the map is a relative coordinate system, so it is not possible to verify the similarity between the past map and the current map. The no-navigation line may be a three-dimensional surface. After the host system or the 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 resumes operation from operation sequence (4). In other words, while the target ship 20 is being guided, the target radar measures the distance at the target berth, and the GNSS / INS integrated navigation system 200 measures the position and attitude of the target ship 20. While the target ship 20 is being guided, the point cloud generation unit 120 generates an updated map based on the results of the distance measurement at the target berth by the target radar and the results of the positioning of the position and attitude of the target ship 20 by the GNSS / INS integrated navigation system 200. The updated map is a map that shows the navigable area at the target berth and indicates absolute coordinates. If there is a difference between the navigable area shown in the updated map and the navigable area shown in the target map, the navigation scenario generation unit 140 stops guiding the target ship 20.
[0109] FIG. 15 is a diagram illustrating the operation sequence of the automatic docking system 90 in use case 2. Use case 2 is a specific example of a case in which a high-end cruiser with two engines docks at a home berth. Use case 2 corresponds to a standard case during actual operation. The processing flow in use case 2 is similar to the processing flow in use case 1. In use case 1, the target vessel 20 can approach the quay quite closely to generate a map. However, in use case 2, the target vessel 20 may collide with the pier if it attempts to approach the quay. Therefore, the user or a higher-level system, etc., must change the docking mode depending on the use case. The user may set a use case using an input / output device. A use case near the target position may be automatically set based on the target position by referring to a correspondence table of use cases based on a predetermined target position.
[0110] 16 to 20 are diagrams for explaining, in chronological order, the operation sequence of the automatic docking system 90 in Use Case 2. A specific example of the operation sequence will be explained using Fig. 16 to 20. In this example, the general MAKLINK method is used.
[0111] (1) Start of approach The target vessel 20 heads toward the wharf to search for the edge of the pier, as shown in Figure 16. The docking mode of the target vessel 20 is set to a mode corresponding to Use Case 2.
[0112] (2) Start of map generation After observing the edge of the pier, the target vessel 20 proceeds in the direction where the edge of the berth is thought to be located in order to search for the edge of the berth, as shown in FIG. 17. After determining the width of the home berth, the target vessel 20 searches within the detected home berth, as shown in FIG. 18. The width of the home berth is calculated based on the observed edge of the pier and the observed edge of the berth. As shown in FIG. 19, the target vessel 20 enters the detected home berth and maps the shape of the home berth. The subsequent processing is the same as that in Use Case 1. Note that FIG. 20 shows the generated map, the target coordinates and target attitude, and each waypoint indicated by the planned route.
[0113] ***Explanation of Effects of Embodiment 1*** According to this embodiment, a map showing docking positions is automatically generated in real time, eliminating the need to register docking positions in advance. Furthermore, according to this embodiment, even if other ships are anchored at the berth, the ship can dock at any berth while automatically avoiding the other ships. According to this embodiment, a navigation scenario can be generated using absolute coordinates. Therefore, in operations where the docking position is fixed, such as in the operation of a cruiser, it is possible to specify the docking coordinates in advance from a control host system, etc. Therefore, the user does not need to manually set the docking coordinates. Note that if the docking position changes, the user can also manually set the coordinates of the docking position. According to this embodiment, the docking position can be specified using absolute coordinates. Therefore, according to this embodiment, it is also possible to generate a navigation scenario for an area outside the ranging range of the millimeter-wave radar 50, for which no map has been generated.
[0114] ***Other Configurations*** <Modification 1> Fig. 21 shows an example of the hardware configuration of the automatic docking device 100 according to this modification. The automatic docking device 100 includes a processing circuit 18 instead 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 hardware that realizes at least a part of the components included in the automatic docking device 100. The processing circuit 18 may be dedicated hardware, or may be a processor that executes a program stored in the memory 12.
[0115] When the processing circuit 18 is dedicated hardware, the processing circuit 18 may be, for example, a single circuit, a multiple 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 include multiple processing circuits that replace the processing circuit 18. The multiple processing circuits share the role of the processing circuit 18.
[0116] In the automatic docking device 100, some functions may be realized by dedicated hardware, and the remaining functions may be realized by software or firmware.
[0117] The processing circuitry 18 is realized by, for example, hardware, software, firmware, or a combination of these. The processor 11, memory 12, auxiliary storage device 13, and processing circuitry 18 are collectively referred to as the "processing circuitry." In other words, the functions of each functional component of the automatic docking device 100 are realized by the processing circuitry.
[0118] ***Other Embodiments*** Although the first embodiment has been described, it is also possible to combine multiple parts of this embodiment and implement it. Alternatively, it is also possible to implement this embodiment in part. In addition, this embodiment may be modified in various ways as needed, and may be implemented in any combination, either as a whole or in part. Note that the above-described embodiments are essentially preferred examples and are not intended to limit the scope of the present disclosure, its applications, and uses. The procedures described using flowcharts, etc. may be modified as appropriate.
[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 Acceleration sensor, 42 Angular velocity sensor, 50 Millimeter wave radar, 51 Synthesizer, 52 Oscillator, 53 Millimeter wave transmitting / receiving antenna, 54 Mixer, 55 Signal processing unit, 56 Symbol acquisition and tracking unit, 60 Magnetic direction sensor, 61 Switch, 90 Automatic docking system, 100 Automatic docking device, 110 Data collection unit, 120 Point cloud generation unit, 121 Point cloud extraction unit, 122 Coordinate conversion unit, 123 Map generation unit, 130 Speed and direction detection unit, 131 Speed profile unit, 132 Radar / Body coordinate conversion unit, 140 Navigation scenario generation unit, 141 target position input unit, 142 route planning unit, 143 guidance control unit, 190 map data, 200 GNSS / INS integrated navigation device, 220 strapdown calculation unit, 230 satellite observation data estimation unit, 240 error estimation unit, 241 high-precision positioning calculation unit, 300 position and attitude localization device, 310 initial value calculation unit, 311 input correction unit, 312 orientation update unit, 313 coordinate conversion unit, 314 latitude and longitude update unit, 315 output correction unit, 320 localization unit, 321 running localization unit, 322 stopping localization unit, 323 stopping judgment 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 Memory unit, 391 GNSS data, 392 velocity / angular velocity data, 393 acceleration data, 394 angular velocity data.
Claims
1. An automatic docking device for controlling automatic docking of a target ship equipped with a GNSS (Global Navigation Satellite System) / INS (Inertial Navigation System) hybrid navigation device and a target radar that measures the relative positions of surrounding objects using electromagnetic waves at a target berth, comprising: a point cloud generation unit that converts relative coordinates corresponding to each point of a point cloud corresponding to the results of the distance measurement by the target radar at the target berth into absolute coordinates based on the results of the positioning and attitude of the target ship measured by the GNSS / INS hybrid navigation device, and generates a target map that is a map showing absolute coordinates, which is a map showing the navigable area at the target berth, based on the absolute coordinates corresponding to each converted point; an automatic docking device comprising: a navigation scenario generation unit that, when a target position and target attitude of the target ship at the target berth are specified based on the target map, generates a plan to guide the target ship to arrive at the target position at the target attitude based on the target map and the position and attitude of the target ship measured by the GNSS / INS integrated navigation device.
2. An automatic docking device as described in claim 1, wherein when the target radar measures the distance at the target berth, the distance between the target ship and the quay of the target berth is a distance according to the shape of the target berth.
3. An automatic docking device as claimed in claim 1 or 2, wherein the target radar is a millimeter wave radar.
4. An automatic docking device as described in claim 3, wherein, when each point in the point cloud is taken as a target point, at the time when the target point is measured by the millimeter wave radar, the distance between the millimeter wave radar and the target point satisfies a ranging reference distance condition, and the angle between the center of the millimeter wave radar and the target point satisfies a ranging reference angle condition.
5. An automatic docking device as described in claim 3 or 4, wherein the GNSS / INS integrated navigation system corrects its positioning results based on the speed and direction observed by the millimeter wave radar.
6. An automatic docking device as described in any one of claims 1 to 5, wherein the target ship is equipped with a magnetic direction sensor, and the GNSS / INS integrated navigation system corrects the positioning results of the GNSS / INS integrated navigation system based on the direction observed by the magnetic direction sensor.
7. An automatic docking device as described in any one of claims 1 to 6, wherein the navigation scenario generation unit guides the target ship in accordance with the generated plan, while the target ship is being guided, the target radar measures the distance at the target berth and the GNSS / INS integrated navigation system measures the position and attitude of the target ship, while the target ship is being guided, and the point cloud generation unit generates an updated map which is a map showing absolute coordinates and which indicates the navigable area at the target berth based on the results of the distance measurement at the target berth by the target radar and the results of the positioning of the position and attitude of the target ship by the GNSS / INS integrated navigation system while the target ship is being guided, and the navigation scenario generation unit stops guiding the target ship when there is a difference between the navigable area shown in the updated map and the navigable area shown in the target map.
8. An automatic docking system comprising: the GNSS / INS integrated navigation device; a millimeter wave radar as the target radar; and an automatic docking device according to any one of claims 1 to 7.
9. An automatic docking method executed by an automatic docking device which is a computer that controls automatic docking of a target ship equipped with a GNSS (Global Navigation Satellite System) / INS (Inertial Navigation System) composite navigation system and a target radar that measures the relative positions of surrounding objects using electromagnetic waves at a target berth, the automatic docking device converts relative coordinates corresponding to each point of a point cloud corresponding to the results of the distance measurement by the target radar at the target berth into absolute coordinates based on the results of the positioning and attitude of the target ship measured by the GNSS / INS composite navigation system, and generates a target map which is a map showing absolute coordinates and which shows the navigable area at the target berth, based on the absolute coordinates corresponding to each converted point, An automatic docking method in which, when a target position and target attitude of the target ship at the target berth are specified based on the target map, the automatic docking device generates a plan to guide the target ship to reach the target position at the target attitude based on the target map and the position and attitude of the target ship measured by the GNSS / INS integrated navigation system.
10. An automatic docking program executed by an automatic docking device, which is a computer that controls automatic docking of a target ship equipped with a GNSS (Global Navigation Satellite System) / INS (Inertial Navigation System) hybrid navigation system and a target radar that uses electromagnetic waves to measure the relative positions of surrounding objects at a target berth, comprising: a point cloud generation process that converts relative coordinates corresponding to each point of a point cloud corresponding to the results of the distance measurement by the target radar at the target berth into absolute coordinates based on the results of the positioning and attitude of the target ship measured by the GNSS / INS hybrid navigation system, and generates a target map that is a map showing absolute coordinates, which is a map showing the navigable area at the target berth, based on the absolute coordinates corresponding to each converted point; an automatic docking program that causes the automatic docking device to execute a navigation scenario generation process that, when a target position and target attitude of the target ship at the target berth are specified based on the target map, generates a plan to guide the target ship to reach the target position at the target attitude based on the target map and the position and attitude of the target ship measured by the GNSS / INS integrated navigation device.
Citation Information
Patent Citations
Automatic docking device
JP2022106753A
Navigation system
WO2018100751A1
Ship docking assistance device
WO2021085274A1
Cited By
Ship automatic berthing safety control system
CN121364724A
Tight coupling navigation positioning method based on 4D millimeter wave radar / inertial sensor
CN121475208A
