JP2023105152A - Estimation device, control method, program, and storage medium - Google Patents

Abstract

To provide an estimation device with which it is possible to estimate the current position with high accuracy.SOLUTION: A driving assistance system comprises: an onboard device 1 mounted in a vehicle, for exercising control that pertains to assistance in driving the vehicle; a lidar 2; a gyro sensor 3; and a vehicle speed sensor 4. The onboard device 1 acquires a measured value ztk from the lidar 2 that indicates a positional relationship between a reference landmark Lk of an index k and the host vehicle. The onboard device 1 corrects a pre-estimate value x-t having been estimated from the movement speed measured by the vehicle speed sensor 4 and the angular speed measured by the gyro sensor 3, on the basis of a position vector mk of the reference landmark Lk included in a map database 10 and the measured value ztk, and thereby calculates a post-estimate value x^t.SELECTED DRAWING: Figure 5

Description

The present invention relates to technology for estimating a current position with high accuracy.

Conventionally, techniques for measuring distances to objects existing in the vicinity have been known. For example, in Patent Document 1, a vehicle is equipped with a lidar that scans in the horizontal direction while intermittently emitting laser light and receives the reflected light (scattered light) to detect point groups on the surface of an object. Examples are disclosed. Further, Patent Literature 2 discloses a point search device having a map database containing latitude and longitude information of a plurality of points to be searched.

JP 2014-089691 A JP 2015-135695 A

In fields such as autonomous driving, it is necessary to estimate the current position with high accuracy. Methods may not be enough. Patent Documents 1 and 2 do not disclose any method for calculating the absolute position of the vehicle with high accuracy.

SUMMARY OF THE INVENTION The present invention has been made to solve the problems described above, and a main object of the present invention is to provide an estimation device capable of estimating a current position with high accuracy.

The invention according to claim 1 is an estimating device comprising: an acquisition unit that acquires map information; and a first estimation unit for estimating the position of the moving object based on the position information of the object and the first information included in the map information.

Further, the invention according to claim 9 is a control method executed by an estimation device, in which an acquisition step of acquiring map information and first information indicating a distance and an angle to an object existing in a first range are obtained. and a first estimation step of estimating the position of the moving object based on the position information of the object and the first information included in the map information. .

Further, the invention according to claim 10 is a program executed by a computer, and includes an acquisition unit that acquires map information, and acquires first information that indicates the distance and angle to an object existing in the first range. The computer is caused to function as a first acquisition unit and a first estimation unit that estimates the position of the moving object based on the position information of the object and the first information included in the map information. .

1 is a schematic configuration diagram of a driving support system according to a first embodiment; FIG. 3 is a block diagram showing a functional configuration of an in-vehicle device; FIG. It is the figure which represented the own vehicle position on the two-dimensional rectangular coordinate. FIG. 5 is a diagram showing a schematic relationship between a prediction step and a measurement update step; It is a block diagram which shows the functional structure of the own vehicle position estimation part in 1st Example. FIG. 10 is a diagram showing the position of the vehicle before and after the movement assuming circular motion by using each variable; It is a figure which shows the relationship between the position of a landmark, and a self-vehicle position. FIG. 4 is a diagram showing a functional configuration of a landmark extraction block; FIG. FIG. 4 is a diagram showing the relationship between a lidar scan range and a landmark search range; 4 is a flow chart showing a procedure of processing executed by a vehicle position estimator in the first embodiment; 9 is a flowchart showing details of landmark extraction processing; FIG. 11 is a schematic configuration diagram of a driving support system according to a second embodiment; FIG. 11 is a block diagram showing a functional configuration of a vehicle position estimator in the second embodiment; FIG. It is a figure which shows the relationship between the position of a landmark, and a self-vehicle position.

According to a preferred embodiment of the present invention, the estimating device includes an acquisition unit that acquires map information, and a first acquisition unit that acquires first information indicating the distance and angle to an object existing in the first range. and a first estimation unit for estimating the position of the moving object based on the position information of the object and the first information included in the map information.

The estimation device includes an acquisition unit, a first acquisition unit, and a first estimation unit. The acquisition unit acquires map information. The first acquisition unit acquires first information indicating a distance and an angle to an object existing in the first range (that is, the positional relationship between the moving body and the feature existing in the first range). The first estimation unit estimates the position of the moving object based on the position information of the object and the first information included in the map information. In this aspect, the estimation device can accurately estimate the position of the moving object by using the position information of the feature registered in the map information.

In one aspect of the estimating device, the estimating device further includes a second estimating unit that calculates a first estimated position that is an estimated position of the current position of the moving object, and the first estimating unit includes the object and the The position of the moving object is estimated based on the first estimated position and the difference between second information indicating the positional relationship with the first estimated position and the first information. According to this aspect, the estimating device can highly accurately estimate the position of the moving object based on the first estimated position calculated by the second estimating section and the difference between the second information and the first information.

In another aspect of the estimating device, the second estimating section calculates the first estimated position based on at least the estimated position of the moving object a predetermined time ago. As a result, the estimation device can suitably estimate the current position of the mobile object by taking into account the estimated position of the mobile object a predetermined time ago.

In another aspect of the estimating device, the estimating device further includes a second acquiring unit that acquires the control information of the moving object, and the second estimating unit includes the estimated position of the moving object a predetermined time ago and the moving object. The first estimated position is calculated based on the body control information. According to this aspect, the estimating device can calculate the first estimated position with high accuracy and low computational load from the estimated position of the moving object a predetermined time ago.

In another aspect of the estimating device, the second estimating unit calculates the first estimated position in a prediction step, and calculates in the previous prediction step based on the difference between the first information and the second information. and an updating step in which the first estimating unit corrects the first estimated position obtained by the prediction step alternately, and in the predicting step, the current time The second estimation unit calculates the first estimated position corresponding to . In this aspect, the estimating device performs the update step and the prediction step alternately to correct the previously calculated first estimated position, while calculating the first estimated position corresponding to the current time with high accuracy and low accuracy. It can be calculated according to the load.

