WO2024075498A1 - 自己位置推定方法及び自己位置推定装置 - Google Patents

自己位置推定方法及び自己位置推定装置 Download PDF

Info

Publication number
WO2024075498A1
WO2024075498A1 PCT/JP2023/033647 JP2023033647W WO2024075498A1 WO 2024075498 A1 WO2024075498 A1 WO 2024075498A1 JP 2023033647 W JP2023033647 W JP 2023033647W WO 2024075498 A1 WO2024075498 A1 WO 2024075498A1
Authority
WO
WIPO (PCT)
Prior art keywords
reliability
angle
observation
value
detection target
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Ceased
Application number
PCT/JP2023/033647
Other languages
English (en)
French (fr)
Inventor
佳奈子 酒井
千加夫 土谷
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nissan Motor Co Ltd
Original Assignee
Nissan Motor Co Ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nissan Motor Co Ltd filed Critical Nissan Motor Co Ltd
Publication of WO2024075498A1 publication Critical patent/WO2024075498A1/ja
Anticipated expiration legal-status Critical
Ceased legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
    • G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled

Definitions

  • 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.
  • 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 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.
  • 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.
  • 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.
  • 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).
  • 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.
  • 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.
  • position and angle information is obtained as the observation value of the detection target.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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), ).
  • the judgment history storage unit 16 associates the observed value of the detection target with the detected reliability and stores it.
  • 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.
  • 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.
  • the reliability associated with this past observed value for example, the reliability of the latest observed value can be detected.
  • 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.
  • 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.
  • 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.
  • 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.
  • step S304 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.
  • 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.
  • 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.
  • step S308 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.
  • step 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.
  • 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.
  • 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.
  • step S324 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.
  • 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.
  • 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.
  • 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.
  • position reliability and angle reliability
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • an estimation method such as a Kalman filter is used to estimate a plausible vehicle angle.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • an estimation method such as a Kalman filter is used to estimate a likely vehicle position.
  • 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.
  • 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.
  • step S01 positioning process
  • step S02-1 traveling trajectory calculation process
  • step S02-1 traveling trajectory calculation 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.
  • 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).
  • 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.
  • 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.
  • 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.
  • the self-position estimation device 2 outputs the estimated vehicle angle and vehicle position as the vehicle's self-position.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • 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.
  • step S01 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.
  • the fourth modified example can be applied to the first to fourth embodiments.
  • 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.
  • 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.
  • step S02 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.

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

自己位置推定方法は、人工衛星から受信する信号から移動体の位置及び角度を検出対象の観測値として計測し（Ｓ０１）、所定時刻における検出対象の観測値から所定距離遡る走行軌跡、又は所定時刻における検出対象の観測値を所定時刻から所定時間遡る移動体の走行軌跡を、移動体の相対的な移動量に基づいて算出し（Ｓ０２）、算出された走行軌跡と走行軌跡に対応する区間における過去の観測値との乖離度、及び過去の観測値に関連付けて記憶されている信頼度に基づいて、所定時刻の検出対象の観測値の信頼度を検出し（Ｓ０３）、所定時刻の検出対象の観測値と検出された信頼度とを関連付けて記憶し（Ｓ０４）、信頼度が第１所定値以上の観測値に基づいて移動体の自己位置を推定する（Ｓ０５）。

Description

