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

Abstract

To provide an information processing device, etc., capable of improving the estimating accuracy of a self-position in a vessel.SOLUTION: The information processing device has predicted position acquisition means, estimated position calculation means, state information acquisition means, and speed calculation means. The predicted position acquisition means acquires a predicted position of a vessel. The estimated position calculation means calculates an estimated position of the vessel in which the predicted position is updated on the basis of collation between data based on an output of an external sensor provided in the vessel and map data. The state information acquisition means acquires state information of the vessel. The speed calculation means calculates a speed of the vessel at a first processing time on the basis of the estimated position at the first processing time, the estimated position at a second processing time immediately before the first processing time, and a time constant set on the basis of the state information.SELECTED DRAWING: Figure 22

Description

[Technical Field]

[0001] This invention relates to the estimation of a ship's own position. [Background technology]

[0002] A technique for estimating the self-position of a moving object is conventionally known, which involves comparing (matching) shape data of surrounding objects measured using measuring devices such as laser scanners with map information that has been pre-stored with the shapes of surrounding objects. For example, Patent Document 1 discloses an autonomous mobile system that determines whether a detected object in a voxel, which is divided into spaces according to a predetermined rule, is a stationary or moving object, and performs matching between map information and measurement data for voxels in which stationary objects exist. Patent Document 2 also discloses a scan matching method that estimates the self-position by comparing voxel data, which includes the mean vector and covariance matrix of stationary objects for each voxel, with point cloud data output by a lidar. [Prior art documents] [Patent Documents]

[0003] [Patent Document 1] International release WO2013 / 076829 [Patent Document 2] International release WO2018 / 221453 [Overview of the project] [Problems that the invention aims to solve]

[0004] Recently, autonomous ship navigation systems are being studied not only in the automotive sector but also in the maritime sector, and accurate self-position estimation is equally important for safe autonomous operation. In the open ocean, self-positioning is possible using GNSS (Global Navigation Satellite System) because there are few structures in the surrounding area. However, in urban coastal areas and rivers, the presence of high-rise buildings and other structures reduces the number of receivable satellites and causes multipath interference, resulting in poor GNSS signal reception and often making accurate positioning impossible. Therefore, research is underway to enable highly accurate self-position estimation using scan matching, as described above, even in ships.

[0005] However, in the case of ships, a problem arises in that the influence of currents and waves manifests as errors in the estimated position, resulting in a decrease in the accuracy of self-position estimation.

[0006] This invention was made to solve the above-mentioned problems, and its main objective is to provide an information processing device capable of improving the accuracy of self-position estimation in ships. [Means for solving the problem]

[0007] The invention described in the claim is an information processing device comprising: a prediction position acquisition means for acquiring the predicted position of a vessel; and an estimated position calculation means for calculating the estimated position of the vessel with the predicted position updated based on the comparison of data based on the output of an external sensor installed on the vessel with map data. The parameters indicating the acceleration and deceleration of the vessel are any of the following: the acceleration of the vessel, the change in the throttle lever of the vessel, the change in the propeller speed of the vessel, or the change in the impeller speed of the vessel. Get parameters Acquisition means, the estimated position at the first processing time, and the estimated position at the second processing time immediately preceding the first processing time, The time constant set by applying the parameters obtained at the first processing time to a predetermined formula. It is possible to change the characteristics in accordance with the changes in [the element]. First-order lag low-pass filter The system includes a speed calculation means for calculating the speed of the vessel at the first processing time based on the above.

[0008] Furthermore, the invention described in the claim is a control method performed by a computer, which acquires the predicted position of a vessel, and calculates the estimated position of the vessel with the predicted position updated based on the comparison of data based on the output of external sensors installed on the vessel with map data, The parameters indicating the acceleration and deceleration of the vessel are any of the following: the acceleration of the vessel, the change in the throttle lever of the vessel, the change in the propeller speed of the vessel, or the change in the impeller speed of the vessel. The estimated position at the first processing time and the estimated position at the second processing time immediately preceding the first processing time are obtained, The time constant set by applying the parameters obtained at the first processing time to a predetermined formula. It is possible to change the characteristics in accordance with the changes in [the element]. First-order lag low-pass filter Based on this, the speed of the vessel at the first processing time is calculated.

[0009] Furthermore, the invention described in the claim is a program that acquires the predicted position of a vessel, calculates the estimated position of the vessel with the predicted position updated based on the comparison of data based on the output of external sensors installed on the vessel with map data, The parameters indicating the acceleration and deceleration of the vessel are any of the following: the acceleration of the vessel, the change in the throttle lever of the vessel, the change in the propeller speed of the vessel, or the change in the impeller speed of the vessel. The estimated position at the first processing time and the estimated position at the second processing time immediately preceding the first processing time are obtained, The time constant set by applying the parameters obtained at the first processing time to a predetermined formula. It is possible to change the characteristics in accordance with the changes in [the element]. First-order lag low-pass filter Based on this, the computer is instructed to perform a process to calculate the speed of the vessel at the first processing time. [Brief explanation of the drawing]

[0010] [Figure 1A] A diagram showing the schematic configuration of the driver assistance system according to the first embodiment. [Figure 1B] A diagram illustrating the angular velocity used in self-localization. [Figure 2] A block diagram showing an example of the hardware configuration of the information processing device according to the first embodiment. [Figure 3] A diagram showing the self-position that the self-position estimation unit should estimate, represented in 3D Cartesian coordinates. [Figure 4] A diagram illustrating an example of the general data structure of voxel data (VD). [Figure 5] A diagram illustrating an example of the processing performed by the self-localization unit. [Figure 6] A diagram illustrating the relationship between the confidence level value NRV and the confidence level index NRI. [Figure 7] A diagram showing the relationship between the confidence index NRI and the time constant τ. [Figure 8] A diagram showing the relationship between the confidence level (NRV) and the time constant τ. [Figure 9A] This figure shows an example of calculating velocity in the world coordinate system without any filtering. [Figure 9B] This figure shows an example of calculating the velocity in the world coordinate system while keeping the filter time constant τ fixed. [Figure 10A] This figure shows an example of calculating velocity in the world coordinate system without any filtering. [Figure 10B] This figure shows an example of calculating the velocity in the world coordinate system while keeping the filter time constant τ fixed. [Figure 11A] This figure shows an example of calculating velocity in the world coordinate system without any filtering. [Figure 11B] This figure shows an example of calculating velocity in the world coordinate system while varying the filter's time constant τ. [Figure 12A] This diagram illustrates an example of ship self-position estimation without any filters. [Figure 12B] This diagram illustrates an example of ship self-position estimation with a filter enabled. [Figure 12C] This diagram illustrates an example of ship self-position estimation with a filter enabled. [Figure 13] A diagram showing an example of the functional block of the self-position estimation unit according to the first embodiment. [Figure 14] A flowchart illustrating an example of the procedure for self-localization. [Figure 15] A diagram showing the setup and scanning range of the lidar during the experiment. [Figure 16A] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 16B] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 16C]This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 16D] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 16E] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 16F] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 16G] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 17A] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 17B] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 17C] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 17D] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 17E] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 17F] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 17G] This figure shows the self-localization results for a comparative example where velocity is calculated without filtering. [Figure 18A] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 18B] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 18C] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 18D] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 18E]This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 18F] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 18G] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 19A] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 19B] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 19C] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 19D] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 19E] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 19F] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 19G] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 19H] This figure shows the self-position estimation results for an example in which velocity is calculated with a filter enabled. [Figure 20] A diagram showing the schematic configuration of the driver assistance system according to the second embodiment. [Figure 21] A block diagram showing an example of the hardware configuration of the information processing device according to the second embodiment. [Figure 22] A diagram showing an example of the functional block of the self-position estimation unit according to the second embodiment. [Figure 23] This diagram illustrates the calculations performed when converting acceleration measured by an acceleration sensor to acceleration in the world coordinate system. [Figure 24] A block diagram showing an example of the hardware configuration of an information processing device according to the third embodiment. [Figure 25]A diagram showing an example of the functional block of the self-position estimation unit according to the third embodiment. [Modes for carrying out the invention]

