WO2024075498A1

WO2024075498A1 - Localization method and localization device

Info

Publication number: WO2024075498A1
Application number: PCT/JP2023/033647
Authority: WO
Inventors: 佳奈子酒井; 千加夫土谷
Original assignee: 日産自動車株式会社
Priority date: 2022-10-04
Filing date: 2023-09-15
Publication date: 2024-04-11

Abstract

This localization method comprises: measuring, as observation values of an object being detected, the position and the angle of a moving object from a signal received from a satellite (S01); calculating, on the basis of a relative amount of movement of the moving object, a travel path tracing back by a predetermined distance from the observation values of the object being detected at a predetermined time, or a travel path of the moving object tracing back by a predetermined time from the observation values of the object being detected at the predetermined time (S02); detecting the confidence level of the observation values of the object being detected at the predetermined time on the basis of a deviation between the calculated travel path and the past observation values in a section corresponding to the travel path, and confidence levels stored in association with the past observation values (S03); storing the observation values of the object being detected at the predetermined time in association with the detected confidence level (S04); and localizing the moving object on the basis of observation values having a confidence level higher than or equal to a first predetermined value (S05).

Description

Self-location estimation method and self-location estimation device

This disclosure relates to a self-location estimation method and a self-location estimation device.

The abnormal value determination device disclosed in Patent Document 1 determines a predetermined time so that the difference between the cumulative error of the speed information detected by the inertial navigation system and the pseudo-distance error based on the GPS reception information is within a predetermined range. The device then estimates the position of the moving body based on the speed information and pseudo-distance at each time within this predetermined time, and determines whether there is an abnormality in the pseudo-distance based on the residual between the estimated position and the pseudo-distance.

JP 2012-207919 A

However, the standard for determining anomalies is the position of the moving object estimated from GPS reception information and information detected by an inertial navigation system. Therefore, the standard for determining anomalies is itself subject to outliers in the GPS reception information, which can lead to erroneous determination of pseudorange anomalies.

The present disclosure has been made in consideration of the above problems, and its purpose is to provide a self-location estimation method and device that can accurately detect the reliability of observed values determined from signals received from artificial satellites.

The self-location estimation method according to one or more embodiments measures the position and angle of a moving body from a signal received from an artificial satellite as an observation value of a detection target, calculates a travel trajectory of the moving body going back a predetermined distance from the observation value of the detection target at a predetermined time, or a travel trajectory of the moving body going back a predetermined time from the observation value of the detection target at a predetermined time, based on the relative movement amount of the moving body, detects the reliability of the observation value of the detection target at the predetermined time based on the calculated travel trajectory, the deviation from past observation values in the section corresponding to the travel trajectory, and the reliability stored in association with the past observation value, stores the observation value of the detection target at the predetermined time in association with the detected reliability, and estimates the self-location of the moving body based on the observation value whose reliability is equal to or greater than a first predetermined value.

According to this disclosure, it is possible to accurately detect the reliability of the observation values determined from the signals received from the artificial satellites.

FIG. 1 is a block diagram showing the configuration of a self-location estimation device 1 according to the first to third embodiments and their modified examples. FIG. 2 is a flowchart showing a self-location estimation method using the self-location estimation device 1 of FIG. 1, which is related to the first to third embodiments and their modified examples. FIG. 3 is a flowchart showing a specific processing example (second embodiment) of step S03 in the flowchart of FIG. FIG. 4 is a flowchart showing a specific processing example (third embodiment) of step S03 in the flowchart of FIG. FIG. 5 is a block diagram showing the configuration of a self-location estimation device 2 according to the fourth embodiment and its modified example. FIG. 6 is a flowchart showing a self-location estimation method using the self-location estimation device 2 of FIG. 5, which is an embodiment of the present invention and a modification of the embodiment. Figure 7 is a conceptual diagram showing the vehicle position (Pgn(t)) as an observation value to be detected, the vehicle positions (Pgn(t), Pgn(t-1), Pgn(t-2), ...) as examples of past observation values in a section (window size) 32 corresponding to a driving trajectory 31, and the distances (D(t-1), D(t-2), ...) as examples of deviation degrees.

Next, one or more embodiments will be described in detail with reference to the drawings. In the description, the same parts will be given the same reference numerals and duplicate descriptions will be omitted.

First Embodiment
A configuration of a self-position estimation device 1 according to a plurality of embodiments including a first embodiment will be described with reference to Fig. 1. The self-position estimation device 1 includes a positioning unit 11, a travel trajectory calculation unit 14, a reliability detection unit 15, a determination history storage unit 16 (an example of a reliability storage unit), and a self-position estimation unit 17. In the embodiment, a vehicle is exemplified as a moving body, but the present invention can also be applied to moving bodies that do not have wheels, such as ships, airplanes, and rockets other than vehicles.

The positioning unit 11 measures the position and angle of a vehicle (an example of a moving object) from a signal received from an artificial satellite as an observation value of the detection target. The positioning unit 11 determines the position and angle of the vehicle using a GNSS (Global Navigation Satellite System). For this purpose, the positioning unit 11 includes, for example, a GPS (Global Positioning System) receiver that receives signals from artificial satellites that constitute the GNSS, and further includes a calculation unit that calculates the absolute position (e.g., latitude, longitude, altitude) and absolute angle (e.g., azimuth angle, elevation angle) of the vehicle in a global coordinate system from the received signal received by the receiver. The "observation value of the detection target" refers to the observation value of the target for which reliability is to be detected, and for example, the most recent observation value in a time series of multiple observation values repeatedly measured by the positioning unit 11 becomes the observation value of the detection target.