In another aspect of the estimation device, the first acquisition unit includes an irradiation unit that emits laser light while changing the irradiation direction, a light receiving unit that receives the laser light reflected by the object, and the Measurement comprising: an output unit configured to output the first information based on a received light signal output by a light receiving unit, the irradiation direction corresponding to the laser light received by the light receiving unit, and a response delay time of the laser light. Obtaining the first information from the device. According to this aspect, the first acquisition unit can suitably generate and output the first information indicating the distance and angle to the object existing in the first range. In addition, in this aspect, since the three-dimensional features registered in the map information are measured, even in various situations such as the disappearance of white lines on the road surface due to snow accumulation, nighttime, etc., it can be used as a suitable reference. The first information can be generated by measuring the distance and bearing to the feature.

In another aspect of the estimation device, the feature is an artificial object. As a result, the first acquisition unit can generate the first information more stably than when targeting natural objects.

In another aspect of the estimation device, the feature is an artificial object arranged periodically. This allows the estimation device to periodically estimate the position of the mobile object.

According to another preferred embodiment of the present invention, there is provided a control method executed by an estimating device, comprising: an acquiring step of acquiring map information; The method includes a first obtaining step of obtaining information, and a first estimating step of estimating the position of the moving object based on the first information and the positional information of the object included in the map information. By executing this control method, the estimating device can accurately estimate the position of the moving object using the position information of the feature registered in the map information.

According to another preferred embodiment of the present invention, there is provided a program executed by a computer, which acquires map information, and first information indicating the distance and angle to an object existing in a first range. The computer is caused to function as a first acquisition unit that acquires and a first estimation unit that estimates the position of the moving object based on the position information of the object and the first information included in the map information. By executing this program, the computer can accurately estimate the position of the moving object using the position information of the feature registered in the map information. Preferably, the program is stored in a storage medium.

Preferred embodiments of the present invention will be described below with reference to the drawings.

<First embodiment>
(1) Schematic Configuration FIG. 1 is a schematic configuration diagram of the driving support system according to the first embodiment. The driving support system shown in FIG. 1 is mounted on a vehicle and includes an in-vehicle device 1 that performs control related to driving support of the vehicle, a lidar (Light Detection and Ranging, or Laser Illuminated Detection And Ranging) 2, and a gyro sensor 3. and a vehicle speed sensor 4 .

The in-vehicle device 1 is electrically connected to the rider 2, the gyro sensor 3, and the vehicle speed sensor 4, and based on the outputs of these, the position of the vehicle in which the in-vehicle device 1 is mounted (also referred to as "vehicle position"). make an estimate. Based on the estimation result of the position of the vehicle, the vehicle-mounted device 1 performs automatic operation control of the vehicle so that the vehicle travels along the set route. The in-vehicle device 1 stores a map database (DB: DataBase) 10 that stores road data and information (also referred to as "landmark information") on landmarks that serve as landmarks provided near roads. Landmark information is information in which at least an index assigned to each landmark is associated with landmark position information. Based on this landmark information, the in-vehicle device 1 limits the search range for landmarks by the rider 2, compares the output with the output of the rider 2, etc., and estimates the position of the vehicle. Hereinafter, a landmark that the vehicle-mounted device 1 uses as a reference for estimating the position of the vehicle will be referred to as a "reference landmark Lk", and the index of the reference landmark Lk will be "k". The reference landmark Lk is an example of the "object" in the present invention.

Landmarks that are candidates for the reference landmark Lk are, for example, features such as kilometer posts, 100m posts, delineators, traffic infrastructure facilities (e.g., signs, direction signs, traffic lights), telephone poles, and streetlights that are periodically arranged along the road. is. Preferably, the above-mentioned landmarks are preferably artificial objects in order to enable stable measurement, and are periodically provided features so as to be able to periodically correct the position of the vehicle. is more preferred. It should be noted that the intervals do not have to be strictly constant, but may be present with a certain degree of periodicity (for example, utility poles, streetlights, etc.). Alternatively, the intervals may be different for each travel area.

The lidar 2 discretely measures the distance to an object existing in the outside world by emitting a pulsed laser over a predetermined angular range in the horizontal and vertical directions, and generates a three-dimensional point indicating the position of the object. Generate group information. In this case, the lidar 2 includes an irradiation unit that irradiates laser light while changing the direction of irradiation, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and scan data based on light receiving signals output by the light receiving unit. and an output unit for outputting The scan data is generated based on the irradiation direction corresponding to the laser light received by the light receiving unit and the response delay time of the laser light specified based on the light reception signal described above. In this embodiment, it is assumed that the rider 2 is installed facing the traveling direction of the vehicle so as to scan at least the front of the vehicle. The rider 2, the gyro sensor 3, and the vehicle speed sensor 4 each supply output data to the onboard device 1. FIG. The vehicle-mounted device 1 is an example of the "estimating device" in the present invention, and the rider 2 is an example of the "measuring device" in the present invention.

FIG. 2 is a block diagram showing the functional configuration of the vehicle-mounted device 2. As shown in FIG. The in-vehicle device 2 mainly has an interface 11 , a storage section 12 , an input section 14 , a control section 15 and an output section 16 . These elements are interconnected via bus lines.

The interface 11 acquires output data from sensors such as the rider 2 , the gyro sensor 3 , and the vehicle speed sensor 4 and supplies the output data to the control unit 15 .

The storage unit 12 stores programs executed by the control unit 15 and information necessary for the control unit 15 to execute predetermined processing. In this embodiment, the storage unit 12 stores a map DB 10 including landmark information. Note that the map DB 10 may be updated periodically. In this case, for example, the control unit 15 receives partial map information related to the area to which the vehicle position belongs from a server device that manages map information via a communication unit (not shown), and reflects it in the map DB 10 .

The input unit 14 is a button, a touch panel, a remote controller, a voice input device, etc. for user operation, and accepts input specifying a destination for route search, input specifying ON and OFF of automatic driving, etc. . The output unit 16 is, for example, a display, a speaker, or the like that outputs based on the control of the control unit 15 .

