JP7308141B2 - Self-position estimation method and self-position estimation device - Google Patents

Description

The present invention relates to a self-position estimation method and a self-position estimation device.

Self-position estimation that estimates the position and orientation of the vehicle by determining the two-dimensional transformation that minimizes the distance between the target position measured by multiple detection units and the target position on the learning map. A device has been proposed (see Patent Document 1).

JP 2015-148601 A

According to the technique described in Patent Document 1, the position and orientation of a vehicle on map data are estimated by maximum likelihood estimation based on the positions of targets in the vehicle coordinate system measured by a plurality of detection units. Therefore, if the measurement error of one of the plurality of detectors is large, the accuracy of the vehicle position and orientation determined by maximum likelihood estimation will be affected by the measurement result of that one detector. There is a problem.

The present invention has been made in view of the above problems, and its object is to combine the measurement results of a plurality of detection units to accurately estimate the position and orientation of a vehicle on map data. An object of the present invention is to provide an estimation method and a self-position estimation device.

In order to solve the above-described problems, a self-position estimation method and a self-position estimation device according to one aspect of the present invention use measurement results from a plurality of detection units that detect targets around a vehicle to generate map data. When estimating the position and orientation of the vehicle above, the position of the target along the longitudinal direction of the vehicle and the width direction of the vehicle in the vehicle coordinate system, their errors, and the direction from the vehicle to the target The angle and its error are estimated, and the position of the target along the front-back direction of the vehicle, the position of the target along the width direction of the vehicle, and the direction from the vehicle to the target at which the estimated error is the smallest. Each angle is selected and the position and orientation of the vehicle is estimated based on the selected position and angle.

According to the present invention, even if the measurement result of one of the plurality of detection units is significantly different from the measurement results of the other detection units, the measurement results of the plurality of detection units are combined. Therefore, the position and orientation of the vehicle can be estimated with high accuracy.

FIG. 1 is a block diagram showing the configuration of a self-position estimation device according to the first embodiment of the present invention. FIG. 2 is a flow chart showing the processing procedure of the self-position estimation device according to the first embodiment of the present invention. FIG. 3 is a schematic diagram showing the relationship between detection results by a plurality of detection units and estimated target positions. FIG. 4 is a block diagram showing the configuration of a self-position estimation device according to the second embodiment of the present invention. FIG. 5 is a diagram for explaining a profile creation method by the self-position estimation device according to the second embodiment of the present invention. FIG. 6 is a diagram for explaining a method of specifying a detection unit having an error by the self-position estimation device according to the second embodiment of the present invention. FIG. 7 is a flow chart showing the processing procedure of the self-position estimation device according to the second embodiment of the present invention. FIG. 8 is a flow chart showing a processing procedure of identifying processing of the detection unit by the self-position estimation device according to the second embodiment of the present invention. FIG. 9 is a diagram for explaining the effects of the self-position estimation device according to the second embodiment of the present invention.

[First embodiment]
Embodiments of the present invention will now be described in detail with reference to the drawings. In the explanation, the same reference numerals are given to the same parts, and redundant explanations are omitted.

[Configuration of self-localization device]
FIG. 1 is a block diagram showing the configuration of the self-position estimation device according to this embodiment. As shown in FIG. 1 , the self-position estimation device according to this embodiment includes a plurality of detection units 40 and a controller 100 . Note that the self-position estimation device is mounted on a vehicle (not shown).

The detection unit 40 outputs a relative positional relationship with the vehicle of a target that serves as a reference for determining the position of the vehicle.

Examples of the detection unit 40 include a white line detection unit that recognizes white lines around the vehicle and a stop line detection unit that detects stop lines based on an image captured by a camera mounted on the vehicle. In addition, the radar or lidar mounted on the vehicle detects road signs, buildings, and other three-dimensional objects (stationary objects) around the vehicle (stationary objects). There is a position detection unit based on the global positioning system (GPS) for detecting the position of the vehicle. The detection unit 40 also includes a position estimation unit that estimates the position of the vehicle from the vehicle model. For example, the vehicle model may be a model that estimates the position of the vehicle by obtaining odometry from the wheel speed and steering angle of the vehicle. Stationary targets such as displays on the road around the vehicle such as white lines and stop lines, road signs, and three-dimensional objects such as buildings around the vehicle are hereinafter referred to as targets.

The controller 100 (an example of a control unit or processing unit) is a general-purpose microcomputer including a CPU (Central Processing Unit), memory, and an input/output unit. A computer program (self-position estimation program) for functioning as a self-position estimation device is installed in the controller 100 . By executing a computer program, the controller 100 functions as a plurality of information processing circuits (31, 33, 71, 73, 75, 91, 93) included in the self-position estimation device.

Here, an example of realizing a plurality of information processing circuits (31, 33, 71, 73, 75, 91, 93) included in the self-position estimation device by software is shown. However, it is also possible to configure the information processing circuits (31, 33, 71, 73, 75, 91, 93) by preparing dedicated hardware for executing each information processing described below. Also, the plurality of information processing circuits (31, 33, 71, 73, 75, 91, 93) may be configured by individual hardware. Furthermore, the information processing circuits (31, 33, 71, 73, 75, 91, 93) may also be used as an electronic control unit (ECU) used for other vehicle-related controls.

The controller 100 includes, as a plurality of information processing circuits (31, 33, 71, 73, 75, 91, 93), an estimator 31, an error estimator 33, a first selector 71, a second selector 73, a third selector A unit 75 , an output unit 91 , and an error integration unit 93 are provided.

Based on the output result of the detection unit 40, the estimation unit 31 calculates the position of the target to be detected by the detection unit 40 along the longitudinal direction of the vehicle and the position along the width direction of the vehicle as viewed from the vehicle coordinate system. and the angle of the direction from the vehicle toward the target with reference to the longitudinal direction of the vehicle. As a premise below, the output results from the plurality of detection units 40 are assumed to be the output results for the same target.