The travel trajectory calculation unit 14 calculates a travel trajectory of the vehicle going back a predetermined distance from the observation value of the detection target at a predetermined time, or a travel trajectory of the vehicle going back a predetermined time from the observation value of the detection target from the predetermined time, based on the relative movement amount of the vehicle. For example, the travel trajectory calculation unit 14 calculates a travel trajectory of the vehicle going back a predetermined distance from the latest observation value as the starting point in the opposite direction to the traveling direction of the vehicle, or a travel trajectory of the vehicle going back an elapsed time from the time when the latest observation value was measured (predetermined time) toward the past.

The driving trajectory can be calculated based on the relative movement amount of the vehicle. In this case, the self-location estimation device 1 may further include an IMU device (Inertial Measurement Unit) 12. The IMU device 12 is a device that detects three-dimensional inertial motion (translational motion and rotational motion in three orthogonal axial directions), and includes an acceleration sensor that detects translational motion, an angular velocity (gyro) sensor that detects rotational motion, and a calculation unit that integrates the acceleration of the translational motion over time to calculate the speed of the translational motion. The driving trajectory, which serves as the detection standard for the reliability of the observation value, is no longer affected by the observation value measured by the positioning unit 11, and erroneous judgment of reliability due to outliers in the observation value can be suppressed.

Specifically, first, position and angle information is obtained as the observation value of the detection target. Using the observation value of the detection target as the starting point, the position and angle at the previous time are estimated by subtracting from the starting point the speed and angular velocity at the time closest to the time when the observation value of the detection target was measured. Next, the position and angle information at the time two times before is estimated by subtracting from the estimated position and angle the speed and angular velocity at the time closest to the time corresponding to this estimated value. This subtraction is repeated within a predetermined distance or elapsed time from the starting point to calculate the travel trajectory of the position and angle. As described above, since the observation value is used only as the starting point in calculating the travel trajectory, the travel trajectory that is the detection standard for the reliability of the observation value is not influenced by the observation value measured by the positioning unit 11, and it is possible to suppress erroneous judgment of reliability due to outliers of the observation value. In other words, by calculating the travel trajectory of the vehicle based on the relative movement amount of the vehicle such as the speed and angular velocity of the vehicle, it is not influenced by the unreliable observation value measured by the positioning unit 11.

The reliability detection unit 15 detects the reliability of the observation value of the detection target at a specified time based on the degree of deviation between the driving trajectory calculated by the driving trajectory calculation unit 14 and the past observation value in the section corresponding to the driving trajectory, and the reliability stored in the judgment history storage unit 16 in association with the past observation value.

Specifically, the reliability detection unit 15 extracts past observation values at each time within the section corresponding to the driving trajectory from the judgment history storage unit 16, and calculates the deviation between the extracted past observation value and the driving trajectory. For example, if the observation value is the vehicle position, the Euclidean distance between the driving trajectory of the vehicle position at each time and the observation value of the vehicle position (observation position) is calculated as the deviation. On the other hand, if the observation value is the vehicle angle, the angle difference between the driving trajectory of the vehicle angle at each time and the observation value of the vehicle angle (observation angle) is calculated as the deviation. "Past observation value" means an observation value measured earlier than the observation value to be detected. "Past observation value" may be limited to an observation value whose reliability has been detected by the reliability detection unit 15. Past observation values whose reliability has been detected are stored in the judgment history storage unit 16 in association with the reliability, as described below. The "reliability" detected by the reliability detection unit 15 may be a binary classification such as "high" or "low," or may be a multi-value classification of three or more values.

The judgment history storage unit 16 stores the observed value of the detection target at a specific time in association with the reliability detected by the reliability detection unit 15.

The self-position estimation unit 17 estimates the vehicle's own position based on observed values whose reliability is equal to or greater than a first predetermined value. The self-position estimation unit 17 can estimate the self-position using known technology. For example, based on observed values (vehicle's absolute position and absolute angle) and the vehicle's relative movement amount (vehicle speed and angular velocity), an estimation method such as a Kalman filter is used to estimate the most likely vehicle position and angle as the vehicle's own position. Here, observed values that are detected to have low reliability can be eliminated in advance or set to have a small contribution to the self-position estimation process.

The self-position estimation device 1 may further include a memory unit 13 that stores data indicating the vehicle speed and angular velocity detected by the IMU device 12 and data indicating the observation values measured by the positioning unit 11. These data are used by the driving trajectory calculation unit 14 and the reliability detection unit 15.