The control unit 15 includes a CPU that executes programs and the like, and controls the entire in-vehicle device 1 . In this embodiment, the control unit 15 has an own vehicle position estimation unit 17 and an automatic driving control unit 18 .

The vehicle position estimator 17 calculates the output data of the gyro sensor 3 and the vehicle speed sensor 4 based on the measured values of the distance and angle of the rider 2 with respect to the reference landmark Lk and the position information of the reference landmark Lk extracted from the map DB 10. Correct the vehicle position estimated from At this time, the vehicle position estimator 17 estimates the vehicle position from the output data of the gyro sensor 3 and the vehicle speed sensor 4 based on a state estimation method based on Bayesian estimation, and the vehicle position calculated in the previous prediction step. A measurement update step for correcting the estimated vehicle position is alternately performed. In the first embodiment, an example using an extended Kalman filter will be described later as an example of a state estimation method based on Bayesian estimation. The vehicle position estimation unit 17 is the "acquisition unit", the "first acquisition unit", the "first estimation unit", the "second estimation unit", and the "second acquisition unit" in the present invention, and a computer that executes a program. is an example.

The automatic driving control unit 18 refers to the map DB 10 and automatically drives the vehicle based on the set route and the vehicle position estimated by the vehicle position estimation unit 17 . The automatic driving control unit 18 sets the target trajectory based on the set route, and controls the vehicle so that the vehicle position estimated by the vehicle position estimating unit 17 has a deviation within a predetermined width from the target trajectory. to control the position of the vehicle by transmitting guidance signals.

(2) Vehicle Position Estimation Using Extended Kalman Filter Next, vehicle position estimation processing by the vehicle position estimator 17 will be described.

(2-1) Basic Description Below, the basic items that are prerequisites for the processing executed by the vehicle position estimation unit 17 will be described. In the following description, the vehicle position is represented by a state variable vector "x=(x, y, θ)". In addition, the tentative estimated value estimated in the prediction step is marked with “ ^- ” above the character representing the estimated value, and the more accurate estimated value updated in the measurement update step is indicated by the value. Add " ^{^} " above the character to represent. In this specification, for the sake of convenience, a character with "^" or "-" above any symbol is represented as "A ^{^} " or " ^A- " (where "A" is an arbitrary character).

FIG. 3 is a diagram showing the state variable vector x in two-dimensional rectangular coordinates. As shown in FIG. 3, the vehicle position on the plane defined on the xy two-dimensional orthogonal coordinates is represented by the coordinates "(x, y)" and the vehicle azimuth "θ". Here, the azimuth .theta. is defined as the angle formed by the traveling direction of the vehicle and the x-axis. The coordinates (x, y) indicate an absolute position, for example a combination of latitude and longitude.

FIG. 4 is a diagram showing a schematic relationship between the prediction step and the measurement update step. In the state estimation method based on Bayesian estimation such as the Kalman filter, as described above, estimation values are sequentially calculated and updated by two-step processing in which a prediction step and a measurement update step are alternately performed. Therefore, in this embodiment, the prediction step and the measurement update step are repeated to sequentially calculate and update the estimated value of the state variable vector x. Hereinafter, the state variable vector of the reference time (that is, the current time) “t” to be calculated is expressed as “x ⁻ _t ” or “x ^{^} _t ”.

In the prediction step, the vehicle position estimator 17 calculates the moving speed “ ^v _” and the angular velocity “ω” ( These are collectively expressed as “control value u _t =(v _t , ω _t ) ^T ”), the estimated value of the vehicle position at time t (also referred to as “preliminary estimated value”) x ^- Calculate _t . The control value u _t is an example of "control information" in the present invention, and the pre-estimated value x ⁻ _t and the post-estimated value x ^̂ _t are examples of the "first estimated position" in the present invention. At the same time, the vehicle position estimating unit 17 calculates the covariance matrix (also referred to as the “prior covariance matrix”) “Σ ⁻ _t ” corresponding to the error distribution of the prior estimation value x ⁻ _t from the previous measurement. It is calculated from the covariance matrix “Σ ^̂ _t−1 ” at time t−1 calculated in the update step.

In the measurement update step, the vehicle position estimating unit 17 obtains the measurement value “z _t ” of the reference landmark Lk by the rider 2 and the measurement process by the rider 2 by modeling from the pre-estimated value x ⁻ _t . , respectively, ^of the reference landmark _Lk . The measured value _zt is a two-dimensional vector representing the distance and scan angle of the reference landmark Lk measured by the rider 2 at time t, as will be described later. Then, the vehicle position estimator 17 multiplies the difference between the measured value _zt and the measured estimated value z ^{^} _t by the Kalman gain " _Kt ", which is obtained separately, as shown in the following equation (1), and calculates Add to the a priori estimate _x ⁻ _t to compute an updated state variable vector (also called “posterior estimate”) ^x̂t .

Further, in the measurement update step, the vehicle position estimation unit 17 updates the covariance matrix (also referred to as a “posterior covariance matrix”) Σ ^{^} _t corresponding to the error distribution of the posterior estimated value x ^{^} in the same manner as in the prediction step. It is obtained from the covariance matrix Σ ^- _t .

(2-2) Processing Overview FIG. 5 is a block diagram showing the functional configuration of the vehicle position estimation unit 17. As shown in FIG. The vehicle position estimation unit 17 mainly includes a state transition model block 20, a covariance calculation block 21, a landmark extraction block 22, a measurement model block 23, a covariance calculation block 24, and a Kalman gain calculation block 25. , a covariance update block 26 , operation blocks 31 to 33 , and unit delay blocks 34 and 35 .

A state transition model ^{( velocity} behavior _{model )} block ₂₀ obtains an a _priori estimated value x ⁻ _t =(x ⁻ _t , y ⁻ _t , θ ⁻ _t ) ^T is calculated. FIG. 6 shows the pre-estimated values “x ⁻ _t ”, “y ⁻ _t ”, and “θ ⁻ _t ” at the reference time t and the posterior estimated values “x ^{^} _t-1 ”, “y ^{^} _t−1 ” and “θ ^̂ _t−1 ”. According to the geometric relationship shown in FIG. 6, the pre-estimated value x ⁻ _t = (x ⁻ _t , y ⁻ _t , θ ⁻ _t ) ^T is represented by the following equation (2). Note that “Δt” represents the time difference between time t and time t−1.