[0011] In one preferred embodiment of the present invention, the information processing device includes: prediction position acquisition means for acquiring the predicted position of a vessel; estimated position calculation means for calculating the estimated position of the vessel with the predicted position updated based on the comparison of data based on the output of external sensors provided on the vessel with map data; state information acquisition means for acquiring state information of the vessel; and speed calculation means for calculating the speed of the vessel at the first processing time based on the estimated position at the first processing time, the estimated position at the second processing time immediately preceding the first processing time, and a time constant set based on the state information.

[0012] The above-described information processing device includes a predicted position acquisition means, an estimated position calculation means, a state information acquisition means, and a speed calculation means. The predicted position acquisition means acquires the predicted position of the vessel. The estimated position calculation means calculates the estimated position of the vessel, with the predicted position updated, based on the comparison of data based on the output of external sensors installed on the vessel with map data. The state information acquisition means acquires the state information of the vessel. The speed calculation means calculates the speed of the vessel at the first processing time based on the estimated position at the first processing time, the estimated position at the second processing time immediately preceding the first processing time, and a time constant set based on the state information. This improves the accuracy of self-position estimation in the vessel.

[0013] In one embodiment of the information processing device described above, the state information includes the acceleration of the vessel.

[0014] In one embodiment of the above-described information processing device, the time constant is set to a value that decreases in proportion to the acceleration of the vessel.

[0015] In one embodiment of the information processing device described above, the state information includes the amount of change in the throttle lever of the vessel.

[0016] In one embodiment of the information processing device described above, the time constant is set to a value that decreases in accordance with the amount of change in the throttle lever of the vessel.

[0017] In one embodiment of the information processing device described above, the state information includes the amount of change in the propeller rotation speed of the ship.

[0018] In one embodiment of the above-described information processing device, the time constant is set to a value that decreases in proportion to the change in the propeller rotation speed of the ship.

[0019] In one embodiment of the information processing device described above, the state information includes the amount of change in the impeller rotation speed of the vessel.

[0020] In one embodiment of the information processing device described above, the time constant is set to a value that decreases in proportion to the change in the rotational speed of the ship's impeller.

[0021] In one embodiment of the information processing device described above, the predicted position acquisition means acquires the predicted position at the first processing time based on the estimated position at the second processing time and the speed of the vessel at the second processing time.

[0022] In another embodiment of the present invention, a control method executed by a computer acquires the predicted position of a vessel, calculates the estimated position of the vessel with updated predicted position based on the comparison of data based on the output of external sensors installed on the vessel with map data, acquires the status information of the vessel, and calculates the speed of the vessel at the first processing time based on the estimated position at the first processing time, the estimated position at the second processing time immediately preceding the first processing time, and a time constant set based on the status information. This improves the accuracy of self-position estimation in the vessel.

[0023] In yet another embodiment of the present invention, the program causes a computer to perform the following processes: acquire the predicted position of a vessel; calculate the estimated position of the vessel with updated predicted position based on the comparison of data from external sensors installed on the vessel with map data; acquire status information of the vessel; and calculate the speed of the vessel at the first processing time based on the estimated position at the first processing time, the estimated position at the second processing time immediately preceding the first processing time, and a time constant set based on the status information. By executing this program on a computer, the above information processing device can be realized. This program can be stored and used on a storage medium. [Examples]

[0024] Preferred embodiments of the present invention will be described below with reference to the drawings. Note that any character with "·", "^", or "-" above it will be referred to as "A" in this specification for convenience. · "A^" or "A - This is represented as (where "A" is any letter).

[0025] <First Example> First, let's describe the first embodiment.

[0026] [Overview of the driver assistance system] Figure 1A shows a schematic configuration of a driver assistance system according to the first embodiment. The driver assistance system according to this embodiment includes an information processing device 1 that moves together with the ship, which is a moving object, and a group of sensors 2 mounted on the ship. Hereafter, the ship that moves together with the information processing device 1 will also be referred to as the "target ship".

[0027] The information processing device 1 is electrically connected to the sensor group 2 and estimates the position of the target vessel (also called "self-position") based on the outputs of the various sensors included in the sensor group 2. Based on the self-position estimation result, the information processing device 1 provides driving support such as automatic driving control of the target vessel. Driving support includes docking support such as automatic docking. Here, "docking" includes not only docking the target vessel at a quay but also docking the target vessel at a structure such as a pier. The information processing device 1 may be a navigation device installed on the target vessel or an electronic control device built into the vessel.

[0028] Furthermore, the information processing device 1 stores a map database (DB:DataBase) 10 containing voxel data "VD". Voxel data VD is data that records position information of stationary structures for each voxel, which represents a cube (normal grid) that is the smallest unit of three-dimensional space. Voxel data VD includes data that represents the measured point cloud data of stationary structures within each voxel using a normal distribution, and is used for scan matching using NDT (Normal Distributions Transform), as described later. The information processing device 1 uses NDT scan matching to estimate, for example, the position on the plane, height position, yaw angle, pitch angle, and roll angle of the target vessel. Unless otherwise specified, the self-position is assumed to include attitude angles such as the yaw angle of the target vessel.

[0029] Figure 1B is a diagram illustrating the angular velocity used for self-position estimation. Sensor group 2 includes various external and internal sensors installed on the target vessel. In this embodiment, sensor group 2 includes a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 3, a GPS (Global Positioning System) receiver 5, and an Inertial Measurement Unit (IMU) 6 that measures the angular velocity of the target vessel in three axes. Specifically, as shown in Figure 1B, the IMU 6 measures the angular velocity ω with respect to the direction of travel of the target vessel as the axis. x And the angular velocity ω of the target vessel with respect to its lateral (left-right) axis. y And the angular velocity ω of the target vessel with respect to the vertical axis. z And, measure.

[0030] LIDA 3 discretely measures the distance to an object in the external environment by emitting a pulsed laser within a predetermined angular range in the horizontal and vertical directions, and generates three-dimensional point cloud data indicating the position of the object. In this case, LIDA 3 has an irradiation unit that irradiates laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and an output unit that outputs scan data (points that constitute the point cloud data, hereafter also called "measurement points") based on the light receiving signal output by the light receiving unit. The measurement points are generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the response delay time of the laser light specified based on the light receiving signal described above. Generally, the accuracy of the LIDA's distance measurement is higher the closer the distance to the object, and lower the accuracy the farther the distance. Note that LIDA 3 is not limited to the scan-type LIDA described above, but may also be a flash-type LIDA that generates three-dimensional data by diffusing laser light into the field of view of a two-dimensional array sensor.

[0031] Furthermore, the sensor group 2 may have a receiver that generates positioning results from a GNSS other than GPS, instead of the GPS receiver 5.

[0032] [Configuration of the information processing device] Figure 2 is a block diagram showing an example of the hardware configuration of an information processing device according to the first embodiment. The information processing device 1 mainly comprises an interface 11, a memory 12, and a controller 13. These elements are interconnected via a bus line.

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

[0034] Memory 12 is composed of various volatile and non-volatile memories such as RAM (Random Access Memory), ROM (Read Only Memory), hard disk drive, and flash memory. Memory 12 stores programs for the controller 13 to execute predetermined processes. Note that the programs executed by the controller 13 may be stored in storage media other than memory 12.

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

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

[0037] The controller 13 includes one or more processors such as a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), and a TPU (Tensor Processing Unit), and controls the entire information processing device 1. In this case, the controller 13 performs processing related to self-localization and driving assistance by executing programs stored in memory 12, etc.