In the self-location estimation device 1 shown in FIG. 1, the memory unit 13, the driving trajectory calculation unit 14, the reliability detection unit 15, the judgment history memory unit 16, and the self-location estimation unit 17 can be realized by a general-purpose computer equipped with a CPU (Central Processing Unit), memory, and input/output units. A computer program for functioning as the self-location estimation device 1 is installed in the microcomputer. By executing the computer program, the computer functions as the multiple information processing circuits (14, 15, 17) equipped in the self-location estimation device 1. The memory unit 13 and the judgment history memory unit 16 can be realized by an external storage device such as a memory or a hard disk drive connected to the microcomputer. Note that, although an example of realizing the multiple information processing circuits equipped in the self-location estimation device 1 by software is shown here, it is also possible to configure the information processing circuits by preparing dedicated hardware. The multiple information processing circuits may also be configured by individual hardware.

2 and 7, a self-location estimation method according to the first embodiment using the self-location estimation device 1 of FIG. 1 will be described. The operation flow shown in FIG. 2 is repeatedly performed at a predetermined period. FIG. 7 is a conceptual diagram showing the vehicle position (Pgn(t)) as an observation value of the detection target, the vehicle positions (Pgn(t), Pgn(t-1), Pgn(t-2), ...) as examples of past observation values in a section (window size) 32 corresponding to the driving trajectory 31, and the distances (D(t-1), D(t-2), ...) as examples of deviations. In step S01 (positioning process), the positioning unit 11 measures the vehicle position Pgn(t) and angle from a signal received from an artificial satellite as the observation value of the detection target. Proceeding to step S02 (travel trajectory calculation process), the travel trajectory calculation unit 14 calculates a travel trajectory 31 of the vehicle going back a predetermined distance from the vehicle position Pgn(t) as the observed value of the detection target, or a travel trajectory 31 of the vehicle going back a predetermined time from the observed value of the detection target, based on the relative movement amount of the vehicle. Proceeding to step S03 (reliability detection process), the reliability detection unit 15 detects the reliability of the observed value of the detection target based on the deviation between the travel trajectory 31 calculated in the travel trajectory calculation process and the past observed value in the section (window size) 32 corresponding to the travel trajectory 31, and the reliability stored in association with the past observed value. The "deviation" is, for example, the distance between the travel trajectory 31 and each position of the vehicle (Pgn(t-1), Pgn(t-2), ...) measured by the positioning unit 11. The distance is, for example, the distance (D(t-1), D(t-2), ...) between each position of the vehicle (Pgn(t-1), Pgn(t-2), ...) and each point (Pod(t-1), Pod(t-2), ...) on the travel trajectory 31 that is closest to each position of the vehicle (Pgn(t-1), Pgn(t-2), ...). Proceeding to step S04 (reliability storage process), the judgment history storage unit 16 associates the observed value of the detection target with the detected reliability and stores it. Proceeding to step S05 (self-position estimation process), the self-position estimation unit 17 estimates the vehicle's own position based on the observed value whose reliability is equal to or greater than a first predetermined value. By repeatedly executing the above-mentioned operation flow, a time series of multiple observed values associated with reliability is obtained and stored as past observed values in the judgment history storage unit 16. Using the reliability associated with this past observed value, for example, the reliability of the latest observed value can be detected.

As explained above, the self-location estimation device 1 in FIG. 1 and the self-location estimation method in FIG. 2 use the vehicle's travel trajectory, which does not depend on GNSS observation values, as the criterion for judging reliability. This makes it possible to suppress erroneous judgments of reliability caused by deterioration in the accuracy of the judgment criterion due to outliers in GNSS observation values. In addition, when judging the reliability of the latest observation values, the reliability of past observation values is taken into consideration. This makes it possible to accurately judge the reliability even if there is a certain amount of error in the GNSS observation values.

Second Embodiment
In the second embodiment, a specific operation example of the reliability detection unit 15 in Fig. 1 and a specific processing example of step 03 (reliability detection processing) in Fig. 2 will be described with reference to Fig. 3. The self-location estimation device 1 and the self-location estimation method according to the second embodiment are generally the same as those in Fig. 1 and Fig. 2, and therefore will not be described again. The subject of all operations in steps S301 to S310 described below is the reliability detection unit 15 in Fig. 1.

First, in step S301, the counter provided in the reliability detection unit 15 is reset to zero. Proceeding to step S302, one past observation value within the window size is selected. The "window size" means the section corresponding to the driving trajectory calculated in the driving trajectory calculation process (step 02). Proceeding to step S303, it is determined whether the reliability of the selected past observation value is higher than a first reference value. If the reliability is higher than the first reference value (YES in step S303), proceed to step S304, and if the reliability is equal to or lower than the first reference value (NO in step S303), proceed to step S306. In step S304, the deviation between the past observation value and the driving trajectory is calculated, and it is determined whether the deviation is equal to or lower than a threshold value. If the deviation is equal to or lower than the threshold value (YES in step S304), proceed to step S305, and if the deviation is greater than the threshold value (NO in step S304), proceed to step S306. In step S305, the counter is incremented by 1. Then, proceed to step S306 to determine whether or not all past observations within the window size have been selected. If all observations have not been selected (NO in step S306), return to step S302, select one past observation within the window size that has not yet been selected, and perform the processes in steps S303 to S305.