自己位置推定方法及び自己位置推定装置
　本開示は、自己位置推定方法及び自己位置推定装置に関する。
　特許文献１に開示された異常値判定装置は、慣性航法装置で検出された速度情報の累積誤差とＧＰＳ受信情報に基づく擬似距離誤差との差が所定範囲内の値となるように所定時間を決定する。そして、この所定時間内の各時刻における速度情報及び擬似距離に基づいて移動体の位置を推定し、推定した位置と擬似距離との残差に基づいて、擬似距離の異常を判定する。
特開２０１２－２０７９１９号公報
　しかし、異常判定の基準として、ＧＰＳ受信情報と慣性航法装置で検出された情報とから推定された移動体の位置を用いている。このため、異常判定の基準自体がＧＰＳ受信情報の外れ値の影響を受けて、擬似距離の異常を誤判定してしまう場合がある。
　本開示は、上記課題に鑑みてなされたものであり、その目的は、人工衛星から受信する信号から測位された観測値の信頼度を正確に検出することができる自己位置推定方法及び自己位置推定装置を提供することにある。
　1又はそれ以上の実施形態に係る自己位置推定方法は、人工衛星から受信する信号から移動体の位置及び角度を検出対象の観測値として計測し、所定時刻における検出対象の観測値から所定距離遡る走行軌跡、又は所定時刻における検出対象の観測値を所定時刻から所定時間遡る移動体の走行軌跡を、移動体の相対的な移動量に基づいて算出し、算出された走行軌跡、走行軌跡に対応する区間における過去の観測値との乖離度、及び過去の観測値に関連付けて記憶されている信頼度に基づいて、所定時刻の検出対象の観測値の信頼度を検出し、所定時刻の検出対象の観測値と検出された信頼度とを関連付けて記憶し、信頼度が第１所定値以上の観測値に基づいて移動体の自己位置を推定する。
　本開示によれば、人工衛星から受信する信号から測位された観測値の信頼度を正確に検出することができる。
図１は、第１～第３実施形態及びその変形例に係る自己位置推定装置１の構成を示すブロック図である。 図２は、図１の自己位置推定装置１を用いた自己位置推定方法であって、第１～第３実施形態及びその変形例に係る自己位置推定方法を示すフローチャートである。 図３は、図２のフローチャートにおけるステップＳ０３の具体的な処理例（第２実施形態）を示すフローチャートである。 図４は、図２のフローチャートにおけるステップＳ０３の具体的な処理例（第３実施形態）を示すフローチャートである。 図５は、第４実施形態及びその変形例に係る自己位置推定装置２の構成を示すブロック図である。 図６は、図５の自己位置推定装置２を用いた自己位置推定方法であって、第４実施形態及びその変形例に係る自己位置推定方法を示すフローチャートである。 図７は、検出対象の観測値としての車両の位置（Ｐｇｎ（ｔ））と、走行軌跡３１に対応する区間（ウィンドウサイズ）３２における過去の観測値の一例としての車両の位置（Ｐｇｎ（ｔ）、Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）と、乖離度の一例としての距離（Ｄ（ｔ－１）、Ｄ（ｔ－２）、・・・）を示す概念図である。
　次に、図面を参照して、１又はそれ以上の実施の形態を詳細に説明する。説明において、同一のものには同一符号を付して重複する説明を省略する。
　（第１実施形態）
　図１を参照して、第１実施形態を含む複数の実施形態に係る自己位置推定装置１の構成を説明する。自己位置推定装置１は、測位部１１と、走行軌跡算出部１４と、信頼度検出部１５と、判定履歴記憶部１６（信頼度記憶部の一例）と、自己位置推定部１７と、を有する。実施形態では、移動体として車両を例示して説明するが、車両以外の船、飛行機、ロケットなど、車輪を有していない移動体にも適用可能である。
　測位部１１は、人工衛星から受信する信号から車両（移動体の一例）の位置及び角度を検出対象の観測値として計測する。測位部１１は、ＧＮＳＳ（Global Navigation Satellite System／全球測位衛星システム）を用いて車両の位置及角度を求める。このため、測位部１１は、ＧＮＳＳを構成する人工衛星から信号を受信する、例えばＧＰＳ（Global Positioning System／全地球測位システム）受信機を備え、更に、受信機が受信した受信信号からグローバルな座標系における車両の絶対位置（例えば、緯度、経度、高度）及び絶対角度（例えば、方位角、仰角）を演算する演算部を備える。「検出対象の観測値」とは、信頼度を検出する対象の観測値を意味し、例えば、測位部１１が繰り返し計測した複数の観測値の時系列の内の最新の観測値が、検出対象の観測値となる。
　走行軌跡算出部１４は、所定時刻における検出対象の観測値から所定距離遡る走行軌跡、又は検出対象の観測値を所定時刻から所定時間遡る車両の走行軌跡を、車両の相対的な移動量に基づいて算出する。例えば、走行軌跡算出部１４は、最新の観測値を起点として、起点から車両の進行方向とは逆方向に所定距離だけ遡る車両の走行軌跡、又は最新の観測値が計測された時刻（所定時刻）から過去に向かって経過時間遡る間の車両の走行軌跡を算出する。
　走行軌跡は、車両の相対的な移動量に基づいて算出することができる。この場合、自己位置推定装置１はＩＭＵ装置（Inertial Measurement Unit／慣性計測装置）１２を更に備えていても構わない。ＩＭＵ装置１２は、３次元の慣性運動（直行３軸方向の並進運動および回転運動）を検出する装置であり、並進運動を検出する加速度センサと、回転運動を検出する角速度(ジャイロ)センサと、並進運動の加速度を時間で積分して並進運動の速度を演算する演算部とを備える。観測値の信頼性の検出基準となる走行軌跡は、測位部１１が計測する観測値の影響を受けることが無くなり、観測値の外れ値による信頼性の誤判定を抑制することができる。
　具体的には、先ず、検出対象の観測値として位置と角度の情報が得られている。検出対象の観測値を起点として、起点から、検出対象の観測値を計測した時刻に最も近い時刻における速度と角速度を差し引くことにより、一つ前の時刻における位置と角度を推定する。次に、推定した位置と角度から、この推定値に対応する時刻に最も近い時刻における速度と角速度を差し引くことにより、二つ前の時刻における位置と角度情報を推定する。この差し引きを、起点から所定距離内又は経過時間内、繰り返すことで、位置及び角度の走行軌跡を算出する。以上説明したように、走行軌跡の算出において観測値は起点としてのみ用いられるため、観測値の信頼性の検出基準となる走行軌跡は、測位部１１が計測する観測値の影響を受けることが無くなり、観測値の外れ値による信頼性の誤判定を抑制することができる。換言すれば、車両の走行軌跡を車両の速度及び角速度のような車両の相対的な移動量に基づいて算出することにより、測位部１１が計測する信頼性の低い観測値の影響を受けることが無くなる。
　信頼度検出部１５は、走行軌跡算出部１４で算出された走行軌跡と走行軌跡に対応する区間における過去の観測値との乖離度、及び過去の観測値に関連付けて判定履歴記憶部１６に記憶されている信頼度に基づいて、所定時刻の検出対象の観測値の信頼度を検出する。
　具体的には、信頼度検出部１５は、走行軌跡に対応する区間内の各時刻における過去の観測値を判定履歴記憶部１６から抽出し、抽出された過去の観測値と走行軌跡との乖離度を算出する。例えば、観測値が車両の位置である場合、各時刻における車両の位置の走行軌跡と車両の位置の観測値（観測位置）とのユークリッド距離を、乖離度として算出する。一方、観測値が車両の角度である場合、各時刻における車両の角度の走行軌跡と車両の角度の観測値（観測角度）との角度差を、乖離度として算出する。「過去の観測値」とは、検出対象の観測値よりも過去に計測された観測値を意味する。「過去の観測値」は、信頼度検出部１５によって信頼度が検出された観測値に限っても構わない。信頼度が検出された過去の観測値は、後述するように、判定履歴記憶部１６に信頼度に紐づけて記憶されている。信頼度検出部１５が検出する「信頼度」は、「高い」又は「低い」のような二値分類であってもよいし、３つ以上の多値分類であっても構わない。
　判定履歴記憶部１６は、所定時刻の検出対象の観測値と信頼度検出部１５により検出された信頼度とを関連付けて記憶する。
　自己位置推定部１７は、信頼度が第１所定値以上の観測値に基づいて車両の自己位置を推定する。自己位置推定部１７は、既知の技術を用いて自己位置を推定することができる。例えば、観測値（車両の絶対位置と絶対角度）と車両の相対的な移動量（車速と角速度）に基づいて、カルマンフィルタなどの推定手法を用いて、尤もらしい車両の位置及び角度を車両の自己位置として推定する。ここで、観測値のうち低い信頼度が検出された観測値は、事前に排除するか、又は自己位置の推定処理への寄与度が小さくなるように設定することができる。
　自己位置推定装置１は、ＩＭＵ装置１２により検出された車両の速度及び角速度を示すデータと、測位部１１により計測された観測値を示すデータとを記憶する記憶部１３を更に有していてもよい。これらのデータは、走行軌跡算出部１４及び信頼度検出部１５によって利用される。
　図１に示す自己位置推定装置１のうち、記憶部１３と、走行軌跡算出部１４と、信頼度検出部１５と、判定履歴記憶部１６と、自己位置推定部１７とは、ＣＰＵ（中央処理装置）、メモリ、及び入出力部を備える汎用のコンピュータで実現可能である。マイクロコンピュータには、自己位置推定装置１として機能させるためのコンピュータプログラムがインストールされている。コンピュータプログラムを実行することにより、コンピュータは自己位置推定装置１が備える複数の情報処理回路（１４、１５、１７）として機能する。記憶部１３及び判定履歴記憶部１６は、メモリ又はマイクロコンピュータに接続されたハードディスクドライブなどの外部記憶装置により実現可能である。なおここでは、ソフトウェアによって自己位置推定装置１が備える複数の情報処理回路を実現する例を示すが、専用のハードウェアを用意して情報処理回路を構成することも可能である。また複数の情報処理回路を個別のハードウェアにより構成してもよい。
　図２及び図７を参照して、図１の自己位置推定装置１を用いた第１実施形態に係る自己位置推定方法を説明する。図２に示す動作フローは、予め定められた周期で、繰り返し実施される。図７は、検出対象の観測値としての車両の位置（Ｐｇｎ（ｔ））と、走行軌跡３１に対応する区間（ウィンドウサイズ）３２における過去の観測値の一例としての車両の位置（Ｐｇｎ（ｔ）、Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）と、乖離度の一例としての距離（Ｄ（ｔ－１）、Ｄ（ｔ－２）、・・・）を示す概念図である。ステップＳ０１（測位処理）において、測位部１１は、人工衛星から受信する信号から車両の位置Ｐｇｎ（ｔ）及び角度を検出対象の観測値として計測する。ステップＳ０２（走行軌跡算出処理）に進み、走行軌跡算出部１４は、検出対象の観測値としての車両の位置Ｐｇｎ（ｔ）から所定距離遡る走行軌跡３１、又は検出対象の観測値を所定時刻から所定時間遡る車両の走行軌跡３１を、車両の相対的な移動量に基づいて算出する。ステップＳ０３（信頼度検出処理）に進み、信頼度検出部１５は、走行軌跡算出処理で算出された走行軌跡３１と走行軌跡３１に対応する区間（ウィンドウサイズ）３２内における過去の観測値との乖離度、及び過去の観測値に関連付けて記憶されている信頼度に基づいて、検出対象の観測値の信頼度を検出する。「乖離度」とは、例えば、走行軌跡３１と測位部１１により計測された車両の各位置（Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）との距離である。距離とは、例えば、車両の各位置（Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）と車両の各位置（Ｐｇｎ（ｔ－１）、Ｐｇｎ（ｔ－２）、・・・）に最も近い走行軌跡３１上の各地点（Ｐｏｄ（ｔ－１）、Ｐｏｄ（ｔ－２）、・・・）との距離（Ｄ（ｔ－１）、Ｄ（ｔ－２）、・・・）である。ステップＳ０４（信頼度記憶処理）に進み、判定履歴記憶部１６は、検出対象の観測値と検出された信頼度とを関連付けて記憶する。ステップＳ０５（自己位置推定処理）に進み、自己位置推定部１７は、信頼度が第１所定値以上の観測値に基づいて車両の自己位置を推定する。上記した動作フローを繰り返し実行する事により、信頼度が関連付けられた複数の観測値の時系列が求められ、判定履歴記憶部１６に過去の観測値として記憶される。この過去の観測値に関連付けた信頼度を用いて、例えば最新の観測値の信頼度を検出することができる。
　以上説明したように、図１の自己位置推定装置１及び図２の自己位置推定方法は、信頼度の判定基準として、ＧＮＳＳによる観測値に依存しない車両の走行軌跡を用いている。このため、ＧＮＳＳ観測値の外れ値による判定基準の精度悪化に起因する信頼度の誤判定を抑制できる。また、最新の観測値の信頼度を判定する際に、過去の観測値の信頼度を考慮している。このため、ＧＮＳＳ観測値に一定誤差が生じている場合であっても、正確な信頼度判定が可能となる。
　（第２実施形態）
　第２実施形態では、図３を参照して、図１の信頼度検出部１５の具体的な動作例、及び図２のステップ０３（信頼度検出処理）の具体的な処理例を説明する。第２実施形態に係る自己位置推定装置１及び自己位置推定方法の全体は、図１及び図２と同じであり説明を省略する。以下に述べる全てのステップＳ３０１～Ｓ３１０の動作主体は、図１の信頼度検出部１５である。
　先ず、ステップＳ３０１において、信頼度検出部１５が備えるカウンタをゼロにリセットする。ステップＳ３０２に進み、ウィンドウサイズ内の過去の観測値を１つ選択する。「ウィンドウサイズ」とは、走行軌跡算出処理（ステップ０２）で算出された走行軌跡に対応する区間を意味する。ステップＳ３０３に進み、選択した過去の観測値の信頼度が第１基準値よりも高いか否かを判断する。信頼度が第１基準値よりも高い場合（ステップＳ３０３でＹＥＳ）、ステップＳ３０４へ進み、信頼度が第１基準値以下である場合（ステップＳ３０３でＮＯ）、ステップＳ３０６へ進む。ステップＳ３０４において、過去の観測値と走行軌跡との乖離度を算出し、乖離度が閾値以下か否かを判定する。乖離度が閾値以下である場合（ステップＳ３０４でＹＥＳ）、ステップＳ３０５に進み、乖離度が閾値よりも大きい場合（ステップＳ３０４でＮＯ）、ステップＳ３０６に進む。ステップＳ３０５において、カウンタを１足し合わせる。ステップＳ３０６に進み、ウィンドウサイズ内の過去の観測値を全て選択したか否かを判断する。全ての観測値を選択していない場合（ステップＳ３０６でＮＯ）、ステップＳ３０２に戻り、未だ選択されていないウィンドウサイズ内の過去の観測値を１つ選択し、ステップＳ３０３～３０５の処理を行う。
　全ての観測値を選択した場合（ステップＳ３０６でＹＥＳ）、ステップＳ３０７に進み、走行軌跡に対応する区間（ウィンドウサイズ）における過去の観測値の全体数に対して、ステップＳ３０４で乖離度が閾値以下と判定された過去の観測値の数が占める割合を算出する。ステップＳ３０８に進み、この割合が所定割合以上であるか否かを判断する。前記した割合が所定割合以上である場合（ステップＳ３０８でＹＥＳ）、ステップＳ３０９に進み、ステップＳ０１で計測された検出対象の観測値の信頼性は高いと判断する。前記した割合が所定割合未満である場合（ステップＳ３０８でＮＯ）、ステップＳ３１０に進み、ステップＳ０１で計測された検出対象の観測値の信頼性は低いと判断する。ステップＳ３０８～Ｓ３１０に依れば、ウィンドウサイズ内の過去の観測値の全体数に対して乖離度が閾値以下と判定された過去の観測値の数が占める割合に基づいて、検出対象の観測値の信頼度を検出することができる。ここでは、信頼度を二値分類する例を示すが、多値分類してもよい。すなわち、前記した割合が大きい程、検出対象の観測値の信頼度を高く検出することができる。
　信頼度検出部１５は、走行軌跡と過去の観測値のうち走行軌跡に対応した区間における観測値との乖離度を算出し、乖離度と閾値とを比較し、閾値以下の観測値の割合が多い場合に少ない場合よりも高い信頼度を算出する。信頼度検出部１５は、ウィンドウサイズ内の観測値全体の平均値に対する閾値判断ではなく、観測値の各々に対して閾値判断を行う。このため、ウィンドウサイズ内に大きな誤差を含む観測値があった場合でも、その観測値の影響を受けることなく、正確な信頼度の判定が可能となる。
　（第３実施形態）
　第３実施形態では、図４を参照して、図１の信頼度検出部１５の他の具体的な動作例、及び図２のステップ０３（信頼度検出処理）の他の具体的な処理例を説明する。第３実施形態に係る自己位置推定装置１及び自己位置推定方法の全体は、図１及び図２と同じであり説明を省略する。以下に述べる全てのステップＳ３２１～Ｓ３２６の動作主体は、図１の信頼度検出部１５である。
　ステップＳ３２１において、ウィンドウサイズ内の過去の観測値と走行軌跡との乖離度を算出する。ウィンドウサイズ内に複数の過去の観測値がある場合には、全ての過去の観測値について、走行軌跡との乖離度を算出する。ステップＳ３２２に進み、乖離度の各々に対して、乖離度に対応する過去の観測値の信頼度に応じた重み付けをする。具体的には、信頼度が高い程、乖離度を大きくする（重くする）。ステップＳ３２３に進み、重み付けして計算した乖離度の平均値（以後、「加重平均値」という）を算出する。ステップＳ３２４に進み、加重平均値が閾値以下か否かを判断する。加重平均値が閾値以下である場合（ステップＳ３２４でＹＥＳ）、ステップＳ３２５に進み、ステップＳ０１で計測された検出対象の観測値の信頼性は高いと判断する。加重平均値が閾値よりも大きい場合（ステップＳ３２４でＮＯ）、ステップＳ３２６に進み、ステップＳ０１で計測された検出対象の観測値の信頼性は低いと判断する。ステップＳ３２４～Ｓ３２６に依れば、ウィンドウサイズ内の過去の観測値と走行軌跡との乖離度の加重平均値に基づいて、検出対象の観測値の信頼度を検出することができる。ここでは、信頼度を二値分類する例を示すが、多値分類してもよい。すなわち、加重平均値が小さい程、検出対象の観測値の信頼度を高く検出することができる。
　信頼度検出部１５は、走行軌跡と過去の観測値のうち走行軌跡に対応した区間における過去の観測値との乖離度に対して、過去の観測値に対応して記憶されている信頼度に応じた重み付けをする。そして、加重平均値が小さい程、検出対象の観測値の信頼度を高く検出する。過去の観測値の信頼度を考慮した加重平均値を用いて検出対象の観測値の信頼度を検出する。これにより、ある特定の過去の観測値と走行軌跡との乖離度が大きくても、その過去の観測値の信頼度が低い場合には乖離度に付される重みが軽くなるので、加重平均値は大きくならない。すなわち、ウィンドウサイズ内に大きな誤差を含む観測値があっても、その観測値の影響を受けることなく、正確な信頼度の判定が可能となる。
　（第４実施形態）
　第１乃至第３実施形態では、検出対象の観測値としての車両の位置（以後、「観測位置」という）及び角度（以後、「観測角度」という）の双方の信頼度（以後、「位置信頼度」及び「角度信頼度」という）を同時に検出し、車両の位置及び角度を同時に推定する例を示した。しかし、位置の走行軌跡を算出する際に起点として用いる観測角度に誤差があると、位置の走行軌跡が誤って算出され、ひいては位置信頼度の誤判定につながる可能性がある。そこで、第４実施形態では、位置信頼性と角度信頼性を個別に検出する例を説明する。具体的には、先ず、角度信頼度を検出して角度を推定し、その後に、推定した角度を、位置の走行軌跡を算出する際に起点として用いる例を説明する。
　図５を参照して、第４実施形態に係る自己位置推定装置２の構成を説明する。自己位置推定装置２は、測位部１１と、ＩＭＵ装置１２と、角度部２ａと、位置部２ｂとを有する。自己位置推定装置２のうち、データ記憶及び演算処理に関する機能ブロックが、角度部１ａと位置部１ｂとに分割された点が、図１と相違している。一方、図５の測位部１１及びＩＭＵ装置１２は、図１のそれらと同じであり、ここでは説明を割愛する。
　角度部１ａには、角度記憶部１３ａと、角度走行軌跡算出部１４ａと、角度信頼度検出部１５ａと、角度判定履歴記憶部１６ａと、角度推定部１７ａと、が含まれる。
　角度記憶部１３ａは、ＩＭＵ装置１２により検出された車両の角速度を示すデータと、測位部１１により計測された観測角度を示すデータとを記憶する。これらのデータは、角度走行軌跡算出部１４ａ、角度信頼度検出部１５ａ及び角度判定履歴記憶部１６ａによって利用される。
　角度走行軌跡算出部１４ａは、検出対象の観測角度から所定距離遡る角度の走行軌跡、又は検出対象の観測角度を計測した時刻から所定時間遡る車両の角度の走行軌跡を、算出する。角度走行軌跡算出部１４ａは、ＩＭＵ装置１２から得られる車両の相対的な移動量に基づいて角度の走行軌跡を算出する。観測角度の信頼性の検出基準となる角度の走行軌跡は、測位部１１が計測する観測角度の影響を受けることが無くなり、観測角度の外れ値による信頼性の誤判定を抑制することができる。
　具体的には、先ず、検出対象の観測角度を起点として、起点から、検出対象の観測角度を計測した時刻に最も近い時刻における角速度を差し引くことにより、一つ前の時刻における車両の角度を推定する。次に、推定した角度から、この推定角度に対応する時刻の直前の時刻における角速度を差し引くことにより、二つ前の時刻における角度を推定できる。この差し引きを、起点から所定距離内、もしくは検出された時刻からの経過時間が所定時間内である限り繰り返すことで、角度の走行軌跡を算出する。角度の走行軌跡の算出において観測角度は起点としてのみ用いられるため、観測角度の信頼性の検出基準となる角度の走行軌跡は、測位部１１が計測する観測角度の影響を受けることが無くなり、観測角度の外れ値による角度信頼性の誤判定を抑制することができる。すなわち、車両の角度の走行軌跡を車両の相対的な移動量に基づいて算出することにより、測位部１１が計測する信頼性の低い観測角度の影響を受けることが無くなる。
　角度信頼度検出部１５ａは、角度走行軌跡算出部１４ａで算出された角度の走行軌跡と角度の走行軌跡に対応する区間における過去の観測角度との乖離度と、過去の観測角度に関連付けて角度判定履歴記憶部１６ａに記憶されている角度信頼度とに基づいて、検出対象の観測角度の角度信頼度を検出する。具体的には、角度信頼度検出部１５ａは、角度の走行軌跡に対応する区間内の各時刻における過去の観測角度を角度判定履歴記憶部１６ａから抽出し、抽出された過去の観測角度と角度の走行軌跡との角度差を、角度の乖離度として算出する。
　角度信頼度検出部１５ａは、第２実施形態（図３）又は第３実施形態（図４）で説明した図２のステップＳ０３（信頼度検出処理）の詳細な具体例を車両の観測角度に適用することにより、検出対象の観測角度の角度信頼度を検出することができる。
　角度判定履歴記憶部１６ａは、検出対象の観測角度と検出された角度信頼度とを関連付けて記憶する。
　角度推定部１７ａは、信頼度が第３所定値以上の観測角度に基づいて車両の角度を推定する。角度推定部１７ａは、既知の技術を用いて角度を推定することができる。例えば、観測角度（車両の絶対角度）と車両の相対的な移動量（車速と角速度）に基づいて、カルマンフィルタなどの推定手法を用いて、尤もらしい車両の角度を推定する。ここで、観測角度のうち低い信頼度が検出された観測角度は、事前に排除するか、又は角度の推定処理への寄与度が小さくなるように設定することができる。
　位置部１ｂには、位置記憶部１３ｂと、位置走行軌跡算出部１４ｂと、位置信頼度検出部１５ｂと、位置判定履歴記憶部１６ｂと、位置推定部１７ｂと、が含まれる。
　位置記憶部１３ｂは、ＩＭＵ装置１２により検出された車両の速度及び角速度を示すデータと、測位部１１により計測された観測位置を示すデータとを記憶する。これらのデータは、位置走行軌跡算出部１４ｂ、位置信頼度検出部１５ｂ及び位置判定履歴記憶部１６ｂによって利用される。
　位置走行軌跡算出部１４ｂは、測位部１１により計測された検出対象の観測位置及び角度推定部１７ａにより推定された角度から、車両の位置の走行軌跡を算出する。位置走行軌跡算出部１４ｂは、ＩＭＵ装置１２から得られる車両の相対的な移動量に基づいて位置の走行軌跡を算出する。観測位置の信頼性の検出基準となる位置の走行軌跡は、測位部１１が計測する観測位置の影響を受けることが無くなり、観測位置の外れ値による信頼性の誤判定を抑制することができる。
　具体的には、先ず、検出対象の観測位置及び角度推定部１７ａにより推定された車両の角度を起点として、起点から、検出対象の観測位置を計測した時刻に最も近い時刻における速度と角速度を差し引くことにより、一つ前の時刻における車両の位置と角度を推定する。次に、推定した位置と角度から、この推定位置及び推定角度に対応する時刻の直前の時刻における速度と角速度を差し引くことにより、二つ前の時刻における位置と角度を推定できる。この差し引きを、起点から所定距離内、もしくは検出された時刻からの経過時間が所定時間内である限り繰り返すことで、位置の走行軌跡を算出する。位置の走行軌跡の算出において観測位置は起点としてのみ用いられるため、観測位置の信頼性の検出基準となる位置の走行軌跡は、測位部１１が計測する観測位置の影響を受けることが無くなり、観測位置の外れ値による位置信頼性の誤判定を抑制することができる。すなわち、車両の位置の走行軌跡を車両の相対的な移動量に基づいて算出することにより、測位部１１が計測する信頼性の低い観測位置の影響を受けることが無くなる。
　位置信頼度検出部１５ｂは、位置走行軌跡算出部１４ｂで算出された位置の走行軌跡と位置の走行軌跡に対応する区間における過去の観測位置との乖離度と、過去の観測位置に関連付けて位置判定履歴記憶部１６ｂに記憶されている位置信頼度とに基づいて、検出対象の観測位置の位置信頼度を検出する。具体的には、位置信頼度検出部１５ｂは、位置の走行軌跡に対応する区間内の各時刻における過去の観測位置を位置判定履歴記憶部１６ｂから抽出し、抽出された過去の観測位置と位置の走行軌跡とのユークリッド距離を、位置の乖離度として算出する。
　位置信頼度検出部１５ｂは、第２実施形態（図３）又は第３実施形態（図４）で説明した図２のステップＳ０３（信頼度検出処理）の詳細な具体例を車両の観測位置に適用することにより、検出対象の観測位置の位置信頼度を検出することができる。
　位置判定履歴記憶部１６ｂは、検出対象の観測位置と検出された位置信頼度とを関連付けて記憶する。
　位置推定部１７ｂは、信頼度が第４所定値以上の観測位置及び角度推定部１７ａにより推定された車両の角度に基づいて車両の位置を推定する。位置推定部１７ｂは、既知の技術を用いて位置を推定することができる。例えば、観測位置（車両の絶対位置）、角度推定部１７ａにより推定された車両の角度、及び車両の相対的な移動量（車速と角速度）に基づいて、カルマンフィルタなどの推定手法を用いて、尤もらしい車両の位置を推定する。ここで、観測位置のうち低い信頼度が検出された観測位置は、事前に排除するか、又は位置の推定処理への寄与度が小さくなるように設定することができる。
　最後に、自己位置推定装置２は、角度推定部１７ａにより推定された車両の角度及び位置推定部１７ｂにより推定された車両の位置を、車両の自己位置として出力する。
　図６を参照して、図５の自己位置推定装置２を用いた第４実施形態に係る自己位置推定方法を説明する。図６に示す動作フローは、予め定められた周期で、繰り返し実施される。ステップＳ０１（測位処理）において、測位部１１は、人工衛星から受信する信号から前記車両の位置及び角度を検出対象の観測値として計測する。ステップＳ０２－１（走行軌跡算出処理）に進み、角度走行軌跡算出部１４ａは、検出対象の観測角度から、車両の角度の走行軌跡を算出する。ステップＳ０３－１（信頼度検出処理）に進み、角度信頼度検出部１５ａは、角度の走行軌跡、角度の走行軌跡に対応する区間における過去の観測角度との乖離度、及び過去の観測角度に関連付けている角度信頼度に基づいて、検出対象の観測角度の角度信頼度を検出する。ステップＳ０４－１（信頼度記憶処理）に進み、角度判定履歴記憶部１６ａは、検出対象の観測角度と検出された角度信頼度とを関連付けて記憶する。ステップＳ０５－１（自己位置推定処理）に進み、角度推定部１７ａは、角度信頼度が第３所定値以上の観測角度に基づいて車両の角度を推定する。
　ステップＳ０２－２（走行軌跡算出処理）に進み、位置走行軌跡算出部１４ｂは、検出対象の観測位置及び自己位置推定処理（ステップＳ０５－１）において推定された車両の角度から、車両の位置の走行軌跡を算出する。ステップＳ０３－２（信頼度検出処理）に進み、位置信頼度検出部１５ｂは、位置の走行軌跡、位置の走行軌跡に対応する区間における過去の観測位置との乖離度、及び過去の観測位置に関連付けている位置信頼度に基づいて、検出対象の観測位置の位置信頼度を検出する。ステップＳ０４－２（信頼度記憶処理）に進み、位置判定履歴記憶部１６ｂは、検出対象の観測位置と検出された位置信頼度とを関連付けて記憶する。ステップＳ０５－２（自己位置推定処理）に進み、位置推定部１７ｂは、位置信頼度が第４所定値以上の観測位置及びステップＳ０５－１で推定された移動体の角度に基づいて車両の位置を推定する。ステップＳ０６に進み、自己位置推定装置２は、推定された車両の角度及び車両の位置を、車両の自己位置として出力する。
　上記した動作フローを繰り返し実行する事により、信頼度が関連付けられた複数の観測位置の時系列及び複数の観測角度の時系列がそれぞれ求められ、位置判定履歴記憶部１６ｂ及び角度判定履歴記憶部１６ａに過去の観測位置及び過去の観測角度としてそれぞれ記憶される。この過去の観測位置及び過去の観測角度に関連付けた位置信頼度及び角度信頼度を用いて、例えば最新の観測位置及び最新の観測角度の信頼度をそれぞれ検出することができる。
　位置信頼度の検出基準となる位置の走行軌跡を算出する際に用いる起点として車両の位置と角度の情報が必要となる。起点となる角度の情報が正確で無いと、誤った位置の走行軌跡が算出され、ひいては位置信頼度の誤判定につながる。そこで、先ず、測位部１１が計測する観測角度の角度信頼度を算出し、車両の角度を推定する。その後、推定された角度を用いて、位置の走行軌跡を算出し、位置信頼度を検出し、車両の位置を推定する。よって、誤った位置走行軌跡に起因する位置信頼度の誤判定を抑制できる。
　（第１変形例）
　以下、上記した各実施形態の第１乃至第５変形例を説明する。第１乃至第５変形例の２以上の変形例を任意に組み合わせて各実施形態に適用することもできる。
　第１変形例では、過去の観測値の信頼度に関する情報量が十分でない場合の対応について説明する。例えば、測位部１１が測位を開始した直後は、測位部１１は観測対象の観測値を測定できるが、過去の観測値の信頼度が未だ十分に記憶されていないため、過去の観測値の信頼度に関する情報量が十分でない。
　そこで、信頼度検出部１５は、走行軌跡に対応する区間（ウィンドウサイズ内）における過去の観測値のうち、信頼性が関連付けられていない過去の観測値の数が第２所定値以上ある場合、信頼性が関連付けられていない過去の観測値に対して、ステップＳ３０３に示す第１基準値よりも高い信頼度が検出されていると見做して、信頼度検出処理（ステップＳ０３）を行う。これにより、測位処理（Ｓ０１）の開始直後のような過去の観測値の信頼度に関する情報が十分でない場合であっても、検出対象の観測値の信頼度を検出することができる。第１変形例は、第１乃至第４実施形態に適用することができる。
　（第２変形例）
　第２変形例では、起点から遡る距離又は時間に応じて閾値を制御する変形例を説明する。検出対象の観測値から遡る距離が長い程、又は検出対象の観測値を計測した時刻から遡る時間が長い程、ステップＳ０２で算出される走行軌跡に含まれる車両の位置又は角度の誤差は大きくなる。そこで、信頼度検出部１５は、信頼度検出処理（ステップＳ０３）において、走行軌跡に対応する区間（ウィンドウサイズ内）における過去の観測値の、検出対象の観測値から遡る距離が長い程、又は検出対象の観測値を計測した時刻から遡る時間が長い程、図３のステップＳ３０４で用いる閾値を大きく設定する。これにより、走行軌跡に含まれる位置又は角度の誤差に起因する信頼度の誤判定を抑制できる。第１変形例は、第２実施形態及び第４実施形態に適用することができる。
　（第３変形例）
　第３変形例では、起点から遡る距離又は時間に応じて乖離度を制御する変形例を説明する。検出対象の観測値から遡る距離が長い程、又は検出対象の観測値を計測した時刻から遡る時間が長い程、ステップＳ０２で算出される走行軌跡に含まれる車両の位置又は角度の誤差は大きくなる。そこで、信頼度検出部１５は、信頼度検出処理（ステップＳ０３）において、走行軌跡に対応する区間（ウィンドウサイズ内）における過去の観測値の、検出対象の観測値から遡る距離が長い程、又は検出対象の観測値を計測した時刻から遡る時間が長い程、乖離度を短く算出する。これにより、走行軌跡に含まれる位置又は角度の誤差に起因する信頼度の誤判定を抑制できる。第３変形例は、第１乃至第４実施形態に適用することができる。
　（第４変形例）
　第４変形例では、測位処理（ステップＳ０１）での観測値の測定精度が悪い場合の対応について説明する。一般的に，高層ビルが立ち並ぶ都市部では，衛星の電波がビル等に反射してしまうことで測位精度が大きく悪化することが知られている。都市部の高層ビルの他にも、山間、トンネルなどでは電波障害が発生し、測位精度が低下、又は測位が不可能となる。
　そこで、測位処理（ステップＳ０１）での観測値の測定精度が第２基準値よりも低い車両の走行距離が長い程、走行軌跡算出部１４は、走行軌跡算出処理（ステップＳ０２）において前記した所定距離又は前記した所定時間を長く設定する。これにより、走行軌跡算出部１４が算出する走行軌跡に対応する区間が長くなる。よって、当該区間における過去の観測値の信頼度が全て低いという状況を抑制して、信頼度の検出精度を高く維持することができる。第４変形例は、第１乃至第４実施形態に適用することができる。
　（第５変形例）
　第５変形例では、ＩＭＵ装置１２の測定精度が悪い場合の対応について説明する。例えば、温度及び湿度などの環境の変化、又は角速度センサ及び加速度センサの経時劣化によってＩＭＵ装置１２の測定精度が悪くなる。車両の速度及び加速度の誤差は、車両の相対的な移動量の精度、及び相対的な移動量に基づき算出される走行軌跡の算出精度を悪化させる。また、非舗装路等の上下方向（Ｚ方向）の加速度または峠道などの左右方向（Ｚ軸周り）の角速度等が大きいシーンでも、走行軌跡の算出精度が悪化する場合がある。
　そこで、走行軌跡算出部１４は、車両の相対的な移動量の誤差が大きい程、走行軌跡算出処理（ステップＳ０２）において、起点から遡る所定距離又は所定時間を短く設定する。これにより、走行軌跡形状の誤差に起因する信頼度の誤判定を抑制できる。第５変形例は、第１乃至第４実施形態に適用することができる。
　以上、実施形態に沿って本発明の内容を説明したが、本発明はこれらの記載に限定されるものではなく、種々の変形及び改良が可能であることは、当業者には自明である。この開示の一部をなす論述及び図面は本発明を限定するものであると理解すべきではない。この開示から当業者には様々な代替実施形態、実施例及び運用技術が明らかとなろう。
　本発明はここでは記載していない様々な実施形態等を含むことは勿論である。したがって、本発明の技術的範囲は上記の説明から妥当な特許請求の範囲に係る発明特定事項によってのみ定められるものである。
　本願は、２０２２年１０月４日に日本国特許庁に出願された特願２０２２－１６０２５９号に基づく優先権を主張するものであり、その全ての開示内容は引用によりここに援用される。
　１、２　自己位置推定装置
　１１　測位部
　１４　走行軌跡算出部
　１５　信頼度検出部
　１６　判定履歴記憶部（信頼度記憶部）
　１７　自己位置推定部
　Ｓ０１　測位処理
　Ｓ０２、Ｓ０２－１、Ｓ０２－２　走行軌跡算出処理
　Ｓ０３、Ｓ０３－１、Ｓ０３－２　信頼度検出処理
　Ｓ０４、Ｓ０４－１、Ｓ０４－２　信頼度記憶処理
　Ｓ０５、Ｓ０５－１、Ｓ０５－２　自己位置推定処理

Claims (9)

  1. 　人工衛星から受信する信号から移動体の位置及び角度を検出対象の観測値として計測する測位処理と、
    　所定時刻における前記検出対象の観測値から所定距離遡る走行軌跡、又は前記所定時刻における前記検出対象の観測値を前記所定時刻から所定時間遡る前記移動体の走行軌跡を、前記移動体の相対的な移動量に基づいて算出する走行軌跡算出処理と、
    　前記走行軌跡算出処理で算出された前記走行軌跡と前記走行軌跡に対応する区間における過去の観測値との乖離度、及び前記過去の観測値に関連付けて記憶されている信頼度に基づいて、前記所定時刻の検出対象の観測値の信頼度を検出する信頼度検出処理と、
    　前記所定時刻の検出対象の観測値と検出された信頼度とを関連付けて記憶する信頼度記憶処理と、
    　前記信頼度が第１所定値以上の観測値に基づいて前記移動体の自己位置を推定する自己位置推定処理と、を有する自己位置推定方法。
  2. 　前記信頼度検出処理において、
    　　第１基準値よりも高い信頼度が関連付けられた前記過去の観測値を抽出し、
    　　抽出された前記過去の観測値と前記走行軌跡との前記乖離度が閾値以下か否かを判定し、
    　　前記走行軌跡に対応する区間における前記過去の観測値の全体数に対して、前記乖離度が前記閾値以下と判定された前記過去の観測値の数が占める割合が大きい程、前記検出対象の観測値の信頼度を高く検出する請求項１記載の自己位置推定方法。
  3. 　前記信頼度検出処理において、
    　　前記乖離度の各々に対して、前記乖離度に対応する前記過去の観測値の信頼度に応じた重み付けをし、
    　　重み付けして計算した前記乖離度の平均値が小さい程、前記検出対象の観測値の信頼度を高く検出する請求項１記載の自己位置推定方法。
  4. 　前記信頼度検出処理において、前記走行軌跡に対応する区間における前記過去の観測値の、前記検出対象の観測値から遡る距離が長い程、又は前記検出対象の観測値を計測した時刻から遡る時間が長い程、前記閾値を大きく設定する請求項２記載の自己位置推定方法。
  5. 　前記信頼度検出処理において、前記走行軌跡に対応する区間における前記過去の観測値の、前記検出対象の観測値から遡る距離が長い程、又は前記検出対象の観測値を計測した時刻から遡る時間が長い程、前記乖離度を短く算出する請求項３記載の自己位置推定方法。
  6. 　前記測位処理において、前記移動体の位置及び角度を検出対象の観測位置及び検出対象の観測角度として計測し、
    　前記走行軌跡算出処理において、前記所定時刻の検出対象の観測角度から、前記移動体の角度の走行軌跡を前記移動体の相対的な移動量に基づいて算出し、
    　前記信頼度検出処理において、前記角度の走行軌跡と前記角度の走行軌跡に対応する区間における過去の観測角度との乖離度、及び前記過去の観測角度に関連付けている角度信頼度に基づいて、前記所定時刻の検出対象の観測角度の角度信頼度を検出し、
    　前記信頼度記憶処理において、前記所定時刻の検出対象の観測角度と検出された角度信頼度とを関連付けて記憶し、
    　前記自己位置推定処理において、前記角度信頼度が第３所定値以上の観測角度に基づいて前記移動体の角度を推定し、
    　前記走行軌跡算出処理において、前記所定時刻の検出対象の観測位置及び前記自己位置推定処理において推定された前記移動体の角度から、前記移動体の位置の走行軌跡を前記移動体の相対的な移動量に基づいて算出し、
    　前記信頼度検出処理において、前記位置の走行軌跡と前記位置の走行軌跡に対応する区間における過去の観測位置との乖離度、及び前記過去の観測位置に関連付けている位置信頼度に基づいて、前記所定時刻の検出対象の観測位置の位置信頼度を検出し、
    　前記信頼度記憶処理において、前記所定時刻の検出対象の観測位置と検出された位置信頼度とを関連付けて記憶し、
    　前記自己位置推定処理において、前記位置信頼度が第４所定値以上の観測位置及び推定された前記移動体の角度に基づいて前記移動体の位置を推定し、推定された前記移動体の角度及び前記移動体の位置を、前記移動体の自己位置として出力する請求項１記載の自己位置推定方法。
  7. 　前記測位処理での前記観測値の測定精度が低いほど、前記走行軌跡算出処理において前記所定距離又は前記所定時間を長く設定する請求項１記載の自己位置推定方法。
  8. 　前記走行軌跡算出処理において、前記移動体の相対的な移動量の誤差が大きい程、前記所定距離又は前記所定時間を短く設定する請求項１記載の自己位置推定方法。
  9. 　人工衛星から受信する信号から移動体の位置及び角度を検出対象の観測値として計測する測位部と、
    　所定時刻における前記検出対象の観測値から所定距離遡る走行軌跡、又は前記所定時刻における前記検出対象の観測値を前記所定時刻から所定時間遡る前記移動体の走行軌跡を、前記移動体の相対的な移動量に基づいて算出する走行軌跡算出部と、
    　前記走行軌跡算出部で算出された前記走行軌跡と前記走行軌跡に対応する区間における過去の観測値との乖離度、及び前記過去の観測値に関連付けて記憶されている信頼度に基づいて、前記所定時刻の検出対象の観測値の信頼度を検出する信頼度検出部と、
    　前記所定時刻の検出対象の観測値と検出された信頼度とを関連付けて記憶する信頼度記憶部と、
    　前記信頼度が第１所定値以上の観測値に基づいて前記移動体の自己位置を推定する自己位置推定部と、を有する自己位置推定装置。
PCT/JP2023/033647 2022-10-04 2023-09-15 自己位置推定方法及び自己位置推定装置 Ceased WO2024075498A1 (ja)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
JP2022160259A JP2025176719A (ja) 2022-10-04 2022-10-04 自己位置推定方法及び自己位置推定装置
JP2022-160259 2022-10-04

Publications (1)

Publication Number Publication Date
WO2024075498A1 true WO2024075498A1 (ja) 2024-04-11

Family

ID=90607891

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/JP2023/033647 Ceased WO2024075498A1 (ja) 2022-10-04 2023-09-15 自己位置推定方法及び自己位置推定装置

Country Status (2)

Country Link
JP (1) JP2025176719A (ja)
WO (1) WO2024075498A1 (ja)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005326196A (ja) * 2004-05-13 2005-11-24 Xanavi Informatics Corp 現在位置検出装置および現在位置検出方法
JP2012007939A (ja) * 2010-06-23 2012-01-12 Aisin Aw Co Ltd 軌跡情報生成装置、方法およびプログラム
JP2013003049A (ja) * 2011-06-20 2013-01-07 Sony Corp 経路比較装置、経路比較方法、及びプログラム
JP2017009429A (ja) * 2015-06-22 2017-01-12 本田技研工業株式会社 位置特定装置、ナビゲーション装置、位置特定方法、および位置特定プログラム

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2005326196A (ja) * 2004-05-13 2005-11-24 Xanavi Informatics Corp 現在位置検出装置および現在位置検出方法
JP2012007939A (ja) * 2010-06-23 2012-01-12 Aisin Aw Co Ltd 軌跡情報生成装置、方法およびプログラム
JP2013003049A (ja) * 2011-06-20 2013-01-07 Sony Corp 経路比較装置、経路比較方法、及びプログラム
JP2017009429A (ja) * 2015-06-22 2017-01-12 本田技研工業株式会社 位置特定装置、ナビゲーション装置、位置特定方法、および位置特定プログラム

Also Published As

Publication number Publication date
JP2025176719A (ja) 2025-12-05

Similar Documents

Publication Publication Date Title
CN106289275B (zh) 用于改进定位精度的单元和方法
US12405112B2 (en) Inertial navigation system capable of dead reckoning in vehicles
JP4964047B2 (ja) 位置検出装置及び位置検出方法
US11441907B2 (en) Positioning device and positioning method
JP5855249B2 (ja) 測位装置
US11079494B2 (en) Positioning device
US20050049787A1 (en) GPS/dead-reckoning combination system and operating method thereof
JP5554560B2 (ja) 測位信頼度評価装置、測位信頼度評価方法、および、測位信頼度評価プログラム
JP6149699B2 (ja) 車両用走行軌跡算出装置
CN114384574B (zh) 用于确定车辆的定位装置的定位结果的完整性信息的方法
US20220063642A1 (en) Method for Determining an Integrity Range
KR102093743B1 (ko) 센서 융합을 이용한 차선 레벨 자동차 위치 정보 측위 시스템
WO2020149014A1 (ja) 衛星選択装置、及びプログラム
JP2004069536A (ja) データ検定装置および方法
JP6248559B2 (ja) 車両用走行軌跡算出装置
JP5222814B2 (ja) 測位方法および装置
JP4931113B2 (ja) 自車位置決定装置
JP6962040B2 (ja) 方位推定装置
JP6820762B2 (ja) 位置推定装置
JP2025176719A (ja) 自己位置推定方法及び自己位置推定装置
CN109827569A (zh) 无人车定位方法及系统
US12111403B2 (en) Error and integrity evaluation via motion prediction
WO2024214215A1 (ja) 測位装置、測位方法、及びプログラム
CN120252753A (zh) 一种航向角初始化方法、装置、设备及存储介质
CN119452272A (zh) 用于确定用于定位车辆的参数估计的完整性区域的方法

Legal Events

Date Code Title Description
121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 23874627

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: DE

122 Ep: pct application non-entry in european phase

Ref document number: 23874627

Country of ref document: EP

Kind code of ref document: A1

NENP Non-entry into the national phase

Ref country code: JP