Information processing device, control method, program and storage medium

Abstract

To provide an information processing device capable of improving an accuracy of self-position estimation on a ship.SOLUTION: An information processing device includes predicted position acquisition means, estimation position calculation means, reliability calculation means, and speed calculation means. The predicted position acquisition means acquires a predicted position of a ship. The estimation position calculation means calculates an estimation position of the ship obtained by updating the predicted position on the basis of a verification of data based on an output of an external field sensor installed on the ship with map data. The reliability calculation means calculates a degree of reliability of the verification. The speed calculation means calculates a velocity of the ship at a first processing time on the basis of the estimation position at the first processing time, an estimation position at a second processing time immediately before the first processing time, and a time constant set based on at least the degree of reliability.SELECTED DRAWING: Figure 13

Description

[Technical Field]

[0001] The present invention relates to ship localization. [Background technology]

[0002] Conventionally, there is known a technique for estimating the self-position of a moving object by comparing (matching) shape data of surrounding objects measured using a measurement device such as a laser scanner with map information in which the shapes of surrounding objects are stored in advance. For example, Patent Document 1 discloses an autonomous mobile system that determines whether a detected object in a voxel obtained by dividing a space according to a predetermined rule is a stationary object or a moving object, and matches the map information with the measurement data for voxels in which a stationary object exists. Furthermore, Patent Document 2 discloses a scan matching method that estimates the self-position by comparing voxel data including the mean vector and covariance matrix of a stationary object for each voxel with point cloud data output by a lidar. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] International Publication WO2013 / 076829 [Patent Document 2] International Publication No. WO2018 / 221453 Summary of the Invention [Problem to be solved by the invention]

[0004] Recently, autonomous ship-steering systems have been studied not only in the automotive field but also in ships, and highly accurate self-location estimation is equally important for safe autonomous maneuvering. In the open ocean, where there are few structures in the vicinity, self-location can be determined using the Global Navigation Satellite System (GNSS). However, along urban coasts and rivers, where high-rise buildings and other structures are located nearby, the GNSS radio wave reception environment is poor due to factors such as a reduced number of receiving satellites and multipath, making highly accurate positioning impossible. Therefore, studies are being conducted to enable highly accurate self-location estimation using the above-mentioned scan matching for ships as well.

[0005] However, in the case of ships, the influence of tides and waves appears as errors in the estimated position, which causes a problem of reduced accuracy in self-position estimation.

[0006] The present invention has been made to solve the above-mentioned problems, and a main object of the present invention is to provide an information processing device that can improve the accuracy of self-position estimation on a ship. [Means for solving the problem]

[0007] The invention described in the claims is an information processing device comprising: a predicted position acquisition means for acquiring a predicted position of a ship; an estimated position calculation means for calculating an estimated position of the ship by updating the predicted position based on a comparison between data based on the output of an external sensor provided on the ship and map data; a reliability calculation means for calculating the reliability of the comparison; and a speed calculation means for calculating the speed of the ship at the first processing time based on the estimated position at a first processing time, the estimated position at a second processing time immediately before the first processing time, and a time constant set based on at least the reliability.

[0008] The claimed invention is also a control method executed by a computer, which acquires a predicted position of a ship, calculates an estimated position of the ship by updating the predicted position based on a comparison between data based on the output of an external sensor installed on the ship and map data, calculates the reliability of the comparison, and calculates the speed of the ship at the first processing time based on the estimated position at a first processing time, the estimated position at a second processing time immediately before the first processing time, and a time constant set based on at least the reliability.

[0009] The invention described in the claims is also a program that causes a computer to perform the following processes: acquire a predicted position of a ship; calculate an estimated position of the ship by updating the predicted position based on a comparison between data based on the output of an external sensor installed on the ship and map data; calculate the reliability of the comparison; and calculate the speed of the ship at the first processing time based on the estimated position at a first processing time, the estimated position at a second processing time immediately before the first processing time, and a time constant set based on at least the reliability. [Brief explanation of the drawings]

[0010] [Figure 1A] 1 is a diagram showing a schematic configuration of a driving assistance system according to a first embodiment. [Figure 1B] FIG. 2 is a diagram for explaining angular velocity used in self-position estimation. [Figure 2] FIG. 1 is a block diagram showing an example of a hardware configuration of an information processing apparatus according to a first embodiment. [Figure 3] FIG. 2 is a diagram showing the self-position to be estimated by the self-position estimation unit in three-dimensional Cartesian coordinates. [Figure 4] FIG. 2 is a diagram showing an example of a schematic data structure of voxel data VD. [Figure 5] FIG. 4 is a diagram for explaining an example of processing performed by a self-position estimation unit. [Figure 6] FIG. 10 is a diagram showing the relationship between the reliability value NRV and the reliability index NRI. [Figure 7] FIG. 10 is a graph showing the relationship between the reliability index NRI and the time constant τ. [Figure 8]FIG. 10 is a diagram showing the relationship between the reliability value NRV and the time constant τ. [Figure 9A] FIG. 10 is a diagram showing an example of a case where a velocity in a world coordinate system is calculated without a filter. [Figure 9B] FIG. 10 is a diagram showing an example of a case where a velocity in a world coordinate system is calculated with a time constant τ of a filter fixed. [Figure 10A] FIG. 10 is a diagram showing an example of a case where a velocity in a world coordinate system is calculated without a filter. [Figure 10B] FIG. 10 is a diagram showing an example of a case where a velocity in a world coordinate system is calculated with a time constant τ of a filter fixed. [Figure 11A] FIG. 10 is a diagram showing an example of a case where a velocity in a world coordinate system is calculated without a filter. [Figure 11B] FIG. 10 is a diagram showing an example in which the velocity in the world coordinate system is calculated while changing the time constant τ of the filter. [Figure 12A] FIG. 10 is a diagram for explaining an example of a case where a ship's self-position is estimated without a filter. [Figure 12B] FIG. 10 is a diagram for explaining an example of a case where a ship's self-position is estimated with a filter. [Figure 12C] FIG. 10 is a diagram for explaining an example of a case where a ship's self-position is estimated with a filter. [Figure 13] FIG. 2 is a diagram showing an example of functional blocks of a self-position estimation unit according to the first embodiment. [Figure 14] 10 is a flowchart showing an example of a procedure for self-position estimation processing. [Figure 15] FIG. 1 is a diagram showing the installation state and scanning range of the lidar during the experiment. [Figure 16A] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 16B] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 16C] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 16D]FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 16E] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 16F] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 16G] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 17A] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 17B] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 17C] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 17D] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 17E] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 17F] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 17G] FIG. 10 is a diagram showing a result of self-position estimation according to a comparative example in which velocity is calculated without a filter. [Figure 18A] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 18B] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 18C] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 18D] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 18E] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 18F]FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 18G] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 19A] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 19B] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 19C] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 19D] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 19E] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 19F] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 19G] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 19H] FIG. 10 is a diagram showing a result of self-location estimation according to an embodiment in which a velocity is calculated with a filter. [Figure 20] FIG. 10 is a diagram showing a schematic configuration of a driving assistance system according to a second embodiment. [Figure 21] FIG. 10 is a block diagram showing an example of the hardware configuration of an information processing apparatus according to a second embodiment. [Figure 22] FIG. 10 is a diagram showing an example of functional blocks of a self-position estimation unit according to a second embodiment. [Figure 23] FIG. 2 is a diagram showing an outline of calculations performed when converting acceleration measured by an acceleration sensor into acceleration in a world coordinate system. [Figure 24] FIG. 10 is a block diagram showing an example of the hardware configuration of an information processing apparatus according to a third embodiment. [Figure 25] FIG. 11 is a diagram showing an example of functional blocks of a self-position estimation unit according to a third embodiment. DETAILED DESCRIPTION OF THE INVENTION