If all the observation values are selected (YES in step S306), the process proceeds to step S307, where the ratio of the number of past observation values whose deviations are determined to be equal to or less than the threshold in step S304 to the total number of past observation values in the section (window size) corresponding to the driving trajectory is calculated. The process proceeds to step S308, where it is determined whether this ratio is equal to or greater than a predetermined ratio. If the ratio is equal to or greater than the predetermined ratio (YES in step S308), the process proceeds to step S309, where it is determined that the reliability of the observation value of the detection target measured in step S01 is high. If the ratio is less than the predetermined ratio (NO in step S308), the process proceeds to step S310, where it is determined that the reliability of the observation value of the detection target measured in step S01 is low. According to steps S308 to S310, the reliability of the observation value of the detection target can be detected based on the ratio of the number of past observation values whose deviations are determined to be equal to or less than the threshold to the total number of past observation values within the window size. Here, an example of binary classification of reliability is shown, but multi-value classification is also possible. In other words, the higher the ratio, the higher the reliability of the observed value of the detection target can be detected.

The reliability detection unit 15 calculates the degree of deviation between the driving trajectory and past observed values in a section corresponding to the driving trajectory, compares the degree of deviation with a threshold, and calculates a higher reliability when the proportion of observed values below the threshold is high than when it is low. The reliability detection unit 15 performs a threshold judgment on each observed value, rather than a threshold judgment on the average value of all observed values within the window size. Therefore, even if there is an observed value with a large error within the window size, it is possible to accurately determine the reliability without being affected by the observed value.

Third Embodiment
In the third embodiment, another specific operation example of the reliability detection unit 15 in Fig. 1 and another specific processing example of step 03 (reliability detection processing) in Fig. 2 will be described with reference to Fig. 4. The self-location estimation device 1 and the self-location estimation method according to the third embodiment are generally the same as those in Figs. 1 and 2, and therefore will not be described again. The subject of all operations in steps S321 to S326 described below is the reliability detection unit 15 in Fig. 1.

In step S321, the deviation between the past observation value within the window size and the driving trajectory is calculated. If there are multiple past observation values within the window size, the deviation between all past observation values and the driving trajectory is calculated. Proceed to step S322, and each deviation is weighted according to the reliability of the past observation value corresponding to the deviation. Specifically, the higher the reliability, the larger (heavier) the deviation is. Proceed to step S323, and the average value of the deviations calculated by weighting (hereinafter referred to as the "weighted average value"). Proceed to step S324, and determine whether the weighted average value is equal to or less than a threshold value. If the weighted average value is equal to or less than the threshold value (YES in step S324), proceed to step S325, and determine that the reliability of the observation value of the detection target measured in step S01 is high. If the weighted average value is greater than the threshold value (NO in step S324), proceed to step S326, and determine that the reliability of the observation value of the detection target measured in step S01 is low. According to steps S324 to S326, the reliability of the observed value of the detection target can be detected based on the weighted average value of the deviation between the past observed values within the window size and the driving trajectory. Here, an example is shown in which the reliability is classified into two values, but it may also be classified into multiple values. In other words, the smaller the weighted average value, the higher the reliability of the observed value of the detection target can be detected.

The reliability detection unit 15 weights the degree of deviation between the driving trajectory and past observation values in a section corresponding to the driving trajectory according to the reliability stored in correspondence with the past observation values. The smaller the weighted average value, the higher the reliability of the observation value of the detection target is detected. The reliability of the observation value of the detection target is detected using a weighted average value that takes into account the reliability of the past observation values. As a result, even if the degree of deviation between a certain past observation value and the driving trajectory is large, if the reliability of that past observation value is low, the weight assigned to the degree of deviation is lighter, so the weighted average value does not become large. In other words, even if there is an observation value with a large error within the window size, it is possible to accurately determine the reliability without being affected by the observation value.

Fourth Embodiment
In the first to third embodiments, the reliability (hereinafter, referred to as "position reliability" and "angle reliability") of both the position (hereinafter, referred to as "observation position") and angle (hereinafter, referred to as "observation angle") of the vehicle as the observation value of the detection target are simultaneously detected, and the position and angle of the vehicle are simultaneously estimated. However, if there is an error in the observation angle used as a starting point when calculating the traveling trajectory of the position, the traveling trajectory of the position may be calculated erroneously, which may lead to an erroneous determination of the position reliability. Therefore, in the fourth embodiment, an example in which the position reliability and the angle reliability are detected separately will be described. Specifically, an example in which the angle reliability is first detected to estimate the angle, and then the estimated angle is used as a starting point when calculating the traveling trajectory of the position will be described.

The configuration of the self-location estimation device 2 according to the fourth embodiment will be described with reference to FIG. 5. The self-location estimation device 2 has a positioning unit 11, an IMU device 12, an angle unit 2a, and a position unit 2b. The self-location estimation device 2 differs from FIG. 1 in that the functional blocks related to data storage and arithmetic processing in the self-location estimation device 2 are divided into an angle unit 1a and a position unit 1b. On the other hand, the positioning unit 11 and IMU device 12 in FIG. 5 are the same as those in FIG. 1, and a description thereof will be omitted here.

The angle unit 1a includes an angle memory unit 13a, an angle travel trajectory calculation unit 14a, an angle reliability detection unit 15a, an angle determination history memory unit 16a, and an angle estimation unit 17a.