Therefore, the state transition model block 20 uses the posterior estimated value x ^̂ _t− 1 at time t−1 and the control value u _t =(v _t , ω _t ) ^T based on equation (2) to perform the prior estimation Calculate the value x ⁻ _t .

The covariance calculation block 21 calculates the matrix “Rt” representing the error distribution obtained by transforming the error distribution of the control value u _t into the three-dimensional space of the state variable vector (x, y, θ), and the state shown in equation (2). The prior covariance matrix Σ ⁻ _t is calculated based on equation (3) using the Jacobian matrix “Gt”, which is the linearized transition model around ^x̂t ₋₁ .

Here, the Jacobian matrix Gt is represented by the following equation (4).

The landmark extraction block 22 uses the output of the lidar 2 and the pre-estimated value x ⁻ _t to extract a position vector “m _k = (m _{k, x} , m _{k, y} )”. In addition, the landmark extraction block 22 supplies the measured value “z _t ^k =(r _t ^k ,φ _t ^k ) ^T ” of the rider 2 corresponding to the extracted reference landmark Lk of the index k to the calculation block 31. . Here, the measured value z _t ^k is the distance “r _t ^k ” to the landmark of index k and the scan angle “φ _t ^k ” to the landmark of index k when the front direction of the vehicle is 0 degrees. A vector value to be an element. The measured value _ztk is an example ^of "first information" in the present invention. A method of identifying the reference landmark Lk executed by the landmark extraction block 22 will be described later.

The measurement model block 23 calculates the estimated ^value of the measured value z t _k from the position vector m k _of the reference landmark L k with index k and the pre-estimated value x ⁻ _t , “z ^{^} _t ^k = (r ^{^} _t ^k , φ ^{^} _t ^k ) ^T '' is calculated and supplied to the calculation block 31 . where “r ^{^} _t ^k ” denotes the distance to the landmark with index k relative to the prior estimate x ⁻ _t , and “φ ^{^} _t ^k ” is the distance relative to the prior estimate x ⁻ _t . 4 shows the scan angle for the landmark of index k when FIG. 7 is a diagram showing the relationship between the position of the reference landmark Lk and the position of the vehicle. Based on the geometrical relationship shown in FIG. 7, the distance r ^{^} _t ^k is represented by Equation (5) below.

Moreover, the following formula (6) is established based on the relationship shown in FIG.

Therefore, the scan angle φ ^̂ _t ^k is expressed as in Equation (7) below.

Therefore, the measurement model block 23 refers to _the equations (5) and (7) ^to calculate ^the estimated value z ^{^} _tk = (r _^ ^tk , φ ^{^} _tk ^{) T} of the measured value ^ztk , It is supplied to the calculation block 31 . The ^estimated value z ^{^} _tk =(r ^{^} _tk , φ ^{^} _tk ) ^T is an example ^{of the} "second information" in the present invention.

Furthermore, the measurement model block 23 calculates the Jacobian matrix “H _t ^k ” by linearizing the measurement model shown in equations (5) and (7) around the pre-estimated value x ⁻ _t . Here, the Jacobian matrix H _t ^k is represented by the following equation (8).

The measurement model block 23 supplies the Jacobian matrix H _t ^k to the covariance calculation block 24, the Kalman gain calculation block 25 and the covariance update block 26 respectively.

The covariance calculation block 24 calculates a covariance matrix “St _k ^” necessary for calculating the Kalman gain K _t ^k based on the following equation (9).

The covariance calculation block 24 supplies the calculated covariance matrix S _t ^k to the Kalman gain calculation block 25 .

The Kalman gain calculation block 25 calculates the Kalman gain K _t ^k based on the following equation (10).

The covariance update block 26 updates the prior covariance matrix Σ ⁻ _t supplied from the covariance calculation block 21 , the Jacobian matrix H _t ^k supplied from the measurement model block 23 , and the Kalman gain calculation block 25 . Based on the gain K _t ^k , the posterior covariance matrix Σ ^̂ _t is calculated based on the following equation (11) using the identity matrix "I".

The calculation block 31 calculates the difference between the measured value z _t ^k supplied from the landmark extraction block 22 and the estimated value z ^{^} _t ^k supplied from the measurement model block 23 (that is, "z _t ^k −z ^{^} _t ^k "). Calculate The calculation block 32 multiplies the value calculated by the calculation block 31 by the Kalman gain K _t ^k supplied from the Kalman gain calculation block 25 . The calculation block 33 adds the value calculated by the calculation block 32 to the pre-estimation value x ⁻ _t as shown in the following equation (12) to calculate the posterior estimate value x ^̂ .

As described above, the vehicle position estimation unit 17 can perform state estimation with high accuracy by sequentially repeating the prediction step and the measurement update step. Various filters developed for Bayesian estimation can be used as the state estimation filter used in these steps, in addition to the extended Kalman filter. For example, an unscented Kalman filter, a particle filter, or the like may be used instead of the extended Kalman filter.

(2-3) Details of Landmark Extraction Block FIG. 8 is a diagram showing the functional configuration of the landmark extraction block 22. As shown in FIG. As shown in FIG. 8 , the landmark extraction block 22 has a search candidate selection block 41 , a measurement estimated value calculation block 42 , a search range narrowing block 43 and an extraction block 44 .

The search candidate selection block 41 recognizes the scan range (also referred to as “scan range Rsc”) by the rider 2 based on the pre-estimated value x ⁻ _{t ,} and stores the landmarks existing within the recognized scan range Rsc in the map DB 10. Select from In this case, the search candidate selection block 41 defines an area that is ±90° from the azimuth θ ⁻ _t and is within the measurable distance of the rider 2 from the position (x ⁻ _t , y ⁻ _t ) as the scan range Rsc. set. Then, the search candidate selection block 41 extracts the position vector m _k =(m _{k, x} , m _{k, y} ) corresponding to the index k from the map DB 10 . Scan range Rsc is an example of the "first range" in the present invention.