[0011] In one preferred embodiment of the present invention, an information processing device includes a predicted position acquisition means for acquiring a predicted position of a ship, an estimated position calculation means for calculating an estimated position of the ship by updating the predicted position based on a comparison between data based on the output of an external sensor provided on the ship and map data, a reliability calculation means for calculating the reliability of the comparison, and a speed calculation means for calculating the speed of the ship at the first processing time based on the estimated position at a first processing time, the estimated position at a second processing time immediately before the first processing time, and a time constant set based on at least the reliability.

[0012] The information processing device includes a predicted position acquisition means, an estimated position calculation means, a reliability calculation means, and a speed calculation means. The predicted position acquisition means acquires the predicted position of the ship. The estimated position calculation means calculates an estimated position of the ship with the predicted position updated based on a comparison between data based on the output of an external sensor provided on the ship and map data. The reliability calculation means calculates the reliability of the comparison. The speed calculation means calculates the speed of the ship at the first processing time based on 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 based on at least the reliability. This improves the accuracy of self-position estimation on the ship.

[0013] In one aspect of the information processing device, the time constant is set to a value that decreases according to the magnitude of the reliability.

[0014] In one aspect of the information processing device, the time constant is set based on the reliability and the acceleration of the ship.

[0015] In one aspect of the above information processing device, 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 ship at the second processing time.

[0016] In one aspect of the information processing device, the reliability calculation means calculates the reliability based on at least a score value indicating a degree of matching of the matching.

[0017] In one aspect of the above information processing device, the data is second point cloud data, which is point cloud data obtained by downsampling first point cloud data, which is point cloud data output by the external sensor, and the reliability calculation means calculates the reliability based on at least the size of the downsampling or the number of measurement points of the first point cloud data.

[0018] In one aspect of the information processing device, the reliability calculation means calculates the reliability based on at least a ratio at which the data is associated with the map data.

[0019] In another embodiment of the present invention, a control method executed by a computer acquires a predicted position of a ship, calculates an estimated position of the ship by updating the predicted position based on a comparison between data based on outputs of external sensors provided on the ship and map data, calculates a reliability of the comparison, and calculates a speed of the ship at the first processing time based on the estimated position at a first processing time, the estimated position at a second processing time immediately before the first processing time, and a time constant set based on the reliability, thereby improving the accuracy of self-position estimation of the ship.

[0020] In yet another embodiment of the present invention, a program causes a computer to execute the following processes: acquire a predicted position of a ship; calculate an estimated position of the ship by updating the predicted position based on a comparison between data based on the output of an external sensor provided on the ship and map data; calculate the reliability of the comparison; and calculate the speed of the ship at the first processing time based on the estimated position at a first processing time, the estimated position at a second processing time immediately before the first processing time, and a time constant set based on the reliability. By executing this program on a computer, the above-mentioned information processing device can be realized. This program can be stored in a storage medium and used. [Example]

[0021] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For the sake of convenience, in this specification, a character with "·", "^" or "-" added to any symbol will be referred to as "A · ", "A^" or "A - " (where "A" is any letter).

[0022] <First Example> First, the first embodiment will be described.

[0023] [Overview of driving assistance system] 1A is a diagram showing the schematic configuration of a driving assistance system according to a first embodiment. The driving assistance system according to this embodiment includes an information processing device 1 that moves together with a ship, which is a moving body, and a sensor group 2 mounted on the ship. Hereinafter, the ship that moves together with the information processing device 1 will also be referred to as the "target ship."

[0024] The information processing device 1 is electrically connected to the sensor group 2, and estimates the position of the target ship on which the information processing device 1 is installed (also referred to as "self-position") based on the outputs of various sensors included in the sensor group 2. Then, the information processing device 1 performs driving assistance such as automatic driving control of the target ship based on the results of estimating its self-position. Driving assistance also includes docking assistance such as automatic docking. Here, "docking" includes not only docking the target ship at a quay, but also docking the target ship at a structure such as a pier. The information processing device 1 may be a navigation device installed on the target ship, or an electronic control device built into the ship.

[0025] The information processing device 1 also stores a map database (DB: DataBase) 10 containing voxel data "VD." The voxel data VD is data that records position information and the like of stationary structures for each voxel, which represents a cube (regular lattice), the smallest unit of three-dimensional space. The voxel data VD includes data that expresses measured point cloud data of stationary structures within each voxel using a normal distribution, and is used for scan matching using NDT (Normal Distributions Transform), as described below. The information processing device 1 estimates, for example, the planar position, height position, yaw angle, pitch angle, and roll angle of the target ship through NDT scan matching. Unless otherwise specified, the self-position is assumed to also include the attitude angle, such as the yaw angle, of the target ship.