[0038] Furthermore, the controller 13 functionally includes a self-position estimation unit 15. The controller 13 functions as a "predicted position acquisition means," a "estimated position calculation means," a "state information acquisition means," a "speed calculation means," and a computer that executes programs.

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

[0040] [NDT Scan Matching] Next, we will explain the position estimation based on NDT scan matching performed by the self-position estimation unit 15.

[0041] Figure 3 shows the self-position to be estimated by the self-position estimation unit 15, represented in three-dimensional Cartesian coordinates. As shown in Figure 3, the self-position in three-dimensional space defined on the three-dimensional Cartesian coordinate system (x,y,z) is represented by the coordinates "(x,y,z)", the roll angle "φ", the pitch angle "θ", and the yaw angle (direction) "ψ". Here, the roll angle φ is defined as the rotation angle around the direction of travel of the target vessel, the pitch angle θ is the elevation angle of the direction of travel of the target vessel relative to the xy plane, and the yaw angle ψ is defined as the angle between the direction of travel of the target vessel and the x-axis. The coordinates (x,y,z) are, for example, absolute positions corresponding to a combination of latitude, longitude, and altitude, or world coordinates indicating a position with a predetermined point as the origin. The self-position estimation unit 15 then performs self-position estimation using these x,y,z,φ,θ, andψ as estimation parameters.

[0042] Next, the voxel data VD used for NDT scan matching will be described. The voxel data VD includes data representing the measured point cloud data of the stationary structures in each voxel by a normal distribution.

[0043] FIG. 4 is a diagram showing an example of the schematic data structure of the voxel data VD. The voxel data VD includes information on parameters when representing the point cloud in a voxel by a normal distribution. In this embodiment, as shown in FIG. 4, it includes a voxel ID, voxel coordinates, a mean vector, and a covariance matrix.

[0044] The "voxel coordinates" indicate the absolute three-dimensional coordinates of a reference position such as the center position of each voxel. Each voxel is a cube obtained by dividing space into a grid, and since its shape and size are determined in advance, it is possible to specify the space of each voxel using the voxel coordinates. The voxel coordinates may be used as the voxel ID.

[0045] The "mean vector" and the "covariance matrix" indicate the mean vector and covariance matrix corresponding to the parameters when representing the point cloud in the target voxel by a normal distribution. Let the coordinates of an arbitrary point "i" in an arbitrary voxel "n" be X n (i)=[x n (i), y n (i), z n (i)] T and defined, and if the number of point clouds in voxel n is "N n ", then the mean vector "μ n " and the covariance matrix "V n " in voxel n are represented by the following equations (1) and (2), respectively.

[0046]

Equation

Equation

[0047] Next, we will explain the overview of NDT scan matching using voxel data VD.

[0048] Scan matching using NDT for ships involves estimation parameters that include the amount of movement in 3D space (here, xyz coordinates) and the orientation of the ship. P=[t x , t y , t z , t φ , t θ , t ψ ] T This will lead to the estimation of "t x " is the amount of movement in the x direction, "t y " is the amount of movement in the y direction, "t z " is the amount of movement in the z direction, "t φ " is the roll angle, "t θ " is the pitch angle, "t ψ The symbol " indicates the yaw angle.

[0049] Also, the coordinates of the point cloud data output by the LIDA3 are X L (j) = [x n (j), y n (j), z n (j)] T Therefore, X L (j) Average value "L' n This is expressed by the following equation (3). Through this operation, the point cloud data is downsampled.

[0050]

number

[0051] Then, using the estimated parameter P mentioned above, the mean value L' is transformed based on a known coordinate transformation process. Hereafter, the transformed coordinates will be referred to as "L'. n "

[0052] Then, the self-position estimation unit 15 searches for voxel data VD that corresponds to the point cloud data converted to an absolute coordinate system (also called the "world coordinate system") which is the same coordinate system as the map DB10, and the mean vector μ contained in that voxel data VD. n and the covariance matrix V n Using this, the evaluation function value of voxel n (also called the "individual evaluation function value") "E n The self-position estimation unit 15 calculates the individual evaluation function value E of voxel n based on the following equation (4). n Calculate.

[0053]

number

[0054] The self-localization unit 15 then calculates an overall evaluation function value (also called a "score value") "E(k)" for all voxels subject to matching, as shown by the following equation (5). The score value E serves as an indicator of the goodness of fit of the matching.

[0055]

number

[0056] Subsequently, the self-localization unit 15 calculates the estimated parameter P that maximizes the score value E(k) using an arbitrary root-finding algorithm such as Newton's method. Then, the self-localization unit 15 calculates the position (also called the "DR position") calculated by dead reckoning at processing index number (hereinafter referred to as processing number) k. DR By applying the estimated parameter P to (k), the self-position based on NDT scan matching (also called "NDT position") "X NDT Calculate (k). Note that the DR position and NDT position include position and orientation. Here, DR position X DR (k) corresponds to the provisional self-position before the calculation of the estimated self-position X^(k), and the predicted self-position "X -It is also written as "(k)". In this case, the NDT position X NDT (k) is expressed by the following equation (6).

[0057]

number

[0058] Then, the self-position estimation unit 15 determines the NDT position X NDT Let (k) be considered as the final self-position estimation result (also called the "estimated self-position") at processing number k, "X^(k)".

[0059] [Posture prediction] The self-position estimation unit 15 calculates the angular velocity "φ" at processing number k in the world coordinate system based on the following equations (7) to (9). · (k) ``θ'' · (k) and "ψ · The following equations (7) to (9) calculate "(k)". In the following equations, "φ^(k-1)", "θ^(k-1)", and "ψ^(k-1)" represent the estimated self-pose at processing number k-1 in the world coordinate system. Also, in the following equations (7) to (9), "ω x (k) ``ω'' y (k) and "ω z "(k)" represents the angular velocity at processing number k in the ship coordinate system as measured by IMU6.

[0060]

number

number

number

[0061] Furthermore, the self-position estimation unit 15 calculates the angular velocity "φ" for the following equations (10) to (12). · (k) ``θ'' · (k) and "ψ ·By applying "(k)" and the estimated self-poses "φ^(k-1)", "θ^(k-1)", and "ψ^(k-1)", the predicted self-pose "φ" at processing number k in the world coordinate system is obtained. - (k) ``θ'' - (k) and "ψ - Calculate "(k)". In the following formulas (10) to (12), "Δt(k)" represents the time difference obtained by subtracting the processing time "t(k-1)" of process number k-1 from the processing time "t(k)" of process number k.

[0062]

number

number

number

[0063] In other words, the second term on the right-hand side of the above equations (10) to (12) represents the amount of change corresponding to the magnitude of the change in attitude that is predicted to have occurred between processing number k-1 and processing number k.

[0064] Then, the self-position estimation unit 15 predicts the self-pose "φ - (k) ``θ'' - (k) and "ψ - By applying the same calculations as the aforementioned NDT scan matching to (k), the estimated self-poses "φ^(k)", "θ^(k)", and "ψ^(k)" at processing number k in the world coordinate system are calculated.

[0065] [Position prediction and speed calculation] The self-position estimation unit 15 calculates the following equations (13) to (15) using the estimated self-position "x^(k-1)", "y^(k-1)", and "z^(k-1)" at processing number k-1 in the world coordinate system, and the velocity "x" at processing number k-1 in the world coordinate system. · (k-1), y · (k-1) and "z· By applying "(k-1)", the predicted self-position "x" at processing number k in the world coordinate system is obtained. - (k) , y - (k) and "z - Calculate (k).

[0066]

number

number

number

[0067] In other words, the second term on the right-hand side of the above equations (13) to (15) represents the amount of change corresponding to the magnitude of the predicted change in position that occurred between processing number k-1 and processing number k.