The measurement estimated _value ^calculation block ⁴² calculates _{an estimated} value z _^ ^{tk =} (r ^{^} Calculate _t ^k , φ ^{^} _t ^k ) ^T . Specifically, the measurement estimated value calculation block 42 calculates the estimated value z ^̂ _t ^k = (r ^̂ _t ^k , φ ^̂ _t ^k ) ^T based on Equations (5) and (7) described above.

The search range narrowing block 43 determines that the scan angle is within a predetermined search angle width " _Δφ " from the angle φ ^̂ _t ^k from the scan range Rsc, and the distance measurement range is from the distance r ^̂ _t ^k to the search distance width. A range of "Δ _r " (also called "search range Rtag") is set. ^The search angle width _Δφ and ^the search distance width _Δr are set in advance based on _experiments and the like, taking into consideration the possible error between the measured value _ztk and the estimated value ^ẑtk . ^The _above ^{error is the} prior ^estimate x ⁻ _t ⁼ ₍ x ^{− t} _, ^y _{− t} , θ ⁻ _t ) depends on the accuracy of the estimation of ^T and the accuracy of the lidar 2 outputting the scan data corresponding to the measurements z _t ^k . Therefore, preferably, the search angle width _Δφ and the search distance width _Δr are set in consideration of at least one of the estimation accuracy of the pre-estimated value x ⁻ _t and the accuracy of the rider 2 . Specifically, the search angle width Δ _φ and the search distance width Δ _r become shorter as the pre-estimated value x ⁻ _t has a higher estimation accuracy and as the rider 2 has a higher accuracy.

The extraction block 44 extracts the measured value z _t ^k corresponding to the point group of the reference landmark Lk from all the scan data of the rider 2 at the time t based on the search range Rtag set by the search range narrowing block 43 . Specifically, the extraction block 44 determines that there exists a measured value z _t ⁱ (“i” is the index of each beam emitted by the lidar 2 in one scan) that satisfies the following equations (13) and (14). determine whether or not to

Then, the ^extraction block ⁴⁴ _determines that the search _candidate selection block ₄₁ ^selects Determine that the landmark actually exists. Then, the extraction block 44 supplies the position vector m ^k corresponding to the index k to the measurement model block 23, resulting in a set of (r _t ⁱ , φ _t ⁱ ) that satisfies the equations (13) and (14). The measured value z _t ⁱ is supplied to the calculation block 31 as the measured value z _t ^k corresponding to the point group of the reference landmark Lk.

In this case, preferably, the extraction block 44 further performs a process of selecting scan data corresponding to the reference landmark Lk from the scan data within the search range Rtag, and the measured value _zt to be supplied to the calculation block 31. ^k should be determined.

For example, the extraction block 44 acquires the shape information of the landmark selected by the search candidate selection block 41 from the map DB 10, and performs matching processing with the three-dimensional shape formed by the point cloud of the scan data within the search range Rtag. , a measured value z _t ^k corresponding to the point group of the reference landmark Lk is selected. In another example, the extraction block 44 examines the magnitude of the received light intensity corresponding to the scan data within the search range Rtag and compares it with preset threshold information. Choose a measurement z _t ^k . In addition, the extraction block 44 may supply any one measured value ^ztk to the operation ^block 31 when there are a plurality of selected measured values _ztk , and ^from these _measured values _ztk A representative value calculated or selected by statistical processing may be supplied to the calculation block 31 .

FIG. 9 is a diagram showing the relationship between the scan range Rsc and the search range Rtag. In the example of FIG. 9, the scan range Rsc is a semi-circular area within the measurable distance from the vehicle within the range of 90° left and right from the front of the vehicle. An arrow 70 in FIG. 9 indicates the scanning direction.

In the example of FIG. 9, first, the search candidate selection block 41 identifies the scanning range Rsc based on the pre-estimated value x ⁻ _t , considers the landmarks existing within the scanning range Rsc as the reference landmarks Lk, and determines the corresponding Choose a position vector ^mk . Then, the ^measurement estimated ^{value calculation} _{block 42 calculates} an estimated value z _t ^k = (r ^{t k ,} _φ ^{t k} ) Calculate ^T. The search range narrowing block 43 sets the search range Rtag based on equations (13) and (14). When the scan data exists within the set search range Rtag, the extraction block 44 supplies the position vector ^mk to the measurement model block 23 and calculates the measured ^value _ztk based on the scan data within the search range Rtag. It feeds into block 31 . Note that when scan data does not exist within the set search range Rtag, the vehicle position estimation unit 17 determines that the reference landmark Lk could not be detected by the rider 2, and calculates the pre-estimated value x ⁻ _t Set the estimate x ^̂ _t and set the prior covariance matrix Σ ⁻ _t to the posterior covariance matrix Σ ^̂ _t .

(3) Processing Flow (3-1) Processing Outline FIG. 10 is a flow chart showing the procedure of processing executed by the vehicle position estimation unit 17 in the first embodiment. The own vehicle position estimation unit 17 repeatedly executes the flowchart of FIG. 10 .

First, the vehicle position estimator 17 takes in the posterior estimated value x ^{^} _t-1 and the posterior covariance matrix Σ ^{^} _t-1 at time t-1, and obtains from the gyro sensor 3 and the vehicle speed sensor 4 at time t A control value u _t is obtained (step S101). Next, the state transition model block 20 of the vehicle position estimator 17 calculates the pre-estimated value x ⁻ _t based on Equation (2) (step S102). Then, the covariance calculation block 21 of the vehicle position estimation unit 17 calculates the prior covariance matrix Σ ^- _t based on the equation (3) (step S103).

Next, the landmark extraction block 22 of the vehicle position estimation unit 17 refers to the map DB 10 and executes landmark extraction processing shown in FIG. 11 (step S104). Then, the vehicle position estimating unit 17 determines whether the position vector ^mk of the landmark registered in the map DB 10 and the scan data of the rider 2 are associated with each other (step S105). In this case, the vehicle position estimation unit 17 determines whether or not a predetermined flag indicating that the above-described association has been established has been set, based on the landmark extraction processing shown in FIG. 11 .