[0026] FIG. 1B is a diagram for explaining angular velocity used for self-position estimation. The sensor group 2 includes various external and internal sensors provided on the target ship. In this embodiment, the sensor group 2 includes a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 3, a GPS (Global Positioning System) receiver 5, and an Inertial Measurement Unit (IMU) 6 that measures the angular velocity of the target ship in three axial directions. Specifically, as shown in FIG. 1B, the IMU 6 measures the angular velocity ω x and the angular velocity ω about the axis of the lateral direction (left-right direction) of the target ship y and the angular velocity ω of the target ship about the vertical axis. z and are measured.

[0027] The LIDAR 3 emits a pulsed laser beam over a predetermined angular range in the horizontal and vertical directions to discretely measure the distance to an object in the external world and generate three-dimensional point cloud data indicating the position of the object. In this case, the LIDAR 3 includes an irradiation unit that irradiates laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and an output unit that outputs scan data (points that constitute the point cloud data, hereinafter also referred to as "measurement points") based on the light receiving signal output by the light receiving unit. The measurement points are generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the response delay time of the laser light identified based on the above-mentioned light receiving signal. Note that, in general, the closer the distance to the object, the higher the accuracy of the LIDAR's distance measurement value, and the farther the distance, the lower the accuracy. Note that the LIDAR 3 is not limited to the above-mentioned scan-type LIDAR, but may also be a flash-type LIDAR that generates three-dimensional data by irradiating a diffused laser beam within the field of view of a two-dimensional array sensor.

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

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

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

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

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

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

[0034] The controller 13 includes one or more processors such as a CPU (Central Processing Unit), a GPU (Graphics Processing Unit), and a TPU (Tensor Processing Unit), and controls the entire information processing device 1. In this case, the controller 13 executes programs stored in the memory 12 or the like to perform processing related to self-position estimation, driving assistance, and the like.

[0035] Moreover, functionally, the controller 13 has a self-position estimation unit 15. The controller 13 functions as a "predicted position acquisition means," "estimated position calculation means," "reliability calculation means," "velocity calculation means," a computer that executes a program, and the like.

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

[0037] [NDT Scan Matching] Next, the position estimation based on NDT scan matching executed by the self-position estimation unit 15 will be described.

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

[0039] Next, we will explain the voxel data VD used in NDT scan matching. The voxel data VD includes data in which measured point cloud data of a stationary structure in each voxel is expressed using a normal distribution.

[0040] 4 is a diagram showing an example of a schematic data structure of the voxel data VD. The voxel data VD includes parameter information when a point group in a voxel is expressed by a normal distribution, and in this embodiment, as shown in FIG. 4, includes a voxel ID, voxel coordinates, a mean vector, and a covariance matrix.

[0041] "Voxel coordinates" indicate the absolute three-dimensional coordinates of a reference position, such as the center position of each voxel. Each voxel is a cube that divides space into a grid, and since its shape and size are predetermined, it is possible to identify the space of each voxel using its voxel coordinates. Voxel coordinates may also be used as a voxel ID.

[0042] The "mean vector" and "covariance matrix" refer to the mean vector and covariance matrix, which are parameters when expressing the point cloud in the target voxel as a normal distribution. Note that the coordinates of an arbitrary point "i" in an arbitrary voxel "n" are X n (i)=[x n (i), y n (i), z n (i)] T and the number of points in voxel n is defined as "N n ", then the mean vector at voxel n is "μ n ” and the covariance matrix “V n " are expressed by the following formulas (1) and (2), respectively.

[0043]

number

number

[0044] Next, an overview of NDT scan matching using voxel data VD will be explained.

[0045] Scan matching using NDT assuming a ship is performed using estimated parameters that are based on the amount of movement in three-dimensional space (here, xyz coordinates) and the orientation of the ship. P=[t x , t y , t z , t φ , t θ , t ψ ] T Here, "t x " is the amount of movement in the x direction, "t y " is the amount of movement in the y direction, "t z " is the amount of movement in the z direction, "t φ ” is the roll angle, “t θ ” is the pitch angle, “t ψ " indicates the yaw angle.

[0046] In addition, the coordinates of the point cloud data output by the lidar 3 are X L (j)=[x n (j), y n (j), z n (j)] T Then, X L The average value of (j) "L' n " is expressed by the following equation (3): By such a calculation, the point cloud data is downsampled.

[0047]

number

[0048] Then, using the estimated parameter P, the average value L' is subjected to coordinate transformation based on a known coordinate transformation process. Hereinafter, the transformed coordinates will be referred to as "L n "

[0049] Then, the self-position estimation unit 15 searches for voxel data VD that is associated with the point cloud data converted into an absolute coordinate system (also called a "world coordinate system") that is the same coordinate system as the map DB 10, and calculates the mean vector μ n and the covariance matrix V n Using the above, the evaluation function value of voxel n (also called "individual evaluation function value") "E n In this case, the self-position estimation unit 15 calculates the individual evaluation function value E of the voxel n based on the following equation (4): n Calculate.

[0050]

number

[0051] Then, the self-position estimation unit 15 calculates a comprehensive evaluation function value (also called a "score value") "E(k)" for all voxels to be matched, as shown in the following formula (5). The score value E is an index showing the compatibility of the matching.

[0052]

number

[0053] Thereafter, the self-position estimation unit 15 calculates the estimation parameter P that maximizes the score value E(k) by using an arbitrary root-finding algorithm such as Newton's method. Then, the self-position estimation unit 15 calculates the position (also referred to as "DR position") "X" calculated by dead reckoning at the processing index number (hereinafter referred to as the processing number) k. DR By applying the estimation parameter P to "(k)", the self-position based on NDT scan matching (also called "NDT position") "X NDT (k)" is calculated. The DR position and NDT position include the position and the orientation. Here, the 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 - In this case, NDT position X NDT (k) is expressed by the following equation (6).

[0054]

number

[0055] Then, the self-position estimation unit 15 calculates the NDT position X NDT (k) is regarded as the final self-location estimation result (also called "estimated self-location") "X^(k)" at process number k.

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

[0057]

number