The angle memory unit 13a stores data indicating the angular velocity of the vehicle detected by the IMU device 12 and data indicating the observed angle measured by the positioning unit 11. These data are used by the angle travel trajectory calculation unit 14a, the angle reliability detection unit 15a, and the angle determination history memory unit 16a.

The angle travel trajectory calculation unit 14a calculates the travel trajectory of the angle going back a predetermined distance from the observation angle of the detection target, or the travel trajectory of the vehicle's angle going back a predetermined time from the time when the observation angle of the detection target was measured. The angle travel trajectory calculation unit 14a calculates the travel trajectory of the angle based on the relative movement amount of the vehicle obtained from the IMU device 12. The travel trajectory of the angle, which is the detection standard for the reliability of the observation angle, is no longer affected by the observation angle measured by the positioning unit 11, and erroneous judgment of reliability due to outliers in the observation angle can be suppressed.

Specifically, first, the angle of the vehicle at the previous time is estimated by subtracting from the starting point the angular velocity at the time closest to the time when the observation angle of the detection target was measured. Next, the angle at the time two times before can be estimated by subtracting from the estimated angle the angular velocity at the time immediately before the time corresponding to this estimated angle. This subtraction is repeated as long as it is within a predetermined distance from the starting point or the elapsed time from the detection time is within a predetermined time, and the travel trajectory of the angle is calculated. Since the observation angle is used only as the starting point in calculating the travel trajectory of the angle, the travel trajectory of the angle, which is the detection standard for the reliability of the observation angle, is not influenced by the observation angle measured by the positioning unit 11, and erroneous determination of the reliability of the angle due to outliers in the observation angle can be suppressed. In other words, by calculating the travel trajectory of the vehicle angle based on the relative movement amount of the vehicle, it is not influenced by the unreliable observation angle measured by the positioning unit 11.

The angle reliability detection unit 15a detects the angle reliability of the observation angle of the detection target based on the deviation between the angle travel trajectory calculated by the angle travel trajectory calculation unit 14a and the past observation angle in the section corresponding to the angle travel trajectory, and the angle reliability stored in the angle determination history storage unit 16a in association with the past observation angle. Specifically, the angle reliability detection unit 15a extracts the past observation angle at each time in the section corresponding to the angle travel trajectory from the angle determination history storage unit 16a, and calculates the angle difference between the extracted past observation angle and the angle travel trajectory as the angle deviation.

The angle reliability detection unit 15a can detect the angle reliability of the observation angle to be detected by applying a detailed example of step S03 (reliability detection process) in FIG. 2 described in the second embodiment (FIG. 3) or the third embodiment (FIG. 4) to the observation angle of the vehicle.

The angle determination history storage unit 16a stores the observed angle of the detection target in association with the detected angle reliability.

The angle estimation unit 17a estimates the vehicle angle based on the observed angle whose reliability is equal to or greater than a third predetermined value. The angle estimation unit 17a can estimate the angle using known technology. For example, based on the observed angle (absolute angle of the vehicle) and the relative movement amount of the vehicle (vehicle speed and angular velocity), an estimation method such as a Kalman filter is used to estimate a plausible vehicle angle. Here, among the observed angles, observed angles for which a low reliability has been detected can be eliminated in advance or set so that their contribution to the angle estimation process is small.

The position unit 1b includes a position memory unit 13b, a position travel trajectory calculation unit 14b, a position reliability detection unit 15b, a position determination history memory unit 16b, and a position estimation unit 17b.

The position memory unit 13b stores data indicating the vehicle speed and angular velocity detected by the IMU device 12, and data indicating the observed position measured by the positioning unit 11. These data are used by the position travel trajectory calculation unit 14b, the position reliability detection unit 15b, and the position determination history memory unit 16b.

The position travel trajectory calculation unit 14b calculates the travel trajectory of the vehicle's position from the observed position of the detection target measured by the positioning unit 11 and the angle estimated by the angle estimation unit 17a. The position travel trajectory calculation unit 14b calculates the travel trajectory of the position based on the relative movement amount of the vehicle obtained from the IMU device 12. The travel trajectory of the position, which is the detection standard for the reliability of the observed position, is no longer affected by the observed position measured by the positioning unit 11, and erroneous judgment of reliability due to outliers in the observed position can be suppressed.

Specifically, first, the vehicle's position and angle at the previous time are estimated by subtracting from the starting point the speed and angular velocity at the time closest to the time at which the observation position of the detection target was measured, using the observation position and angle estimation unit 17a of the detection target as the starting point. Next, the position and angle at the time two times before can be estimated by subtracting from the estimated position and angle the speed and angular velocity at the time immediately before the time corresponding to this estimated position and estimated angle. This subtraction is repeated as long as it is within a predetermined distance from the starting point or the elapsed time from the detection time is within a predetermined time, thereby calculating the travel trajectory of the position. Since the observation position is used only as the starting point in calculating the travel trajectory of the position, the travel trajectory of the position, which is the detection standard for the reliability of the observation position, is not influenced by the observation position measured by the positioning unit 11, and it is possible to suppress erroneous determination of the reliability of the position due to outliers of the observation position. In other words, by calculating the travel trajectory of the vehicle's position based on the relative movement amount of the vehicle, it is not influenced by the unreliable observation position measured by the positioning unit 11.

The position reliability detection unit 15b detects the position reliability of the observation position of the detection target based on the deviation between the driving trajectory of the position calculated by the position driving trajectory calculation unit 14b and the past observation position in the section corresponding to the driving trajectory of the position, and the position reliability stored in the position determination history storage unit 16b in association with the past observation position. Specifically, the position reliability detection unit 15b extracts the past observation positions at each time in the section corresponding to the driving trajectory of the position from the position determination history storage unit 16b, and calculates the Euclidean distance between the extracted past observation positions and the driving trajectory of the position as the position deviation.

The position reliability detection unit 15b can detect the position reliability of the observation position of the detection target by applying a detailed example of step S03 (reliability detection process) in FIG. 2 described in the second embodiment (FIG. 3) or the third embodiment (FIG. 4) to the observation position of the vehicle.

The position determination history storage unit 16b stores the observed position of the detection target in association with the detected position reliability.

The position estimation unit 17b estimates the position of the vehicle based on the observed position whose reliability is equal to or greater than a fourth predetermined value and the vehicle angle estimated by the angle estimation unit 17a. The position estimation unit 17b can estimate the position using known technology. For example, based on the observed position (absolute position of the vehicle), the vehicle angle estimated by the angle estimation unit 17a, and the relative movement amount of the vehicle (vehicle speed and angular velocity), an estimation method such as a Kalman filter is used to estimate a likely vehicle position. Here, observed positions that have been detected to have a low reliability can be eliminated in advance or set to have a low contribution to the position estimation process.

Finally, the self-position estimation device 2 outputs the vehicle angle estimated by the angle estimation unit 17a and the vehicle position estimated by the position estimation unit 17b as the vehicle's self-position.

With reference to FIG. 6, a self-location estimation method according to the fourth embodiment using the self-location estimation device 2 of FIG. 5 will be described. The operation flow shown in FIG. 6 is repeatedly performed at a predetermined cycle. In step S01 (positioning process), the positioning unit 11 measures the position and angle of the vehicle from a signal received from an artificial satellite as an observation value of the detection target. Proceeding to step S02-1 (traveling trajectory calculation process), the angle traveling trajectory calculation unit 14a calculates the traveling trajectory of the angle of the vehicle from the observation angle of the detection target. Proceeding to step S03-1 (reliability detection process), the angle reliability detection unit 15a detects the angle reliability of the observation angle of the detection target based on the traveling trajectory of the angle, the deviation from the past observation angle in the section corresponding to the traveling trajectory of the angle, and the angle reliability associated with the past observation angle. Proceeding to step S04-1 (reliability storage process), the angle determination history storage unit 16a associates and stores the observation angle of the detection target with the detected angle reliability. Proceed to step S05-1 (self-position estimation process), and the angle estimator 17a estimates the vehicle angle based on the observed angle whose angle reliability is equal to or greater than a third predetermined value.

Proceeding to step S02-2 (travel trajectory calculation process), the position travel trajectory calculation unit 14b calculates the travel trajectory of the vehicle's position from the observed position of the detection target and the vehicle angle estimated in the self-position estimation process (step S05-1). Proceeding to step S03-2 (reliability detection process), the position reliability detection unit 15b detects the position reliability of the observed position of the detection target based on the travel trajectory of the position, the deviation from a past observed position in the section corresponding to the travel trajectory of the position, and the position reliability associated with the past observed position. Proceeding to step S04-2 (reliability storage process), the position determination history storage unit 16b associates and stores the observed position of the detection target with the detected position reliability. Proceeding to step S05-2 (self-position estimation process), the position estimation unit 17b estimates the vehicle's position based on the observed position whose position reliability is equal to or greater than a fourth predetermined value and the angle of the moving body estimated in step S05-1. Proceeding to step S06, the self-position estimation device 2 outputs the estimated vehicle angle and vehicle position as the vehicle's self-position.

By repeatedly executing the above-mentioned operation flow, a time series of multiple observation positions and a time series of multiple observation angles with associated reliability are obtained, and are stored as past observation positions and past observation angles in the position determination history storage unit 16b and the angle determination history storage unit 16a, respectively. Using the position reliability and angle reliability associated with these past observation positions and past observation angles, it is possible to detect, for example, the reliability of the latest observation position and the latest observation angle, respectively.

Information on the vehicle's position and angle is required as the starting point used when calculating the driving trajectory of the position that serves as the detection standard for the position reliability. If the information on the angle that serves as the starting point is not accurate, the driving trajectory of the incorrect position will be calculated, which will ultimately lead to an erroneous determination of the position reliability. Therefore, first, the angle reliability of the observation angle measured by the positioning unit 11 is calculated, and the angle of the vehicle is estimated. Then, the estimated angle is used to calculate the driving trajectory of the position, the position reliability is detected, and the vehicle's position is estimated. This makes it possible to suppress erroneous determination of the position reliability caused by an erroneous position driving trajectory.

(First Modification)
The following describes first to fifth modified examples of each of the above-described embodiments. Any combination of two or more of the first to fifth modified examples may be applied to each embodiment.

In the first modified example, we will explain how to deal with the case where there is insufficient information regarding the reliability of past observation values. For example, immediately after the positioning unit 11 starts positioning, the positioning unit 11 can measure the observation value of the observation target, but the reliability of past observation values has not yet been fully stored, so there is insufficient information regarding the reliability of past observation values.

Therefore, when the number of past observation values not associated with reliability among the past observation values in the section (within the window size) corresponding to the driving trajectory is equal to or greater than a second predetermined value, the reliability detection unit 15 assumes that a reliability higher than the first reference value shown in step S303 has been detected for the past observation values not associated with reliability, and performs a reliability detection process (step S03). This makes it possible to detect the reliability of the observation value to be detected even when there is insufficient information regarding the reliability of the past observation values, such as immediately after the start of the positioning process (S01). The first modified example can be applied to the first to fourth embodiments.

(Second Modification)
In the second modified example, a modified example in which the threshold value is controlled according to the distance or time going back from the starting point will be described. The longer the distance going back from the observation value of the detection target, or the longer the time going back from the time when the observation value of the detection target was measured, the larger the error of the position or angle of the vehicle included in the travel trajectory calculated in step S02. Therefore, in the reliability detection process (step S03), the reliability detection unit 15 sets the threshold value used in step S304 of FIG. 3 to be larger the longer the distance going back from the observation value of the detection target of the past observation value in the section (within the window size) corresponding to the travel trajectory, or the longer the time going back from the time when the observation value of the detection target was measured. This makes it possible to suppress erroneous determination of the reliability caused by the error of the position or angle included in the travel trajectory. The first modified example can be applied to the second and fourth embodiments.

(Third Modification)
In the third modified example, a modified example in which the deviation is controlled according to the distance or time going back from the starting point will be described. The longer the distance going back from the observation value of the detection target, or the longer the time going back from the time when the observation value of the detection target was measured, the larger the error of the position or angle of the vehicle included in the travel trajectory calculated in step S02. Therefore, in the reliability detection process (step S03), the reliability detection unit 15 calculates a shorter deviation as the distance going back from the observation value of the detection target of the past observation value in the section (within the window size) corresponding to the travel trajectory goes back from the observation value of the detection target, or the longer the time going back from the time when the observation value of the detection target was measured. This makes it possible to suppress erroneous determination of the reliability caused by the error of the position or angle included in the travel trajectory. The third modified example can be applied to the first to fourth embodiments.

(Fourth Modification)
In the fourth modified example, a response when the measurement accuracy of the observed value in the positioning process (step S01) is poor will be described. It is generally known that in urban areas where high-rise buildings stand side by side, the positioning accuracy is significantly deteriorated due to the satellite radio waves being reflected by the buildings. In addition to high-rise buildings in urban areas, radio interference occurs in mountain areas, tunnels, etc., which reduces the positioning accuracy or makes positioning impossible.

Therefore, the longer the travel distance of a vehicle in which the measurement accuracy of the observed value in the positioning process (step S01) is lower than the second reference value, the longer the travel trajectory calculation unit 14 sets the above-mentioned specified distance or the above-mentioned specified time in the travel trajectory calculation process (step S02). This makes the section corresponding to the travel trajectory calculated by the travel trajectory calculation unit 14 longer. Therefore, it is possible to suppress a situation in which the reliability of all past observation values in that section is low, and to maintain high reliability detection accuracy. The fourth modified example can be applied to the first to fourth embodiments.

(Fifth Modification)
In the fifth modified example, a response when the measurement accuracy of the IMU device 12 is poor will be described. For example, the measurement accuracy of the IMU device 12 deteriorates due to environmental changes such as temperature and humidity, or due to deterioration over time of the angular velocity sensor and acceleration sensor. Errors in the speed and acceleration of the vehicle deteriorate the accuracy of the relative movement amount of the vehicle and the calculation accuracy of the travel trajectory calculated based on the relative movement amount. In addition, the calculation accuracy of the travel trajectory may deteriorate even in a scene where the acceleration in the vertical direction (Z direction) such as an unpaved road or the angular velocity in the horizontal direction (around the Z axis) such as a mountain pass is large.

Then, the greater the error in the relative movement amount of the vehicle, the shorter the predetermined distance or time going back from the starting point in the traveling trajectory calculation process (step S02) is set by the traveling trajectory calculation unit 14. This makes it possible to suppress erroneous determination of reliability caused by errors in the traveling trajectory shape. The fifth modified example can be applied to the first to fourth embodiments.

The contents of the present invention have been described above in accordance with the embodiments, but the present invention is not limited to these descriptions, and it will be obvious to those skilled in the art that various modifications and improvements are possible. The descriptions and drawings that form part of this disclosure should not be understood as limiting the present invention. Various alternative embodiments, examples, and operating techniques will be apparent to those skilled in the art from this disclosure.

The present invention naturally includes various embodiments not described here. Therefore, the technical scope of the present invention is determined only by the invention-specific matters related to the scope of the claims that are appropriate from the above explanation.

This application claims priority to Patent Application No. 2022-160259, filed with the Japan Patent Office on October 4, 2022, the entire disclosure of which is incorporated herein by reference.

REFERENCE SIGNS

LIST