Then, when the above-mentioned correspondence is established (step S105; Yes), the covariance calculation block 24 calculates the covariance matrix S _t ^k based on the above equation (9) (step S106). Next, the Kalman gain calculation block 25 calculates the Kalman gain K _t ^k based on the above equation (10) (step S107). Then ^, _{the calculation block 33 calculates the measured value z t} ^k _supplied from the landmark extraction block 22 and the estimated value The posterior estimated value _x̂t is calculated ^by adding _{a value obtained by multiplying the difference from ẑtk (that is, "ztk−ẑtk "} ^{) by} _the ^{Kalman gain Ktk (} _step S108). Also, the covariance update block 26 receives the prior covariance matrix Σ ⁻ _t supplied from the covariance calculation block 21 , the Jacobian matrix H _t ^k supplied from the measurement model block 23 , and the Kalman gain calculation block 25 . The posterior covariance matrix Σ ^̂ _t is calculated based on the Kalman gain K _t ^k and the above equation (11) (step S109). Thereafter, the unit delay block 34 supplies the posterior estimate x ^{^} _t as the posterior estimate x ^{^} _t-1 to the state transition model block 20, and the unit delay block 35 converts the posterior covariance matrix Σ ^{^} _t to the posterior covariance It is supplied to covariance calculation block 21 as matrix Σ ^̂ _t−1 .

On the other hand, if the above-mentioned association cannot be made (step S105; No), the vehicle position estimation unit 17 sets the prior estimated value x ⁻ _t to the posterior estimated value x ^̂ _t , and the prior covariance matrix Σ ⁻ Set _t to _the posterior covariance matrix ^Σ̂t (step S110).

(3-2) Landmark Extraction Processing FIG. 11 is a flowchart showing details of the landmark extraction processing in step S104 of FIG.

First, the search range narrowing block 43 of the landmark extraction block 22 takes in scan data of the rider 2 (step S201). Here, it is assumed that the lidar 2 emits "n" beams whose emission angles are gradually changed by scanning for one cycle, and by measuring the received light intensity and the response time of the reflected light of each beam, Assume that measured values z _t ¹ to z _t ⁿ are output respectively.

Next, the search candidate selection block 41 selects the position vector ^mk of the landmark existing in the scanning range Rsc of the rider 2 from the map DB 10 (step S202). In this case, the search candidate selection block 41 specifies, as the scan range Rsc, an area that is ±90° from the direction θ ⁻ _t and is within the measurable distance of the rider 2 from the position (x ⁻ _t , y ⁻ _t ). and extracts from the map DB 10 a position vector _mk indicating a position within the identified area.

