JP7613900B2 - Information processing device, control method, program, and storage medium - Google Patents

Description

This disclosure relates to position estimation.

Conventionally, there is known a technique for estimating a vehicle's own position by comparing (matching) shape data of surrounding objects measured using a measuring 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 map information with measurement data for voxels in which stationary objects exist. In addition, Patent Document 2 discloses a scan matching method that estimates a vehicle's own position by comparing voxel data, including the mean vector and covariance matrix of stationary objects for each voxel, with point cloud data output by a lidar.

International Publication WO2013/076829 International Publication WO2018/221453

Recently, automatic ship-steering systems have been studied not only in the automotive field but also in ships, and accurate self-location estimation is equally important for safe automatic ship-steering. In the open sea, there are few structures in the vicinity, so self-location can be determined using the Global Navigation Satellite System (GNSS). However, in urban coasts and rivers, where there are high-rise buildings nearby, the reception environment for GNSS radio waves is poor due to a reduced number of receiving satellites and multipath, and accurate positioning is often not possible. Therefore, in the case of ships, the above-mentioned high-precision position estimation using scan matching is preferably applied.

On the other hand, when the self-location estimation process begins, both automobiles and ships use the position determined by GNSS as their initial position. GNSS positioning involves errors, but if the errors are not large, it can be used as the initial position for the above-mentioned scan matching, resulting in highly accurate position estimation. However, if the GNSS reception environment is poor, a location far away may be determined as the initial position, and it may not be possible to properly obtain the optimal solution in scan matching. Therefore, in order to properly estimate the position in such a case, it is necessary to search for a likely initial position. Also, even if the estimated self-location deviates significantly for some reason, it is necessary to similarly set a likely initial position.

This disclosure has been made to solve the above problems, and its main objective is to provide an information processing device that can perform self-location estimation in an optimal manner.

The claimed invention is
a position acquisition means for acquiring a position of a moving object based on a first position calculation method;
a reference position searching means for setting a search range for searching for a reference position of the second position calculation method based on the position measurement position, and performing a first comparison within the search range in which voxel data representing the position of an object for each voxel, which is a unit area, is compared with point cloud data based on an output of a measuring device provided on the moving body based on the reference position, and searching for the reference position where a first evaluation index of the first comparison is equal to or greater than a first predetermined value ;
a position estimating means for estimating a position of the moving object by the second position calculation method based on a second comparison between the voxel data and the point cloud data ,
The reference position search means determines a size of the search range based on accuracy information representing accuracy of positioning of the positioning position;
If an estimated position of the moving object determined at the immediately preceding processing time exists, and a second evaluation index of the position estimation by the second position calculation method calculated based on a result of the second matching at the immediately preceding processing time is less than a second predetermined value,
The reference position search means searches for the reference position based on the point cloud data at a current processing time,
The position estimation means converts a coordinate system of the point cloud data at a current processing time into a coordinate system of the voxel data based on the reference position;
the position estimation means, when an estimated position of the moving body determined at the immediately preceding processing time exists and the second evaluation index is equal to or greater than the second predetermined value, converts the coordinate system of the point cloud data at the current processing time into the coordinate system of the voxel data based on the estimated position of the moving body determined at the immediately preceding processing time;
It is an information processing device.

The claimed invention also includes:
A computer-implemented control method, comprising:
a position acquisition step of acquiring a position measured by the moving object using a first position calculation method;
a reference position searching step of setting a search range for searching for a reference position of the second position calculation method based on the position measurement position, and performing a first comparison within the search range in which voxel data representing the position of an object for each voxel, which is a unit area, is compared with point cloud data based on an output of a measuring device provided on the moving body based on the reference position, and searching for the reference position where a first evaluation index of the first comparison is equal to or greater than a first predetermined value ;
and a position estimating step of estimating a position of the moving object by the second position calculation method based on a second comparison between the voxel data and the point cloud data ,
The reference position search step includes determining a size of the search range based on accuracy information representing accuracy of positioning of the positioning position;
If an estimated position of the moving object determined at the immediately preceding processing time exists, and a second evaluation index of the position estimation by the second position calculation method calculated based on a result of the second matching at the immediately preceding processing time is less than a second predetermined value,
The reference position search step searches for the reference position based on the point cloud data at a current processing time;
The position estimation step converts a coordinate system of the point cloud data at a current processing time into a coordinate system of the voxel data based on the reference position;
The position estimation process is a control method for converting the coordinate system of the point cloud data at the current processing time into the coordinate system of the voxel data based on the estimated position of the moving body determined at the immediately previous processing time when an estimated position of the moving body determined at the immediately previous processing time exists and the second evaluation index is equal to or greater than the second specified value .

The claimed invention also includes:
a position acquisition means for acquiring a position of a moving object based on a first position calculation method;
a reference position searching means for setting a search range for searching for a reference position of the second position calculation method based on the position measurement position, and performing a first comparison within the search range in which voxel data representing the position of an object for each voxel, which is a unit area, is compared with point cloud data based on an output of a measuring device provided on the moving body based on the reference position, and searching for the reference position where a first evaluation index of the first comparison is equal to or greater than a first predetermined value ;
causing a computer to function as a position estimation unit that estimates a position of the moving object by the second position calculation method based on a second comparison between the voxel data and the point cloud data ;
The reference position search means determines a size of the search range based on accuracy information representing accuracy of positioning of the positioning position;
If an estimated position of the moving object determined at the immediately preceding processing time exists, and a second evaluation index of the position estimation by the second position calculation method calculated based on a result of the second matching at the immediately preceding processing time is less than a second predetermined value,
The reference position search means searches for the reference position based on the point cloud data at a current processing time,
The position estimation means converts a coordinate system of the point cloud data at a current processing time into a coordinate system of the voxel data based on the reference position;
The position estimation means is a program that, when an estimated position of the moving body determined at the immediately previous processing time exists and the second evaluation index is equal to or greater than the second specified value, converts the coordinate system of the point cloud data at the current processing time into the coordinate system of the voxel data based on the estimated position of the moving body determined at the immediately previous processing time .

FIG. 1 is a schematic configuration diagram of a driving assistance system. FIG. 2 is a block diagram showing a functional configuration of an in-vehicle device. 2 is a diagram showing a self-position to be estimated by a self-position estimation unit in two-dimensional orthogonal coordinates. FIG. 2 shows an example of a schematic data structure of voxel data. 4 is an example of a functional block of a self-position estimation unit. This shows the positional relationship between voxels in which voxel data exists and measurement points indicating positions near these voxels on a two-dimensional plane in the world coordinate system. 1 shows an overhead view of a vehicle near a road with an intersection. 1 shows an overhead view of a road showing GNSS positioning positions and initial location search ranges. 1A is a diagram clearly showing voxels within an initial position search range, and FIG. 1B is a diagram showing a frequency distribution of z-coordinate values indicated by voxel data within the initial position search range. 1 shows an overhead view clearly indicating candidate voxels and the order of the candidate voxels. 13 is a graph showing the transition of the association ratio when a simple NDT process is performed. 1 shows an overhead view clearly indicating virtual voxels and their order. 13 is a graph showing the transition of the association ratio when a simple NDT process is performed. 1 is an example of a flowchart illustrating an overview of a self-position estimation process according to an embodiment. 11 is a flowchart illustrating an example of a procedure of an initial position setting process according to an embodiment when a target moving object is a vehicle. 13 is a flowchart illustrating an example of a procedure of an initial position setting process according to an embodiment when a target moving body is a ship. 13 is an example of a flowchart illustrating an overview of a self-position estimation process according to a modified example. 13 is an example of a flowchart illustrating a procedure of an initial position setting process according to a modified example in which the target moving body is a vehicle. 13 is an example of a flowchart illustrating a procedure of an initial position setting process according to a modified example in which the target moving body is a ship.