number

number

[0058] Furthermore, the self-position estimation unit 15 calculates the angular velocity "φ · (k)," "θ · (k)" and "ψ · (k)" and the estimated self-orientation "φ^(k-1)", "θ^(k-1)" and "ψ^(k-1)" are applied to obtain the predicted self-orientation "φ^(k-1)" at the processing number k in the world coordinate system. - (k)," "θ - (k)" and "ψ - In the following formulas (10) to (12), "Δt(k)" represents the time difference obtained by subtracting the processing time "t(k-1)" of processing number k-1 from the processing time "t(k)" of processing number k.

[0059]

number

number

number

[0060] That is, the second term on the right side of the above equations (10) to (12) represents the amount of change corresponding to the magnitude of the change in posture predicted to have occurred between process number k-1 and process number k.

[0061] Then, the self-position estimation unit 15 calculates the predicted self-attitude "φ - (k)," "θ - (k)" and "ψ - By applying the same calculations as the NDT scan matching described above to "(k)", the estimated self-orientation "φ^(k)", "θ^(k)" and "ψ^(k)" at processing number k in the world coordinate system are calculated.

[0062] [Position prediction and speed calculation] The self-position estimation unit 15 calculates the estimated self-positions "x^(k-1)", "y^(k-1)", and "z^(k-1)" at the process number k-1 in the world coordinate system and the velocity "x^(k-1)" at the process number k-1 in the world coordinate system using the following formulas (13) to (15). · (k-1)", "y · (k-1)" and "z · (k-1)" and the predicted self-position "x - (k)," "y - (k)" and "z - (k) is calculated.

[0063]

number

number

number

[0064] That is, the second term on the right side of the above equations (13) to (15) represents the amount of change corresponding to the magnitude of the position change predicted to have occurred between process number k-1 and process number k.

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

[0066] Thereafter, the self-position estimation unit 15 calculates the velocity “x · (k)," "y · (k)" and "z · (k)". In the following formulas (16) to (18), "τ" represents a time constant, and "s" represents a Laplace operator.

[0067]

number

number

number

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

[0069] [Calculation of time constant τ] The self-position estimation unit 15 calculates the reliability value NRV (NDT Reliability Value) based on the following equation (19).

[0070]

number

[0071] In the above formula (19), "DSS" represents the downsampling size. The downsampling performed by formula (3) is performed by dividing the space into grids of an appropriate size and calculating the average value for each grid. When the amount of point cloud data from the LIDAR 3 is large, the amount of data can be reduced by increasing the grid size, making it possible to complete the NDT matching process within a specified time. Therefore, as the amount of point cloud data obtained when the LIDAR 3 performs one scanning cycle increases, "DSS" increases and the reliability value NRV also increases. In other words, when "DSS" takes a large value, the reliability of the estimation result of the self-localization increases.

[0072] In the above formula (19), "DAR" represents the ratio at which the downsampled point cloud data is associated with the map. Therefore, in the point cloud data obtained when the LIDAR 3 performs one scanning cycle, if there is little occlusion and there is little change in the actual situation compared to the map stored in the map DB 10, "DAR" becomes large and the reliability value NRV also becomes large. In other words, if "DAR" takes a large value, the reliability of the estimation result of the self-localization becomes high.

[0073] "Score" in the above formula (19) is a value corresponding to the score value E described above. Therefore, when the estimation result of the self-location estimation is close to the optimal solution, "Score" becomes large and the reliability value NRV also becomes large. In other words, when "Score" takes a large value, the reliability of the estimation result of the self-location estimation becomes high.

[0074] The self-location estimation unit 15 applies the reliability value NRV to any one of the following formulas (20) to (23) to calculate a reliability index NRI (NDT Reliability Index) by converting the reliability value NRV into a value in the range of 0 to 1. The relationship between the reliability value NRV and the reliability index NRI is shown in Fig. 6. Fig. 6 is a diagram showing the relationship between the reliability value NRV and the reliability index NRI.

[0075]

number

number

number

number

[0076] The self-location estimation unit 15 calculates the time constant τ by applying the reliability index NRI to the following equation (24). The relationship between the reliability index NRI and the time constant τ is shown in FIG. 7. The relationship between the reliability value NRV and the time constant τ is shown in FIG. 8. FIG. 7 is a diagram showing the relationship between the reliability index NRI and the time constant τ. FIG. 8 is a diagram showing the relationship between the reliability value NRV and the time constant τ.

[0077]

number

[0078] Incidentally, the estimated self-position calculated by the self-position estimation unit 15 contains an error, although to varying degrees. Therefore, the difference from the immediately preceding estimated self-position also contains an error, and the calculated value of the velocity in the world coordinate system also contains an error. In other words, the larger the error in the estimated self-position, the larger the error in the calculated value of the velocity in the world coordinate system. If a large error occurs in the calculated value of the velocity, the predicted position calculated by the above formulas (13) to (15) may deviate significantly, which may result in 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.

[0079] The error in the estimated self-position can be assumed to be random noise. Therefore, it is considered possible to remove the error in the estimated self-position using a method similar to the method of removing high-frequency noise components using a low-pass filter. In this embodiment, for example, as shown in the above formulas (16) to (18), the high-frequency noise components are suppressed by using a first-order delay low-pass filter (1 / (τs+1)). However, when using such a low-pass filter, there is a risk of a disadvantage in that it cannot follow fast movements.

[0080] Therefore, in this embodiment, the time constant τ of the low-pass filter is changed depending on the reliability of the self-location estimation. Therefore, for example, in a situation where the reliability of the self-location estimation is high and the error is small, the time constant τ of the low-pass filter can be set to a small value to widen the band and improve tracking ability. Also, for example, in a situation where the reliability of the self-location estimation is low and the error is large, the time constant τ of the low-pass filter can be set to a large value to suppress noise as much as possible. Note that, hereinafter, the low-pass filter will also be simply referred to as a filter.

[0081] Figures 9A, 10A, and 11A are diagrams showing an example of a case where velocity in the world coordinate system is calculated without a filter. More specifically, Figures 9A, 10A, and 11A show a state where a sudden change in velocity occurs two seconds after the ship starts moving, and noise occurs four seconds after the ship starts moving. Figures 9B and 10B are diagrams showing an example of a case where velocity in the world coordinate system is calculated with the filter time constant τ fixed. Figure 11B is a diagram showing an example of a case where velocity in the world coordinate system is calculated while changing the filter time constant τ.