The estimation by the estimation unit 31 is performed for each of the multiple detection units 40 . That is, the estimating unit 31 determines, for each detecting unit 40, the position of the vehicle in the longitudinal direction (first coordinate value), the position of the vehicle in the width direction (second coordinate value), and the angle indicating the direction toward the target (second coordinate value). 3 coordinate values), the estimation result is output.

The error estimator 33 estimates an error for the output result of the estimator 31 based on the output result of the detector 40 . More specifically, based on the output result of the detection unit 40, for each detection unit 40, the error included in the position of the vehicle in the front-rear direction (first error) and the error included in the position in the width direction of the vehicle (first error) are calculated. 2 error), and an estimation result is output for each error (third error) included in the angle indicating the direction toward the target.

For example, FIG. 3 shows, as output results from the plurality of detection units 40, a first output result (position P1 (X1, Y1), error range R1) from the first detection unit 40, and a second detection A second output result from unit 40 (position P2 (X2, Y2), error range R2) is shown. The estimation by the estimating unit 31 is performed for each of the plurality of detecting units 40, and the estimating unit 31 sets "coordinate value X1 of position P1" and "coordinate value X2 of position P2" as the position (first coordinate value) of the vehicle in the longitudinal direction. is output as the estimation result. In addition, the estimating unit 31 outputs “coordinate value Y1 of position P1” and “coordinate value Y2 of position P2” as the positions (second coordinate values) in the width direction of the vehicle as estimation results.

Further, in FIG. 3, the error estimating unit 33 uses the "error ΔX1", "error ΔX2,” “error ΔY1,” and “error ΔY2” are output as estimation results. Here, "error ΔX1" and "error ΔX2" are first errors, and "error ΔY1" and "error ΔY2" are second errors.

Although the third coordinate value and the third error are not explicitly shown in FIG. 3, the estimating section 31 and the error estimating section 33 similarly output estimation results for the angle indicating the direction toward the target. Also, in FIG. 3, only two output results of the plurality of detection units 40 are shown. Do the same process.

There are various methods for estimating the error by the error estimator 33 . For example, the error may be estimated based on the error information output by the detection unit 40 itself, or the error may be estimated based on an error model based on the characteristics of each detection unit 40. . In addition to the output result of the detection unit 40, the controller 100 holds a vehicle model that expresses the travel locus of the vehicle. , and an error model based on the characteristics of each detection unit 40 may be used to estimate the error.

There are various error models used by the error estimator 33 . For example, when the detection unit 40 includes a camera, the error model may be created by actually measuring the relationship between the moving speed of the camera with respect to the target and the error that occurs. Here, in general, as the moving speed increases, the generated error increases. Therefore, it is possible to create an error model as an approximation formula for evaluating the actually measured error as a function of the moving speed.

Also, the error model may be created by actually measuring the relationship between the imaging distance of the camera to the target and the error that occurs.

In addition to the above, the camera may be modeled by calculation based on the lens parameters of the camera, and the error model may be created based on the characteristics of the modeled camera.

There are also various models for vehicle models that express the travel locus of the vehicle. For example, it may be a model that predicts the position of the vehicle after a predetermined period of time by actually measuring the vehicle's travel locus based on odometry and a satellite positioning system mounted on the vehicle.

The first selection unit 71 selects, as a first selection value, the first coordinate value with the smallest first error among the plurality of first coordinate values estimated by the estimation unit 31 . Also, the minimum first error is selected as the minimum first error. In the example shown in FIG. 3, the first selection unit 71 selects a plurality of first coordinate values because the smallest first error is "error ΔX1" among "error ΔX1" and "error ΔX2" which are the first errors. The "coordinate value X1" associated with the "error ΔX1" is selected as the first selection value from the "coordinate value X1" and "coordinate value X2". Also, the first selection unit 71 selects “error ΔX1” as the minimum first error.

The second selection unit 73 selects, as a second selection value, the second coordinate value with the smallest second error among the plurality of second coordinate values estimated by the estimation unit 31 . Also, the smallest second error is selected as the smallest second error. In the example shown in FIG. 3, the second selection unit 73 selects a plurality of second coordinate values because the smallest second error is the "error ΔY2" among the "error ΔY1" and the "error ΔY2" that are the second errors. "Coordinate value Y2" associated with "Error ΔY2" is selected as the second selection value from "Coordinate value Y1" and "Coordinate value Y2". Also, the second selection unit 73 selects “error ΔY2” as the minimum second error.

The third selection unit 75 selects, as a third selection value, the third coordinate value with the smallest third error among the plurality of third coordinate values estimated by the estimation unit 31 . Also, the smallest third error is selected as the smallest third error. The third selection unit 75 selects the third selection value and the minimum third error based on a plurality of third coordinate values, similarly to the first selection unit 71 and the second selection unit 73 .

Based on the first selection value, the second selection value, and the third selection value selected by the first selection unit 71, the second selection unit 73, and the third selection unit 75, respectively, the output unit 91 performs the detection by the detection unit 40. Estimate the position and angle of the target of interest. In addition, the output unit 91 estimates the position and orientation of the vehicle based on the estimated position and angle of the target. That is, the output unit 91 stores map data including target positions in advance, and selects the first selection value, the second selection value, The position and orientation of the vehicle on the map data are estimated by collating the third selection value with the map data.

In the example shown in FIG. 3, the output unit 91 converts the first selection value "coordinate value X1" and the second selection value "coordinate value Y2" to the target position PE (X1, Y2). We estimate that

Although the third selection value is not explicitly shown in FIG. 3, the output unit 91 similarly estimates the angle indicating the direction toward the target based on the third selection value.

The error integration unit 93 outputs the Estimate the error regarding the estimated position and angle of the target. As a result, the error regarding the position and orientation of the vehicle on the map data estimated by the output unit 91 is estimated.