According to a preferred embodiment of the present invention, an information processing device has a position acquisition means for acquiring a positioning position of a moving body by a first position calculation method, a reference position setting means for setting a search range based on the positioning position and setting a reference position for a second position calculation method within the search range, and a position estimation means for estimating the position of the moving body by the second position calculation method based on the reference position, and the reference position setting means determines the size of the search range based on accuracy information representing the accuracy of the positioning of the positioning position. According to this aspect, the information processing device can appropriately determine the search range of the reference position used in the second position calculation method depending on the accuracy of the first position calculation method.

In one aspect of the information processing device, the first position calculation method is positioning using GNSS, and the reference position setting means determines the size of the search range based on accuracy information of the GNSS. With this aspect, the information processing device can set the first position calculation method to GNSS, and appropriately determine the range for searching for the reference position used in the second position calculation method according to the accuracy information of the GNSS.

In another aspect of the information processing device, the reference position setting means reduces the size of the search range as the accuracy indicated by the accuracy information increases. With this aspect, the information processing device can appropriately determine the size of the search range for the reference position used in the second position calculation method.

In another aspect of the information processing device, the position estimation means performs the position estimation as the second position calculation method based on matching voxel data representing the position of an object for each voxel, which is a unit area, with point cloud data based on the output of a measuring device provided on the moving body. With this aspect, the information processing device can perform position estimation of the moving body with high accuracy.

In another aspect of the information processing device, when there is no estimated position of the moving body determined at the immediately preceding processing time, the position estimation means converts the coordinate system of the point cloud data into the coordinate system of the voxel data based on the reference position. With this aspect, the information processing device can preferably perform the coordinate conversion of the point cloud data required for the execution of the second position calculation means using the reference position set by the reference position setting means.

In another aspect of the information processing device, the moving body is a vehicle, and the reference position setting means determines a candidate voxel in which the vehicle exists based on a frequency distribution of the height direction positions of each voxel present within the search range. With this aspect, the information processing device can suitably determine a candidate voxel in which a vehicle exists, taking into account the general tendency that the road surfaces of each road tend to be roughly the same in height.

In another aspect of the information processing device, the moving body is a ship, and the reference position setting means determines a virtual voxel for which no voxel data exists within the search range as a candidate voxel for which the ship exists. In general, considering that no voxel data exists on water, the candidate voxel for which the ship exists can be suitably determined.

In another aspect of the information processing device, the reference position setting means performs position search and evaluation index calculation using the simplified second position calculation method with each of the candidate voxels as a search start position, and determines the result of the simplified second position calculation method in which the evaluation index is greater than a predetermined value as the reference position. With this aspect, the information processing device can determine the reference position with high accuracy while taking into account the processing load.

In another aspect of the information processing device, when an estimated position of the moving body determined at the immediately preceding processing time exists and the evaluation index of the second position calculation method at the immediately preceding processing time is less than a predetermined value, the reference position setting means determines the reference position, and the position estimation means converts the coordinate system of the point cloud data at the current processing time into the coordinate system of the voxel data based on the reference position. With this aspect, when the accuracy of the second position calculation method deteriorates, the information processing device can preferably recover from a state in which a matching deviation or the like occurs due to falling into a local solution in the second position calculation method by performing coordinate conversion of the point cloud data based on the reference position set by the reference position setting means.

In another aspect of the information processing device, the position estimation means converts the coordinate system of the point cloud data at the current processing time into the coordinate system of the voxel data based on the estimated position when an estimated position of the moving body determined at the immediately preceding processing time exists and the evaluation index is equal to or greater than the predetermined value. In this way, when the accuracy of the second position calculation method is maintained, the information processing device performs coordinate conversion of the point cloud data based on the position calculation result of the second position calculation method.

According to another preferred embodiment of the present invention, a control method executed by a computer includes a position acquisition step of acquiring a positioning position of a moving body by a first position calculation method, a reference position setting step of setting a search range based on the positioning position and setting a reference position for a second position calculation method within the search range, and a position estimation step of estimating the position of the moving body by the second position calculation method based on the reference position, and the reference position setting step determines the size of the search range based on accuracy information representing the accuracy of the positioning of the positioning position. By executing this control method, the computer can appropriately determine the search range of the reference position used in the second position calculation method according to the accuracy of the first position calculation method.

According to yet another preferred embodiment of the present invention, the program causes a computer to function as a position acquisition means for acquiring a positioning position of a moving body by a first position calculation method, a reference position setting means for setting a search range based on the positioning position and setting a reference position for a second position calculation method within the search range, and a position estimation means for estimating the position of the moving body by the second position calculation method based on the reference position, and the reference position setting means determines the size of the search range based on accuracy information representing the accuracy of the positioning of the positioning position. By executing the program, the computer can appropriately determine the search range of the reference position used in the second position calculation method according to the accuracy of the first position calculation method. Preferably, the program is stored in a storage medium.

Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings. For the sake of convenience, any symbol followed by "^" or "-" will be expressed as "A^" or " ^A- " (where "A" is any character) in this specification.

(1) Overview of the driving assistance system
1 is a schematic diagram of a driving assistance system according to the present embodiment. The driving assistance system includes an information processing device 1 that moves together with a moving body, such as a vehicle or a ship, and a sensor group 2 mounted on the vehicle or the ship. Hereinafter, the vehicle or the ship that moves together with the information processing device 1 is also referred to as a "target moving body."

The information processing device 1 is electrically connected to the sensor group 2, and estimates the position (also called "self-position") of the target moving object in which the information processing device 1 is installed based on the output of various sensors included in the sensor group 2. Then, based on the result of estimating the self-position, the information processing device 1 performs driving assistance such as automatic driving control of the target moving object so that the target moving object travels along a route to a set destination. The information processing device 1 may be a navigation device provided in the target moving object, or may be an electronic control device built into a vehicle or ship.

The information processing device 1 also stores a map database (DB: DataBase) 10 including voxel data "VD". The voxel data VD is data that records the position information of a stationary structure for each voxel that indicates a cube (regular lattice) that is the smallest unit of three-dimensional space. The voxel data VD includes data that represents the measured point cloud data of the stationary structure in 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 at least the position and yaw angle of the target moving object on a plane by NDT scan matching. The information processing device 1 may further estimate the height position, pitch angle, and roll angle of the target moving object. Unless otherwise specified, the self-position is also assumed to include the attitude angle such as the yaw angle of the target moving object.

The sensor group 2 includes various external and internal sensors provided on the target moving body. In this embodiment, the sensor group 2 includes a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 3, a speed sensor 4 that detects the speed of the target moving body, a GPS (Global Positioning Satellite) receiver 5, and a gyro sensor 6 that detects the angular velocity of the target moving body.

The lidar 3 emits a pulsed laser in 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 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 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. In general, the closer the distance to the object, the higher the accuracy of the distance measurement value of the lidar, and the farther the distance, the lower the accuracy. The lidar 3 is an example of a "measurement device" in this invention.

In addition, instead of or in addition to the gyro sensor 6, the sensor group 2 may have an inertial measurement unit (IMU) that measures the acceleration and angular velocity of the target moving object in three axial directions. Also, instead of the GPS receiver 5, the sensor group 2 may have a receiver that generates positioning results of a GNSS other than GPS.

(2) Configuration of the information processing device
2 is a block diagram showing an example of a hardware configuration of the information processing device 1. The information processing device 1 mainly includes an interface 11, a memory 12, and a controller 13. These elements are connected to each other via a bus line.

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 sensors such as the lidar 3, the gyro sensor 6, the speed sensor 4, and the GPS receiver 5, and supplies the data to the controller 13. The interface 11 also supplies, for example, a signal related to the control of the target moving object generated by the controller 13 to the electronic control unit (ECU) of the target moving object. The interface 11 may be a wireless interface such as a network adapter for wireless communication, or may be a hardware interface for connecting to an external device via a cable or the like. The interface 11 may perform interface operations with various peripheral devices such as an input device, a display device, and a sound output device.

The memory 12 is composed of 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 a program for the controller 13 to execute a predetermined process. The program executed by the controller 13 may be stored in a storage medium other than the memory 12.