After that, the measurement estimated value calculation block 42 calculates the estimated value ^z _{t k} ⁼ (r ^{t k} , φ ^t _k ⁾ of the measured value z _t ^k when the position vector m _k is measured from the prior estimated value x ⁻ _t . ^T is calculated (step S203). Specifically, the measurement estimated value calculation block 42 calculates the estimated value z ^̂ _t ^k = (r ^̂ _t ^k , φ ^̂ _t ^k ) ^T based on Equations (5) and (7) described above.

Next, the search range narrowing block 43 narrows down the scan angles φ _t ⁱ (i=1 to n) that satisfy equation (13) (step S204). The extraction block 44 determines whether or not there is a measured value z _t ⁱ having the scan angle φ _t ⁱ that satisfies the equation (13) and the distance r ^{^} _t ⁱ is within the range shown in the equation (14). (Step S205). Then, among the measured values z _t ⁱ having the scan angle φ _t ⁱ that satisfies the equation (13), if there is a value whose distance r _t ⁱ is within the range shown by the equation (14) (step S205; Yes), extraction The block 44 further executes processing (known shape/feature extraction processing, use of received light intensity, etc.) to identify the measured value corresponding to the reference landmark Lk from the measured values z _t ⁱ included in this search range. Then, the finally selected measured value is regarded as z _t ^k and extracted (step S206). Then, the extraction block 44 outputs the extracted position vector ^mk and the measured value _ztk , which is the scan data corresponding to the position vector ^mk , and sets a flag indicating that the association is completed ( ^step S207). This flag is referenced in the determination process of step S105 of FIG. 10 described above.

On the other hand ^, the extraction block 44 determines if there is no measured value z _t ⁱ that satisfies the formula ( ₁₃ ) and the distance r ^{^} _t ⁱ is within the range shown in the formula (14) (step S205; No), the flag indicating that the association is established is not set (step S208).

<Second embodiment>
FIG. 12 is a block configuration diagram of a driving support system according to the second embodiment. In the example of FIG. 12 , the vehicle-mounted device 1 is electrically connected to the orientation sensor 5 . The azimuth sensor 5 is, for example, a geomagnetic sensor, a compass, or a GPS compass, and supplies azimuth information corresponding to the traveling direction of the vehicle to the in-vehicle device 1 . The in-vehicle device 1 has the configuration shown in FIG. 2 as in the first embodiment, and includes landmark information registered in the map DB 10 and outputs from the rider 2, the gyro sensor 3, the vehicle speed sensor 4, and the direction sensor 5. Based on the data, the position of the vehicle is estimated. Hereinafter, the same reference numerals are given to the same components as in the first embodiment, and the description thereof will be omitted.

FIG. 13 shows a schematic configuration of the vehicle position estimating section 17A of the vehicle-mounted device 1 in the second embodiment. In the example of FIG. 13 , the vehicle position estimator 17A has a landmark extraction block 22 and a position estimation block 28 . Hereinafter, the direction of the vehicle at the reference time t output by the direction sensor 5 will be denoted as “θ _t ”, and the estimated value of the vehicle position at the reference time t output by the position estimation block 28 will be denoted as “x ⁻ _t = (x ⁻ _t , y ⁻ _t )”.

The landmark extraction block 22 has a search candidate selection block 41, a measurement estimated value calculation block 42, a search range narrowing block 43, and an extraction block 44, and includes a position vector ^mk of the reference landmark Lk and a lidar 2, and outputs the measured values z _t ^k which are the scan data of 2.

Specifically, in the first embodiment, the search candidate selection block 41 specifies the scan range Rsc based on the pre-estimated value x ⁻ _t supplied from the state transition model block 20 . Instead of this, in the second embodiment, the search candidate selection block 41 first determines the xy position of the vehicle based on the geometrical relationship shown in FIG. 6, as in the state transition model block 20 of the first embodiment. Compute a tentative estimate of the coordinates. Specifically, the search candidate selection block 41 uses the estimated value x ⁻ _t−1 of the vehicle position estimated by the position estimation block 28 one time ago, the direction θ _t−1 measured one time ago, and the control Based on the value u _t =(v _t , ω _t ) ^T , a provisional estimate of the xy coordinates of the vehicle position is calculated according to equation (2). Then, the search candidate selection block 41 specifies the scan range Rsc based on the calculated provisional estimated values of the xy coordinates of the vehicle position and the azimuth _θt . Specifically, the search candidate selection block 41 specifies, as the scan range Rsc, an area that is ±90° from the azimuth θ _t and is within the measurable range of the rider 2 from the calculated xy coordinates of the vehicle position. . Then, the search candidate selection block 41 extracts from the map DB 10 the position vector _mk of the landmark indicating the position within the identified area.

A measurement estimated value calculation block 42 calculates the temporary estimated values of the xy coordinates of the vehicle position calculated by the search candidate selection block 41 (corresponding to the pre-estimated values x ⁻ _t in the first embodiment) and the position extracted from the map DB 10. Based on vector _mk and with ^reference to equations (5 ⁾ and (7), an estimated value z ^{^} _tk ⁼ (r ^{^} _tk , φ ^{^} _tk ) ^T of the measured value _ztk ^is calculated. Note that the measurement estimated value calculation block 42, when referring to equations (5) and (7), instead of the prior estimated value x ⁻ _t = (x ⁻ _t , y ⁻ _t ), the search candidate selection block 41 A tentative estimate of the calculated xy coordinates of the vehicle position is used, and the direction θ _t is used instead of the direction θ ⁻ _t . Then, the search range narrowing block 43 sets the search range Rtag shown in equations (13) and (14), and the extraction block 44 satisfies the equations (13) and (14) (r _t ⁱ , φ _t If there is a set of measurements z _t ⁱ (here z _t ^k ) for ⁱ ), then the measurements and the position vector m ^k are provided to the position estimation block 28 . In this case, preferably, the extraction block 44 should further execute a process of selecting scan data corresponding to the reference landmark Lk from the scan data within the search range Rtag and supply it, as in the first embodiment. A metric z _t ^k may be determined.

The position estimation block 28 ^uses the position vector ^mk and the ^measured value _ztk = ( _rtk , _φtk ) ^T supplied from the extraction block 44 ^and the direction _θt output by the direction sensor 5 to Compute the position estimate x ⁻ _t = (x ⁻ _t , y ⁻ _t ). Specifically, when the absolute value |θ _t +φ _t ^k | is less than 90°, the position estimation block 28 calculates the estimated value x ⁻ _t = ( x ⁻ _t , y ⁻ _t ).

On the other hand, if the absolute value |θ _t +φ _t ^k | is greater than 90°, the position estimation block 28 calculates the estimated value x ⁻ _t = ( x ⁻ _t , y ⁻ _t ).

Further, when the absolute value |θ _t +φ _t ^k | is 90°, the position estimation block 28 calculates the estimated value x ⁻ _t = (x ⁻ _t , y ⁻ _t ).

Here, a method of deriving equations (15) to (18) will be described.

^First , since the distance r _t ^k and the scan _{angle φ} ^t _k have ^the geometrical ^relationship _shown in ^{FIG .} =(x ⁻ _t , y ⁻ _t ) and the orientation θ _t have the same relationships as the above equations (5) and (6) respectively. Here, the following formula (19) corresponds to a modified formula of formula (6), and formula (20) corresponds to a modified formula of formula (5).

Then, by substituting equation (19) into equation (20) and arranging it, the following equation (21) is obtained.

Since the sign “±” is included in equation (21), the magnitude relationship between the x-coordinate value “x ⁻ _{t ”} of the estimated value x ⁻ t and the x-coordinate value “m _k,x ” of the position vector m ^k is It is necessary to divide cases based on FIGS. 14A to 14C are diagrams showing the magnitude relationship between the x-coordinate value “x ⁻ _t ” of the estimated value x ⁻ _t and the x-coordinate value “m _{k, x} ” of the position vector m ^k . Specifically, FIG. 14A shows a case where the x-coordinate value “m _{k, x} ” of the position vector m ^k is larger than the x-coordinate value “x ⁻ _t ” of the estimated value x ⁻ _t . FIG. 14(B) shows a case where the x-coordinate value “x ⁻ _t ” of the estimated value x ⁻ _t and the x-coordinate value “m _k,x ” of the position vector m ^k are equal, and FIG. 14(C) indicates the case where the x ^- coordinate value “m _{k , x} ” of the position vector m ^k is smaller than the x-coordinate value “x ⁻ _t ” of the estimated value x − t.

As shown in FIG. 14A, when “θ _t +φ _t ^k ” is less than 90°, “x ⁻ _t <m _k,x ” holds. Also, as shown in FIG. 14B, when "θ _t +φ _t ^k " is 90°, "x ⁻ _t =m _k,x " is established. Furthermore, as shown in FIG. 14(C), when “θ _t +φ _t ^k ” is greater than 90°, “x ⁻ _t >m _{k, x} ” holds.

Therefore, including the case where "θ _t +φ _t ^k " becomes a negative value,
(a) |θ _t +φ _t ^k |<90° → x ⁻ _t < m _{k, x}
(b) |θ _t +φ _t ^k |=90° → x ⁻ _t = m _{k, x}
(c) |θ _t +φ _t ^k |>90° → x ⁻ _t > m _{k, x}
becomes. In the case of (a) above, since the left side of the equation (21) is a positive value, the sign of the right side is "+", so the above equation (15) is derived. In the case of (b) above, the relationship “x ⁻ _t =m _k,x ” shown in Equation (18) holds. Furthermore, in the case of (c) above, since the left side of the equation (21) is a negative value, the sign of the right side is "-", so the above equation (17) is derived. Also, the y-coordinate value “y ⁻ _t ” of the estimated value x ⁻ _t is represented by Equation (16) by transforming Equation (19).

As described above, according to the second embodiment, the vehicle position estimator 17A uses the direction θ _t obtained from the direction sensor 5 to calculate the position vector ^mk of the landmark registered in the map DB 10. and the measured value _ztk =(r ^{^} _tk , φ ^{^} _tk ^{) T} , the estimated ^{value xt} =( ^xt _, ^yt ₎ ^of the vehicle position can _be calculated favorably.

Note that if the position vector ^mk of the landmark registered in the map DB 10 and the scan data of the rider 2 cannot be associated with each other, the vehicle position estimation unit 17A determines the vehicle position calculated by the search candidate selection block 41. A tentative estimate of the xy coordinates of the vehicle position is set as the estimate of the own vehicle position x ⁻ _t = (x ⁻ _t , y ⁻ _t ). That is, in this case, the estimated vehicle position x ⁻ _t = (x ⁻ _t , y ⁻ _t ) is the estimated vehicle position x ⁻ _t−1 estimated by the position estimation block 28 one time earlier, It is an estimated value calculated based on the control value u _t measured by the vehicle speed sensor 4 and the gyro sensor 5 at the reference time t and the heading θ _t−1 measured one time before.

<Modification>
Modifications suitable for the first and second embodiments will be described below.

(Modification 1)
In the first and second embodiments, the search candidate selection block 41 extracts position vectors of a plurality of landmarks existing within the scan range Rsc from the map DB 10, and performs matching processing with the scan data of the rider 2. good.

In general, when a vehicle is running, there are obstacles such as other vehicles in front of and to the side of the vehicle, so that the scan data of the reference landmark Lk selected from the map DB 10 cannot be obtained from the rider 2 (i.e., occlusion). occurs. Considering the above, in this modification, the search candidate selection block 41 extracts the position vectors of a plurality of landmarks existing within the scan range Rsc from the map DB 10, so that occlusion is detected for any of the landmarks. Even if occlusion occurs, the vehicle position is estimated based on landmarks where occlusion does not occur. As a result, the reference landmark Lk required for estimating the position of the vehicle can be detected with a higher probability. Also, if multiple landmarks can be extracted and all of them are available, the measurement update step can be performed multiple times (multiple landmarks can be used to correct the pre-estimation). can). In this case, the accuracy of estimating the position of the vehicle can be improved statistically.

A supplementary description of the processing of this modified example will be given with reference to FIG. 11 . In step S202, the landmark extraction block 22 refers to the map DB 10, and if it determines that a plurality of landmarks exist within the scan range Rsc, selects position vectors of at least two landmarks from the map DB 10. do. Then, at S203, the landmark extraction block 22 computes a measurement estimate for the selected position vector. Then, in steps S204 and S205, the landmark extraction block 22 sets a search range Rtag shown in equations (13) and (14) for each calculated measurement estimated value, and Attempt to extract scan data corresponding to the landmarks selected in If this association is successful, the measured value and position vector corresponding to the scan data are output in step S207.

A supplementary explanation will be given of the processing when a plurality of landmarks are extracted and all of them can be used, with reference to FIG. In this case, a plurality of landmarks have been extracted in the landmark extraction process shown in S104, and it is determined as Yes in step S105. A series of processing from S106 to S109 that follows is processing to be performed on each extracted landmark. , this series of processes is processed in a loop for the number of extracted landmarks.

(Modification 2)
In the second embodiment, the vehicle position estimator 17A may estimate the direction θ _t based on the output of the gyro sensor 3 instead of specifying the direction θ _t based on the output of the direction sensor 5 .

In this case, the vehicle position estimating unit 17A, similarly to FIG. 6 and Equation (2), calculates the direction θ _t−1 calculated one time before, the angular velocity ω _t and the time between time t−1 and time t. By adding the value (ω _t Δt) multiplied by the width Δt, the estimated value of the azimuth θ _t at the reference time is calculated. Also according to this example, similarly to the second embodiment, the estimated value x ⁻ _t of the vehicle position can be preferably calculated without using Bayesian estimation such as an extended Kalman filter.

(Modification 3)
The measured value z _t ^k for the landmark with index k by the rider 2 is obtained by combining the distance “ _rt ^k ” and the scan angle “φ _t ^k ” with respect to the landmark with index k when the front direction of the vehicle is 0 degrees. Although the above explanation is given assuming that the vector values are the elements, some lidar products convert the distance and angle to the target into coordinate values in a three-dimensional space before outputting them. For example, in the case of a lidar that scans in two dimensions, using r _t ^k and φ _t ^k , the output is in the form of rectangular coordinates x _t ^k = r _t ^k cos _{φ t} ^k and y _t ^k = r _t ^k sin φ _t ^k . done. For a lidar product capable of obtaining such an output, for example, the values of r _t ^k and φ _t ^k are simply back-calculated from the values of x t k and y _t ^k , and then the ^method described in the _present invention is applied. Good luck.

(Modification 4)
In the in-vehicle device 1 , instead of storing the map DB 10 in the storage unit 12 , a server device (not shown) may have the map DB 10 . In this case, the vehicle-mounted device 1 acquires the necessary landmark information by communicating with the server device through a communication unit (not shown).

1 vehicle-mounted device 2 rider 3 gyro sensor 4 vehicle speed sensor 5 direction sensor 10 map DB

Claims (1)

an acquisition unit that acquires map information;
a first acquisition unit that acquires first information indicating the distance and angle to an object existing in the first range;
a first estimating unit that estimates the position of the mobile object based on the position information of the object and the first information included in the map information;
An estimating device comprising: