Self-position estimation method and self-position estimation device

Abstract

To accurately detect the reliability of observed positioning values obtained from signals received from artificial satellites.SOLUTION: The self-position estimation method 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 (S01), 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 calculates 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 the relative movement amount of the moving body (S02), detects the reliability of the observation value of the detection target at the predetermined time based on the degree of deviation between the calculated travel trajectory and past observation values in a section corresponding to the travel trajectory, and the reliability stored in association with the past observation value (S03), associates the observation value of the detection target at the predetermined time with the detected reliability and stores it (S04), and estimates the self-position of the moving body based on the observation value whose reliability is equal to or greater than a first predetermined value (S05).SELECTED DRAWING: Figure 2

Description

[Technical Field]

[0001] The present invention relates to a self-location estimation method and a self-location estimation device. [Background technology]

[0002] The abnormal value determination device disclosed in Patent Document 1 determines a predetermined time so that the difference between the cumulative error of velocity information detected by an inertial navigation system and the pseudorange error based on GPS reception information falls within a predetermined range.The device then estimates the position of a moving object based on the velocity information and pseudorange at each time within the predetermined time, and determines whether the pseudorange is abnormal based on the residual between the estimated position and the pseudorange. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] Japanese Patent Application Laid-Open No. 2012-207919 Summary of the Invention [Problem to be solved by the invention]

[0004] However, since the standard for anomaly detection is the position of the moving object estimated from the GPS reception information and the information detected by the inertial navigation system, the standard for anomaly detection itself is affected by outliers in the GPS reception information, and there are cases where anomalies in the pseudorange are erroneously detected.

[0005] The present invention has been made in consideration of the above-mentioned problems, and its purpose is to provide a self-position estimation method and a self-position estimation device that can accurately detect the reliability of observation values ​​determined from signals received from artificial satellites. [Means for solving the problem]

[0006] In order to solve the above-mentioned problems, a self-location estimation method according to one embodiment of the present invention measures the position and angle of a moving body from a signal received from a satellite as an observation value of a detection target, calculates a travel trajectory of the moving body that goes back a predetermined distance from the observation value of the detection target at a predetermined time, or a travel trajectory of the moving body that goes 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 a 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 values, associates the observation value of the detection target at a predetermined time with the detected reliability, and stores it, and estimates the self-location of the moving body based on observation values ​​whose reliability is equal to or greater than a first predetermined value. [Effects of the Invention]

[0007] According to the present invention, it is possible to accurately detect the reliability of observed values ​​determined from signals received from artificial satellites. [Brief explanation of the drawings]

[0008] [Figure 1] FIG. 1 is a block diagram showing the configuration of a self-position estimation device 1 according to the first to third embodiments and their modifications. [Figure 2] FIG. 2 is a flowchart showing a self-location estimation method using the self-location estimation device 1 of FIG. 1, which is a self-location estimation method according to the first to third embodiments and their modifications. [Figure 3] FIG. 3 is a flowchart showing a specific processing example (second embodiment) of step S03 in the flowchart of FIG. [Figure 4] FIG. 4 is a flowchart showing a specific processing example (third embodiment) of step S03 in the flowchart of FIG. [Figure 5] FIG. 5 is a block diagram showing the configuration of a self-position estimation device 2 according to the fourth embodiment and its modified example. [Figure 6]FIG. 6 is a flowchart showing a self-location estimation method using the self-location estimation device 2 of FIG. 5, which is a self-location estimation method according to the fourth embodiment and its modified example. [Figure 7] Figure 7 is a conceptual diagram showing the vehicle position (Pgn(t)) as an observation value to be detected, 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 distances (D(t-1), D(t-2), ...) as examples of deviations. DETAILED DESCRIPTION OF THE INVENTION

[0009] Next, an embodiment of the present invention will be described in detail with reference to the drawings. In the description, the same components are designated by the same reference numerals and redundant description will be omitted.

[0010] (First embodiment) The configuration of a self-position estimation device 1 according to multiple embodiments including the first embodiment will be described with reference to Fig. 1. The self-position estimation device 1 includes a positioning unit 11, a traveling 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 used as an example of a moving body, but the present invention can also be applied to moving bodies other than vehicles that do not have wheels, such as ships, airplanes, and rockets.

[0011] The positioning unit 11 measures the position and angle of a vehicle (an example of a moving object) as an observation value of a detection target from signals received from artificial satellites. The positioning unit 11 determines the position and angle of the vehicle using a GNSS (Global Navigation Satellite System). To this end, the positioning unit 11 includes, for example, a GPS (Global Positioning System) receiver that receives signals from artificial satellites that make up 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 signals received by the receiver. The "observation value of a detection target" refers to the observation value of a target whose reliability is to be detected. For example, the most recent observation value among a time series of multiple observation values ​​repeatedly measured by the positioning unit 11 is the observation value of the detection target.

[0012] 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 at 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 starting point in the opposite direction to the traveling direction of the vehicle, using the latest observation value as the starting point, or a travel trajectory of the vehicle going back a predetermined time from the time when the latest observation value was measured (predetermined time).

[0013] The travel trajectory can be calculated based on the relative movement amount of the vehicle. In this case, the self-localization estimation device 1 may further include an IMU (Inertial Measurement Unit) 12. The IMU 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 calculates the speed of the translational motion by integrating the acceleration of the translational motion over time. The travel trajectory, which serves as a detection standard for the reliability of the observation value, is no longer affected by the observation value measured by the positioning unit 11, making it possible to suppress erroneous determination of reliability due to outliers in the observation value.

[0014] Specifically, position and angle information is first obtained as observed values ​​of the detection target. Using the observed value of the detection target as a starting point, the position and angle at the immediately preceding time are estimated by subtracting from the starting point the velocity and angular velocity at the time closest to the time at which the observed value of the detection target was measured. Next, the position and angle information at the time two times prior are estimated by subtracting from the estimated position and angle the velocity and angular velocity at the time closest to the time corresponding to the estimated value. This subtraction is repeated within a predetermined distance or elapsed time from the starting point to calculate the travel trajectory of position and angle. As described above, because the observed value is used only as a starting point in calculating the travel trajectory, the travel trajectory, which serves as a detection standard for the reliability of the observation value, is not affected by the observation value measured by the positioning unit 11, thereby preventing erroneous determination of reliability due to outliers in the observation value. In other words, by calculating the vehicle's travel trajectory based on the vehicle's relative movement amount, such as the vehicle's velocity and angular velocity, the travel trajectory is not affected by unreliable observation values ​​measured by the positioning unit 11.

[0015] 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 past observation values ​​in the section corresponding to the driving trajectory, and the reliability stored in the judgment history memory unit 16 in association with the past observation values.

[0016] Specifically, the reliability detection unit 15 extracts past observation values ​​at each time within a section corresponding to the travel trajectory from the determination history storage unit 16, and calculates the degree of deviation between the extracted past observation value and the travel trajectory. For example, if the observation value is the vehicle position, the Euclidean distance between the travel trajectory of the vehicle position at each time and the observation value of the vehicle position (observation position) is calculated as the degree of deviation. On the other hand, if the observation value is the vehicle angle, the angle difference between the travel trajectory of the vehicle angle at each time and the observation value of the vehicle angle (observation angle) is calculated as the degree of deviation. The "past observation value" refers to an observation value measured earlier than the observation value to be detected. The "past observation value" may be limited to an observation value for which reliability has been detected by the reliability detection unit 15. The past observation values ​​for which reliability has been detected are stored in the determination 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.

[0017] The determination history storage unit 16 stores the observed value of the detection target at a predetermined time and the reliability detected by the reliability detection unit 15 in association with each other.

[0018] 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 the observed values ​​(the 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 a 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.

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

[0020] Of the self-location estimation device 1 shown in FIG. 1, the storage unit 13, the travel trajectory calculation unit 14, the reliability detection unit 15, the determination history storage 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) included in the self-location estimation device 1. The storage unit 13 and the determination history storage unit 16 can be realized by memory or an external storage device such as a hard disk drive connected to the microcomputer. Note that, while an example is shown in which the multiple information processing circuits included in the self-location estimation device 1 are realized by software, it is also possible to configure the information processing circuits using dedicated hardware. The multiple information processing circuits may also be configured as separate hardware.

[0021] 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 cycle. FIG. 7 is a conceptual diagram showing the vehicle position (Pgn(t)) as an observation value of the detection target, 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 travel trajectory 31, and 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 that traces 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 that traces 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 degree of deviation between the travel trajectory 31 calculated in the travel trajectory calculation process and past observed values ​​within the section (window size) 32 corresponding to the travel trajectory 31, and the reliability stored in association with the past observed values. The "degree of 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 vehicle position (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 vehicle position (Pgn(t-1), Pgn(t-2), ...). Proceeding to step S04 (reliability storage process), the determination 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 self-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 in the determination history storage unit 16 as past observed values.The reliability associated with the previous observations can be used to determine the reliability of the most recent observation, for example.

[0022] As described above, the self-localization device 1 in FIG. 1 and the self-localization method in FIG. 2 use the vehicle's travel trajectory, which does not depend on GNSS observation values, as a criterion for determining reliability. This makes it possible to suppress erroneous determination of reliability caused by deterioration in the accuracy of the determination criterion due to outliers in GNSS observation values. Furthermore, when determining the reliability of the latest observation values, the reliability of past observation values ​​is taken into consideration. This makes it possible to accurately determine reliability even when a certain amount of error occurs in the GNSS observation values.

[0023] (Second embodiment) In the second embodiment, a specific example of operation of the reliability detection unit 15 in Fig. 1 and a specific example of processing 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 the same as those in Figs. 1 and 2, and therefore their description will be omitted. The reliability detection unit 15 in Fig. 1 performs all of the steps S301 to S310 described below.

[0024] First, in step S301, the counter included 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" refers to the section corresponding to the travel trajectory calculated in the travel trajectory calculation process (step S302). 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), it proceeds to step S304. If the reliability is equal to or lower than the first reference value (NO in step S303), it proceeds to step S306. In step S304, the deviation between the past observation value and the travel trajectory is calculated, and it is determined whether the deviation is equal to or lower than a threshold. If the deviation is equal to or lower than the threshold (YES in step S304), it proceeds to step S305. If the deviation is greater than the threshold (NO in step S304), it proceeds to step S306. In step S305, the counter is incremented by 1. Then, in step S306, it is determined whether or not all past observation values ​​within the window size have been selected. If all past observation values ​​have not been selected (NO in step S306), the process returns to step S302, where one past observation value within the window size that has not yet been selected is selected, and steps S303 to S305 are performed.

[0025] If all the observed values ​​have been selected (YES in step S306), the process proceeds to step S307, where the ratio of the number of past observed values ​​whose deviations were determined to be equal to or less than the threshold in step S304 to the total number of past observed values ​​in the section (window size) corresponding to the travel 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 observed values ​​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 observed values ​​of the detection target measured in step S01 is low. According to steps S308 to S310, the reliability of the observed values ​​of the detection target can be detected based on the ratio of the number of past observed values ​​whose deviations were determined to be equal to or less than the threshold to the total number of past observed values ​​within the window size. Here, an example is shown in which the reliability is classified into two values, but multi-value classification is also possible. That is, the larger the ratio, the higher the reliability of the observed value of the detection target can be detected.

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

[0027] (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 entirely the same as those in Figs. 1 and 2, and therefore description thereof will be omitted. The reliability detection unit 15 in Fig. 1 performs all of the operations in steps S321 to S326 described below.

[0028] In step S321, the deviation between the past observation values ​​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. Proceeding to step S322, 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. Proceeding to step S323, the average value of the weighted deviations (hereinafter referred to as the "weighted average value") is calculated. Proceeding to step S324, it is determined whether the weighted average value is equal to or less than a threshold. If the weighted average value is equal to or less than the threshold (YES in step S324), proceed to step S325, where it is determined that the reliability of the observation values ​​of the detection target measured in step S01 is high. If the weighted average value is greater than the threshold (NO in step S324), proceed to step S326, where it is determined that the reliability of the observation values ​​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 past observed values ​​within the window size and the traveling trajectory. Here, an example is shown in which the reliability is classified into two values, but multi-value classification is also possible. In other words, the smaller the weighted average value, the higher the reliability of the observed value of the detection target can be detected.

[0029] The reliability detection unit 15 weights the degree of deviation between the travel trajectory and past observation values ​​in a section corresponding to the travel trajectory according to the reliability stored for the past observation value. 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 value. As a result, even if the degree of deviation between a certain past observation value and the travel trajectory is large, if the reliability of that past observation value is low, the weight assigned to the deviation is lighter, and 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, the reliability can be accurately determined without being affected by that observation value.

[0030] (Fourth embodiment) In the first to third embodiments, an example was described in which the reliability (hereinafter referred to as "position reliability" and "angle reliability") of both the vehicle position (hereinafter referred to as "observed position") and angle (hereinafter referred to as "observed angle") as observed values ​​of the detection target were simultaneously detected, and the vehicle position and angle were simultaneously estimated. However, if there is an error in the observed angle used as a starting point when calculating the traveling trajectory of the position, the traveling trajectory of the position may be calculated incorrectly, which may ultimately lead to an erroneous determination of the position reliability. Therefore, in the fourth embodiment, an example will be described in which the position reliability and the angle reliability are detected separately. Specifically, an example will be described 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.

[0031] The configuration of a 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 the IMU device 12 in Fig. 5 are the same as those in Fig. 1, and therefore a description thereof will be omitted here.

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

[0033] The angle storage 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 traveling trajectory calculation unit 14a, the angle reliability detection unit 15a, and the angle determination history storage unit 16a.

[0034] 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 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 it is possible to suppress erroneous determination of reliability due to outliers in the observation angle.

[0035] Specifically, the system first estimates the vehicle's angle at the immediately preceding time by subtracting from the observed angle of the target vehicle the angular velocity at the time closest to the time at which the observed angle of the target vehicle was measured. Next, the system estimates the angle at the time two times prior by subtracting from the estimated angle the angular velocity at the time immediately preceding the time corresponding to the estimated angle. This subtraction is repeated as long as the vehicle is within a predetermined distance from the origin or within a predetermined time since the detection time. Because the observed angle is used only as the origin in calculating the angle's traveling trajectory, the angle's traveling trajectory, which is the basis for detecting the reliability of the observed angle, is not affected by the observed angle measured by the positioning unit 11, thereby preventing erroneous determination of the angle's reliability due to outliers in the observed angle. In other words, calculating the vehicle's traveling trajectory based on the vehicle's relative movement distance eliminates the influence of the unreliable observed angle measured by the positioning unit 11.

[0036] 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 angle travel trajectory calculation unit 14a and a past observation angle in a section corresponding to the angle travel trajectory, and the angle reliability associated with the past observation angle and stored in angle determination history storage unit 16a. Specifically, angle reliability detection unit 15a extracts, from angle determination history storage unit 16a, past observation angles at each time in the section corresponding to the angle travel trajectory, and calculates the angle difference between the extracted past observation angle and the angle travel trajectory as the angle deviation.

[0037] The angle reliability detection unit 15a can detect the angle reliability of the observation angle 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 angle of the vehicle.

[0038] The angle determination history storage unit 16a stores the observation angle of the detection target and the detected angle reliability in association with each other.

[0039] The angle estimation unit 17a estimates the vehicle angle based on the observation angle whose reliability is equal to or greater than a third predetermined value. The angle estimation unit 17a can estimate the angle using known techniques. For example, the angle estimation unit 17a estimates a likely vehicle angle using an estimation method such as a Kalman filter based on the observation angle (absolute angle of the vehicle) and the relative movement amount of the vehicle (vehicle speed and angular velocity). Here, among the observation angles, observation angles whose reliability is detected to be low can be eliminated in advance or set so that their contribution to the angle estimation process is reduced.

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

[0041] The position storage unit 13b stores data indicating the speed and angular velocity of the vehicle 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 storage unit 16b.

[0042] 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 it is possible to suppress erroneous determination of reliability due to outliers of the observed position.

[0043] Specifically, the system first estimates the vehicle's position and angle at the immediately preceding time by subtracting from the origin the speed and angular velocity at the time closest to the time at which the target's position was measured. Next, the system estimates the vehicle's position and angle at the immediately preceding time by subtracting from the estimated position and angle the speed and angular velocity at the time immediately preceding the time corresponding to the estimated position and angle. This subtraction is repeated as long as the vehicle is within a predetermined distance from the origin or within a predetermined time since the detection time. The system calculates the vehicle's travel trajectory. Because the observed position is used only as the origin in calculating the vehicle's travel trajectory, the vehicle's travel trajectory, which is used as a criterion for detecting the reliability of the observed position, is not affected by the observed position measured by the positioning unit 11. This prevents erroneous determination of the reliability of the position due to outliers in the observed position. In other words, calculating the vehicle's travel trajectory based on the vehicle's relative movement distance eliminates the influence of unreliable observed positions measured by the positioning unit 11.

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

[0045] 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.

[0046] The position determination history storage unit 16b stores the observed position of the detection target and the detected position reliability in association with each other.

[0047] The position estimation unit 17b estimates the position of the vehicle based on the observed position having a reliability 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 techniques. For example, the position estimation unit 17b estimates a likely vehicle position using an estimation method such as a Kalman filter 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). Here, observed positions detected to have low reliability can be excluded in advance or set to have a low contribution to the position estimation process.

[0048] 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 self-position of the vehicle.

[0049] 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 signals received from artificial satellites as observation values ​​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 vehicle's angle from the observed 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 observed angle of the detection target based on the traveling trajectory of the angle, the deviation from a past observed angle in a section corresponding to the traveling trajectory of the angle, and the angle reliability associated with the past observed angle. Proceeding to step S04-1 (reliability storage process), the angle determination history storage unit 16a associates and stores the observed angle of the detection target with the detected angle reliability. Proceeding to step S05-1 (self-position estimation process), angle estimation unit 17a estimates the angle of the vehicle based on the observed angle whose angle reliability is equal to or greater than a third predetermined value.

[0050] Proceeding to step S02-2 (traveling trajectory calculation process), the position traveling trajectory calculation unit 14b calculates the traveling 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 traveling trajectory of the position, the deviation from a past observed position in a section corresponding to the traveling 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 the observed position of the detection target with the detected position reliability and stores it. 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 object 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 self-position of the vehicle.

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

[0052] Information on the vehicle's position and angle is required as the starting point used when calculating the travel 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 inaccurate, the travel 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 vehicle angle is estimated. Then, the estimated angle is used to calculate the travel trajectory of the position, the position reliability is detected, and the vehicle position is estimated. This makes it possible to suppress erroneous determination of the position reliability caused by an erroneous position travel trajectory.

[0053] (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 can also be applied to each embodiment.

[0054] In the first modification, we will explain how to deal with the case where there is insufficient information about the reliability of past observation values. For example, immediately after positioning unit 11 starts positioning, 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 about the reliability of past observation values.

[0055] Therefore, when the number of past observation values ​​not associated with reliability among past observation values ​​in a section (within the window size) corresponding to the traveling trajectory is equal to or greater than a second predetermined value, the reliability detection unit 15 regards the past observation values ​​not associated with reliability as having a higher reliability than the first reference value shown in step S303, 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 about the reliability of past observation values, such as immediately after the start of the positioning process (S01). The first modification can be applied to the first to fourth embodiments.

[0056] (Second Modification) In the second modification, a modification in which the threshold 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 in the position or angle of the vehicle included in the traveling trajectory calculated in step S02. Therefore, in the reliability detection process (step S03), the reliability detection unit 15 sets a larger threshold used in step S304 of FIG. 3 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 for past observation values ​​in the section (within the window size) corresponding to the traveling trajectory. This makes it possible to suppress erroneous determination of reliability due to errors in the position or angle included in the traveling trajectory. The first modification can be applied to the second and fourth embodiments.

[0057] (Third Modification) In the third modified example, a modification is described in which the deviation degree is controlled according to the distance or time going back from the starting point. The longer the distance going back from the observed value of the detection target or the longer the time going back from the time when the observed value of the detection target was measured, the larger the error in the position or angle of the vehicle included in the traveling trajectory calculated in step S02. Therefore, in the reliability detection process (step S03), the reliability detection unit 15 calculates a smaller deviation degree the longer the distance going back from the observed value of the detection target or the longer the time going back from the time when the observed value of the detection target was measured for past observed values ​​in the section (within the window size) corresponding to the traveling trajectory. This makes it possible to reduce erroneous determination of reliability due to errors in the position or angle included in the traveling trajectory. The third modified example can be applied to the first to fourth embodiments.

[0058] (Fourth Modification) In the fourth modification, we will explain how to deal with cases where the measurement accuracy of the observed values ​​in the positioning process (step S01) is poor. It is generally known that in urban areas where high-rise buildings line the streets, positioning accuracy deteriorates significantly due to satellite radio waves being reflected by buildings, etc. In addition to high-rise buildings in urban areas, radio wave interference occurs in mountainous areas, tunnels, etc., which reduces positioning accuracy or makes positioning impossible.

[0059] Therefore, the longer the travel distance of a vehicle for 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 predetermined distance or the predetermined time in the travel trajectory calculation process (step S02). This increases the length of the section corresponding to the travel trajectory calculated by the travel trajectory calculation unit 14. This prevents a situation in which all past observation values ​​in that section have low reliability, and makes it possible to maintain high reliability detection accuracy. The fourth modified example can be applied to the first to fourth embodiments.

[0060] (Fifth Modification) In the fifth modification, we will explain how to deal with cases where the measurement accuracy of the IMU device 12 is poor. 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 vehicle's speed and acceleration deteriorate the accuracy of the vehicle's relative movement amount and the calculation accuracy of the travel trajectory calculated based on the relative movement amount. Furthermore, the calculation accuracy of the travel trajectory may also deteriorate in situations where the acceleration in the vertical direction (Z direction), such as on an unpaved road, or the angular velocity in the horizontal direction (around the Z axis), such as on a mountain pass, is large.

[0061] Therefore, the larger 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 due to errors in the traveling trajectory shape. The fifth modified example can be applied to the first to fourth embodiments.

[0062] Although the present invention has been described above based on the embodiments, it will be apparent to those skilled in the art that the present invention is not limited to these descriptions and 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 become apparent to those skilled in the art from this disclosure.

[0063] The present invention naturally includes various embodiments not described herein. Therefore, the technical scope of the present invention is defined only by the invention-specifying matters according to the scope of the claims that are appropriate from the above description. [Explanation of symbols]

[0064] 1, 2 Self-position estimation device 11 Positioning unit 14. Driving trajectory calculation unit 15 Reliability detection unit 16. Judgment history memory unit (reliability memory unit) 17 Self-position estimation part S01 Positioning processing S02, S02-1, S02-2 Driving trajectory calculation process S03, S03-1, S03-2 Reliability detection process S04, S04-1, S04-2 Reliability memory processing S05, S05-1, S05-2 Self-position estimation process

Claims

1. 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 that calculates a travel trajectory of the moving body going back a predetermined distance from the 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 process for detecting the reliability of the observation value of the detection target at the predetermined time based on a degree of 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 object based on the observed value having a reliability equal to or greater than a first predetermined value; A self-location estimation method comprising:

2. In the reliability detection process, extracting the past observations associated with a reliability higher than a first reference value; determining whether the deviation between the extracted past observation value and the travel trajectory is equal to or less than a threshold value; The reliability of the detection target observation value is detected to be higher as the ratio of the number of past observation values ​​whose deviation is determined to be equal to or less than the threshold to the total number of past observation values ​​in the section corresponding to the travel locus increases. The method for estimating a self-location according to claim 1 .

3. In the reliability detection process, weighting each of the deviations according to the reliability of the past observation value corresponding to the deviation; The smaller the weighted and calculated average value of the deviation is, the higher the reliability of the observed value of the detection target is detected. The method for estimating a self-location according to claim 1 .

4. In the reliability detection process, the threshold value is set to a larger value as the distance going back from the observation value of the detection target to the past observation values ​​in the section corresponding to the travel locus is longer or the time going back from the time when the observation value of the detection target was measured is longer. The self-location estimation method according to claim 2 .

5. 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 travel locus, or the longer the time going back from the time when the observation value of the detection target was measured, the shorter the deviation degree is calculated to be. The self-location estimation method according to claim 3 .

6. In the positioning process, a position and an angle of the moving body are measured as an observation position and an observation angle of the detection target; In the travel locus calculation process, a travel locus of the angle of the moving body is calculated based on the 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, detecting angle reliability of the observation angle of the detection target at the predetermined time based on a deviation between the travel locus of the angle and a past observation angle in a section corresponding to the travel locus 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 whose angle reliability is equal to or greater than a third predetermined value; in the travel trajectory calculation process, calculating a travel trajectory of the position of the moving body 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 degree of deviation between the travel locus of the position and a past observation position in a section corresponding to the travel locus 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 and the detected position reliability are stored in association with each other; In the self-position estimation process, a 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 a self-position of the moving body. The method for estimating a self-location according to claim 1 .

7. 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 locus calculation process.

8. 2. The method for estimating a position according to claim 1, wherein the predetermined distance or the predetermined time is set shorter in the travel trajectory calculation process as the error in the relative movement amount of the mobile object increases.

9. a positioning unit that measures the position and angle of the moving object from signals received from an artificial satellite as observed values ​​of the detection target; 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 the 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 that is 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 whose reliability is equal to or greater than a first predetermined value; A self-location estimation device having the above configuration.

Images