The memory 12 also stores a map DB 10 that includes voxel data VD.

The map DB 10 may be stored in a storage device external to the information processing device 1, such as a hard disk connected to the information processing device 1 via the interface 11. The storage device may be a server device that communicates with the information processing device 1. The storage device may also be composed of multiple devices. The map DB 10 may also be updated periodically. In this case, for example, the controller 13 receives partial map information related to the area to which the controller 13's own position belongs from the server device that manages the map information via the interface 11, and reflects the partial map information in the map DB 10.

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

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-location estimation by executing a program stored in the memory 12, etc.

Functionally, the controller 13 also has a self-position estimation unit 15. The controller 13 functions as a "position acquisition means," a "reference position setting means," a "position estimation means," a computer that executes a program, etc.

The self-position estimation unit 15 estimates its own position by performing NDT-based scan matching (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 after the point cloud data is downsampled.

(3) Position estimation based on NDT scan matching
Next, a description will be given of position estimation based on NDT scan matching executed by the self-position estimation unit 15. Position estimation based on GNSS is an example of a "first position calculation method" in the present invention, and position estimation based on NDT scan matching is an example of a "second position calculation method" in the present invention.

3 is a diagram showing the self-position to be estimated by the self-position estimation unit 15 in two-dimensional orthogonal coordinates. As shown in FIG. 3, the self-position on a plane defined on the two-dimensional orthogonal coordinates of xy is represented by the coordinates "(x, y)" and the orientation (yaw angle) "ψ" of the target moving body. Here, the yaw angle ψ is defined as the angle between the traveling direction of the target moving body and the x-axis. The coordinates (x, y) are, for example, absolute positions corresponding to a combination of latitude and longitude, or world coordinates indicating a position with a predetermined point as the origin. The self-position estimation unit 15 performs self-position estimation using these x, y, and ψ as estimation parameters. Note that the self-position estimation unit 15 may perform self-position estimation in which at least one of the height position, pitch angle, and roll angle of the target moving body in the three-dimensional orthogonal coordinate system is further estimated as an estimation parameter in addition to x, y, and ψ.

Next, we will explain the voxel data VD used in NDT scan matching. The voxel data VD includes data that represents the measured point cloud data of stationary structures in each voxel using a normal distribution.

Figure 4 shows an example of a schematic data structure of the voxel data VD. The voxel data VD includes parameter information when expressing the point cloud within a voxel with a normal distribution, and in this embodiment, as shown in Figure 4, includes a voxel ID, voxel coordinates, a mean vector, and a covariance matrix.

"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 determined in advance, it is possible to identify the space of each voxel by its voxel coordinates. The voxel coordinates may also be used as a voxel ID.

The "mean vector" and "covariance matrix" refer to the mean vector and covariance matrix that correspond to the parameters when expressing the point group 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 "N _n ", the mean vector "μ _n " and the covariance matrix "V _n " in voxel n are expressed by the following formulas (1) and (2), respectively.

Next, we will provide an overview of NDT scan matching using voxel data VD.

Scan matching by NDT assuming a vehicle or ship is performed by estimating parameters based on the amount of movement in the horizontal plane (here, xy coordinates) and the orientation of the vehicle or ship.
P=[t _x , t _y , t _ψ ] ^T
Here, "t _x " indicates the amount of movement in the x direction, "t _y " indicates the amount of movement in the y direction, and "t _ψ " indicates the yaw angle.

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, the average value "L' _n " of X _L (j) is expressed by the following formula (3).

そして、上述の推定パラメータＰを用い、平均値Ｌ´を座標変換すると、変換後の座標「Ｌ_ｎ」は、以下の式（４）により表される。

Then, when the average value L' is coordinate-transformed using the above-mentioned estimated parameter P, the transformed coordinate "L _n " is expressed by the following equation (4).

そして、自己位置推定部１５は、地図ＤＢ１０と同一の座標系である絶対的な座標系（「ワールド座標系」とも呼ぶ。）に変換した点群データに対応付けられるボクセルデータＶＤを探索し、そのボクセルデータＶＤに含まれる平均ベクトルμ_ｎと共分散行列Ｖ_ｎとを用い、ボクセルｎの評価関数値（「個別評価関数値」とも呼ぶ。）「Ｅ_ｎ」を算出する。この場合、自己位置推定部１５は、以下の式（５）に基づき、ボクセルｎの個別評価関数値Ｅ_ｎを算出する。

Then, the self-position estimation unit 15 searches for voxel data VD associated with the point cloud data converted into an absolute coordinate system (also called "world coordinate system") that is the same coordinate system as the map DB 10, and calculates an evaluation function value (also called "individual evaluation function value") "E _n " of voxel n using the mean vector μ _n and covariance matrix V _n included in the voxel data VD. In this case, the self-position estimation unit 15 calculates the individual evaluation function value E _n of voxel n based on the following formula (5).

Then, the self-position estimation unit 15 calculates an overall evaluation function value (also called a "score value") "E(k)" for all voxels that are the subject of matching, as shown in the following formula (6). The score value E is an index that indicates the degree of suitability of the matching.

その後、自己位置推定部１５は、ニュートン法などの任意の求根アルゴリズムによりスコア値Ｅ（ｋ）が最大となるとなる推定パラメータＰを算出する。そして、自己位置推定部１５は、時刻ｋにおいてデッドレコニングにより算出した位置（「ＤＲ位置」とも呼ぶ。）「Ｘ_ＤＲ（ｋ）」に対し、推定パラメータＰを適用することで、ＮＤＴスキャンマッチングに基づく自己位置（「ＮＤＴ位置」とも呼ぶ。）「Ｘ_ＮＤＴ（ｋ）」を算出する。ここで、ＤＲ位置Ｘ_ＤＲ（ｋ）は、推定自己位置Ｘ＾（ｋ）の算出前の暫定的な自己位置に相当し、予測自己位置「Ｘ^－（ｋ）」とも表記する。この場合、ＮＤＴ位置Ｘ_ＮＤＴ（ｋ）は、以下の式（７）により表される。

Thereafter, the self-position estimation unit 15 calculates an estimation parameter P that maximizes the score value E(k) by an arbitrary root-finding algorithm such as Newton's method. The self-position estimation unit 15 then calculates a self-position (also called "NDT position") "X _NDT (k)" based on NDT scan matching by applying the estimation parameter P to the position (also called "DR position") "X _DR (k)" calculated by dead reckoning at time k. Here, the DR position X _DR (k) corresponds to a provisional self-position before the calculation of the estimated self-position X^(k), and is also expressed as a predicted self-position "X ^- (k)". In this case, the NDT position X _NDT (k) is expressed by the following formula (7).

そして、自己位置推定部１５は、ＮＤＴ位置Ｘ_ＮＤＴ（ｋ）を、現在の処理時刻ｋでの最終的な自己位置の推定結果（「推定自己位置」とも呼ぶ。）「Ｘ＾（ｋ）」とみなす。

Then, the self-position estimation unit 15 regards the NDT position X _NDT (k) as the final self-position estimation result (also called "estimated self-position") "X^(k)" at the current processing time k.

Figure 5 is an example of a functional block of the self-position estimation unit 15. As shown in Figure 5, the self-position estimation unit 15 has a dead reckoning block 21, a DR position calculation block 22, a coordinate transformation block 23, a point cloud data matching block 24, and an NDT position calculation block 25.

The dead reckoning block 21 uses the moving speed and angular velocity of the target moving object based on the outputs of the speed sensor 4, GPS receiver 5, gyro sensor 6, etc. to determine the moving distance and azimuth change from the previous time. The DR position calculation block 22 calculates the DR position X _DR (k) at time k by adding the moving distance and azimuth change determined by dead reckoning to the estimated self-position X ^{^} (k-1) at time k-1, which is the immediately previous processing time. This DR position X _DR (k) is the self-position determined at time k based on dead reckoning, and corresponds to the predicted self-position X ^- (k).

Also, immediately after the start of self-location estimation, in cases where there is no estimated self-location X ^{^} (k-1) at time k-1, the dead reckoning block 21 sets an initial position (also called "initial position") equivalent to the estimated self-location X ^{^} (k-1) at time k-1 by a method to be described later. Then, the dead reckoning block 21 calculates a DR position _XDR by adding the movement distance and azimuth change obtained by dead reckoning to the set initial position. The setting of the initial position is performed in the same manner not immediately after the start of the self-location estimation process, but also when the reliability of the NDT scan matching is reduced. The method of setting the initial position will be described in detail in the section "(4) Setting the initial position ". The initial position is an example of the "reference position" in the present invention.

The coordinate transformation block 23 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 23 performs coordinate transformation of the point cloud data at time k, for example, based on the predicted self-position output by the DR position calculation block 22 at time k. Note that the process of transforming point cloud data in a coordinate system based on a LIDAR installed on a moving body 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.

The point cloud data matching block 24 matches the point cloud data in the world coordinate system output by the coordinate transformation block 23 with the voxel data VD expressed in the same world coordinate system to match the point cloud data and the voxels. The NDT position calculation block 25 calculates an individual evaluation function value based on the formula (5) for each voxel that is matched with the point cloud data, and calculates an estimated parameter P that maximizes the score value E(k) based on the formula (6). Then, the NDT position calculation block 25 calculates an NDT position X NDT (k) at time k, which is determined by applying the estimated parameter P calculated at time k to the DR position X _DR (k) output by the DR position calculation block 22, based on the formula (7). The NDT position calculation block 25 outputs the _{NDT position X NDT (} k) as an estimated self-position X^(k) at time k.

Here, we will provide additional explanation using a simple example on the specific steps for associating measurement points with voxel data VD.

Figure 6 shows the positional relationship between voxels "Vo1" to "Vo6" in which voxel data VD exists on a two-dimensional x-y plane in the world coordinate system, and measurement points 61 to 65 that indicate positions near these voxels. For ease of explanation, it is assumed here that the z coordinate in the world coordinate system of the center positions of voxels Vo1 to Vo6 is the same as the z coordinate in the world coordinate system of measurement points 61 to 65.

First, the coordinate conversion block 23 converts the point cloud data including the measurement points 61 to 65 into the world coordinate system. After that, the point cloud data matching block 24 performs a rounding process on the fractions of the measurement points 61 to 65 in the world coordinate system. In the example of FIG. 6, since the size of each voxel, which is a cube, is 1 m, the point cloud data matching block 24 rounds off the decimal points of the x, y, and z coordinates of each of the measurement points 61 to 65.

Next, the point cloud data matching block 24 compares the voxel data VD corresponding to the voxels Vo1 to Vo6 with the coordinates of each of the measurement points 61 to 65 to determine the voxels corresponding to each of the measurement points 61 to 65. In the example of FIG. 6, the (x, y) coordinates of the measurement point 61 become (2, 1) due to the above-mentioned rounding, so the point cloud data matching block 24 matches the measurement point 61 with the voxel Vo1. Similarly, the (x, y) coordinates of the measurement points 62 and 63 become (3, 2) due to the above-mentioned rounding, so the point cloud data matching block 24 matches the measurement points 62 and 63 with the voxel Vo5. Also, the (x, y) coordinates of the measurement point 64 become (2, 3) due to the above-mentioned rounding, so the point cloud data matching block 24 matches the measurement point 64 with the voxel Vo6. On the other hand, since the (x, y) coordinates of the measurement point 65 become (4, 1) due to the above-mentioned rounding, the point cloud data matching block 24 determines that there is no voxel data VD corresponding to the measurement point 65. The NDT position calculation block 25 then estimates the estimated parameters P using the measurement point and voxel data VD that have been matched by the point cloud data matching block 24. Note that in this example, the number of measurement points after downsampling is five, but the number of corresponding measurement points Nc is four. The number of corresponding measurement points Nc is smaller when there are few structures in the surrounding space and it is also smaller when a phenomenon occurs in which the lidar's light beam is blocked by another moving object or the like near the target moving object (called occlusion).

(4) Setting the initial position
Next, the setting of the initial position by the self-position estimation unit 15 will be described.

(4-1) Overview In general, the self-position estimation unit 15 determines a search range for the initial position based on the GNSS positioning position and accuracy information, and determines the initial position based on the processing results of simple NDT scan matching for each candidate voxel within the search range.

Here, we will first explain the problems that arise when using the GNSS positioning position as the initial position, with reference to Figure 7.

Figures 7(A) and 7(B) show overhead views of a target moving object (here, a vehicle) near a road with an intersection. Here, Figure 7(A) shows the measurement points converted into the global coordinate system based on the GNSS positioning results and the ranging range of the LIDAR 3 (see circle 70) when the GNSS positioning accuracy is good. Also, Figure 7(B) shows the measurement points converted into the global coordinate system based on the GNSS positioning results and the ranging range of the LIDAR 3 (see circle 71) when the GNSS positioning accuracy is poor. Also, in Figure 7(B), the GNSS positioning results (position and orientation) are indicated by arrow 73.

If the GNSS positioning accuracy is good, self-location estimation by NDT scan matching can be performed without any problems by using the GNSS-based positioning as the initial position. In the example of Figure 7 (A), the measurement point after coordinate transformation roughly matches the surface position of the measured feature and is properly associated with the voxel data VD with which it should originally be associated, so NDT scan matching can be performed with high accuracy. On the other hand, if the GNSS positioning accuracy is poor, as shown in Figure 7 (B), there may be a large deviation between the point cloud data and the voxel data VD with which it should originally be associated, and NDT scan matching may not reach an optimal solution and may fall into a local solution.

Taking the above into consideration, the self-position estimation unit 15 according to this embodiment determines the search range for the initial position (also called the "initial position search range Ri") based on the GNSS positioning position and accuracy information, and searches for the initial position. Below, we will explain separately the case where the target moving body is a vehicle and the case where the target moving body is a ship.

(4-2) When the target moving body is a vehicle The setting of the initial position search range Ri when the target moving body is a vehicle will be specifically described. Fig. 8(A) shows an overhead view of a road clearly indicating the position measured by the GNSS, and Fig. 8(B) shows an overhead view of a road clearly indicating the initial position search range Ri set based on the position measured by the GNSS.

First, the self-position estimation unit 15 acquires the positioning position and accuracy information by the GNSS based on the output signal of the GPS receiver 5. Here, the accuracy information may be, for example, PDOP (Position Dilution Of Precision), which is the position accuracy degradation rate, or 2DRMS (Twice the Distance Root Mean Square), which is the standard deviation of the error. Then, as shown in FIG. 8 (B), the self-position estimation unit 15 sets a square initial position search range Ri centered on the GNSS positioning position and having a size according to the accuracy indicated by the accuracy information. In this case, the size of the initial position search range Ri is set to be smaller as the accuracy indicated by the accuracy information is higher. For example, a lookup table or a formula indicating the relationship between the accuracy indicated by the accuracy information and the size of the initial position search range Ri is stored in the memory 12, and the self-position estimation unit 15 determines the size of the initial position search range Ri from the accuracy information based on the information stored in the memory 12. Note that the shape of the initial position search range Ri is not limited to a square, and may be a circle.

Next, the self-position estimation unit 15 creates a frequency distribution of the z coordinate values (coordinate values in the height direction) indicated by the voxel data VD within the initial position search range Ri, and considers voxels corresponding to the voxel data VD having high-frequency z coordinate values as candidates for the road surface. Figure 9 (A) is a diagram clearly showing the voxels within the initial position search range Ri shown in Figure 8 (B), and Figure 9 (B) shows the frequency distribution of the z coordinate values indicated by the voxel data VD within the initial position search range Ri.

As shown in FIG. 9(A), the self-location estimation unit 15 first recognizes voxels within the set initial position search range Ri, and extracts voxel data VD corresponding to the voxels from the map DB 10. The self-location estimation unit 15 then generates a frequency distribution of z coordinate values as shown in FIG. 9(B). In the frequency distribution shown in FIG. 9(B), local peaks are formed in the ranges of 4 m to 6 m and 12 m to 14 m. Therefore, the self-location estimation unit 15 considers voxels that fall within the above-mentioned peak ranges in the frequency distribution to be voxels that include the road surface and are candidates for the location where the target moving body is located (also called "candidate voxels").

The self-position estimation unit 15, for example, sets a predetermined threshold value for the frequency (6 in the example of FIG. 9(B)) and regards one or more ranges that are equal to or greater than the threshold value as peak ranges in the frequency distribution. The above-mentioned threshold value may be a variable value that varies depending on the number of voxels in the initial position search range Ri in which the voxel data VD exists or the size of the initial position search range Ri, etc.

Next, the self-location estimation unit 15 arranges the candidate voxels in order of proximity to the GNSS positioning position indicated by the arrow 73 (specifically, serial numbers are assigned in order of proximity). FIG. 10(A) shows an overhead view showing the candidate voxels, and FIG. 10(B) shows an overhead view showing the order of proximity to the GNSS positioning position on the candidate voxels. As shown in FIG. 10(A), in this case, voxels including a road surface close to the GNSS positioning position and voxels at the same height as the road surface are extracted as candidate voxels. Then, as shown in FIG. 10(B), serial numbers are assigned to the candidate voxels in order of proximity to the GNSS positioning position indicated by the arrow 73. Note that although numbers are given up to 20 in FIG. 10(B), in reality, serial numbers are assigned to all candidate voxels.

Next, the self-position estimation unit 15 sequentially selects candidate voxels according to the assigned order, and executes simple NDT scan matching (simple NDT processing) using a position based on the position of the selected candidate voxel as the search start position. Then, when the reliability index of the executed simple NDT processing is equal to or greater than a predetermined threshold, the self-position estimation unit 15 sets the result of the simple NDT processing as the initial position.

The simplified NDT process corresponds to a process in which the number of iterations (number of calculations of score value E(k)) for finding an optimal solution is reduced compared to when the NDT position is calculated, to shorten the calculation time, and thus perform rough matching. Note that in the simplified NDT process, instead of or in addition to reducing the number of iterations compared to when the NDT position is calculated, the size of downsampling applied to the point cloud data output by the lidar 3 may be made larger than when the normal NDT position is calculated. This also reduces the number of measurement points that are the subject of NDT scan matching, and shortens the calculation time of the simplified NDT process.

In addition, in this embodiment, the ratio of the number of measurement points associated with the voxel data VD among the number of measurement points of the point cloud data used in NDT scan matching (also called the "data association ratio (DAR)") is used as an example of an index of the reliability of the simplified NDT processing. In general, the lower the association ratio (DAR), the lower the position estimation accuracy by NDT scan matching tends to be.

Figure 11 is a graph showing the transition of the matching ratio DAR when the simplified NDT process is performed according to the sequence shown in Figure 10 (B). Figure 11 clearly shows the threshold value (here, 0.7) set for the matching ratio DAR for determining the voxel that is the initial position.

As shown in FIG. 11, the self-position estimation unit 15 selects candidate voxels according to the assigned order, sets a search start position based on the position corresponding to the selected candidate voxel, performs simple NDT processing, calculates the association ratio DAR, and compares it with a threshold value. In this case, for example, the x and y coordinate values of the search start position may be set to the same as the x and y coordinate values of the target candidate voxel, and the z coordinate value of the search start position may be set to the z coordinate value of the target candidate voxel plus the vehicle height. The vehicle height is pre-stored in the memory 12, for example.

In the example of FIG. 11, the self-position estimation unit 15 determines that the association ratio DAR exceeds 0.7 when the search start position is set based on the position corresponding to the candidate voxel assigned number 17 and the simple NDT process is performed. Therefore, in this case, the self-position estimation unit 15 sets the result of the simple NDT process corresponding to the candidate voxel assigned number 17 as the initial position.

(4-3) When the target moving body is a ship Next, a case where the target moving body is a ship will be described. First, the self-position estimation unit 15 acquires the GNSS-based positioning position and accuracy information based on the output signal of the GPS receiver 5, and sets an initial position search range Ri that is a square (or may be a circle) centered on the GNSS-based positioning position and has a size according to the accuracy indicated by the accuracy information. The setting of the initial position search range Ri is the same as when the target moving body is a vehicle, and therefore a detailed description will be omitted here.

Next, the self-position estimation unit 15 determines that locations within the initial position search range Ri where no voxel data VD exists are likely to be on the water, and acquires the coordinates of the locations where no voxel data VD exists. Here, since the voxel coordinate unit is used, the self-position estimation unit 15 acquires virtual voxels (also called "virtual voxels") where no voxel data VD exists as candidate voxels that include the water surface and are candidates for the location where the target moving body exists. The self-position estimation unit 15 then arranges the candidate voxels, which are virtual voxels, in order of proximity to the GNSS positioning position.

Figure 12 (A) shows an overhead view showing candidate voxels within the initial position search range Ri. Also, Figure 12 (B) shows an overhead view showing the order of proximity of the candidate voxels to the GNSS positioning position indicated by the arrow 73.

As shown in FIG. 12(A), virtual voxels corresponding to above-water positions of a river 75 and ground positions where no voxel data VD exists are extracted as candidate voxels. Then, as shown in FIG. 12(B), serial numbers are assigned to the candidate voxels in order of proximity to the GNSS positioning position indicated by the arrow 73. Note that although numbers go up to 13 in FIG. 12(B), in reality, serial numbers are assigned to all candidate voxels.

Next, the self-position estimation unit 15 sequentially selects candidate voxels according to the assigned order, and executes the simplified NDT process using a position based on the position of the selected candidate voxel as the search start position. Then, when the association ratio DAR is equal to or greater than a predetermined threshold, the self-position estimation unit 15 sets the corresponding result of the simplified NDT process as the initial position.

Figure 13 is a graph showing the transition of the association ratio DAR when the simplified NDT process is performed according to the sequence shown in Figure 12 (B). Figure 13 clearly shows the threshold value (here, 0.7) set for the association ratio DAR for determining the voxel that is the initial position.

As shown in FIG. 13, the self-location estimation unit 15 selects candidate voxels, which are virtual voxels, according to the assigned order, sets a search start position based on the position corresponding to the selected candidate voxel, performs simple NDT processing, calculates the association ratio DAR, and compares it with a threshold. In this case, for example, the x and y coordinate values of the search start position may be the same as the x and y coordinate values of the target candidate voxel, and the z coordinate value of the search start position may be a value obtained by adding the height of the ship from the water surface to the predicted tide level. In this case, for example, the self-location estimation unit 15 receives information on the predicted tide level corresponding to the point where the target moving object is located from a server device that manages the current predicted tide levels at each point via the interface 11. In addition, the self-location estimation unit 15 may calculate the height of the ship from the water surface based on the output of the sensor group 2, etc., or it may be a fixed value pre-stored in the memory 12.

In the example of FIG. 13, the self-position estimation unit 15 determines that the association ratio DAR exceeds 0.7 when the search start position corresponding to the candidate voxel assigned number 6 is set and the simple NDT process is performed. Therefore, in this case, the self-position estimation unit 15 sets the result of the simple NDT process corresponding to the candidate voxel assigned number 6 as the initial position.

(5) Processing flow
Next, a process flow relating to self-location estimation executed by the self-location estimating unit 15 of the information processing device 1 will be described with reference to FIGS.

(5-1) Processing Overview Fig. 14 is an example of a flowchart showing an overview 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. 14 when it becomes necessary to perform self-location estimation, such as when the power is turned on.

First, the self-position estimation unit 15 executes an initial position setting process, which is a process for setting an initial position (step S11). Details of the initial position setting process will be described later with reference to Figures 15 and 16.

Next, the self-position estimation unit 15 performs dead reckoning based on the moving speed and angular velocity of the target moving object detected based on the speed sensor 4 and the gyro sensor 6, etc., and the previous estimated self-position, and calculates the DR position _XDR, which is the predicted self-position (step S12). Next, the self-position estimation unit 15 converts the point cloud data based on the output of the LIDAR 3 into point cloud data in the world coordinate system (step S13). In addition, the self-position estimation unit 15 may perform downsampling of the point cloud data output by the LIDAR 3 immediately before or after the coordinate conversion in step S13.

Then, the self-position estimation unit 15 performs an NDT matching process using the DR position _XDR calculated in step S12 as an initial value to calculate an NDT position _XNDT to be the estimated self-position X^ (step S14). In addition, the controller 13 calculates the association ratio DAR based on the result of association between the point cloud data converted into the world coordinate system and the voxels in which the voxel data VD exists.

Then, the self-position estimation unit 15 judges whether the association ratio DAR is less than a predetermined value (step S15). This predetermined value is, for example, stored in advance in the memory 12. Then, when the self-position estimation unit 15 judges that the association ratio DAR is less than the predetermined value (step S15; Yes), the self-position estimation unit 15 returns the process to step S11. In this case, the self-position estimation unit 15 judges that the reliability of the NDT scan matching (and the calculated NDT position X _NDT ) has decreased, and that position estimation should be performed from the setting of the initial position at the next processing time, and returns the process to step S11. On the other hand, when the self-position estimation unit 15 judges that the association ratio DAR is equal to or greater than the predetermined value (step S15; No), the self-position estimation unit 15 judges that the reliability of the NDT position X _NDT is maintained, and proceeds to the process to step S16.

Next, the self-position estimation unit 15 determines whether or not the self-position estimation process should be ended (step S16). If the controller 13 determines that the self-position estimation process should be ended (step S16; Yes), the controller 13 ends the processing of the flowchart. On the other hand, if the controller 13 determines that the self-position estimation process should be continued (step S16; No), the controller 13 returns the processing to step S12 and estimates the self-position at the next processing time using the estimated self-position X^ calculated in step S14.

(5-2) Initial Position Setting Process: Target Moving Object is a Vehicle FIG. 15 is an example of a flowchart showing the procedure of the initial position setting process executed in step S11 of FIG. 14 when the target moving object is a vehicle.

First, the self-position estimation unit 15 acquires the latitude and longitude position (i.e., x, y coordinate values) and accuracy information based on the output signal of the GPS receiver 5, etc. (step S21). Then, based on the accuracy information acquired in step S21, the self-position estimation unit 15 sets an initial position search range Ri centered on the GNSS positioning position (step S22).

Next, the self-location estimation unit 15 acquires voxel data VD present in the initial position search range Ri from the map DB 10 (step S23). Then, the self-location estimation unit 15 creates a frequency distribution of the z coordinate values (i.e., height) indicated by each voxel data VD acquired in step S23 (step S24). Then, the self-location estimation unit 15 extracts voxels of the voxel data VD having a frequently occurring z coordinate value as candidate voxels from the voxels present in the initial position search range Ri (step S25).

Then, the self-location estimation unit 15 sorts the voxels extracted in step S25 in order of proximity to the GNSS positioning position (here, a two-dimensional position defined by an x coordinate value and a y coordinate value) (step S26).Then, the self-location estimation unit 15 selects the first voxel in the order sorted in step S26 (step S27).

Then, the self-position estimation unit 15 regards the coordinate value of the selected voxel (z coordinate value is a value obtained by adding the vehicle height) as a tentative initial position and performs the simplified NDT process (step S28). The self-position estimation unit 15 also calculates the association ratio DAR in the simplified NDT process.

Then, the self-position estimation unit 15 determines whether the association ratio DAR is greater than a predetermined value (step S29). If the association ratio DAR is greater than the predetermined value (step S29; Yes), the self-position estimation unit 15 considers the result of the simple NDT processing to be reliable and sets the result of the simple NDT processing to the initial position (step S30).

On the other hand, if the association ratio DAR is equal to or less than a predetermined value (step S29; No), the self-location estimation unit 15 determines whether the candidate voxel arranged in step S26 is the last one (step S31). If the self-location estimation unit 15 is the last candidate voxel (step S31; Yes), it expands the initial position search range Ri (step S32). In this case, the self-location estimation unit 15 expands the initial position search range Ri by, for example, shifting the boundaries of the initial position search range Ri outward by 5 m each in the vertical and horizontal directions. After that, the self-location estimation unit 15 returns the process to step S23. In this case, when arranging the candidate voxels in step S26, the self-location estimation unit 15 may exclude candidate voxels for which the simple NDT process has already been performed.

On the other hand, if the self-position estimation unit 15 is not targeting the last candidate voxel (step S31; No), it selects the next candidate voxel (step S33). Then, in step S28, it performs the simplified NDT process on the selected candidate voxel.

(5-3) Initial Position Setting Process: Target Moving Body is a Ship FIG. 16 is an example of a flowchart showing the procedure of the initial position setting process executed in step S11 of FIG. 14 when the target moving body is a ship.

First, the self-location estimation unit 15 acquires the predicted tide level at the location where the target moving object, the ship, is located from public information from the Japan Meteorological Agency, etc. (step S41). In this case, for example, the self-location estimation unit 15 receives information on the predicted tide level corresponding to the location where the target moving object is located from a server device that manages the current predicted tide levels at each location via the interface 11.

Then, the self-position estimation unit 15 acquires the latitude and longitude position (i.e., x, y coordinate values) and accuracy information obtained by GNSS positioning based on the output signal of the GPS receiver 5, etc. (step S42). Then, the self-position estimation unit 15 sets an initial position search range Ri centered on the GNSS positioning position based on the accuracy information acquired in step S42 (step S43).

Next, the self-location estimation unit 15 extracts, as candidate voxels, virtual voxels for which no voxel data VD exists within the initial position search range Ri (step S44). In this case, the self-location estimation unit 15 creates a virtual voxel coordinate list in voxel coordinate units as a list of target virtual voxels. Then, the self-location estimation unit 15 arranges the virtual voxels in order of proximity to the GNSS positioning position (step 45). Then, the self-location estimation unit 15 selects the virtual voxel with the smallest order (step S46).

Next, the self-position estimation unit 15 executes the simplified NDT process using the coordinates of the selected virtual voxel (step S47). In this case, the self-position estimation unit 15 sets the z coordinate value that is the initial position in the simplified NDT process to a value obtained by adding the height from the water surface of the ship, which is the target moving body, to the predicted tide level obtained in step S41.

Next, the self-position estimation unit 15 determines whether the association ratio DAR is greater than a predetermined value (step S48). If the association ratio DAR is greater than the predetermined value (step S48; Yes), the self-position estimation unit 15 sets the result of the simplified NDT processing to the initial position (step S49).

On the other hand, if the association ratio DAR is equal to or less than a predetermined value (step S48; No), the self-location estimation unit 15 determines whether the last voxel in the order arranged in step S45 was the target (step S50). If the last voxel was the target (step S50; Yes), the self-location estimation unit 15 expands the initial position search range Ri (step S51). In this case, the self-location estimation unit 15 shifts the boundaries of the initial position search range Ri outward by 5 m each in the up, down, left, and right directions, and expands the initial position search range Ri. Thereafter, the self-location estimation unit 15 returns the process to step S44. In this case, when arranging the voxels in step S45, the self-location estimation unit 15 may exclude voxels for which the simple NDT process has already been performed.

On the other hand, if the self-position estimation unit 15 does not target the last voxel in the order arranged in step S45 (step S50; No), it selects the next voxel (step S52). Then, in step S47, it performs the simplified NDT process on the selected voxel.

(6) Modifications
Preferred modifications of the above-described embodiment will now be described. The following modifications may be combined and applied to these embodiments.

(Variation 1)
In the initial position setting process executed when the association ratio DAR is less than a predetermined value in step S15 of Fig. 14, the self-position estimation unit 15 may set the initial position search range Ri based on the reliability of the previous estimated self-position and NDT scan matching, instead of setting the initial position search range Ri based on the GNSS positioning result and the accuracy information of the GNSS positioning. Hereinafter, the above initial position setting process is also called "initial position setting process for return". That is, the initial position setting process for return is an initial position setting process executed when the self-position estimation process is repeatedly executed and the index of reliability regarding the calculation of the NDT position X _NDT (here, the association ratio DAR) is less than a predetermined value.

Figure 17 is an example of a flowchart showing an overview of the self-location estimation process according to Modification Example 1. The self-location estimation unit 15 starts the process of the flowchart in Figure 17 when it becomes necessary to perform self-location estimation, such as when the power is turned on.

First, the self-position estimation unit 15 executes the initial position setting process described in Fig. 15 or Fig. 16 (step S61). Next, the self-position estimation unit 15 executes dead reckoning from the moving speed and angular velocity of the target moving object detected based on the speed sensor 4 and the gyro sensor 6, etc., and the previous estimated self-position, and calculates the DR position _XDR, which is the predicted self-position (step S62). Next, the self-position estimation unit 15 converts the point cloud data based on the output of the LIDAR 3 into point cloud data in the world coordinate system (step S63).

Then, the self-position estimation unit 15 performs an NDT matching process using the DR position _XDR calculated in step S62 as an initial value to calculate an NDT position _XNDT that is the estimated self-position X^ (step S64). Furthermore, the controller 13 calculates an association ratio DAR based on the result of association between the point cloud data converted into the world coordinate system and the voxels in which the voxel data VD exists.

Then, the self-position estimation unit 15 judges whether the association ratio DAR is less than a predetermined value (step S65). When the self-position estimation unit 15 judges that the association ratio DAR is less than the predetermined value (step S65; Yes), it executes the initial position setting process for return (step S66). In this case, the self-position estimation unit 15 judges that the reliability of the NDT scan matching (and the calculated NDT position X _NDT ) has decreased, and it is necessary to execute the initial position setting process for return. Details of the initial position setting process for return will be described with reference to FIG. 18 and FIG. 19. On the other hand, when the association ratio DAR is equal to or greater than the predetermined value (step S65; No), the self-position estimation unit 15 judges that the reliability of the NDT position X _NDT is maintained, and proceeds to the process of step S67.

Next, the self-position estimation unit 15 determines whether or not the self-position estimation process should be ended (step S67). If the controller 13 determines that the self-position estimation process should be ended (step S67; Yes), the controller 13 ends the processing of the flowchart. On the other hand, if the controller 13 continues the self-position estimation process (step S67; No), the controller 13 returns to step S62 and estimates the self-position at the next processing time using the estimated self-position X^ calculated in step S64.

Figure 18 is an example of a flowchart showing the steps of the initial position setting process executed in step S66 of Figure 17 when the target moving body is a vehicle.

First, the self-location estimation unit 15 generates reliability information from the association ratio DAR in the NDT scan matching executed immediately before (step S71). The reliability information is information for determining the size of the initial location search range Ri, and may be the association ratio DAR itself, or may be an index value that has a positive correlation with the association ratio DAR and is calculated from the association ratio DAR by a predetermined calculation formula. The self-location estimation unit 15 determines that the smaller the value of the reliability information, the larger the deviation of the self-location, and increases the size of the initial location search range Ri. Also, if the value of the reliability information is relatively large, the self-location estimation unit 15 determines that the deviation of the estimated self-location is small, and reduces the size of the initial location search range Ri. Then, the self-location estimation unit 15 sets the initial location search range Ri from the reliability information (step S72). In this case, the self-position estimating unit 15 sets an initial position search range Ri whose center position is the previously calculated NDT position X _NDT (estimated self-position X^) and whose size increases as the reliability information value decreases.

Then, in steps S73 to S83, the self-location estimation unit 15 executes the same processes as steps S23 to S33 in FIG. 15 based on the set initial position search range Ri. As a result, the self-location estimation unit 15 extracts candidate voxels within the initial position search range Ri, and determines the result of the simplified NDT process of the candidate voxels whose association ratio DAR is greater than a predetermined value as the initial position.

Figure 19 is an example of a flowchart showing the steps of the initial position setting process executed in step S66 of Figure 17 when the target moving body is a ship.

First, the self-location estimation unit 15 acquires the predicted tide level at the location where the ship, which is the target moving object, is located from the public information of the Japan Meteorological Agency (step S91). Next, the self-location estimation unit 15 generates reliability information from the association ratio DAR in the NDT scan matching performed immediately before (step S92). Then, the self-location estimation unit 15 sets an initial location search range Ri from the reliability information (step S93). In this case, the self-location estimation unit 15 sets the initial location search range Ri with the center position set to the NDT position X _NDT (estimated self-location X^) obtained immediately before and the size being increased as the value of the reliability information is smaller.

Then, in steps S94 to S102, the self-location estimation unit 15 executes the same processing as steps S44 to S52 in FIG. 16 based on the set initial position search range Ri. As a result, the self-location estimation unit 15 extracts virtual voxels within the initial position search range Ri, and determines the result of the simplified NDT processing of the virtual voxels whose association ratio DAR is greater than a predetermined value as the initial position.

In this way, according to this modified example, the return initial position setting process can be preferably executed based on the results of the most recent NDT scan matching.

(Variation 2)
The self-position estimation unit 15 may execute a simplified NDT process with each position of all candidate voxels as a search start position, and may determine the result of the simplified NDT process for the voxel with the maximum association ratio DAR as the initial position. For example, in the initial position setting process of the vehicle shown in FIG. 15, the self-position estimation unit 15 extracts candidate voxels in step S25, and then executes the simplified NDT process in step S28 for each of the extracted voxels. Then, the self-position estimation unit 15 determines the result of the simplified NDT process with the highest association ratio DAR as the initial position.

Although this modified example increases processing time, it is possible to favorably improve the reliability of the determined initial position.

(Variation 3)
Instead of using the association ratio DAR as an evaluation index used to determine whether or not the result of the simple NDT processing is to be the initial position (see step S29 in FIG. 15, step S48 in FIG. 16, etc.), another index may be used.

For example, the self-position estimation unit 15 may use the reliability index of NDT scan matching (NRI: NDT Reliability Index) with a value range of 0 to 1 as the above-mentioned evaluation index. In this case, the self-position estimation unit 15 determines the result of the simplified NDT processing in which the reliability index NRI is greater than a predetermined value as the initial position.

Here, the calculation of the reliability index NRI will be described. First, when the downsampling size at each time is "DSS", the self-location estimation unit 15 calculates the reliability value "NRV (NDT Reliability Value)" of the NDT scan matching by the following formula using the score value E and the association ratio DAR.
NRV=DSS×E×DAR (8)
In this case, it is assumed that the downsampling size is adaptively changed at each processing time so that the number of measurement points (also called the "number of corresponding measurement points") associated with the voxel data VD in the point cloud data after downsampling falls within a predetermined target range. For example, when the number of corresponding measurement points is greater than the upper limit of the target range at each processing time, the self-location estimation unit 15 increases the downsampling size at the next processing time by a predetermined rate (e.g., 1.1) or a predetermined value, and when the number of corresponding measurement points is less than the lower limit of the target range, the self-location estimation unit 15 decreases the downsampling size at the next processing time by a predetermined rate or a predetermined value. On the other hand, when the number of corresponding measurement points is within the target range at each processing time, the self-location estimation unit 15 maintains the downsampling size. In such an adaptive setting of the downsampling size, the more the number of corresponding measurement points, the larger the downsampling size, so it is presumed that the reliability of the NDT processing increases as the downsampling size increases.

Next, the self-location estimation unit 15 calculates a reliability index NRI by normalizing the reliability value NRV to a range of 0 to 1. For example, the self-location estimation unit 15 converts the reliability value NRV into the reliability index NRI using any one of the following formulas (8) to (11).

なお、式（８）～式（１１）において、信頼度値ＮＲＶに所定の係数αを乗じてもよい。

In addition, in the formulas (8) to (11), the reliability value NRV may be multiplied by a predetermined coefficient α.

When the reliability index NRI is used as the above-mentioned evaluation index, the score value E is also taken into account, so the number of repetitions in the simple NDT process needs to be increased, which increases the processing time, but improves the strictness of the evaluation index. Note that the self-location estimation unit 15 may use the score value E as the above-mentioned evaluation index instead of the association ratio DAR and the reliability index NRI. In this case, the self-location estimation unit 15 determines the result of the simple NDT process in which the score value E is greater than a predetermined value as the initial position.

Similarly, the self-location estimation unit 15 may use the reliability index NRI or the score value E instead of the association ratio DAR as an evaluation index used in determining whether to perform the initial position setting process and the return initial position setting process (see step S15 in FIG. 14, step S65 in FIG. 17, etc.). Also, the self-location estimation unit 15 may generate reliability information from the reliability index NRI or the score value E instead of the association ratio DAR in step S71 in FIG. 18 or step S92 in FIG. 19 of (Variation 1).

(Variation 4)
The voxel data VD is not limited to a data structure including a mean vector and a covariance matrix as shown in Fig. 4. For example, the voxel data VD may include point cloud data measured by a measurement maintenance vehicle and used to calculate the mean vector and the covariance matrix.

As described above, the controller 13 of the information processing device 1 according to this embodiment acquires the position of the target moving object measured by GNSS, which is the first position calculation method. The controller 13 then sets an initial position search range Ri based on the position measured by GNSS, and sets a reference position for NDT scan matching, which is the second position calculation method, within the initial position search range Ri. The controller 13 then estimates the position of the target moving object by NDT scan matching based on the set reference position. In this case, the controller 13 determines the size of the initial position search range Ri based on accuracy information that indicates the positioning accuracy of the GNSS measured position. This allows the information processing device 1 to accurately search for and set a reference position that serves as a reference in NDT scan matching.

In the above-described embodiment, the program can be stored using various types of non-transitory computer readable media and supplied to a computer, such as a controller. 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)).

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 that can be understood by those skilled in the art can be made to the configuration and details of the present invention within the scope of the present invention. In other words, the present invention naturally includes various modifications and amendments that a person skilled in the art could make in accordance with the entire disclosure, including the scope of the claims, and the technical ideas. In addition, the disclosures of the above cited patent documents and the like are incorporated into this document by reference.