[0082] As shown in Figures 9A and 9B, when the filter time constant τ is fixed to 0.1, it is possible to follow speed changes, but noise cannot be suppressed. Also, as shown in Figures 10A and 10B, when the filter time constant τ is fixed to 1.0, it is possible to suppress noise, but it is not possible to follow speed changes. 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 since the start of speed calculation, and then the filter time constant τ is changed from 0.1 to 1.0 at the timing when 4 seconds have elapsed, thereby enabling noise to be suppressed while following speed changes.

[0083] When the filter of this embodiment is implemented as a program, it is sufficient to convert it in the order of continuous time transfer function (s domain) → discrete time transfer function (z domain) → difference equation (time domain).

[0084] [Filter Effects] Here, the effect exerted by the filter of this embodiment will be explained. In the following explanation, for simplicity, it is assumed that the target ship is moving on a two-dimensional plane and that the direction of the target ship is the same as that of the world coordinate system. In the following explanation, "true position" means the actual position of the target ship, "estimated position" means the estimated self-position of the target ship, and "predicted position" means the predicted self-position of the target ship.

[0085] First, problems that arise when estimating the ship's position without using a filter when the position error becomes large will be described with reference to Fig. 12A. Fig. 12A is a diagram for explaining an example of estimating the ship's position without using a filter.

[0086] At the timings of process numbers k-3 and k-2 in FIG. 12A, the self-position estimation is performed well, so that the true position and the estimated position are almost the same.

[0087] Thereafter, for example, if an unfavorable situation occurs for performing NDT scan matching at the timing of processing number k-1 in Figure 12A, such as the area around the target ship being sparsely populated or a large ship passing close to the target ship, an estimated position that is shifted in the y direction from the true position is 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 includes an erroneous velocity y in the y direction. · (k-1) is included.

[0088] At the timing of processing number k in FIG. 12A, the incorrect estimated position y^(k-1) and the incorrect velocity y · (k-1) and the predicted position y - (k) is calculated, and the predicted position y - (k) will deviate further from the true position. - A large deviation in (k) causes the search range to be exceeded in NDT scan matching. As a result, the estimated position y^(k) is far from the true position. Therefore, y · Since (k) is calculated as a velocity in a direction away from the true position, there is a risk that the estimated position calculated later than in the situation shown in FIG. 12A will become increasingly farther away from the true position.

[0089] In other words, if self-location estimation is performed without using a filter, the deviation (error) in the predicted position will gradually increase due to the occurrence of situations that are unfavorable for performing NDT scan matching, resulting in the problem that it is not possible to obtain an appropriate estimated self-location that approaches the true position.

[0090] Next, the advantages of performing self-location estimation using the filter of this embodiment will be described with reference to Fig. 12B. Fig. 12B and Fig. 12C are diagrams for explaining an example of a case where the self-location of a ship is estimated with a filter.

[0091] At the timings of process numbers k-3 and k-2 in FIG. 12B, the self-location estimation is performed well, so that the true location and the estimated location are almost the same.

[0092] Thereafter, for example, at the timing of processing number k-1 in FIG. 12B, if an unfavorable situation occurs in the execution of NDT scan matching and the value of the reliability index NRI becomes small, the estimated position is likely to be inaccurate, so 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. · (k-1) is calculated.

[0093] At the timing of processing number k in FIG. 12B, the predicted position y - Since the deviation of (k) from the true position is small, it is within the search range of NDT scan matching, and the estimated position y^(k) can approach the true position. · Since (k) is calculated as the velocity in the direction approaching the true position, the estimated position calculated later than the situation shown in FIG. 12B gradually approaches the true position.

[0094] In other words, when self-location estimation is performed using a filter, there is an advantage that an appropriate estimated self-location that approaches the true position can be obtained even when a situation occurs that is unfavorable for performing NDT scan matching.

[0095] In the case of large vessels with large inertial forces and small speed changes, it is considered acceptable to perform self-location estimation using a filter with a certain frequency characteristic that does not cause large speed changes. However, small vessels such as pleasure boats have high acceleration performance and speed changes are not necessarily small.

[0096] Therefore, for example, if the reliability of the self-location estimation is sufficiently high, it is considered better to reduce the value of the time constant τ so that it can handle large acceleration and deceleration that may occur in a small vessel. Therefore, in this embodiment, the time constant τ is set according to the value of the reliability index NRI, so that the self-location estimation is performed using a filter whose characteristics can be adaptively changed.

[0097] Furthermore, for example, as shown in process numbers k-1 and k in FIG. 12C, if the target ship moves significantly in the y direction while the self-position estimation by NDT scan matching is performed correctly, the reliability index NRI value will be large and the value of the filter time constant τ will be small, so the speed y · (k-1) is calculated without suppression. As a result, the velocity y · While (k-1) is calculated as a large value, it is also calculated as a correct value, so the predicted position at process number k is calculated as being close to the true position.

[0098] [Function Block] Fig. 13 is a diagram showing an example of functional blocks of a self-location estimation unit according to Example 1. As shown in Fig. 13, the self-location estimation unit 15 has a dead reckoning block 21, a coordinate transformation block 22, an NDT position calculation block 23, a reliability calculation block 24, a time constant calculation block 25, a velocity calculation block 26, and a filter block 27.