[0068] The self-position estimation unit 15 predicts the self-position "x - (k) , y - (k) and "z - By applying the same calculations as the aforementioned NDT scan matching to (k), the estimated self-positions "x^(k)", "y^(k)", and "z^(k)" at processing number k in the world coordinate system are calculated.

[0069] Subsequently, the self-position estimation unit 15 calculates the velocity "x" at processing number k in the world coordinate system based on the following equations (16) to (18). · (k) , y · (k) and "z · Calculate (k). In the following formulas (16) to (18), "τ" represents the time constant and "s" represents the Laplace operator.

[0070]

number

number

number

[0071] The calculation process using the above formulas (13) to (18) can be illustrated, for example, as shown in Figure 5. In Figure 5, "Most recent estimated position" represents the estimated self-position at processing number k-1, "DR position" represents the predicted self-position at processing number k, and "NDT estimation result" represents the estimated self-position at processing number k. Figure 5 is a diagram illustrating an example of the process performed by the self-position estimation unit.

[0072] [Calculation of time constant τ] The self-localization unit 15 calculates the confidence value NRV (NDT Reliability Value) based on the following formula (19).

[0073]

number

[0074] In equation (19) above, "DSS" represents the downsampling size. Downsampling performed by equation (3) is carried out by dividing the space into grids of an appropriate size and calculating the average value for each grid. When the number of point cloud data from LIDA3 is large, the amount of data can be reduced by increasing the grid size, so that the NDT matching process can be completed within a predetermined time. Therefore, as the amount of point cloud data obtained when LIDA3 performs one period of scanning increases, "DSS" increases and the confidence value NRV also increases. In other words, when "DSS" takes a large value, the reliability of the self-localization estimation result increases.

[0075] In the above formula (19), "DAR" represents the ratio to which the downsampled point cloud data is mapped to the map. Therefore, when the point cloud data obtained after LIDA3 has performed one scan cycle has little occlusion and there is little change in the actual situation relative to the map stored in map DB10, "DAR" will be large and the confidence value NRV will be large. In other words, when "DAR" takes a large value, the reliability of the self-localization estimation result is high.

[0076] In the above formula (19), "Score" corresponds to the score value E mentioned earlier. Therefore, when the self-localization estimation result is close to the optimal solution, "Score" increases, and the confidence value NRV also increases. In other words, a large "Score" indicates a high reliability of the self-localization estimation result.

[0077] The self-localization unit 15 applies the confidence value NRV to one of the following formulas (20) to (23) to calculate the NDT Reliability Index (NRI), which is obtained by converting the confidence value NRV into a value that falls within the range of 0 or greater and 1 or less. The relationship between the confidence value NRV and the NRI is shown in Figure 6. Figure 6 is a diagram showing the relationship between the confidence value NRV and the NRI.

[0078]

number

number

number

number

[0079] The self-localization unit 15 calculates the time constant τ by applying the confidence index NRI to the following formula (24). The relationship between the confidence index NRI and the time constant τ is shown in Figure 7. The relationship between the confidence value NRV and the time constant τ is shown in Figure 8. Figure 7 is a diagram showing the relationship between the confidence index NRI and the time constant τ. Figure 8 is a diagram showing the relationship between the confidence value NRV and the time constant τ.

[0080]

number

[0081] By the way, the estimated self-position calculated by the self-position estimation unit 15 contains errors, albeit to varying degrees. Therefore, the difference from the immediately preceding estimated self-position also contains errors, as does the calculated velocity in the world coordinate system. In other words, the larger the error in the estimated self-position, the larger the error in the calculated velocity in the world coordinate system. If a large error occurs in the calculated velocity, the predicted position calculated by the above formulas (13) to (15) may be significantly off, potentially exceeding the search range in NDT scan matching. Therefore, in order to obtain an accurate predicted position as an initial value in NDT scan matching, it is necessary to reduce the velocity error.

[0082] The error in the estimated self-position can be assumed to be random noise. Therefore, it is thought that the error in the estimated self-position can be removed by a method similar to the method of removing high-frequency noise components with a low-pass filter. In this embodiment, for example, high-frequency noise components are suppressed by using a first-order lag low-pass filter (1 / (τs+1)) as shown in equations (16) to (18) above. However, using such a low-pass filter may result in the disadvantage of not being able to follow fast movements.

[0083] Therefore, in this embodiment, the time constant τ in the low-pass filter is changed according to the reliability of the self-localization estimation. For example, in situations where the reliability of the self-localization estimation is high and the error is considered small, setting the time constant τ of the low-pass filter to a small value widens the bandwidth and improves tracking performance. Conversely, in situations where the reliability of the self-localization estimation is low and the error is considered large, setting the time constant τ of the low-pass filter to a large value suppresses noise as much as possible. Hereafter, the low-pass filter will also be simply referred to as the filter.

[0084] Figures 9A, 10A, and 11A show examples of calculating the velocity in world coordinates without a filter. More specifically, Figures 9A, 10A, and 11A show a sudden change in velocity occurring 2 seconds after the start of the ship's movement, and noise occurring from 4 seconds after the start of the ship's movement. Figures 9B and 10B show examples of calculating the velocity in world coordinates with the filter time constant τ fixed. Figure 11B shows an example of calculating the velocity in world coordinates while varying the filter time constant τ.

[0085] As shown in Figures 9A and 9B, when the filter time constant τ is fixed at 0.1, the filter can follow the speed change, but it cannot suppress the noise. Also, as shown in Figures 10A and 10B, when the filter time constant τ is fixed at 1.0, the filter can suppress the noise, but it cannot follow the speed change. In this embodiment, as shown in Figures 11A and 11B, for example, the filter time constant τ is maintained at 0.1 until just before 4 seconds have elapsed from the start of speed calculation, and then the filter time constant τ is changed from 0.1 to 1.0 at the moment 4 seconds have elapsed, thereby enabling the filter to follow the speed change while suppressing the noise.

[0086] When implementing the filter of this embodiment as a program, the conversion should be performed in the following steps: continuous-time transfer function (s-domain) → discrete-time transfer function (z-domain) → difference equation (time-domain).

[0087] [Effects of filtering] Here, we will explain the effects exhibited by the filter in this embodiment. For simplicity, in the following explanation, we will assume that the target vessel is moving on a two-dimensional plane and that its direction is the same as that of the world coordinate system. Furthermore, in the following explanation, "true position" refers to the actual position of the target vessel, "estimated position" refers to the estimated self-position of the target vessel, and "predicted position" refers to the predicted self-position of the target vessel.

[0088] First, we will explain the problems that arise when performing self-position estimation without using a filter, especially when the positional error is large, referring to Figure 12A. Figure 12A is a diagram illustrating an example of self-position estimation of a ship without a filter.

[0089] At processing times k-3 and k-2 in Figure 12A, the self-localization is performed well, resulting in a near-perfect match between the true position and the estimated position.

[0090] Subsequently, if, for example, at the timing of processing number k-1 in Figure 12A, unfavorable conditions for performing NDT scan matching occur, such as the area around the target vessel being sparse space or a large vessel passing near the target vessel, an estimated position shifted in the y-direction from the true position will be calculated. Therefore, the velocity at processing number k-1, calculated using the difference between the estimated position at processing number k-2 and the estimated position at processing number k-1, will have an incorrect velocity in the y-direction. · (k-1) is included.

[0091] Furthermore, at the timing of processing number k in Figure 12A, the estimated position y^(k-1) is incorrect, and the velocity y is incorrect. · (k-1) and predict the position y- (k) is calculated as a result of the predicted position y - (k) is further deviated from the true position. Also, the predicted position y - A large deviation in (k) causes an overshoot of the search range in NDT scan matching. As a result, the estimated position y^(k) becomes far from the true position. Therefore, y · Because (k) is calculated as the velocity moving away from the true position, there is a risk that the estimated position calculated after the situation shown in Figure 12A will move further and further away from the true position.