For example, the error integration unit 93 estimates the estimated target position and angle error as the sum of the minimum first error, the minimum second error, and the minimum third error.

[Processing procedure of the self-position estimation device]
Next, a processing procedure for self-position estimation by the self-position estimation device according to this embodiment will be described with reference to the flowchart of FIG. The self-position estimation process shown in FIG. 2 starts when the ignition of the vehicle is turned on, and is repeatedly executed while the ignition is on.

First, in step S101, the detection unit 40 outputs the relative positional relationship with the vehicle of a target that serves as a reference for determining the position of the vehicle. Then, based on the output result of the detection unit 40, the estimation unit 31 determines the position of the target to be detected by the detection unit 40 along the longitudinal direction of the vehicle and the width of the vehicle as viewed from the vehicle coordinate system. The position along the direction and the angle of the orientation from the vehicle toward the target are estimated with reference to the longitudinal direction of the vehicle.

In step S<b>103 , the error estimator 33 estimates an error for the output result of the estimator 31 based on the output result of the detector 40 .

In step S<b>105 , the first selection unit 71 selects the first coordinate value with the smallest first error among the plurality of first coordinate values estimated by the estimation unit 31 as the first selection value. Also, the minimum first error is selected as the minimum first error.

In step S<b>107 , the second selection unit 73 selects the second coordinate value with the smallest second error among the plurality of second coordinate values estimated by the estimation unit 31 as the second selection value. Also, the smallest second error is selected as the smallest second error.

In step S<b>109 , the third selection unit 75 selects the third coordinate value with the smallest third error among the plurality of third coordinate values estimated by the estimation unit 31 as the third selection value. Also, the smallest third error is selected as the smallest third error.

In step S<b>111 , the output unit 91 estimates the position and orientation of the vehicle on the map data based on the first selection unit 71 , the second selection unit 73 and the third selection unit 75 .

In step S<b>113 , the error integration unit 93 estimates the error regarding the position and orientation of the vehicle on the map data estimated by the output unit 91 .

[Effects of Embodiment]
As described in detail above, the self-position estimation method and the self-position estimation device according to the present embodiment use the measurement results from a plurality of detection units that detect targets around the vehicle to determine the position on the map data. When estimating the position and orientation of the vehicle, each of the plurality of detection units estimates the position of the target along the longitudinal direction of the vehicle as a first coordinate value in the vehicle coordinate system of the vehicle. estimating the position of the target along the width direction of the vehicle as a first error, estimating the position of the target along the width direction of the vehicle as a second coordinate value, estimating the error of the second coordinate value as a second error, and estimating the longitudinal direction of the vehicle As a reference, the angle of the direction from the vehicle to the target is estimated as the third coordinate value, and the error of the third coordinate value is estimated as the third error. Then, a first coordinate value with the smallest first error among the estimated plurality of first coordinate values is selected as a first selection value, and a second error is selected among the plurality of estimated second coordinate values. is selected as the second selection value, and the third coordinate value with the minimum third error among the plurality of estimated third coordinate values is selected as the third selection value. , the first selection value, the second selection value, and the third selection value are used to estimate the position and orientation of the vehicle.

As a result, even if the measurement result of one of the plurality of detection units is significantly different from the measurement result of the other detection units, the error can be detected from the measurement results of the plurality of detection units. is estimated to be the smallest (position in the longitudinal direction of the vehicle (first coordinate value), position in the width direction of the vehicle (second coordinate value), angle indicating the direction toward the target (third coordinate value)) By combining them, the position and orientation of the vehicle can be estimated with high accuracy.

In the method using maximum likelihood estimation, when a measurement result with a large error is included in a plurality of measurement results obtained by a plurality of detection units, the estimated vehicle position and position are affected by the measurement result with a large error. Orientation accuracy may deteriorate. Ultimately, the maximum likelihood method gives correct results in the limit of infinite number of measurements, but in real measurements only a finite number of measurements are available because only a finite number of detectors are available. do not have. Therefore, the error in the measurement result can be a problem in the method based on maximum likelihood estimation. In comparison, according to the method of the present embodiment, it is possible to prevent deterioration of the accuracy of the estimated vehicle position and orientation due to measurement results with large errors.

Further, in the self-position estimation method and the self-position estimation device according to the present embodiment, the sum of the first error of the first selection value, the second error of the second selection value, and the third error of the third selection value is It may be calculated. This allows the error in the estimated vehicle position and orientation to be evaluated. It can also serve as a basis for evaluating errors in future vehicle position and orientation via a vehicle model.

Furthermore, in the self-position estimation method and self-position estimation device according to the present embodiment, the error model for each detection unit is used to estimate the first error, the second error, and the third error in the detection unit. may Since the error is estimated using the error model prepared for each detection unit, the error can be estimated in consideration of the characteristics of each detection unit. In addition, when the detection unit exhibits behavior that deviates from the error model, it is detected that the detection unit is malfunctioning, or the detection unit cannot perform measurement under normal conditions for some reason. can be detected.

Further, in the self-position estimation method and the self-position estimation device according to the present embodiment, when the detection unit is a camera, the error model is created by actually measuring the relationship between the moving speed of the camera and the error that occurs. may be Furthermore, the error model may be created by actually measuring the relationship between the imaging distance of the camera and the error that occurs. Errors that occur in the camera as the vehicle moves can be accurately evaluated based on actual measurements, and the position and orientation of the vehicle can be estimated with higher accuracy.

Furthermore, in the self-position estimation method and the self-position estimation device according to the present embodiment, the error model may be created by modeling by calculation based on the lens parameters of the camera. Errors that occur in the camera as the vehicle moves can be accurately evaluated based on the modeled characteristics of the camera, and the position and orientation of the vehicle can be estimated more accurately.

Moreover, in the self-position estimation method and the self-position estimation device according to the present embodiment, the position and orientation of the vehicle may be estimated based on the vehicle model of the vehicle. Also, the vehicle model may be created by actual measurement based on odometry and a satellite positioning system mounted on the vehicle. By estimating based on a vehicle model, the amount of deviation between the trajectory of the vehicle and the estimated position and orientation of the vehicle can be used to evaluate the error, improving the estimation of the vehicle position and orientation. .

Furthermore, in the self-position estimation method and the self-position estimation device according to the present embodiment, the vehicle model and the error model for each detection unit are used to estimate the first error, the second error, and the third error in the detection unit. can be anything. Since the error is estimated using the error model prepared for each detection unit, the error can be estimated in consideration of the characteristics of each detection unit.

Further, in the self-position estimation method and the self-position estimation device according to the present embodiment, the position and orientation of the vehicle are determined based on the first selection value, the second selection value, and the third selection value selected in the past. It may be estimated. By using the first selected value, the second selected value, and the third selected value selected in the past for estimation, the amount of data used for estimation can be increased, and the position and orientation of the vehicle can be It can be estimated more accurately.

Furthermore, in the self-position estimation method and the self-position estimation device according to the present embodiment, based on the first error of the first selection value, the second error of the second selection value, and the third error of the third selection value, , to estimate the position and orientation of the vehicle. This allows the error in the estimated vehicle position and orientation to be evaluated. It can also serve as a basis for evaluating errors in future vehicle position and orientation via a vehicle model.

[Second embodiment]
Embodiments of the present invention will now be described in detail with reference to the drawings. In the explanation, the same reference numerals are given to the same parts, and redundant explanations are omitted.

[Configuration of self-localization device]
FIG. 4 is a block diagram showing the configuration of the self-position estimation device according to this embodiment. As shown in FIG. 4, the self-position estimation device according to this embodiment differs from the first embodiment in that it further includes a detection unit evaluation unit 150 .

The detection unit evaluation unit 150 identifies a detection unit 40 having an error from among the plurality of detection units 40, and outputs the identified detection units 40 to the first to third selection units 71 to 75, respectively. As a result, the first to third selection units 71 to 75 remove the coordinate values estimated using the specified measurement results of the detection unit 40, and select the coordinate values that minimize the first to third errors. as the first-third selection values. The detection unit evaluation unit 150 includes a profile generation unit 152 and a detection unit identification unit 154 .

The profile creating unit 152 calculates, for each of the plurality of detecting units 40, the speed of the target along the longitudinal direction of the vehicle in the vehicle coordinate system of the vehicle as the first speed, and calculates the plurality of first speeds calculated within a predetermined period. 1. Memorize the speed and create a profile. Similarly, the profile creating unit 152 calculates the speed of the target along the width direction of the vehicle as a second speed, and stores a plurality of second speeds calculated within a predetermined period to create a profile. Also, the angular velocity in the direction from the vehicle toward the target is calculated as a third velocity with reference to the longitudinal direction of the vehicle, and a profile is created by storing a plurality of third velocities calculated within a predetermined period. Furthermore, the profile creation unit 152 also creates a reference profile that stores a reference speed.

The profile creation unit 152 first calculates the velocity of the target object that the detection unit 40 is to detect for each of the plurality of detection units 40 . For example, when the detection unit 40 is a white line detection unit, the moving speed of the white line is calculated, and when the detection unit 40 is a vehicle position detection system based on GPS, the speed of the vehicle is calculated. The method of calculating the speed differs depending on the detection unit 40, but basically it can be obtained from the amount of change in the position of the target within a predetermined period of time. Then, the profile generator 152 determines the speed of the target along the longitudinal direction of the vehicle as the first speed, the speed of the target along the width direction of the vehicle as the second speed, and the object from the vehicle relative to the longitudinal direction of the vehicle. The angular velocity in the direction toward the target is calculated as the third velocity.

Next, the profile creation unit 152 creates a profile in which a plurality of speeds calculated within a predetermined period are arranged in order of calculated time and recorded. For example, in the profile, 10 velocities calculated every 1 second for 10 seconds are stored in order of calculated time. Then, the profile creation unit 152 creates profiles of the first speed, the second speed, and the third speed for each detection unit 40 .

Also, the profile creating unit 152 creates a reference profile that stores a reference speed. The reference profile is a profile that stores the velocity calculated using the measurement result of the detection section that is considered to be the most stable among the detection sections 40 . For example, it is conceivable to use a profile that stores the speed estimated from the vehicle model as the reference profile. As the vehicle model, odometry obtained from the wheel speed and steering angle of the vehicle can be used. Besides the vehicle model, the velocity calculated from the position of the vehicle detected by GPS may be used to create the reference profile.

Next, the detection unit specifying unit 154 calculates the difference between the first speed stored in the profile and the speed stored in the reference profile. Similarly, the detection unit specifying unit 154 calculates the difference between the second speed stored in the profile and the speed stored in the reference profile, and calculates the difference between the third speed stored in the profile and the speed stored in the reference profile. Calculate the difference between Then, the calculated differences are added to calculate the sum of the differences, and it is determined whether or not the calculated sum of the differences is greater than the first threshold. As a result, when the sum of the calculated differences becomes larger than the first threshold, the detection unit 40 that outputs the measurement result for creating the profile determined to be larger than the first threshold continues to detect the error. It is specified that the detection unit has

Here, a method for identifying detectors that continuously have errors will be described with reference to FIGS. FIG. 5 is a graph showing changes over time in speed calculated from the measurement results of the detection unit 40. FIG. 6 shows changes over time in the sum of the differences between the speed stored in the profile and the speed stored in the reference profile. It is a figure which shows.

As shown in FIG. 5 , the profile generator 152 calculates velocities Va, Vb, Vc, and Vr for each detector 40 from the measurement results of the plurality of detectors 40 . Velocity Va is a velocity calculated from the measurement result of the detection section 40a, velocity Vb is a velocity calculated from the measurement result of the detection section 40b, and velocity Vc is a velocity calculated from the measurement result of the detection section 40c. Also, the speed Vr is a reference speed, and in FIG. 5, the speed obtained from the odometry is used as a reference. As shown in FIG. 5, the speed Va is rapidly decelerating, and the speed Vb is gently decelerating. On the other hand, the velocities Vc and Vr are accelerating.

In FIG. 5, the profile creating unit 152 creates a profile by storing the velocity calculated for a predetermined period of time, for example, 10 seconds, before the current time t0. In FIG. 5, profiles of velocities Va, Vb, and Vc are created respectively, and a profile of velocity Vr is created as a reference profile.

After the profile is created in this way, the detection unit specifying unit 154 then calculates the difference between the speed stored in the profile and the speed stored in the reference profile. Since the profile stores velocities at predetermined times and at predetermined time intervals, the difference between the velocities at the same time is calculated. For example, the difference between the velocity Va at time t5 and the velocity Vr at time t5 is calculated. Then, the difference is calculated for each of the velocities stored in the profile. In the case of FIG. 5, all the speed differences for 10 seconds before the current time t0 are calculated.

Then, the detection unit specifying unit 154 adds all the calculated differences to calculate the sum of the differences, and determines whether or not the sum of the calculated differences is greater than the first threshold. FIG. 6 shows the sum of differences Sa calculated from the velocity Va of FIG. 5, the sum of differences Sb calculated from the velocity Vb of FIG. 5, and the sum of differences Sc calculated from the velocity Vc of FIG. Note that the first threshold is a threshold that is set in order to identify detection units that continuously have an error when the vehicle is traveling straight ahead at a normal speed, and should be set in advance through experiments and simulations. Just do it.

As shown in FIG. 6, the sums of differences Sa and Sb are greater than the first threshold at time t0. That is, it can be seen that the velocities Va and Vb continued to deviate from the reference velocity Vr during the profile creation period. Therefore, the detection unit 40 that outputs the measurement results of calculating the velocities Va and Vb can be identified as the detection unit that continuously has an error.

On the other hand, the sum Sc of differences is less than or equal to the first threshold. That is, it can be seen that the speed Vc always has a small deviation from the reference speed Vr during the profile creation period. Therefore, the detection unit 40 that outputs the measurement result of calculating the velocity Vc can be specified as a detection unit with a small error.

When the detecting portion having the error is thus specified, the detecting portion specifying portion 154 outputs the specified result to the first to third selecting portions 71-75.

After acquiring the identification result, the first selection unit 71 selects the first coordinate value estimated using the measurement result of the detection unit 40 identified as having an error among the plurality of first coordinate values estimated by the estimation unit 31 . Exclude one coordinate value. Then, the first coordinate value with the smallest first error is selected as the first selection value from among the first coordinate values that are not excluded.

Similarly, the second selection unit 73 selects the second coordinate values estimated using the measurement result of the detection unit 40 identified as having an error among the plurality of second coordinate values estimated by the estimation unit 31. to exclude. Then, the second coordinate value with the smallest second error is selected as the second selection value from the second coordinate values that are not excluded.

Further, the third selection unit 75 selects the third coordinate values estimated using the measurement result of the detection unit 40 identified as having an error among the plurality of third coordinate values estimated by the estimation unit 31. exclude. Then, the third coordinate value with the smallest third error is selected as the third selection value from among the third coordinate values that are not excluded.

After that, the output unit 91 outputs the detection unit Estimate the position and angle of the target to be detected by 40 . In addition, the output unit 91 estimates the position and orientation of the vehicle based on the estimated position and angle of the target.

[Processing procedure of the self-position estimation device]
Next, a processing procedure for self-position estimation by the self-position estimation device according to this embodiment will be described with reference to the flowchart of FIG. The self-position estimation process of the present embodiment shown in FIG. 7 differs from the self-position estimation process of the first embodiment shown in FIG. 2 in that step S104 is added.

In step S104, the detection unit evaluation unit 150 identifies the detection units 40 that continuously have errors among the plurality of detection units 40, and selects the identified detection units 40 from the first to third selection units 71-75. , respectively. The detection unit specifying process executed in step S104 will be described below with reference to the flowchart of FIG.

As shown in FIG. 8, in step S201, the profile generator 152 calculates velocities in the vehicle coordinate system of the vehicle for each of the plurality of detectors 40, and in step S203, calculates the plurality of velocities calculated within a predetermined period. Memorize and create a profile. In step S203, the profile creation unit 152 also creates a reference profile that stores a reference speed.

In step S205, the detection unit specifying unit 154 calculates the difference between the speed stored in the profile and the speed stored in the reference profile, adds all the calculated differences, and calculates the sum of the differences.

In step S207, the detection unit specifying unit 154 determines whether or not the sum of the calculated differences is greater than the first threshold. Then, the detection unit 40 that outputs the measurement result for creating the profile that is determined to be larger than the first threshold value is identified as the detection unit that continuously has an error. After that, the detecting portion identifying portion 154 outputs the identification result to the first to third selecting portions 71-75.

When the detection unit having the error is thus identified, the detection unit identification processing executed in step S104 ends.

Thereafter, the self-position estimation process of FIG. 7 proceeds to steps S105-S109. In steps S105 to S109, when the first to third selection units 71 to 75 acquire the identification result of the detection unit having the error, the first to third selection units 71 to 75 select the detection unit having the error among the multiple coordinate values estimated by the estimation unit 31. Exclude the coordinate values estimated using 40 measurements. Then, the coordinate value with the smallest error is selected as the selection value from the coordinate values that have not been excluded.

When the selection value is selected in this way, the processes of steps S111 and S113 are executed, and the self-position estimation process according to this embodiment ends.

[Modification 1]
In the above-described embodiment, the case where the vehicle is traveling straight ahead at a normal speed has been described. The magnitude of the error changes. Therefore, when the vehicle is traveling at a low speed or turning, it is necessary to change the first threshold value of the above-described embodiment.

Therefore, when the speed of the vehicle is equal to or less than a predetermined value, a second threshold value smaller than the first threshold value is used to identify the detectors 40 that continuously have errors. When the vehicle is traveling at a low speed, the error is small, so the second threshold is a smaller value than the first threshold. The second threshold is a threshold set for identifying the detection unit 40 having an error when the vehicle is traveling at a low speed such that the vehicle is almost stopped, and is set in advance through experiments and simulations. be able to.

Further, when the vehicle is turning, the third threshold is used instead of the first threshold to identify the detection unit 40 that continuously has an error. As a method of determining that the vehicle is turning, it is possible to determine by detecting the yaw angle of the vehicle. Note that the third threshold is a threshold set to identify the detection unit 40 having an error when the vehicle is turning at a normal speed, and can be set in advance through experiments or simulations.

[Modification 2]
Some detection units 40 have low stability and may temporarily output measurement results containing large errors. In such a case, the velocities stored in the profile will contain completely different values of velocities.

Therefore, if the velocities stored in the profile include at least one velocity with a completely different value, the detection unit specifying unit 154 determines that the detection unit 40 that outputs the measurement result for creating the profile. are identified as erroneous detectors.

Specifically, the detection unit identifying unit 154 determines whether or not there is a speed greater than the fourth threshold among the speeds stored in the profile. Then, if there is a profile having a velocity greater than the fourth threshold, the detection unit that output the measurement result for creating that profile is identified as the detection unit having an error. Note that the fourth threshold is set to a value larger than the other thresholds because it is set to detect a speed including a large error, and can be set in advance through experiments or simulations.

[Modification 3]
As a method of identifying the detection unit 40 having an error, it is determined whether the detection unit 40 has an error by determining whether the rate of change of velocity, that is, the direction of acceleration (positive or negative) is different. be able to.

Therefore, the detection unit specifying unit 154 calculates acceleration for each velocity stored in the profile, and also calculates acceleration for each velocity stored in the reference profile. Acceleration can be calculated by finding the rate of change between forward and backward velocities. Then, it is determined whether or not the number of times the direction of acceleration calculated from the velocity stored in the profile differs from the direction of acceleration calculated from the reference profile is greater than the fifth threshold. As a result, when the number is greater than the fifth threshold, the detection section that outputs the measurement result for creating a profile greater than the fifth threshold is specified as the detection section having the error. Note that the fifth threshold may be set by verifying in advance the number of times an abnormality in the direction of acceleration occurs in a detection unit that continuously has an error, through experiments or simulations.

[Modification 4]
Even if the error of each speed stored in the profile is small, if there are many speeds with errors, it is considered that the detection unit that output the measurement results for creating such a profile is the detection unit with errors. be done.

Therefore, the detector identification unit 154 compares the speed stored in the profile with the speed stored in the reference profile. Then, if the number of times the speed difference is equal to or greater than the predetermined value is greater than the sixth threshold, the detection unit outputting the measurement result for creating a profile greater than the sixth threshold is determined to be the detection unit having an error. Identify. Note that the sixth threshold may be set by verifying in advance by experiment or simulation the number of times a small error occurs in a detection unit that continuously has an error. Also, the predetermined value for determining whether or not there is a small error is set to a value smaller than the fourth threshold value for determining a large error.

[Effects of Embodiment]
As described in detail above, the self-position estimation method and the self-position estimation device according to the present embodiment calculate the speed of a target, store a plurality of speeds calculated within a predetermined period, and create a profile. Furthermore, a reference profile is created in which reference speeds are stored. Then, the difference between the speed stored in the profile and the speed stored in the reference profile is calculated, and the calculated difference is added to calculate the sum of the differences. As a result, when the sum of the calculated differences is larger than the first threshold value, the detection unit that output the measurement result for creating the profile is specified. When the detection unit is specified in this way, the coordinate values estimated using the measurement result of the specified detection unit are excluded from the estimated multiple coordinate values, and the coordinate value with the minimum error is selected. Select as

As a result, the coordinate values estimated using the measurement results of the detection unit 40 that continuously have errors can be excluded, so the position and orientation of the vehicle can be accurately estimated when estimating the self-position.

In particular, the self-position estimation method and the self-position estimation device according to the present embodiment can prevent erroneous selection of measurement results of the detection unit 40 in which small errors continue to exist. A specific example thereof will be described with reference to FIG.

FIG. 7 shows self-position estimation results when the method of the present embodiment is used and when the method of the present embodiment is not used when there is a detection unit 40 in which small errors continue to exist. As shown in FIG. 7, at point A, self-position P1 estimated without using the method of this embodiment and self-position P2 estimated using the method of this embodiment are at the same position.

After that, time passes and the estimation result of the point B indicates that the self-position P1 estimated without using the method of the present embodiment is ahead of the self-position P2 estimated using the method of the present embodiment. I'm in.

If there is a detection unit 40 that continuously has a small error, the measurement result of the detection unit 40 having a small error will be selected because the error is small at each time point. However, even if the error at each time point is small, if the error is gradually increased due to continued selection, the self-position P1 of the point B will be far ahead of the self-position P2.

On the other hand, the self-position P2 estimated using the method of the present embodiment is estimated by excluding the measurement results of the detection unit 40 that continuously have an error, so the error gradually expands. It is estimated with good precision.

As a result, in the estimation result of the point C, the self-position P1 estimated without using the method of this embodiment protrudes into another lane at the intersection, but the self-position P2 estimated using the method of this embodiment is driving in the lane without sticking out at the intersection.