Reference Signs List 1 Information processing device 2 Sensor group 3 Lidar 4 Speed sensor 5 GPS receiver 6 Gyro sensor 10 Map DB

Claims (9)

a position acquisition means for acquiring a position of a moving object based on a first position calculation method;
a reference position searching means for setting a search range for searching for a reference position of the second position calculation method based on the position measurement position, and performing a first comparison within the search range in which voxel data representing the position of an object for each voxel, which is a unit area, is compared with point cloud data based on an output of a measuring device provided on the moving body based on the reference position, and searching for the reference position where a first evaluation index of the first comparison is equal to or greater than a first predetermined value ;
a position estimating means for estimating a position of the moving object by the second position calculation method based on a second comparison in which the voxel data and the point cloud data are compared based on the reference position ,
The reference position search means determines a size of the search range based on accuracy information representing accuracy of positioning of the positioning position;
If an estimated position of the moving object determined at the immediately preceding processing time exists, and a second evaluation index of the position estimation by the second position calculation method calculated based on a result of the second matching at the immediately preceding processing time is less than a second predetermined value,
The reference position search means searches for the reference position based on the point cloud data at a current processing time,
The position estimation means converts a coordinate system of the point cloud data at a current processing time into a coordinate system of the voxel data based on the reference position;
The position estimation means, when an estimated position of the moving body determined at the immediately previous processing time exists and the second evaluation index is equal to or greater than the second specified value, converts the coordinate system of the point cloud data at the current processing time into the coordinate system of the voxel data based on the estimated position of the moving body determined at the immediately previous processing time . the first position calculation method is positioning using GNSS,
The information processing device according to claim 1 , wherein the reference position searching means determines a size of the search range based on accuracy information of the GNSS. The information processing device according to claim 1 or 2, wherein the reference position search means reduces the size of the search range as the accuracy indicated by the accuracy information increases. The information processing device according to any one of claims 1 to 3, wherein the position estimation means converts the coordinate system of the point cloud data to the coordinate system of the voxel data based on the reference position when there is no estimated position of the moving body determined at the immediately preceding processing time. the moving object is a vehicle,
5. The information processing device according to claim 1 , wherein the reference position searching means determines a candidate voxel in which the vehicle exists based on a frequency distribution of a position in a height direction for each voxel existing within the search range. The moving body is a ship,
5. The information processing apparatus according to claim 1 , wherein the reference position searching means determines a virtual voxel having no voxel data within the search range as a candidate voxel in which the ship is present. A computer-implemented control method, comprising:
a position acquisition step of acquiring a position measured by the moving object using a first position calculation method;
a reference position searching step of setting a search range for searching for a reference position of the second position calculation method based on the position measurement position, and performing a first comparison within the search range in which voxel data representing the position of an object for each voxel, which is a unit area, is compared with point cloud data based on an output of a measuring device provided on the moving body based on the reference position, and searching for the reference position where a first evaluation index of the first comparison is equal to or greater than a first predetermined value ;
a position estimating step of estimating a position of the moving object by the second position calculation method based on a second comparison in which the voxel data and the point cloud data are compared based on the reference position ,
The reference position search step includes determining a size of the search range based on accuracy information representing accuracy of positioning of the positioning position;
If an estimated position of the moving object determined at the immediately preceding processing time exists, and a second evaluation index of the position estimation by the second position calculation method calculated based on a result of the second matching at the immediately preceding processing time is less than a second predetermined value,
The reference position search step searches for the reference position based on the point cloud data at a current processing time;
The position estimation step converts a coordinate system of the point cloud data at a current processing time into a coordinate system of the voxel data based on the reference position;
A control method in which the position estimation process converts the coordinate system of the point cloud data at the current processing time into the coordinate system of the voxel data based on the estimated position of the moving body determined at the immediately previous processing time when an estimated position of the moving body determined at the immediately previous processing time exists and the second evaluation index is equal to or greater than the second specified value . a position acquisition means for acquiring a position of a moving object based on a first position calculation method;
a reference position searching means for setting a search range for searching for a reference position of the second position calculation method based on the position measurement position, and performing a first comparison within the search range in which voxel data representing the position of an object for each voxel, which is a unit area, is compared with point cloud data based on an output of a measuring device provided on the moving body based on the reference position, and searching for the reference position where a first evaluation index of the first comparison is equal to or greater than a first predetermined value ;
making the computer function as a position estimation means for estimating a position of the moving object by the second position calculation method based on a second comparison in which the voxel data and the point cloud data are compared based on the reference position ;
The reference position search means determines a size of the search range based on accuracy information representing accuracy of positioning of the positioning position;
If an estimated position of the moving object determined at the immediately preceding processing time exists, and a second evaluation index of the position estimation by the second position calculation method calculated based on a result of the second matching at the immediately preceding processing time is less than a second predetermined value,
The reference position search means searches for the reference position based on the point cloud data at a current processing time,
The position estimation means converts a coordinate system of the point cloud data at a current processing time into a coordinate system of the voxel data based on the reference position;
The position estimation means, when an estimated position of the moving body determined at the immediately previous processing time exists and the second evaluation index is equal to or greater than the second specified value, converts the coordinate system of the point cloud data at the current processing time into the coordinate system of the voxel data based on the estimated position of the moving body determined at the immediately previous processing time . A storage medium storing the program according to claim 8 .

Images