[0092] In other words, when self-localization is performed without using a filter, the deviation (error) of the predicted position gradually increases due to situations that are inconvenient for performing NDT scan matching, resulting in the problem that it is not possible to obtain an appropriate estimated self-localization that approaches the true position.

[0093] Next, the advantages of performing self-position estimation using the filter of this embodiment will be explained with reference to Figure 12B. Figures 12B and 12C are diagrams illustrating an example of self-position estimation of a ship with the filter enabled.

[0094] At processing times k-3 and k-2 in Figure 12B, the self-localization is performed well, resulting in a near-perfect match between the true position and the estimated position.

[0095] Subsequently, for example, at the timing of processing number k-1 in Figure 12B, if an unfavorable situation occurs during the implementation of NDT scan matching, causing the value of the confidence index NRI to decrease, there is a high possibility that the estimated position is inaccurate. Therefore, the self-position estimation unit 15 sets the time constant τ of the filter used to calculate the velocity to a large value. As a result, large velocity changes are suppressed in y. · (k-1) is calculated.

[0096] Then, at the timing of processing number k in Figure 12B, the predicted position y -Because the deviation of (k) from the true position is small, it is within the search range in NDT scan matching, and the estimated position y^(k) is able to approach the true position. · Because (k) is calculated as the velocity in the direction approaching the true position, the estimated position calculated after the situation shown in Figure 12B gradually approaches the true position.

[0097] In other words, when self-localization is performed using a filter, an advantage arises in that even if unfavorable conditions occur for performing NDT scan matching, it is possible to obtain an appropriate estimated self-localization that approaches the true position.

[0098] Incidentally, in large vessels with high inertial force and small velocity changes, it is considered acceptable to perform self-position estimation using a filter with a constant frequency characteristic that prevents large velocity changes. However, small vessels such as pleasure boats have high acceleration performance, and their velocity changes are not necessarily small.

[0099] Therefore, for example, if the reliability of self-position estimation is sufficiently high, it is considered better to reduce the value of the time constant τ to accommodate the large acceleration and deceleration that may occur in small vessels. Accordingly, in this embodiment, self-position estimation is performed using a filter whose characteristics can be adaptively changed by setting the time constant τ according to the value of the reliability index NRI.

[0100] Furthermore, as shown in processing numbers k-1 and k in Figure 12C, for example, if the target vessel moves significantly in the y-direction, but the self-position estimation by NDT scan matching is performed correctly, the value of the confidence index NRI increases and the value of the filter time constant τ decreases, so the velocity y · (k-1) is calculated without suppression. As a result, velocity y · While (k-1) is calculated as a large value, it is calculated as a correct value, so the predicted position at processing number k is calculated to be close to the true position.

[0101] [Functional block] FIG. 13 is a diagram showing an example of a functional block of the self-position estimation unit according to the first embodiment. As shown in FIG. 13, the self-position estimation unit 15 includes a dead reckoning block 21, a coordinate conversion block 22, an NDT position calculation block 23, a reliability calculation block 24, a time constant calculation block 25, a speed calculation block 26, and a filter block 27.

[0102] The dead reckoning block 21, based on the angular velocities (ω x (k), ω y (k), and ω z (k)) output from the IMU 6 at the timing of process number k, and the estimated self-attitude (φ^(k - 1), θ^(k - 1), and ψ^(k - 1)) at process number k - 1 immediately before process number k calculated by the NDT position calculation block 23, calculates the angular velocities (φ · (k), θ · (k), and ψ · (k)) at process number k. Further, the dead reckoning block 21 calculates the predicted self-attitude (φ - (k), θ - (k), and ψ - (k)) at process number k based on the estimated self-attitude at process number k - 1 calculated by the NDT position calculation block 23 and the angular velocities at process number k. Further, the dead reckoning block 21, based on the estimated self-position (x^(k - 1), y^(k - 1), and z^(k - 1)) at process number k - 1 calculated by the NDT position calculation block 23 and the speed (x · (k - 1), y · (k - 1), and z · (k - 1)) at process number k - 1 output from the speed calculation block 26 through the filter block 27, calculates the predicted self-position (x - (k), y - (k), and z -The (k)) is calculated. If it is immediately after the start of self-position estimation and there is no estimated self-attitude and estimated self-position at processing number k-1, the dead reckoning block 21 calculates the predicted self-attitude at processing number k based on the angular velocity output from the IMU 6, and calculates the predicted self-position at processing number k based on the signal output from the GPS receiver 5.

[0103] The coordinate transformation block 22 transforms the point cloud data based on the output of the lidar 3 into the world coordinate system, which is the same coordinate system as the map DB 10. In this case, the coordinate transformation block 22 performs the coordinate transformation of the point cloud data for processing number k based on the predicted self-attitude and predicted self-position obtained as a result of processing of the dead reckoning block 21, for example. Processes for transforming point cloud data in a coordinate system based on a lidar installed on a mobile body (in this embodiment, a ship) into the coordinate system of the mobile body, and processes for transforming from the coordinate system of the mobile body into the world coordinate system, etc., are disclosed, for example, in International Publication WO2019 / 188745.

[0104] The NDT position calculation block 23 calculates the estimated self-position and estimated self-orientation based on the point cloud data obtained as a result of processing by the coordinate transformation block 22. In this case, the NDT position calculation block 23 matches the point cloud data in the world coordinate system supplied from the coordinate transformation block 22 with the voxel data VD represented in the same world coordinate system to establish a correspondence between the point cloud data and the voxels. The NDT position calculation block 23 also calculates an individual evaluation function value based on formula (4) for each voxel that has been matched with the point cloud data, and calculates the estimated parameter P that maximizes the score value E(k) based on formula (5). Then, based on formula (6), the NDT position calculation block 23 calculates the estimated self-position (x^(k), y^(k), and z^(k)) at processing number k by applying the estimated parameter P obtained at processing number k to the predicted self-position at processing number k obtained as a result of processing by the dead reckoning block 21. Furthermore, the NDT position calculation block 23 calculates the estimated self-pose (φ^(k), θ^(k), and ψ^(k)) at processing number k by performing a similar process to the above. Note that the NDT position calculation block 23 may, for example, calculate the estimated self-position and estimated self-pose after removing erroneous data such as the water surface position and data below it from the point cloud data obtained as a result of the coordinate transformation block 22's processing.

[0105] The confidence calculation block 24 identifies three parameters, DSS, DAR, and Score, based on the processing performed in the NDT position calculation block 23. Then, the confidence calculation block 24 calculates the confidence value NRV using the aforementioned three parameters and converts the calculated confidence value NRV into the confidence index NRI.

[0106] The time constant calculation block 25 calculates the time constant τ using the confidence index NRI obtained as a result of processing by the confidence calculation block 24.

[0107] The velocity calculation block 26 calculates the velocity at processing number k without filtering by dividing the difference between the estimated self-position at processing number k obtained as a result of processing by the NDT position calculation block 23 and the estimated self-position at processing number k-1 by the time difference Δt(k).

[0108] The filter block 27 sets a filter (1 / (τs+1)) using the time constant τ obtained as a result of processing by the time constant calculation block 25, and applies the set filter to the velocity calculated by the velocity calculation block 26, thereby calculating the velocity (x) at processing number k. · (k), y · (k) and z · Calculate (k).

[0109] [Processing flow] Figure 14 is a flowchart showing an example of the procedure for self-position estimation processing performed by the self-position estimation unit 15 of the information processing device 1. The self-position estimation unit 15 starts the processing shown in the flowchart of Figure 8 when it becomes necessary to perform self-position estimation, such as when the power is turned on.

[0110] First, immediately after the start of the self-position estimation process, the self-position estimation unit 15 calculates a predicted self-position from the GPS positioning result based on the data output by the GPS receiver 5 (step S11).

[0111] Next, the self-position estimation unit 15 determines whether or not a sufficient amount of data has been obtained from the lidar 3 to perform NDT scan matching (step S12).

[0112] If the self-position estimation unit 15 does not obtain enough data to perform NDT scan matching (step S12: NO), it will acquire scan data from the lidar 3 again and repeat the process in step S12 based on the acquired data. If the self-position estimation unit 15 obtains enough data to perform NDT scan matching (step S12: YES), it will calculate the time difference Δt(k) (step S13), and further calculate the angular velocity (ω) in the ship coordinate system. x (k), ω y (k) and ω z (k)) is obtained from IMU6 (step S14).

[0113] Subsequently, the self-position estimation unit 15 calculates the angular velocity in the world coordinate system (φ) based on the angular velocity in the ship coordinate system obtained in step S14 and equations (7) to (9). · (k), θ · (k) and ψ · Calculate (k) (step S15).

[0114] Next, the self-position estimation unit 15 predicts its own attitude (φ) in the world coordinate system based on the time difference Δt(k) calculated in step S13, the angular velocity in the world coordinate system calculated in step S15, and equations (10) to (12). - (k), θ - (k) and ψ - Calculate (k) (step S16).

[0115] Next, the self-position estimation unit 15 calculates its predicted self-position (x) in the world coordinate system based on the time difference Δt(k) calculated in step S13 and equations (13) to (15). - (k), y - (k) and z - Calculate (k) (step S17).

[0116] Next, the self-position estimation unit 15 performs NDT scan matching using the predicted self-pose obtained in step S16 and the DR position corresponding to the predicted self-position obtained in step S17 as initial values, thereby calculating the estimated self-position (x^(k), y^(k), and z^(k)) and the estimated self-pose (φ^(k), θ^(k), and ψ^(k)) respectively (step S18).

[0117] Next, the self-localization unit 15 calculates the confidence value NRV based on the three parameters DSS, DAR, and Score identified based on the processing content of step S18, and the formula (19) (step S19).

[0118] Next, the self-localization unit 15 converts the confidence value NRV calculated in step S19 into a confidence index NRI by applying it to one of the formulas (20) to (23) (step S20).

[0119] Next, the self-position estimation unit 15 calculates the time constant τ based on the confidence index NRI obtained in step S20 and formula (24) (step S21).

[0120] Next, the self-position estimation unit 15 calculates the velocity (x) based on the equations (16) to (18) obtained by applying the time difference Δt(k) calculated in step S13 and the time constant τ calculated in step S21 to the filter (1 / (τs+1)). · (k), y · (k) and z · Calculate (k) (step S22).

[0121] After performing step S22, the self-position estimation unit 15 determines whether or not to terminate the self-position estimation process (step S23). If the self-position estimation unit 15 determines that the self-position estimation process should be terminated (step S23: YES), it terminates the process in the flowchart. On the other hand, if the self-position estimation unit 15 decides to continue the self-position estimation process (step S23: NO), it returns to step S12 and calculates the estimated self-position, estimated self-attitude, velocity, etc. for the next processing number.

[0122] [Discussion based on experimental results] We will now discuss the experimental results related to the above-described examples.

[0123] Figure 15 shows the installation status and scan range of the lidar during the experiment. The applicant performed NDT position estimation by matching pre-created coastal voxel data (ND map) for a certain waterway with point cloud data obtained from a lidar installed on a vessel in operation. As illustrated in Figure 15, this experiment was conducted with a medium-range, horizontal 60-degree type lidar, with an operating frequency of 12 Hz (period 83.3 ms), with two lidars installed on each side of the vessel. In addition, RTK-GPS positioning results were used as the ground truth position data for accuracy evaluation.

[0124] In this experiment, downsampling is performed on point cloud data obtained by the lidar. In this case, the downsampling size is adaptively changed at each processing time so that the number of measurement points associated with the voxel data VD in the downsampled point cloud data (also called the "corresponding measurement points") falls within a predetermined target range during NDT scan matching. For example, at each processing time, if the number of corresponding measurement points is greater than the upper limit of the target range, the self-position estimation unit 15 increases the downsampling size at the next processing time by a predetermined rate (e.g., 1.1 times) or a predetermined value, and if the number of corresponding measurement points is less than the lower limit of the target range, it decreases the downsampling size at the next processing time by a predetermined rate or a predetermined value. On the other hand, at each processing time, if the number of corresponding measurement points is within the target range, the self-position estimation unit 15 maintains the downsampling size. With such adaptive setting of the downsampling size, the downsampling size increases as the number of corresponding measurement points increases, so it can be inferred that the reliability of the NDT process increases as the downsampling size increases.

[0125] Figures 16A to 16G and 17A to 17G show the self-position estimation results for a comparative example where velocity is calculated without filtering. Here, Figure 16A shows the number of measurement points in the point cloud data before downsampling, Figure 16B shows the downsampling size, Figure 16C shows the number of measurement points and corresponding measurement points after downsampling, Figure 16D shows the correspondence ratio DAR, Figure 16E shows the score value E, Figure 16F shows the confidence value NRV, and Figure 16G shows the confidence index NRI. Also, Figure 17A shows the velocity in the x direction, Figure 17B shows the velocity in the y direction, and Figure 17C shows the velocity in the z direction. Furthermore, for the RTK-GPS positioning results, Figure 17D shows the error in the direction of travel, Figure 17E shows the error in the lateral direction, Figure 17F shows the error in the height direction, and Figure 17G shows the error in the yaw angle.

[0126] In the experimental results for the comparative example, as shown in Figure 16D, the value of the correspondence ratio DAR decreases around 90-120s, which negatively affects the calculation accuracy of NDT scan matching. As a result of this situation, according to the velocities shown in Figures 17A-17C, disturbances that appear to be offsets occur, particularly around 90-120s. Furthermore, as shown in Figures 17D-17G, the errors in estimated self-position and estimated self-attitude are larger in the areas where velocity disturbances occur compared to other areas. In particular, the error in the direction of travel in Figure 17D, although not shown for convenience, increases to approximately -20m.

[0127] Figures 18A to 18G and 19A to 19H show the self-position estimation results for an embodiment in which velocity is calculated with a filter. Here, Figure 18A shows the number of measurement points in the point cloud data before downsampling, Figure 18B shows the downsampling size, Figure 18C shows the number of measurement points and corresponding measurement points after downsampling, Figure 18D shows the correspondence ratio DAR, Figure 18E shows the score value E, Figure 18F shows the confidence value NRV, and Figure 18G shows the confidence index NRI. Also, Figure 19A shows the time constant τ, Figure 19B shows the velocity in the x direction, Figure 19C shows the velocity in the y direction, and Figure 19D shows the velocity in the z direction. Furthermore, for the RTK-GPS positioning results, Figure 19E shows the error in the direction of travel, Figure 19F shows the error in the lateral direction, Figure 19G shows the error in the height direction, and Figure 19H shows the error in the yaw angle.

[0128] In the experimental results for this embodiment, the speeds in Figures 19B to 19D are generally smoother than those in Figures 17A to 17C, and the turbulence in the speed around 100 to 120 s is reduced. As a result, in the experimental results for this embodiment, as shown in Figures 19E to 19H, the errors in estimated self-position and estimated self-attitude are reduced compared to the experimental results for the comparative example. In particular, the error in the direction of travel in Figure 19E shows that the large error shown in Figure 17D is suppressed, and the error amplitude up to 100 s is also smaller on average. Furthermore, as shown in Figure 18G, the period during which the reliability index NRI value is reduced is shorter than in the experimental results for the comparative example (Figure 16G), indicating that stability has improved compared to the state without the filter.

[0129] As described above, according to this embodiment, the speed of a ship can be calculated using a filter (1 / (τs+1)) in which the time constant τ changes according to the confidence index NRI (confidence value NRV), and the calculated speed of the ship can be used to perform self-position estimation. Therefore, according to this embodiment, the accuracy of self-position estimation in a ship can be improved.

[0130] [Differentiation] The following describes suitable modifications of the above-described embodiments.

[0131] (Variation 1) The self-localization unit 15 may set the time constant τ using the correspondence ratio DAR instead of the confidence index NRI.

[0132] For example, if the value of the correspondence ratio DAR is small, the reliability of the self-localization estimation result is considered to be low and error is likely to be high. Therefore, in the self-localization estimation unit 15 according to this modified example, if the value of the correspondence ratio DAR is small, the value of the time constant τ should be set to a large value.

[0133] Furthermore, for example, when the value of the correspondence ratio DAR is large, the reliability of the self-position estimation result is considered to be high and the error is small. Therefore, in the self-position estimation unit 15 according to this modified example, when the value of the correspondence ratio DAR is large, the value of the time constant τ should be set to a small value.

[0134] In other words, the self-position estimation unit 15 according to this modified example can calculate an appropriate time constant τ by applying a value of "DAR" that falls within the range of 0 to 1 as the value of "NRI" in formula (24).

[0135] (Modification 2) The self-position estimation unit 15 uses the angular velocity (ω) measured by the IMU 6. x (k), ω y (k) and ω z Instead of using (k), for example, by performing calculations using the following formulas (25) to (27), the angular velocity "φ" at processing number k in the world coordinate system can be obtained. · (k) ``θ'' · (k) and "ψ · Alternatively, you can calculate "(k)". In the following formulas (25) to (27), "τ" represents the time constant and "s" represents the Laplace operator.

[0136]

number

number

[0137] That is, according to this modification example, even in a ship without the IMU6, the self-position estimation process can be performed by the operations using the mathematical formulas (25) to (27).

[0138] [Second Embodiment] Next, the second embodiment will be described. In this embodiment, the description of the parts where the same configurations as those in the first embodiment can be applied will be omitted as appropriate, and the description will focus on the parts different from the first embodiment.

[0139] [Outline of the Driving Support System] FIG. 20 is a diagram showing a schematic configuration of a driving support system according to the second embodiment. The driving support system according to this embodiment includes an information processing device 1A provided in a target ship and a sensor group 2A mounted on the target ship.

[0140] The sensor group 2A includes various external sensors and internal sensors provided in the target ship. In this embodiment, the sensor group 2A includes a lidar 3, a GPS receiver 5, an IMU6 that measures the angular velocity of the target ship in three axial directions, and an acceleration sensor 7 that measures the acceleration of the target ship in three axial directions. According to this embodiment, when the IMU6 can measure acceleration, the acceleration sensor 7 may not be included in the sensor group 2A.

[0141] [Configuration of the Information Processing Device] FIG. 21 is a block diagram showing an example of the hardware configuration of the information processing device according to the second embodiment. As shown in FIG. 21, the information processing device 1A has a self-position estimation unit 15A instead of the self-position estimation unit 15 of the information processing device 1.

[0142] [Functional Blocks] Figure 22 shows an example of the functional blocks of the self-position estimation unit according to the second embodiment. As shown in Figure 22, the self-position estimation unit 15A includes a dead reckoning block 21, a coordinate transformation block 22, an NDT position calculation block 23, a time constant calculation block 25A, a velocity calculation block 26, and a filter block 27A.

[0143] The time constant calculation block 25A uses the acceleration in the three axes measured by the acceleration sensor 7 to calculate the time constant τ described later. x , τ y and τ z Calculate.

[0144] Filter block 27A replaces the time constant "τ" in equation (16) with the time constant "τ x Applying this, replace the time constant "τ" in equation (17) with the time constant "τ y Applying this, replace the time constant "τ" in equation (18) with the time constant "τ z By applying this and performing the calculation, the velocity at processing number k in the world coordinate system "x · (k) , y · (k) and "z · Calculate (k).

[0145] [Time constant τ x , τ y and τ z [Calculation of] Here, the time constant τ calculated by the self-position estimation unit 15A is used. x , τ y and τ z The calculation method will be explained below. Note that the following example describes the case where there is no (or almost no) displacement in the roll and pitch directions, and the acceleration changes on a two-dimensional plane.

[0146] The self-position estimation unit 15A uses the acceleration in three axes "a" measured by the acceleration sensor 7 to determine the position. xb "a yb " and "a zb By applying this to the following equations (28)~(30), we can obtain the acceleration in the three axes in the world coordinate system "a x"a y " and "a z The acceleration "a" is calculated. xb " corresponds to the direction of travel of the target vessel x b This is acceleration in a direction, and acceleration "a yb " corresponds to the lateral (left-right) direction of the vessel in question. b This is acceleration in a direction, and acceleration "a zb " corresponds to the vertical direction of the vessel in question, z b This is the acceleration in the axial direction. Furthermore, an overview of the calculations in the following equations (28) to (30) can be shown, for example, as in Figure 23. Figure 23 is a diagram illustrating the overview of the calculations performed when converting the acceleration measured by the acceleration sensor to the acceleration in the world coordinate system.

[0147]

number

number

number

[0148] The self-position estimation unit 15A calculates the acceleration in the three axes in the world coordinate system "a" for the following equations (31) to (33). x "a y " and "a z By applying ", the time constant "τ x "τ y " and "τ z Calculate ".

[0149]

number

number

number

[0150] According to the above formula (31), for example, when the absolute value of the acceleration "a x " is small and the velocity change in the x - direction is small, the time constant τ x is calculated as a large value. Also, according to the above formula (31), for example, when the absolute value of the acceleration "a x " is large and the velocity change in the x - direction is large, the time constant τ x is calculated as a small value.

[0151] According to the above formula (32), for example, when the absolute value of the acceleration "a y " is small and the velocity change in the y - direction is small, the time constant τ y is calculated as a large value. Also, according to the above formula (32), for example, when the absolute value of the acceleration "a y " is large and the velocity change in the y - direction is large, the time constant τ y is calculated as a small value.

[0152] According to the above formula (33), for example, when the absolute value of the acceleration "a z " is small and the velocity change in the z - direction is small, the time constant τ <​​​​​​​​​​​​​​​​​​​​​​​​​​The speed of the vessel can be calculated using s+1)) and the calculated speed of the vessel, and the self-position estimation process can be performed using the calculated speed of the vessel. Therefore, according to this embodiment, the accuracy of self-position estimation in a vessel can be improved.

[0154] (modified version) The following describes suitable modifications of the above-described embodiments.

[0155] The self-position estimation unit 15A may acquire ship state information indicating the state of the target ship and calculate the time constant τ based on the acquired ship state information. In this modified example, the time constant τ is the velocity x · (k), y · (k) and z · When calculating (k), the time constant τ x , τ y and τ z It can be used as a substitute.

[0156] Specifically, the self-position estimation unit 15A acquires, for example, the amount of change SV of the target vessel's throttle lever, the amount of change PV of the target vessel's propeller rotation speed, or the amount of change IV of the target vessel's impeller rotation speed as ship status information.

[0157] Then, the self-position estimation unit 15A calculates the time constant τ by substituting the values ​​of the change amounts SV, PV, or IV acquired as ship state information into "n" in the following formula (34).

[0158]

number

[0159] The amount of change SV in the throttle lever of the target vessel increases as the acceleration (or deceleration) of the vessel increases. Therefore, when the value of the change SV substituted for "n" in equation (34) is large, the time constant τ is calculated as a small value.

[0160] The change in propeller rotation speed PV of the target vessel increases as the acceleration (or deceleration) of the vessel increases. Therefore, when the value of the change in PV substituted for "n" in equation (34) is large, the time constant τ is calculated as a small value.

[0161] The change in impeller rotation speed IV of the target vessel increases as the acceleration (or deceleration) of the vessel increases. Therefore, when the value of the change in IV substituted for "n" in equation (34) is large, the time constant τ is calculated as a small value.

[0162] The self-position estimation unit 15A may acquire the acceleration in the direction of travel of the target vessel, measured by the acceleration sensor 7, as vessel state information. In other words, the vessel state information may include any of the following: the change in the throttle lever SV of the target vessel, the change in the propeller rotation speed PV of the target vessel, the change in the impeller rotation speed IV of the target vessel, or the acceleration of the target vessel.

[0163] <Third Example> Next, a third embodiment will be described. In this embodiment, explanations of parts to which the same configuration as in the first or second embodiment can be applied will be omitted as appropriate, and the explanation will focus on parts that differ from both the first and second embodiments.

[0164] [Overview of the driver assistance system] The driving support system according to this embodiment comprises an information processing device 1B installed on the target vessel and a group of sensors 2A mounted on the target vessel.

[0165] [Configuration of the information processing device] Figure 24 is a block diagram showing an example of the hardware configuration of an information processing device according to the third embodiment. As shown in Figure 24, the information processing device 1B has a self-position estimation unit 15B instead of the self-position estimation unit 15 of the information processing device 1.

[0166] [Function Block] Figure 25 shows an example of the functional blocks of the self-position estimation unit according to the third embodiment. As shown in Figure 25, the self-position estimation unit 15B includes a dead reckoning block 21, a coordinate transformation block 22, an NDT position calculation block 23, a time constant calculation block 25B, a velocity calculation block 26, and a filter block 27.

[0167] The time constant calculation block 25B calculates the time constant τ using the reliability index NRI obtained as a result of processing by the reliability calculation block 24 and the acceleration measured by the acceleration sensor 7.

[0168] [Calculation of time constant τ] Here, we will explain how the self-position estimation unit 15B calculates the time constant τ.

[0169] The self-position estimation unit 15B applies the confidence index NRI to formula (24) to obtain a time constant τ similar to that of the first embodiment. t The self-position estimation unit 15B calculates the acceleration in the three axes (a) measured by the acceleration sensor 7. xb a yb and a zb By applying ) to equations (28) to (30), the acceleration in the three axes in the world coordinate system (a x a y and a z The self-position estimation unit 15B calculates the acceleration in the three axes (a) in the world coordinate system. x a y and a z By applying ) to equations (31) to (33), a time constant τ similar to that in the second embodiment is obtained. x , τ y and τ z The self-position estimation unit 15B calculates the time constant τ. t and τ x (τ) corresponds to the average value t +τ x ) / 2 and the time constant τ t and τ y (τ) corresponds to the average value t +τ y ) / 2 and the time constant τt and τ z (τ) corresponds to the average value t +τ z ) / 2 is obtained as the result of calculating the time constant τ.

[0170] As described above, according to this embodiment, the reliability index NRI and acceleration a x a y and a z The ship's speed can be calculated using a filter (1 / (τs+1)) whose time constant τ changes accordingly, and the calculated ship's speed can be used to perform self-position estimation. Therefore, according to this embodiment, the accuracy of self-position estimation in a ship can be improved.

[0171] In each of the embodiments described above, the program can be stored using various types of non-transitory computer-readable medium and supplied to a control unit, which is a computer. Non-transitory computer-readable medium includes various types of tangible storage medium. Examples of non-transitory computer-readable medium include magnetic storage medium (e.g., flexible disks, magnetic tapes, hard disk drives), magneto-optical storage medium (e.g., magneto-optical disks), CD-ROM (Read Only Memory), CD-R, CD-R / W, and semiconductor memory (e.g., mask ROM, PROM (Programmable ROM), EPROM (Erasable PROM), flash ROM, RAM (Random Access Memory)).

[0172] Although the present invention has been described above with reference to embodiments, the present invention is not limited to the above embodiments. Various modifications to the structure and details of the present invention can be made that are understandable to those skilled in the art within the scope of the present invention. That is, the present invention naturally includes the full disclosure, including the claims, and various modifications and alterations that those skilled in the art could make in accordance with the technical idea. Furthermore, each disclosure of the above-mentioned patent documents and other references is incorporated herein by reference. [Explanation of Symbols]

[0173] 1, 1A, 1B Information Processing Devices 2, 2A Sensor Group 3 Riders 5 GPS receivers 6 IMU 7. Accelerometer 10 Map Database

Claims

1. A means for obtaining the predicted position of a ship, An estimated position calculation means calculates the estimated position of the ship with updated predicted position based on a comparison of data based on the output of external sensors installed on the ship and map data, A parameter acquisition means for acquiring one of the following as parameters indicating the acceleration and deceleration of the vessel: the acceleration of the vessel, the change in the throttle lever of the vessel, the change in the propeller rotation speed of the vessel, or the change in the impeller rotation speed of the vessel. A speed calculation means for calculating the speed of the vessel at the first processing time, based on the estimated position at the first processing time, the estimated position at the second processing time immediately preceding the first processing time, and a first-order lag low-pass filter capable of changing its characteristics in accordance with changes in a time constant set by applying the parameters acquired at the first processing time to a predetermined mathematical formula, An information processing device having

2. The information processing apparatus according to Claim 1, wherein the time constant is set to a value that decreases as the absolute value of the acceleration of the ship, which is the parameter applied to the predetermined mathematical formula, increases.

3. The information processing device according to Claim 1, wherein the time constant is set to a value that decreases as the absolute value of the amount of change of the throttle lever of the ship, which is the parameter applied to the predetermined mathematical formula, increases.

4. The information processing apparatus according to claim 1, wherein the time constant is set to a value that decreases as the absolute value of the change in the propeller rotation speed of the ship, which is the parameter applied to the predetermined mathematical formula, increases.

5. The information processing device according to Claim 1, wherein the time constant is set to a value that decreases as the absolute value of the change in the rotational speed of the ship's impeller, which is the parameter applied to the predetermined mathematical formula, increases.

6. The information processing apparatus according to any one of claims 1 to 5, wherein the predicted position acquisition means acquires the predicted position at the first processing time based on the estimated position at the second processing time and the speed of the vessel at the second processing time.

7. A control method performed by a computer, Obtain the predicted position of the ship, Based on the comparison of data from external sensors installed on the vessel with map data, the estimated position of the vessel with the updated predicted position is calculated. As parameters indicating the acceleration and deceleration of the vessel, one of the following is obtained: the acceleration of the vessel, the change in the throttle lever of the vessel, the change in the propeller rotation speed of the vessel, or the change in the impeller rotation speed of the vessel. A control method for calculating the speed of a vessel at a first processing time, based on the estimated position at a first processing time, the estimated position at a second processing time immediately preceding the first processing time, and a first-order lag low-pass filter whose characteristics can be changed in accordance with a change in a time constant set by applying the parameters acquired at the first processing time to a predetermined mathematical formula.

8. Obtain the predicted position of the ship, Based on the comparison of data from external sensors installed on the vessel with map data, the estimated position of the vessel with the updated predicted position is calculated. As parameters indicating the acceleration and deceleration of the vessel, one of the following is obtained: the acceleration of the vessel, the change in the throttle lever of the vessel, the change in the propeller rotation speed of the vessel, or the change in the impeller rotation speed of the vessel. A program that causes a computer to perform a process to calculate the speed of the vessel at the first processing time, based on the estimated position at the first processing time, the estimated position at the second processing time immediately preceding the first processing time, and a first-order lag low-pass filter whose characteristics can be changed in accordance with a change in a time constant set by applying the parameters acquired at the first processing time to a predetermined mathematical formula.

9. A storage medium storing the program described in Claim 8.

Images