Therefore, by using the method of the present embodiment, it is possible to accurately estimate the position and orientation of the vehicle even when there is a detection unit 40 that continuously has a small error. .

Further, in the self-position estimation method and the self-position estimation device according to the present embodiment, the second threshold smaller than the first threshold is used when the speed of the vehicle is equal to or less than a predetermined value. As a result, the position and orientation of the vehicle can be accurately estimated even when the vehicle is traveling at a low speed such that the vehicle is almost stationary.

Furthermore, in the self-position estimation method and the self-position estimation device according to this embodiment, the third threshold is used instead of the first threshold when the vehicle is turning. This makes it possible to accurately estimate the position and orientation of the vehicle even when the vehicle is turning.

Further, in the self-position estimation method and the self-position estimation device according to the present embodiment, if there is a speed higher than the fourth threshold among the speeds stored in the profile, a profile having a speed higher than the fourth threshold is created. Identify the detection unit that has output the measurement result for This makes it possible to accurately estimate the position and orientation of the vehicle even when the detection unit 40 with low stability outputs measurement results that are completely different from normal values.

Furthermore, in the self-position estimation method and the self-position estimation device according to the present embodiment, acceleration is calculated for each velocity stored in the profile, and acceleration is calculated for each velocity stored in the reference profile. If the number of times that the direction of acceleration calculated from the speed of the profile and the direction of acceleration calculated from the reference profile differ is greater than the fifth threshold, the measurement result for creating a profile greater than the fifth threshold is output. Identify the detected part. As a result, it is possible to continuously identify the detection unit that has an error, so that the position and orientation of the vehicle can be accurately estimated.

Also, in the self-position estimation method and the self-position estimation device according to this embodiment, the speed stored in the profile and the speed stored in the reference profile are compared. Then, if the number of times the speed difference is equal to or greater than the predetermined value is greater than the sixth threshold, the detection unit that outputs the measurement result for creating a profile greater than the sixth threshold is specified. As a result, even if the error in each speed stored in the profile is small, when there are many speeds containing errors, the detection unit that output such measurement results can be specified, so the position and orientation of the vehicle can be accurately determined. can be estimated.

Each function illustrated in the above embodiments may be implemented by one or more processing circuits. Processing circuitry includes programmed processors, electrical circuits, etc., as well as devices such as application specific integrated circuits (ASICs) and circuit components arranged to perform the described functions. etc. are also included.

Although the contents of the present invention have been described above according to the embodiments, it is obvious 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 discussion and drawings forming part of this disclosure should not be understood as limiting the invention. Various alternative embodiments, implementations and operational techniques will become apparent to those skilled in the art from this disclosure.

Of course, the present invention includes various embodiments and the like that are not described here. Therefore, the technical scope of the present invention is defined only by the matters specifying the invention according to the scope of claims that are valid from the above description.

31 estimation unit 33 error estimation unit 40 detection unit 71 first selection unit 73 second selection unit 75 third selection unit 91 output unit 93 error integration unit 100 controller 150 detection unit evaluation unit 152 profile creation unit 154 detection unit identification unit

Claims (18)