[0099] The dead reckoning block 21 calculates the angular velocity (ω x (k), ωy (k) and ω z (k)) and the estimated self-orientation (φ^(k-1), θ^(k-1) and ψ^(k-1)) at the processing number k immediately before the processing number k calculated by the NDT position calculation block 23, the angular velocity (φ · (k), θ · (k) and ψ · Furthermore, the dead reckoning block 21 calculates the predicted self-attitude (φ(k)) at the processing number k based on the estimated self-attitude at the processing number k−1 calculated by the NDT position calculation block 23 and the angular velocity at the processing number k. - (k), θ - (k) and ψ - The dead reckoning block 21 calculates the estimated self-position (x^(k-1), y^(k-1) and z^(k-1)) at the processing number k-1 calculated by the NDT position calculation block 23 and the velocity (x^(k-1), y^(k-1) and z^(k-1)) at the processing number k-1 output from the velocity calculation block 26 via the filter block 27. · (k-1), y · (k-1) and z · (k-1)) and the predicted self-position (x - (k), y - (k) and z - In addition, immediately after the start of self-position estimation, if there are no estimated self-attitude and estimated self-position at processing number k-1, the dead reckoning block 21 calculates a predicted self-attitude at processing number k based on the angular velocity output from the IMU 6, and calculates a predicted self-position at processing number k based on the signal output from the GPS receiver 5, for example.

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

[0101] The NDT position calculation block 23 calculates an estimated self-position and an 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 expressed in the same world coordinate system, thereby associating the point cloud data with the voxels. The NDT position calculation block 23 also calculates an individual evaluation function value based on Equation (4) for each voxel associated with the point cloud data, and calculates an estimated parameter P that maximizes the score value E(k) based on Equation (5). The NDT position calculation block 23 then calculates an estimated self-position (x^(k), y^(k), and z^(k)) for processing number k based on Equation (6) by applying the estimated parameter P calculated for processing number k to the predicted self-position for processing number k obtained as a result of processing by the dead reckoning block 21. Furthermore, the NDT position calculation block 23 performs processing similar to this to calculate the estimated self-orientation (φ^(k), θ^(k), and ψ^(k)) at processing number k. Note that the NDT position calculation block 23 may, for example, calculate the estimated self-position and estimated self-orientation after removing, as erroneous data, data at the water surface position and below that from the point cloud data obtained as a processing result of the coordinate transformation block 22.

[0102] The reliability calculation block 24 identifies three parameters, DSS, DAR, and Score, based on the content of the processing performed in the NDT position calculation block 23. Then, the reliability calculation block 24 calculates a reliability value NRV using the above-mentioned three parameters, and converts the calculated reliability value NRV into a reliability index NRI.

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

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

[0105] The filter block 27 sets a filter (1 / (τs+1)) using the time constant τ obtained as a processing result of the time constant calculation block 25, and applies the set filter to the speed calculated by the speed calculation block 26 to obtain the speed (x · (k), y · (k) and z · (k)) is calculated.

[0106] [Processing flow] Fig. 14 is a flowchart showing an example of the procedure of the self-location estimation process executed by the self-location estimation unit 15 of the information processing device 1. The self-location estimation unit 15 starts the process of the flowchart in Fig. 8 when it becomes necessary to perform self-location estimation, for example, when the power is turned on.

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

[0108] Next, the self-position estimation unit 15 determines whether or not an amount of data that allows NDT scan matching to be performed has been obtained as scan data from the LIDAR 3 (step S12).

[0109] If the self-position estimation unit 15 has not obtained enough data to perform NDT scan matching (step S12: NO), it acquires scan data from the LIDAR 3 again and performs the process of step S12 again based on the acquired data. If the self-position estimation unit 15 has obtained enough data to perform NDT scan matching (step S12: YES), it calculates the time difference Δt(k) (step S13) and further calculates the angular velocity (ω x (k), ω y (k) and ω z (k)) is acquired from the IMU 6 (step S14).

[0110] Thereafter, the self-position estimation unit 15 calculates the angular velocity (φ · (k), θ · (k) and ψ · (k)) is calculated (step S15).

[0111] Next, the self-position estimation unit 15 calculates a predicted self-attitude (φ - (k), θ - (k) and ψ - (k)) is calculated (step S16).

[0112] Next, the self-position estimation unit 15 calculates the predicted self-position (x - (k), y - (k) and z - (k)) is calculated (step S17).

[0113] Next, the self-position estimation unit 15 performs NDT scan matching using the predicted self-orientation 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-orientation (φ^(k), θ^(k) and ψ^(k)) (step S18).

[0114] Next, the self-position estimation unit 15 calculates the reliability value NRV based on the three parameters DSS, DAR, and Score identified based on the processing content of step S18 and on Equation (19) (step S19).

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

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

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

[0118] After performing step S22, the self-position estimation unit 15 determines whether or not the self-position estimation process should be ended (step S23). If the self-position estimation unit 15 determines that the self-position estimation process should be ended (step S23: YES), the self-position estimation unit 15 ends the process of the flowchart. On the other hand, if the self-position estimation process is to be continued (step S23: NO), the self-position estimation unit 15 returns to step S12 and calculates the estimated self-position, estimated self-orientation, speed, etc. for the next process number.

[0119] [Considerations based on experimental results] The experimental results for the above-described embodiment will now be considered.

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

[0121] In this experiment, downsampling was performed on the 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 (also referred to as the "number of corresponding measurement points") associated with the voxel data VD in the downsampled point cloud data in the NDT scan matching falls within a predetermined target range. For example, if the number of corresponding measurement points is greater than the upper limit of the target range at each processing time, the self-localization unit 15 increases the downsampling size at the next processing time by a predetermined rate (e.g., 1.1 times) or a predetermined value. If the number of corresponding measurement points is less than the lower limit of the target range, the self-localization unit 15 decreases the downsampling size at the next processing time by a predetermined rate or a predetermined value. On the other hand, if the number of corresponding measurement points is within the target range at each processing time, the self-localization unit 15 maintains the downsampling size. In this adaptive setting of the downsampling size, the more the number of corresponding measurement points, the larger the downsampling size. Therefore, it is presumed that the larger the downsampling size, the higher the reliability of the NDT processing.

[0122] 16A to 16G and 17A to 17G are diagrams showing the results of self-localization estimation according to a comparative example in which velocity is calculated without a filter. Here, FIG. 16A shows the number of measurement points of the point cloud data before downsampling, FIG. 16B shows the downsampling size, FIG. 16C shows the number of measurement points and the number of corresponding measurement points after downsampling, FIG. 16D shows the association ratio DAR, FIG. 16E shows the score value E, FIG. 16F shows the reliability value NRV, and FIG. 16G shows the reliability index NRI. Also, FIG. 17A shows the velocity in the x direction, FIG. 17B shows the velocity in the y direction, and FIG. 17C shows the velocity in the z direction. Furthermore, for the RTK-GPS positioning results, FIG. 17D shows the error in the heading direction, FIG. 17E shows the error in the lateral direction, FIG. 17F shows the error in the vertical direction, and FIG. 17G shows the error in the yaw angle.

[0123] In the experimental results for the comparative example, as shown in FIG. 16D, the value of the association ratio DAR drops around 90 to 120 s, which adversely affects the calculation accuracy of NDT scan matching. As this occurs, according to the speeds shown in FIGS. 17A to 17C, disturbances that are thought to be offsets occur, particularly around 90 to 120 s. Furthermore, as shown in FIGS. 17D to 17G, the errors in the estimated self-position and estimated self-orientation are larger in the areas where the speed disturbances occur than in other areas. In particular, the error in the direction of travel in FIG. 17D increases to approximately -20 m, although this is not shown for convenience.

[0124] 18A to 18G and 19A to 19H are diagrams illustrating the results of self-localization estimation according to an embodiment in which velocity is calculated with a filter. Here, FIG. 18A illustrates the number of measurement points in the point cloud data before downsampling, FIG. 18B illustrates the downsampling size, FIG. 18C illustrates the number of measurement points and the number of corresponding measurement points after downsampling, FIG. 18D illustrates the association ratio DAR, FIG. 18E illustrates the score value E, FIG. 18F illustrates the reliability value NRV, and FIG. 18G illustrates the reliability index NRI. Also, FIG. 19A illustrates the time constant τ, FIG. 19B illustrates the velocity in the x direction, FIG. 19C illustrates the velocity in the y direction, and FIG. 19D illustrates the velocity in the z direction. Furthermore, for the RTK-GPS positioning results, FIG. 19E illustrates the error in the traveling direction, FIG. 19F illustrates the error in the lateral direction, FIG. 19G illustrates the error in the vertical direction, and FIG. 19H illustrates the error in the yaw angle.

[0125] In the experimental results of this example, the velocities in FIGS. 19B to 19D are generally smoother than those in FIGS. 17A to 17C, and the velocity disturbances around 100 to 120 s are reduced. As a result, in the experimental results of this example, as shown in FIGS. 19E to 19H, the errors in the estimated self-position and estimated self-orientation are reduced compared to the experimental results of the comparative example. In particular, with regard to the error in the traveling direction in FIG. 19E, large errors such as those shown in FIG. 17D are suppressed, and the error amplitude up to 100 s is also smaller on average. Furthermore, as shown in FIG. 18G, the period during which the value of the reliability index NRI is reduced is shorter than in the experimental results of the comparative example (FIG. 16G), and it can be determined that stability is improved compared to the state without a filter.

[0126] As described above, according to this embodiment, the speed of the ship is calculated using a filter (1 / (τs+1)) whose time constant τ varies depending on the reliability index NRI (reliability value NRV), and the self-location estimation process can be performed using the calculated speed of the ship. Therefore, according to this embodiment, the accuracy of self-location estimation on the ship can be improved.

[0127] [Variations] A preferred modification of the above embodiment will now be described.

[0128] (Variation 1) The self-position estimation unit 15 may set the time constant τ using the association ratio DAR instead of the reliability index NRI.

[0129] For example, when the value of the association ratio DAR is small, it is considered that the estimation result of the self-location estimation is unreliable and has many errors. Therefore, when the value of the association ratio DAR is small, the self-location estimation unit 15 according to this modification sets the value of the time constant τ to a large value.

[0130] Furthermore, for example, when the value of the association ratio DAR is large, it is considered that the estimation result of the self-location estimation is highly reliable and has few errors. Therefore, when the value of the association ratio DAR is large, the self-location estimation unit 15 according to this modification may set the value of the time constant τ to a small value.

[0131] That is, the self-position estimation unit 15 according to this modification 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 equation (24).

[0132] (Variation 2) The self-position estimation unit 15 calculates the angular velocity (ω x (k), ω y (k) and ω z Instead of using the equation (k), for example, by performing calculations using the following equations (25) to (27), the angular velocity "φ" at the processing number k in the world coordinate system can be calculated. · (k)," "θ · (k)" and "ψ · (k)" may be calculated. In the following formulas (25) to (27), "τ" represents a time constant, and "s" represents a Laplace operator.

[0133]

number

number

number

[0134] That is, according to this modification, the self-position estimation process can be performed even on a vessel that does not have an IMU 6 mounted thereon, by calculation using the formulas (25) to (27).

[0135] <Second Example> Next, a second embodiment will be described. Note that in this embodiment, explanations of parts to which the same configuration as in the first embodiment can be applied will be omitted as appropriate, and the explanation will focus on parts that are different from the first embodiment.

[0136] [Overview of driving assistance system] 20 is a diagram showing a schematic configuration of a driving assistance system according to Example 2. The driving assistance system according to this example has an information processing device 1A provided on a target vessel and a sensor group 2A mounted on the target vessel.

[0137] The sensor group 2A includes various external and internal sensors provided on the target vessel. In this embodiment, the sensor group 2A includes a lidar 3, a GPS receiver 5, an IMU 6 that measures the angular velocity of the target vessel in three axial directions, and an acceleration sensor 7 that measures the acceleration of the target vessel in three axial directions. Note that, according to this embodiment, if the IMU 6 can measure acceleration, the acceleration sensor 7 does not need to be included in the sensor group 2A.

[0138] [Configuration of information processing device] Fig. 21 is a block diagram showing an example of a hardware configuration of an information processing device according to Example 2. 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.

[0139] [Function Block] Fig. 22 is a diagram showing an example of functional blocks of a self-position estimation unit according to Example 2. As shown in Fig. 22, the self-position estimation unit 15A has 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.

[0140] The time constant calculation block 25A uses the acceleration in the three axial directions measured by the acceleration sensor 7 to calculate a time constant τ x , τ y and τ zCalculate.

[0141] The filter block 27A uses a time constant "τ" instead of the time constant "τ" in equation (16). x ” and replace the time constant “τ” in Equation (17) with the time constant “τ y ” and replace the time constant “τ” in Equation (18) with the time constant “τ z By applying this calculation, the velocity "x" at processing number k in the world coordinate system is calculated. · (k)," "y · (k)" and "z · (k) is calculated.

[0142] [Time constant τ x , τ y and τ z Calculation of Here, the time constant τ x , τ y and τ z In the following, an example will be described in which there is no (or almost no) displacement in the roll direction and pitch direction, and the acceleration changes on a two-dimensional plane.

[0143] The self-position estimation unit 15A estimates the acceleration in the three-axis direction measured by the acceleration sensor 7, xb "," a yb " and "a zb " to the following equations (28) to (30), the acceleration in the three axes directions in the world coordinate system, "a x "," a y " and "a z ” is calculated. xb " corresponds to the direction of travel of the target vessel x b The acceleration in the direction of yb " is the y axis corresponding to the horizontal direction (left and right direction) of the target vessel. b The acceleration in the direction of zb " corresponds to the vertical direction of the target ship z bis the acceleration in the axial direction. The calculation of the following formulas (28) to (30) can be outlined, for example, as shown in FIG. 23. FIG. 23 is a diagram showing the outline of the calculation performed when converting the acceleration measured by the acceleration sensor into the acceleration in the world coordinate system.

[0144]

number

number

number

[0145] The self-position estimation unit 15A calculates the accelerations in the three-axis directions in the world coordinate system "a x "," a y " and "a z By applying the time constant "τ x ", "τ y " and "τ z " is calculated.

[0146]

number

number

number

[0147] According to the above formula (31), for example, the acceleration "a x When the absolute value of ” 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, the acceleration "a x When the absolute value of " is large and the velocity change in the x direction is large, the time constant τ x is calculated as a small value.

[0148] According to the above formula (32), for example, the acceleration "a y When the absolute value of " is small and the change in velocity 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, the acceleration "a y When the absolute value of " is large and the velocity change in the y direction is large, the time constant τ y is calculated as a small value.

[0149] According to the above formula (33), for example, the acceleration "a z When the absolute value of ” is small and the velocity change in the z direction is small, the time constant τ z is calculated as a large value. Also, according to the above formula (33), for example, the acceleration "a z When the absolute value of " is large and the velocity change in the z direction is large, the time constant τ z is calculated as a small value.

[0150] As described above, according to this embodiment, the acceleration a x Depending on the time constant τ x The filter (1 / (τ x s+1)) and acceleration a y Depending on the time constant τ y The filter (1 / (τ y s+1)) and acceleration a z Depending on the time constant τ z The filter (1 / (τ z s+1)) and the speed of the ship is calculated using the calculated speed of the ship, and the self-location estimation process can be performed using the calculated speed of the ship. Therefore, according to this embodiment, the accuracy of the self-location estimation of the ship can be improved.

[0151] (Variation) A preferred modification of the above embodiment will now be described.

[0152] 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. Note that the time constant τ in this modification is calculated based on the speed x· (k), y · (k) and z · When calculating (k), the time constant τ x , τ y and τ z can be used instead of

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

[0154] Then, the self-position estimation unit 15A calculates the time constant τ by substituting the value of the amount of change SV, PV or IV acquired as the vessel state information into "n" in the following equation (34).

[0155]

number

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

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

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

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

[0160] <Third Example> Next, a third embodiment will be described. In this embodiment, the description of parts to which the same configurations as those in the first or second embodiment can be applied will be omitted as appropriate, and the description will focus on parts that are different from both the first and second embodiments.

[0161] [Overview of driving assistance system] The operation assistance system according to this embodiment includes an information processing device 1B provided on a target vessel, and a sensor group 2A mounted on the target vessel.

[0162] [Configuration of information processing device] Fig. 24 is a block diagram showing an example of a hardware configuration of an information processing device according to Example 3. As shown in Fig. 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.

[0163] [Function Block] 25 is a diagram showing an example of functional blocks of a self-position estimation unit according to Example 3. As shown in FIG. 25, the self-position estimation unit 15B has 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.

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

[0165] [Calculation of time constant τ] Here, a method for calculating the time constant τ by the self-position estimation unit 15B will be described.

[0166] The self-position estimation unit 15B applies the reliability index NRI to the equation (24) to obtain the same time constant τ t Furthermore, the self-position estimation unit 15B calculates the acceleration (a xb , a yb and a zb ) into equations (28) to (30), the acceleration in the three axes directions in the world coordinate system (a x , a y and a z ) in the world coordinate system. x , a y and a z ) into the equations (31) to (33), the same time constant τ as in the second embodiment can be obtained. x , τ y and τ z Then, the self-position estimation unit 15B calculates the time constant τ t and τ x corresponds to the average value of (τ t +τ x ) / 2 and the time constant τ t and τ y corresponds to the average value of (τ t +τ y ) / 2 and the time constant τ t and τ z corresponds to the average value of (τ t +τ z ) / 2 is obtained as the calculation result of the time constant τ.

[0167] As described above, according to this embodiment, the reliability index NRI and the acceleration a x , a y and a zThe vessel velocity is calculated using a filter (1 / (τs+1)) whose time constant τ changes depending on the vessel velocity, and the vessel velocity is then used to perform self-location estimation processing. Therefore, according to this embodiment, the accuracy of vessel position estimation can be improved.

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

[0169] Although the present invention has been described above with reference to the embodiments, the present invention is not limited to the above embodiments. Various modifications within the scope of the present invention that would be understood by those skilled in the art can be made to the configuration and details of the present invention. In other words, the present invention naturally includes various modifications and alterations that would be possible for those skilled in the art based on the entire disclosure, including the claims, and the technical ideas. Furthermore, the disclosures of the above-cited patent documents and other documents are incorporated herein by reference. [Explanation of symbols]

[0170] 1, 1A, 1B Information processing equipment 2, 2A sensor group 3 Rider 5 GPS receiver 6 IMU 7 Acceleration Sensor 10 Map DB

Claims

[Claim 1] a predicted position acquisition means for acquiring a predicted position of the ship; an estimated position calculation means for calculating an estimated position of the ship by updating the predicted position based on comparison of data based on outputs of external sensors provided on the ship with map data; a reliability calculation means for calculating the reliability of the matching; a speed calculation means for calculating a speed of the ship at the first processing time based on 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 based on at least the reliability; An information processing device having the above.

Images