1, 2 Self-position estimation device 11 Positioning unit 14 Travel trajectory calculation unit 15 Reliability detection unit 16 Judgment history storage unit (reliability storage unit)
17 Self-position estimation unit S01 Positioning process S02, S02-1, S02-2 Travel trajectory calculation process S03, S03-1, S03-2 Reliability detection process S04, S04-1, S04-2 Reliability storage process S05, S05-1, S05-2 Self-position estimation process

Claims

A positioning process for measuring the position and angle of a moving object as an observation value of a detection target from a signal received from an artificial satellite;
a travel trajectory calculation process for calculating a travel trajectory of the moving body going back a predetermined distance from an observation value of the detection target at a predetermined time, or a travel trajectory of the moving body going back a predetermined time from the observation value of the detection target at the predetermined time, based on a relative movement amount of the moving body;
a reliability detection process for detecting reliability of the observation value of the detection target at the predetermined time based on a deviation between the travel locus calculated in the travel locus calculation process and a past observation value in a section corresponding to the travel locus, and a reliability stored in association with the past observation value;
a reliability storage process for storing the observed value of the detection target at the predetermined time and the detected reliability in association with each other;
a self-location estimation process for estimating a self-location of the moving body based on an observed value having a reliability equal to or greater than a first predetermined value.
In the reliability detection process,
extracting the past observations associated with a confidence level higher than a first reference value;
determining whether the deviation between the extracted past observation value and the travel trajectory is equal to or smaller than a threshold;
2. The self-position estimation method according to claim 1, wherein the reliability of the observation value of the detection target is detected to be higher when the proportion of the number of past observation values for which the deviation is determined to be equal to or less than the threshold value is greater than the total number of past observation values in the section corresponding to the driving trajectory.
In the reliability detection process,
weighting each of the deviations according to the reliability of the past observation value corresponding to the deviation;
2. The method for estimating a position according to claim 1, wherein the smaller the average value of the deviation calculated by weighting is, the higher the reliability of the observed value of the detection target is detected.
The self-location estimation method according to claim 2, wherein in the reliability detection process, the threshold value is set to a larger value the longer the distance going back from the observation value of the detection target in the past observation value in the section corresponding to the driving trajectory, or the longer the time going back from the time when the observation value of the detection target was measured.
The self-position estimation method according to claim 3, wherein in the reliability detection process, the longer the distance going back from the observation value of the detection target to the past observation value in the section corresponding to the driving trajectory, or the longer the time going back from the time when the observation value of the detection target was measured, the shorter the calculated deviation.
In the positioning process, a position and an angle of the moving object are measured as an observation position of a detection target and an observation angle of the detection target;
In the travel trajectory calculation process, a travel trajectory of an angle of the moving body is calculated based on an observation angle of the detection target at the predetermined time, based on a relative movement amount of the moving body;
In the reliability detection process, an angle reliability of the observation angle of the detection target at the predetermined time is detected based on a deviation between the travel trajectory of the angle and a past observation angle in a section corresponding to the travel trajectory of the angle, and an angle reliability associated with the past observation angle;
In the reliability storage process, the observation angle of the detection object at the predetermined time and the detected angle reliability are stored in association with each other;
In the self-position estimation process, an angle of the moving body is estimated based on an observation angle having an angle reliability equal to or greater than a third predetermined value;
in the running trajectory calculation process, a running trajectory of the position of the moving body is calculated based on a relative movement amount of the moving body from the observed position of the detection target at the predetermined time and the angle of the moving body estimated in the self-position estimation process;
In the reliability detection process, a position reliability of the observation position of the detection target at the predetermined time is detected based on a deviation between the travel path of the position and a past observation position in a section corresponding to the travel path of the position, and a position reliability associated with the past observation position;
In the reliability storage process, the observed position of the detection target at the predetermined time is associated with the detected position reliability and stored;
2. The self-location estimation method according to claim 1, wherein, in the self-location estimation process, the position of the moving body is estimated based on an observed position where the position reliability is equal to or greater than a fourth predetermined value and an estimated angle of the moving body, and the estimated angle of the moving body and the position of the moving body are output as the self-location of the moving body.
The self-position estimation method according to claim 1, wherein the lower the measurement accuracy of the observed value in the positioning process, the longer the predetermined distance or the predetermined time is set in the travel trajectory calculation process.
The self-position estimation method according to claim 1, wherein, in the travel trajectory calculation process, the greater the error in the relative movement amount of the moving body, the shorter the predetermined distance or the predetermined time is set.
a positioning unit that measures the position and angle of a moving object as an observation value of a detection target from a signal received from an artificial satellite;
a travel trajectory calculation unit that calculates a travel trajectory of the moving body going back a predetermined distance from an observation value of the detection object at a predetermined time, or a travel trajectory of the moving body going back a predetermined time from the observation value of the detection object at the predetermined time, based on a relative movement amount of the moving body;
a reliability detection unit that detects reliability of the observation value of the detection target at the predetermined time based on a degree of deviation between the travel locus calculated by the travel locus calculation unit and a past observation value in a section corresponding to the travel locus, and a reliability stored in association with the past observation value;
a reliability storage unit that stores the observed value of the detection target at the predetermined time and the detected reliability in association with each other;
a self-position estimation unit that estimates a self-position of the moving object based on an observed value having a reliability equal to or greater than a first predetermined value.