A self-position estimation method for estimating the position and orientation of the vehicle on map data using measurement results from a plurality of detection units that detect targets around the vehicle,
In the vehicle coordinate system of the vehicle, for each of the plurality of detection units,
estimating the position of the target along the longitudinal direction of the vehicle as a first coordinate value, estimating an error of the first coordinate value as a first error;
estimating the position of the target along the width direction of the vehicle as a second coordinate value, estimating an error of the second coordinate value as a second error;
estimating an angle of a direction from the vehicle toward the target with respect to the longitudinal direction of the vehicle as a third coordinate value, and estimating an error of the third coordinate value as a third error;
selecting, as a first selection value, a first coordinate value with the smallest first error among the estimated plurality of first coordinate values;
selecting, as a second selection value, a second coordinate value with the smallest second error among the estimated plurality of second coordinate values;
selecting the third coordinate value with the smallest third error among the estimated plurality of third coordinate values as the third selection value;
A self-position estimation method, comprising estimating the position and orientation of the vehicle based on the first selection value, the second selection value, and the third selection value. The self-localization method according to claim 1,
In the vehicle coordinate system of the vehicle, for each of the plurality of detection units,
calculating a speed of the target along the longitudinal direction of the vehicle as a first speed, storing a plurality of the first speeds calculated within a predetermined period to create a profile;
calculating a speed of the target along the width direction of the vehicle as a second speed, storing a plurality of the second speeds calculated within a predetermined period to create a profile;
calculating an angular velocity in a direction from the vehicle toward the target with reference to the longitudinal direction of the vehicle as a third velocity, storing a plurality of the third velocities calculated within a predetermined period to create a profile;
Create a reference profile that stores the reference speed,
calculating the difference between the first speed stored in the profile and the speed stored in the reference profile, adding the calculated differences to calculate the sum of the differences, and calculating the sum of the differences If it is greater than the first threshold, identify the detection unit that outputs the measurement result for creating the profile,
calculating the difference between the second speed stored in the profile and the speed stored in the reference profile, adding the calculated differences to calculate the sum of the differences, and calculating the sum of the differences If it is greater than the first threshold, identify the detection unit that outputs the measurement result for creating the profile,
calculating the difference between the third speed stored in the profile and the speed stored in the reference profile, adding the calculated differences to calculate the sum of the differences, and calculating the sum of the calculated differences If it is greater than the first threshold, identify the detection unit that outputs the measurement result for creating the profile,
A first coordinate value that minimizes the first error by excluding the first coordinate value estimated using the specified measurement result of the detection unit, among the estimated plurality of first coordinate values. as the first selection value,
A second coordinate value that minimizes the second error by excluding a second coordinate value that is estimated using the specified measurement result of the detection unit, among the plurality of estimated second coordinate values. as the second selection value, and
A third coordinate value that minimizes the third error by excluding the third coordinate value estimated using the specified measurement result of the detection unit, among the plurality of estimated third coordinate values. as the third selection value. The self-localization method according to claim 2,
A self-position estimation method, wherein a second threshold smaller than the first threshold is used when the speed of the vehicle is equal to or less than a predetermined value. The self-position estimation method according to claim 2 or 3,
A self-position estimation method, wherein a third threshold is used instead of the first threshold when the vehicle is turning. The self-position estimation method according to any one of claims 2 to 4,
If there is a velocity greater than a fourth threshold among the first velocities stored in the profile, the detection unit that outputs the measurement result for creating a profile having a velocity greater than the fourth threshold is specified. death,
If there is a velocity greater than a fourth threshold among the second velocities stored in the profile, the detection unit that outputs the measurement result for creating a profile having a velocity greater than the fourth threshold is specified. death,
If there is a velocity greater than a fourth threshold among the third velocities stored in the profile, the detection unit that outputs the measurement result for creating a profile having a velocity greater than the fourth threshold is specified. A self-position estimation method characterized by: The self-position estimation method according to any one of claims 2 to 5,
calculating acceleration for each of the first velocities stored in the profile, calculating acceleration for each of the velocities stored in the reference profile, and calculating the direction of the acceleration calculated from the first velocity and the velocity of the reference profile; If the calculated number of different directions of acceleration is greater than a fifth threshold, identifying the detection unit that outputs the measurement result for creating a profile greater than the fifth threshold,
calculating acceleration for each of the second velocities stored in the profile, calculating acceleration for each of the velocities stored in the reference profile, and calculating the direction of the acceleration calculated from the second velocity and the velocity of the reference profile If the calculated number of different directions of acceleration is greater than a fifth threshold, identifying the detection unit that outputs the measurement result for creating a profile greater than the fifth threshold,
calculating acceleration for each of the third velocities stored in the profile, calculating acceleration for each of the velocities stored in the reference profile, and calculating the direction of the acceleration calculated from the third velocity and the velocity of the reference profile If the number of times that the direction of the calculated acceleration changes is greater than a fifth threshold, the detecting unit that outputs the measurement result for creating a profile that is greater than the fifth threshold is specified. estimation method. The self-position estimation method according to any one of claims 2 to 6,
The first speed stored in the profile and the speed stored in the reference profile are compared, and if the number of times the difference in speed is equal to or greater than a predetermined value is greater than a sixth threshold, the sixth threshold is determined. Identifying the detection unit that outputs measurement results for creating more profiles,
The second speed stored in the profile is compared with the speed stored in the reference profile, and if the number of times the speed difference is equal to or greater than a predetermined value is greater than a sixth threshold, the sixth threshold is determined. Identifying the detection unit that outputs measurement results for creating more profiles,
The third speed stored in the profile is compared with the speed stored in the reference profile, and if the number of times the speed difference is equal to or greater than a predetermined value is greater than the sixth threshold, the sixth threshold is determined. A method for estimating self-location, characterized by identifying the detection unit that has output measurement results for creating more profiles. The self-position estimation method according to any one of claims 1 to 7,
the first error of the first selected value;
the second error of the second selected value;
A method for estimating self-location, wherein the sum of the third errors of the third selection value is calculated. The self-position estimation method according to any one of claims 1 to 8,
A self-position estimation method, wherein the first error, the second error, and the third error in the detection unit are estimated using an error model for each detection unit. The self-localization method according to claim 9,
A method for estimating self-localization, wherein when the detection unit is a camera, the error model is created by actually measuring the relationship between the moving speed of the camera and the error that occurs. The self-position estimation method according to claim 10,
A self-position estimation method, wherein the error model is created by actually measuring the relationship between the imaging distance of the camera and the error that occurs. The self-position estimation method according to claim 10 or 11,
A self-position estimation method, wherein the error model is created by modeling based on the lens parameters of the camera. The self-position estimation method according to any one of claims 1 to 12,
A self-localization method, comprising estimating the position and orientation of the vehicle based on a vehicle model of the vehicle. The self-localization method according to claim 13,
A self-position estimation method, wherein the vehicle model is created by actual measurement based on odometry and a satellite positioning system mounted on the vehicle. The self-localization method according to claim 13 or 14,
A self-position estimation method, wherein the first error, the second error, and the third error in the detection unit are estimated using the vehicle model and the error model for each of the detection units. The self-position estimation method according to any one of claims 1 to 15,
A method for estimating self-position, wherein the position and orientation of the vehicle are estimated based on the first selection value, the second selection value, and the third selection value selected at a past point in time. The self-position estimation method according to any one of claims 1 to 16,
estimating the position and orientation of the vehicle based on the first error of the first selection value, the second error of the second selection value, and the third error of the third selection value; A self-localization method characterized by: a plurality of detection units that detect targets around the vehicle;
a controller;
A self-localization device that estimates the position and orientation of the vehicle on map data,
The controller is
In the vehicle coordinate system of the vehicle, for each of the plurality of detection units,
estimating the position of the target along the longitudinal direction of the vehicle as a first coordinate value, estimating an error of the first coordinate value as a first error;
estimating the position of the target along the width direction of the vehicle as a second coordinate value, estimating an error of the second coordinate value as a second error;
estimating an angle of a direction from the vehicle toward the target with respect to the longitudinal direction of the vehicle as a third coordinate value, and estimating an error of the third coordinate value as a third error;
selecting, as a first selection value, a first coordinate value with the smallest first error among the estimated plurality of first coordinate values;
selecting, as a second selection value, a second coordinate value with the smallest second error among the estimated plurality of second coordinate values;
selecting the third coordinate value with the smallest third error among the estimated plurality of third coordinate values as the third selection value;
A self-position estimation device, wherein the position and orientation of the vehicle are estimated based on the first selection value, the second selection value, and the third selection value.

Images