Estimation device, control method, program, and storage medium

The estimation device uses map information and lidar measurements to accurately determine vehicle position by integrating terrestrial features, addressing the precision limitations of conventional methods and improving estimation accuracy in various conditions.

JP2024161108AInactive Publication Date: 2024-11-15PIONEER IP
View PDF 4 Cites 0 Cited by

Patent Information

Application Number
JP2024144186
Authority / Receiving Office
JP · JP
Patent Type
Applications
Current Assignee / Owner
Filing Date
2024-08-26
Publication Date
2024-11-15
Estimated Expiration
Not applicable · inactive patent

AI Technical Summary

Technical Problem

Conventional methods for estimating the current position of a vehicle lack accuracy, particularly in autonomous driving applications, as they rely mainly on internal sensors that may not provide precise absolute positioning.

Method used

An estimation device that acquires map information and first information indicating distance and angle to objects within a range, using a lidar to measure distances and angles to terrestrial features like kilometer posts and traffic infrastructure, and employs a state estimation method like the extended Kalman filter to calculate the vehicle's position with high accuracy.

Benefits of technology

Enables precise estimation of the vehicle's position by utilizing external features registered in the map information, enhancing accuracy even in challenging conditions such as snowfall or nighttime, and allowing for stable and periodic position estimation.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure 2024161108000001_ABST
    Figure 2024161108000001_ABST
Patent Text Reader

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
Need to check novelty before this filing date? Find Prior Art

Description

[Technical field]

[0001] The present invention relates to a technique for estimating a current position with high accuracy. [Background technology]

[0002] Conventionally, there are known techniques for measuring the distance to a surrounding object. For example, Patent Document 1 discloses an example of a vehicle equipped with a lidar that detects a point cloud on the surface of an object by scanning the horizontal direction while intermittently emitting a laser beam and receiving the reflected light (scattered light). Patent Document 2 discloses a location search device having a map database that includes latitude and longitude information of multiple locations to be searched. [Prior art documents] [Patent documents]

[0003] [Patent Document 1] JP 2014-089691 A [Patent Document 2] JP 2015-135695 A Summary of the Invention [Problem to be solved by the invention]

[0004] In fields such as autonomous driving, it is necessary to estimate the current position with high accuracy, but there are cases where a conventional method of estimating the current position that mainly uses the output of an internal sensor that detects the state of the vehicle is insufficient. Patent Documents 1 and 2 do not disclose any method for calculating the absolute position of a vehicle with high accuracy.

[0005] The present invention has been made in order to solve the above-mentioned problems, and has as its main object to provide an estimation device capable of estimating a current position with high accuracy. [Means for solving the problem]

[0006] The invention described in claim 1 is an estimation device comprising: an acquisition unit that acquires map information; a first acquisition unit that acquires first information indicating a distance and an angle to an object present in a first range; and a first estimation unit that estimates a position of the moving body based on position information of the object contained in the map information and the first information.

[0007] The invention described in claim 9 is a control method executed by an estimation device, characterized in that it includes an acquisition step of acquiring map information, a first acquisition step of acquiring first information indicating a distance and an angle to an object present in a first range, and a first estimation step of estimating a position of the moving body based on position information of the object contained in the map information and the first information.

[0008] The invention described in claim 10 is a program executed by a computer, which causes the computer to function as an acquisition unit that acquires map information, a first acquisition unit that acquires first information indicating a distance and angle to an object present in a first range, and a first estimation unit that estimates the position of the moving body based on position information of the object contained in the map information and the first information. [Brief description of the drawings]

[0009] [Figure 1] 1 is a schematic configuration diagram of a driving assistance system according to a first embodiment. [Diagram 2] FIG. 2 is a block diagram showing a functional configuration of an in-vehicle device. [Diagram 3] This is a diagram showing the vehicle position on a two-dimensional orthogonal coordinate system. [Figure 4] FIG. 1 illustrates a schematic relationship between a prediction step and a measurement update step. [Diagram 5] 3 is a block diagram showing a functional configuration of a vehicle position estimation unit in the first embodiment. FIG. [Figure 6] FIG. 11 is a diagram showing the vehicle positions before and after a circular movement, using various variables. [Figure 7] FIG. 2 is a diagram showing the relationship between landmark positions and the vehicle position. [Figure 8] FIG. 2 is a diagram showing the functional configuration of a landmark extraction block. [Figure 9] FIG. 1 is a diagram showing the relationship between the lidar scanning range and the landmark search range. [Figure 10] 5 is a flowchart showing a procedure of a process executed by a vehicle position estimation unit in the first embodiment. [Figure 11] 13 is a flowchart showing details of a landmark extraction process. [Figure 12] FIG. 11 is a schematic configuration diagram of a driving assistance system according to a second embodiment. [Figure 13] FIG. 11 is a block diagram showing a functional configuration of a vehicle position estimation unit in a second embodiment. [Figure 14] FIG. 2 is a diagram showing the relationship between landmark positions and the vehicle position. DETAILED DESCRIPTION OF THE PREFERRED EMBODIMENTS

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

[0011] 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 present in a first range (i.e., a positional relationship between the moving body and a feature present in the first range). The first estimation unit estimates a position of the moving body based on the position information of the object included in the map information and the first information. In this aspect, the estimation device can accurately estimate the position of the moving body by using the position information of the feature registered in the map information.

[0012] In one aspect of the above estimation device, the estimation device further includes a second estimation unit that calculates a first estimated position that is an estimated position of the current position of the moving object, and the first estimation unit estimates the position of the moving object based on the first estimated position and a difference between the first information and second information indicating a positional relationship between the object and the first estimated position. With this aspect, the estimation device can perform highly accurate estimation of the position of the moving object based on the first estimated position calculated by the second estimation unit and the difference between the second information and the first information.

[0013] In another aspect of the estimation device, the second estimation unit calculates the first estimated position based on an estimated position of the moving body at least a predetermined time ago, thereby enabling the estimation device to appropriately estimate the current position of the moving body by taking into account the estimated position of the moving body a predetermined time ago.

[0014] In another aspect of the estimation device, the estimation device further includes a second acquisition unit that acquires control information of the moving body, and the second estimation unit calculates the first estimated position based on the estimated position of the moving body a predetermined time ago and the control information of the moving body. With this aspect, the estimation device can calculate the first estimated position with high accuracy and low calculation load from the estimated position of the moving body a predetermined time ago.

[0015] In another aspect of the above estimation device, a prediction step in which the second estimation unit calculates the first estimated position and an update step in which the first estimation unit corrects the first estimated position calculated in the immediately preceding prediction step based on a difference between the first information and the second information are alternately executed, and in the prediction step, the second estimation unit calculates the first estimated position corresponding to the current time based on the first estimated position corrected in the update step immediately before the prediction step. In this aspect, the estimation device can calculate the first estimated position corresponding to the current time with high accuracy and low calculation load while correcting the previously calculated first estimated position by alternately executing the update step and the prediction step.

[0016] In another aspect of the estimation device, the first acquisition unit acquires the first information from a measurement device having an irradiating unit that irradiates a laser beam while changing the irradiation direction, a light receiving unit that receives the laser beam reflected by the object, and an output unit that outputs the first information based on a light receiving signal output by the light receiving unit, the irradiation direction corresponding to the laser beam received by the light receiving unit, and a response delay time of the laser beam. With this aspect, the first acquisition unit can preferably generate and output the first information indicating the distance and angle to an object existing in a first range. In addition, with this aspect, since the measurement is performed on a three-dimensional feature registered in map information, the distance and direction to a reference feature can be preferably measured and the first information can be generated even in various situations such as the disappearance of white lines on the road surface due to snowfall or at night.

[0017] In another aspect of the estimation device, the feature is an artificial feature, whereby the first acquisition unit can generate the first information more stably compared to a case in which a natural feature is the target.

[0018] In another aspect of the estimation device, the features are man-made features that are periodically arranged, thereby enabling the estimation device to periodically estimate the position of the moving object.

[0019] According to another preferred embodiment of the present invention, a control method executed by an estimation device includes an acquisition step of acquiring map information, a first acquisition step of acquiring first information indicating a distance and an angle to an object existing in a first range, and a first estimation step of estimating a position of the moving body based on position information of the object included in the map information and the first information. By executing this control method, the estimation device can accurately estimate the position of the moving body by using position information of features registered in the map information.

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

[0021] Preferred embodiments of the present invention will now be described with reference to the drawings.

[0022] <First Example> (1) Schematic configuration Fig. 1 is a schematic diagram of a driving assistance system according to Example 1. The driving assistance system shown in Fig. 1 includes an on-board device 1 that is mounted on a vehicle and controls driving assistance for the vehicle, a Lidar (Light Detection and Ranging, or Laser Illuminated Detection and Ranging) 2, a gyro sensor 3, and a vehicle speed sensor 4.

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

[0024] Candidate landmarks for the reference landmark Lk are, for example, kilometer posts, 100m posts, delineators, traffic infrastructure facilities (e.g., signs, direction signs, traffic signals), utility poles, street lights, and other features that are periodically arranged on the side of the road. Preferably, the above-mentioned landmarks are man-made objects to enable stable measurements, and more preferably, they are periodically arranged features that allow the vehicle position to be periodically corrected. Note that the intervals do not need to be strictly constant, and any object that exists with a certain degree of periodicity (e.g., utility poles, street lights, etc.) may be arranged at different intervals for each driving area.

[0025] The lidar 2 measures the distance to an object in the outside world discretely by emitting a pulsed laser in a predetermined angular range in the horizontal and vertical directions, and generates three-dimensional point cloud information indicating the position of the object. In this case, the lidar 2 has an irradiation unit that irradiates a laser beam while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser beam, and an output unit that outputs scan data based on the light receiving signal output by the light receiving unit. The scan data is generated based on the irradiation direction corresponding to the laser beam received by the light receiving unit and the response delay time of the laser beam specified based on the above-mentioned light receiving signal. In this embodiment, the lidar 2 is installed facing the traveling direction of the vehicle so as to scan at least the area ahead of the vehicle. The lidar 2, the gyro sensor 3, and the vehicle speed sensor 4 each supply output data to the vehicle-mounted device 1. The vehicle-mounted device 1 is an example of an "estimation device" in the present invention, and the lidar 2 is an example of a "measurement device" in the present invention.

[0026] 2 is a block diagram showing the functional configuration of the vehicle-mounted device 2. The vehicle-mounted device 2 mainly includes an interface 11, a storage unit 12, an input unit 14, a control unit 15, and an output unit 16. These elements are connected to each other via a bus line.

[0027] 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 data to the control unit 15.

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

[0029] The input unit 14 is a button, a touch panel, a remote controller, a voice input device, etc. for a user to operate, and accepts inputs specifying a destination for route search, inputs specifying on / off of autonomous driving, etc. The output unit 16 is, for example, a display, a speaker, etc. that perform output based on the control of the control unit 15.

[0030] The control unit 15 includes a CPU that executes a program, and controls the entire vehicle-mounted device 1. In this embodiment, the control unit 15 includes a vehicle position estimation unit 17 and an automatic driving control unit .

[0031] The vehicle position estimation unit 17 corrects the vehicle position estimated from the output data of the gyro sensor 3 and the vehicle speed sensor 4 based on the measurement values ​​of the distance and angle by the lidar 2 relative to the reference landmark Lk and the position information of the reference landmark Lk extracted from the map DB 10. At this time, the vehicle position estimation unit 17 alternately executes a prediction step of estimating 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 a measurement update step of correcting the estimated value of the vehicle position calculated in the immediately preceding prediction step. 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 an example of the "acquisition unit", "first acquisition unit", "first estimation unit", "second estimation unit", "second acquisition unit" and a computer that executes a program in the present invention.

[0032] The automatic driving control unit 18 refers to the map DB 10 and performs automatic driving of 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 a target trajectory based on the set route, and transmits a guide signal to the vehicle to control the position of the vehicle so that the vehicle position estimated by the vehicle position estimation unit 17 deviates from the target trajectory within a predetermined range.

[0033] (2) Vehicle position estimation using an extended Kalman filter Next, the process of estimating the vehicle position by the vehicle position estimating unit 17 will be described.

[0034] (2-1) Basic explanation In the following, basic matters that are the premise of the process executed by the vehicle position estimation unit 17 will be described. In the following description, the vehicle position will be represented by a state variable vector "x=(x, y, θ)". In addition, the provisional estimated value estimated in the prediction step will have " - " is added to the character representing the value, and the more accurate estimates updated in the measurement update step are marked with " ^ For the sake of convenience, in this specification, any symbol with "^" or "-" above it is referred to as "A ^ " or "A - ” (where “A” is any letter).

[0035] Fig. 3 is a diagram showing the state variable vector x in two-dimensional Cartesian coordinates. As shown in Fig. 3, the vehicle position on a plane defined on the two-dimensional Cartesian coordinates of xy is represented by coordinates "(x, y)" and the vehicle's orientation "θ". Here, the orientation θ is defined as the angle between the vehicle's traveling direction and the x-axis. The coordinates (x, y) indicate an absolute position equivalent to, for example, a combination of latitude and longitude.

[0036] FIG. 4 is a diagram showing a schematic relationship between the prediction step and the measurement update step. In a state estimation method based on Bayesian estimation such as the Kalman filter, as described above, the calculation and update of the estimated value are sequentially performed by a two-step process in which the prediction step and the measurement update step are alternately performed. Therefore, in this embodiment, the calculation and update of the estimated value of the state variable vector x are sequentially performed by repeating the prediction step and the measurement update step. Hereinafter, the state variable vector at the reference time (i.e., the current time) "t" to be calculated is referred to as "x - t " or "x ^ t " should be written.

[0037] In the prediction step, the vehicle position estimation unit 17 estimates the state variable vector x ^ t-1The vehicle's moving speed "v" and angular velocity "ω" (collectively referred to as the "control value u t =(v t , ω t ) T ".) is applied to obtain the estimated value of the vehicle position at time t (also called the "prior estimated value") x - t Calculate the control value u t is an example of the "control information" in the present invention, and the prior estimate x - t and the posterior estimate x ^ t is an example of the "first estimated position" in the present invention. At the same time, the vehicle position estimating unit 17 calculates the advance estimated value x - t The covariance matrix (also called the "prior covariance matrix") corresponding to the error distribution of Σ - t ” is the covariance matrix “Σ ^ t-1 " is calculated.

[0038] In the measurement update step, the vehicle position estimation unit 17 updates the measurement value “z t " and the measurement processing by LIDAR 2 is the pre-estimated value x - t The estimated measurement value of the above-mentioned reference landmark Lk obtained by modeling from ^ t " and " are obtained. t is a two-dimensional vector that represents the distance and scan angle of the reference landmark Lk measured by the LIDAR 2 at time t, as described later. Then, the vehicle position estimation unit 17 calculates the measured value z t and the measurement estimate z ^ t The Kalman gain "K t " and multiply this by the prior estimate x - t By adding it to the updated state variable vector (also called the "posterior estimate") x ^ t Calculate.

[0039]

number

[0040] (2-2) Processing Overview 5 is a block diagram showing the functional configuration of the vehicle position estimation unit 17. 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, a Kalman gain calculation block 25, a covariance update block 26, operation blocks 31 to 33, and unit delay blocks 34 and 35.

[0041] The state transition model (speed motion model) block 20 calculates the posterior estimate x at time t-1. ^ t-1 From the control value u t =(v t , ω t ) T Based on the above, the prior estimate x - t =(x - t , y - t , θ - t ) T Figure 6 shows the prior estimate "x - t ", "y - t ","θ - t " and the posterior estimate "x ^ t-1 ", "y ^ t-1 ","θ ^ t-1According to the geometric relationship shown in FIG. 6, the prior estimate x - t =(x - t , y - t , θ - t ) T is expressed by the following formula (2): where "Δt" represents the time difference between time t and time t-1.

[0042]

number

[0043] The covariance calculation block 21 calculates the control value u t The error distribution of the state variable vector (x, y, θ) is transformed into a three-dimensional space, and the state transition model shown in equation (2) is transformed into a matrix "Rt" that shows the error distribution of the state variable vector (x, y, θ) into a three-dimensional space, and the state transition model shown in equation (2) is transformed into ^ t-1 Using the Jacobian matrix Gt linearized around, the prior covariance matrix Σ - t Calculate.

[0044]

number

[0045]

number

[0046] The measurement model block 23 calculates the position vector m of the reference landmark Lk of index k. k and the prior estimate x - t From the measured value z t k The estimated value of z ^ t k =(r ^ t k , φ ^ t k ) T " is calculated and supplied to the calculation block 31. ^ t k " is the prior estimate x - t φ denotes the distance to the landmark of index k based on the distance ^ t k" is the prior estimate x - t FIG. 7 shows the relationship between the position of the reference landmark Lk and the vehicle position. Based on the geometric relationship shown in FIG. 7, the distance r ^ t k is expressed as the following equation (5).

[0047]

number

[0048]

number

[0049]

number

[0050] Furthermore, the measurement model block 23 converts the measurement model shown in equations (5) and (7) into the prior estimate x - t The Jacobian matrix "H t k " is calculated, where the Jacobian matrix H t k is expressed by the following equation (8).

[0051]

number

[0052] The covariance calculation block 24 calculates the Kalman gain K t k The covariance matrix "S t k " is calculated based on the following formula (9).

[0053]

number

[0054] The Kalman gain calculation block 25 calculates the Kalman gain K t k is calculated based on the following formula (10).

[0055]

number

[0056]

number

[0057]

number

[0058] (2-3) Details of the landmark extraction block Fig. 8 is a diagram showing the functional configuration of the landmark extraction block 22. As shown in Fig. 8, the landmark extraction block 22 has a search candidate selection block 41, a measurement estimate value calculation block 42, a search range narrowing block 43, and an extraction block 44.

[0059] The search candidate selection block 41 selects the prior estimate x - t Based on 、 The scan range (also called the "scan range Rsc") by the LIDAR 2 is recognized, and landmarks existing within the recognized scan range Rsc are selected from the map DB 10. In this case, the search candidate selection block 41 selects landmarks based on the direction θ - t ±90° from the position (x - t , y - t ) within the distance measurable by the LIDAR 2 is set as the scan range Rsc. Then, the search candidate selection block 41 selects the position vector m k =(m k,x , m k,y ) is extracted from the map DB 10. The scan range Rsc is an example of the "first range" in the present invention.

[0060] The measurement estimate calculation block 42 calculates the prior estimate x - t and the position vector m corresponding to the index k extracted from the map DB10. k Based on this, the measured value z t k The estimated value of z ^ t k =(r ^ t k , φ ^ t k ) T Specifically, the measurement estimate calculation block 42 calculates the estimate value z ^ t k =(r ^t k , φ ^ t k ) T Calculate.

[0061] The search range narrowing block 43 narrows down the scan range Rsc to a scan angle of φ ^ t k from the predetermined search angle width "Δ φ " and the measurement range is within distance r ^ t k Search distance width "Δ r The range (also called the "search range Rtag") within which the search angle width Δ φ and search distance width Δ r are the expected measured values ​​z t k and the estimated value z ^ t k It is set in advance based on experiments, etc., taking into account the error with the estimated value z ^ t k =(r ^ t k , φ ^ t k ) T The prior estimate x required for the calculation of (see equations 5 and 7) - t =(x - t , y - t , θ - t ) T The estimated accuracy of and the measured value z t k Therefore, it is preferable that the search angle width Δ φ and search distance width Δ r is the prior estimate x - t is set taking into consideration at least one of the estimation accuracy of the search angle and the accuracy of the LIDAR 2. φ and search distance width Δr is the prior estimate x - t The higher the estimation accuracy, the shorter the time. Also, the higher the accuracy of LIDAR 2, the shorter the time.

[0062] The extraction block 44 extracts the measurement value z corresponding to the point group of the reference landmark Lk based on the search range Rtag set by the search range narrowing block 43. t k is extracted from all the scan data of the lidar 2 at time t. Specifically, the extraction block 44 extracts the measured value z t i (where "i" is the index of each beam emitted by LIDAR 2 in one scan) is determined to exist or not.

[0063]

number

[0064]

number

[0065] In this case, preferably, the extraction block 44 further executes a process of selecting scan data corresponding to the reference landmark Lk from the scan data within the search range Rtag, and extracts the measurement value z t k It is advisable to determine the following.

[0066] For example, the extraction block 44 obtains 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 constituted by the point cloud of the scan data within the search range Rtag to obtain the measurement value z t k In another example, the extraction block 44 checks the magnitude of the received light intensity corresponding to the scan data within the search range Rtag and compares it with preset threshold information to select the measurement value z corresponding to the point group of the reference landmark Lk. t k The extraction block 44 selects the selected measurement value z t k If there are multiple values, any one of the measured values ​​z t k may be fed to the calculation block 31, and these measurements z t k Alternatively, a representative value calculated or selected by statistical processing may be supplied to the calculation block 31.

[0067] 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 semicircular area within a range of 90° to the left and right from the front of the vehicle and within a distance measurable from the vehicle. Note that an arrow 70 in Fig. 9 indicates the scan direction.

[0068] In the example of FIG. 9, first, the search candidate selection block 41 selects a priori estimate x - t The scan range Rsc is identified based on the above, and the landmarks present within the scan range Rsc are regarded as reference landmarks Lk, and the corresponding position vector m kThen, the measurement estimate calculation block 42 selects the position vector m of the reference landmark Lk. k and the prior estimate x - t From the measured value z t k The estimated value of z ^ t k =(r ^ t k , φ ^ t k ) T The search range narrowing block 43 sets the search range Rtag based on the formulas (13) and (14). If the scan data exists within the set search range Rtag, the extraction block 44 calculates the position vector m k is supplied to the measurement model block 23, and the measurement value z based on the scan data within the search range Rtag is t k to the calculation block 31. If no scan data exists within the set search range Rtag, the vehicle position estimation unit 17 determines that the reference landmark Lk has not been detected by the LIDAR 2, and supplies the advance estimated value x - t The posterior estimate x ^ t and the prior covariance matrix Σ - t Let us use the posterior covariance matrix Σ ^ t Set to.

[0069] (3) Processing flow (3-1) Processing Overview 10 is a flowchart showing the procedure of the process executed by the vehicle position estimation unit 17 in Example 1. The vehicle position estimation unit 17 repeatedly executes the flowchart in FIG.

[0070] First, the vehicle position estimation unit 17 calculates the posterior estimated value x ^ t-1 and the posterior covariance matrix Σ ^ t-1and a control value u at time t is acquired from the gyro sensor 3 and the vehicle speed sensor 4. t (Step S101). Next, the state transition model block 20 of the vehicle position estimation unit 17 obtains a priori estimated value x - t (Step S102). Then, the covariance calculation block 21 of the vehicle position estimation unit 17 calculates the prior covariance matrix Σ ー t Calculate (step S103).

[0071] Next, the landmark extraction block 22 of the vehicle position estimation unit 17 refers to the map DB 10 and executes a landmark extraction process shown in FIG. 11 (to be described later) (step S104). Then, the vehicle position estimation unit 17 extracts the landmark position vector m k The vehicle position estimation unit 17 determines whether or not the image data has been associated with the scan data of the LIDAR 2 (step S105). In this case, the vehicle position estimation unit 17 determines whether or not a predetermined flag indicating that the above-mentioned association has been established has been set based on the landmark extraction process shown in FIG.

[0072] If the above-mentioned correspondence is established (step S105; Yes), the covariance calculation block 24 calculates the covariance matrix S t k (Step S106). Next, the Kalman gain calculation block 25 calculates the Kalman gain K t k Based on the above-mentioned formula (10) (step S107), the calculation block 33 calculates the a priori estimated value x - t For the measurement value z t k and the estimated value z provided by the measurement model block 23 ^ t k The difference between z t k -z ^ t k") to the Kalman gain K t k The posterior estimate x ^ t (step S108). The covariance update block 26 also calculates the prior covariance matrix Σ - t and the Jacobian matrix H provided by the measurement model block 23 t k and the Kalman gain K provided by the Kalman gain calculation block 25. t k Based on this, the posterior covariance matrix Σ ^ t (step S109). Then, the unit delay block 34 calculates the a posteriori estimate x ^ t The posterior estimate x ^ t-1 to the state transition model block 20, and the unit delay block 35 provides the posterior covariance matrix Σ ^ t Let us use the posterior covariance matrix Σ ^ t-1 to the covariance calculation block 21.

[0073] On the other hand, if the above-mentioned correspondence cannot be established (step S105; No), the vehicle position estimation unit 17 calculates the prior estimated value x - t The posterior estimate x ^ t and the prior covariance matrix Σ - t Let us use the posterior covariance matrix Σ ^ t (step S110).

[0074] (3-2) Landmark extraction processing FIG. 11 is a flowchart showing details of the landmark extraction process in step S104 of FIG.

[0075] First, the search range narrowing block 43 of the landmark extraction block 22 imports scan data of the LIDAR 2 (step S201). Here, the LIDAR 2 emits "n" beams whose emission angles are gradually changed by one scan period, and measures the light receiving intensity and response time of the reflected light of each beam to obtain the measured values ​​z t 1 ~z t n The output shall be:

[0076] Next, the search candidate selection block 41 selects the position vector m of the landmarks present in the scan range Rsc of the lidar 2. k is selected from the map DB 10 (step S202). In this case, the search candidate selection block 41 selects the direction θ - t ±90° from the position (x - t , y - t ) to identify an area within the range where the lidar 2 can measure distance, and obtain a position vector m k is extracted from the map DB10.

[0077] Then, the measurement estimate calculation block 42 calculates the prior estimate x - t from position vector m k When measuring z t k The estimated value of z ^ t k =(r ^ t k , φ ^ t k ) T Specifically, the measurement estimate calculation block 42 calculates the estimate value z ^ t k =(r ^ t k , φ ^t k ) T Calculate.

[0078] Next, the search range narrowing block 43 determines the scan angle φ that satisfies the formula (13). t i (i=1 to n) (step S204). The extraction block 44 narrows down the scan angle φ t i The measured value z t i Of these, distance r ^ t i It is determined whether or not there exists a scan angle φ that satisfies the range shown in the formula (14) (step S205). t i The measured value z t i Of these, distance r t i If there is a range in which z is in the range shown in equation (14) (step S205; Yes), the extraction block 44 extracts the measured value z t i Then, a process is further performed to identify the measurement value corresponding to the reference landmark Lk from among the measured values ​​(known shape and feature extraction process, use of received light intensity, etc.), and the finally selected measurement value is designated as z t k Then, the extraction block 44 extracts the extracted position vector m k and the position vector m k The measured value z, which is the scan data corresponding to t k The CPU 11 outputs the corresponding item and sets a flag indicating that the correspondence has been established (step S207). This flag is referenced in the determination process in step S105 of FIG.

[0079] On the other hand, the extraction block 44 has a scan angle φ that satisfies the formula (13). t i The measured value z t i Of these, distance r ^t i If there is no such range as shown in formula (14) (step S205; No), the flag indicating that the correspondence has been established is not set (step S208).

[0080] <Second Example> FIG. 12 is a block diagram of a driving assistance system according to a second embodiment. In the example of FIG. 12, the vehicle-mounted device 1 is electrically connected to a direction sensor 5. The direction sensor 5 is, for example, a geomagnetic sensor, a compass, or a GPS compass, and supplies information on a direction corresponding to the traveling direction of the vehicle to the vehicle-mounted device 1. The vehicle-mounted device 1 has the configuration shown in FIG. 2 as in the first embodiment, and estimates the vehicle position based on landmark information registered in the map DB 10 and the output data of the LIDAR 2, the gyro sensor 3, the vehicle speed sensor 4, and the direction sensor 5. Hereinafter, the same components as those in the first embodiment are appropriately denoted by the same reference numerals, and the description thereof will be omitted.

[0081] FIG. 13 shows a schematic configuration of the vehicle position estimation unit 17A of the vehicle-mounted device 1 in the second embodiment. In the example of FIG. 13, the vehicle position estimation unit 17A has a landmark extraction block 22 and a position estimation block 28. Hereinafter, the vehicle direction at the reference time t output by the direction sensor 5 is referred to as "θ t " and the estimated value of the vehicle position at the reference time t output by the position estimation block 28 is represented as "x - t =(x - t , y - t )"

[0082] The landmark extraction block 22 includes a search candidate selection block 41, a measurement estimate calculation block 42, a search range narrowing block 43, and an extraction block 44. k and the measurement value z which is the scan data of LIDAR 2 for the reference landmark Lk t k and output.

[0083] Specifically, in the first embodiment, the search candidate selection block 41 receives the prior estimate x - t Instead, in the second embodiment, the search candidate selection block 41 first calculates a tentative estimate of the x and y coordinates of the vehicle position based on the geometric relationship shown in Fig. 6, similar to the state transition model block 20 in the first embodiment. Specifically, the search candidate selection block 41 calculates the estimated value x of the vehicle position estimated by the position estimation block 28 one time ago based on the geometric relationship shown in Fig. 6. - t-1 and the direction θ measured one time ago t-1 and the control value u t =(v t , ω t ) T Based on the above, the search candidate selection block 41 calculates a provisional estimate of the x and y coordinates of the vehicle position according to the formula (2). t Specifically, the search candidate selection block 41 determines the scan range Rsc based on the azimuth θ t The search candidate selection block 41 then determines, as the scan range Rsc, an area that is within ±90° from the calculated x and y coordinates of the vehicle position and within the distance that can be measured by the LIDAR 2. k is extracted from the map DB10.

[0084] The measurement estimate calculation block 42 calculates the provisional estimate of the x and y coordinates of the vehicle position calculated by the search candidate selection block 41 (pre-estimated values ​​x and y in the first embodiment). - t ) and the position vector m k Based on this, the measured value z t k The estimated value of z ^ t k =(r ^ t k , φ ^ t k ) TIn addition, when referring to the formulas (5) and (7), the measurement estimate calculation block 42 calculates the prior estimate x - t =(x - t , y - t ), the provisional estimate of the x and y coordinates of the vehicle position calculated by the search candidate selection block 41 is used, and the direction θ - t Instead of azimuth θ t Then, the search range narrowing block 43 sets the search range Rtag shown in the formulas (13) and (14), and the extraction block 44 selects (r t i , φ t i ) pair of measured values ​​z t i (Here, z t k ) exists, the measurement value and the position vector m k to the position estimation block 28. In this case, preferably, the extraction block 44 further executes a process of selecting scan data corresponding to the reference landmark Lk from the scan data within the search range Rtag, as in the first embodiment, and supplies the measurement value z t k may be determined.

[0085] The position estimation block 28 uses the position vector m k and the measured value z t k =(r t k , φ t k ) T and the direction θ output by the direction sensor 5 t From this, the estimated vehicle position x - t =(x - t , y - t Specifically, the position estimation block 28 calculates the absolute value |θ t +φ t kIf | is less than 90°, the estimated value x - t =(x - t , y - t ) is calculated.

[0086]

number

[0087]

number

[0088]

number

[0089]

number

[0090] First, the distance r t k and the scan angle φt k is the distance r ^ t k and the scan angle φ ^ t k Similarly, since the geometric relationship shown in FIG. 7 is satisfied, the estimated value x - t =(x - t , y - t ) and the direction θ t The same relationships as those of the above-mentioned formulas (5) and (6) hold between these two equations. Here, the following formula (19) corresponds to a modified formula (6), and the following formula (20) corresponds to a modified formula (5).

[0091]

number

[0092]

number

[0093]

number

[0094] As shown in FIG. 14(A), t +φ t k If x is less than 90 degrees, - t <m k、x " is established. Also, as shown in FIG. 14(B), "θ t +φ t k If "x" is 90 degrees, then - t =m k、x Furthermore, as shown in FIG. 14(C), t +φ t k If x is greater than 90°, - t >m k、x " is true.

[0095] Therefore, "θ t +φ t k " can be negative, (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 In the case of (a) above, the left side of equation (21) is positive, so the sign on the right side is "+", and the above equation (15) is derived. In the case of (b) above, the relationship "x - t =m k、x " is true. Furthermore, in the case of (c) above, the left side of equation (21) is negative, so the sign on the right side is "-", leading to the above equation (17). Also, the estimated value x - t The y coordinate value of "y - t " can be expressed as equation (16) by transforming equation (19).

[0096] As described above, according to the second embodiment, the vehicle position estimation unit 17A estimates the vehicle position based on the direction θ t By using this, the position vector m of the landmark registered in the map DB10 can be obtained. k and the measured value z t k =(r ^ t k , φ ^ t k ) T From the estimated vehicle position x - t =(x - t , y - t ) can be suitably calculated.

[0097] The landmark position vector m kIf the vehicle position estimation unit 17A is unable to associate the vehicle position with the scan data of the LIDAR 2, the vehicle position estimation unit 17A converts the tentative estimate of the x and y coordinates of the vehicle position calculated by the search candidate selection block 41 into the estimated value x of the vehicle position. - t =(x - t , y - t ) In this case, the estimated vehicle position x - t =(x - t , y - t ) is the estimated value x of the vehicle position estimated by the position estimation block 28 one time ago. - t-1 and the control value u measured by the vehicle speed sensor 4 and the gyro sensor 5 at the reference time t. t and the azimuth θ measured one hour ago t-1 This is an estimated value calculated based on the above.

[0098] <Modification> Preferred modifications of the first and second embodiments will be described below.

[0099] (Variation 1) In the first and second embodiments, the search candidate selection block 41 may extract position vectors of a plurality of landmarks existing within the scan range Rsc from the map DB 10 and perform a process of matching the position vectors with the scan data of the LIDAR 2.

[0100] Generally, when a vehicle is traveling, there are obstacles such as other vehicles in front of or to the sides of the vehicle, so that the LIDAR 2 cannot obtain scan data of the reference landmark Lk selected from the map DB 10 (i.e., occlusion). Taking the above into consideration, in this modified example, the search candidate selection block 41 extracts position vectors of multiple landmarks present in the scan range Rsc from the map DB 10, and performs vehicle position estimation based on a landmark that is not occluded, even if occlusion occurs for any of the landmarks. This makes it possible to detect the reference landmark Lk required for vehicle position estimation with a higher probability. Furthermore, if multiple landmarks can be extracted and all of them can be used, the measurement update step can be performed multiple times (the advance estimate can be corrected using multiple landmarks). In this case, the estimation accuracy of the vehicle position can be statistically improved.

[0101] The processing of this modified example will be further explained with reference to FIG. 11. In step S202, the landmark extraction block 22 refers to the map DB 10, and if it is determined that multiple landmarks exist within the scan range Rsc, it selects position vectors of at least two or more landmarks from the map DB 10. Then, in S203, the landmark extraction block 22 calculates a measurement estimate value 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 estimate value, and attempts to extract scan data corresponding to the previously selected landmark within each search range. If this association is successful, the measurement value and position vector corresponding to the scan data are output in step S207.

[0102] Further, the process when multiple landmarks are extracted and all of them can be used will be explained with reference to Fig. 10. In this case, multiple landmarks are extracted in the landmark extraction process shown in S104, and the result of step S105 is Yes. The series of processes from S106 to S109 that follow are processes that should be performed for each of the extracted landmarks, and this series of processes is processed in a loop the number of times equal to the number of extracted landmarks, using data corresponding to each landmark (corresponding to the output of step S207) as input.

[0103] (Variation 2) In the second embodiment, the vehicle position estimation unit 17A estimates the direction θ t Instead of specifying the azimuth θ based on the output of the gyro sensor 3, t may be estimated.

[0104] In this case, the vehicle position estimation unit 17A uses the direction θ calculated one time before as in FIG. 6 and formula (2). t-1 For angular velocity ω t multiplied by the time span Δt between time t-1 and time t (ω t Δt) to obtain the azimuth θ at the reference time. t In this example, similarly to the second embodiment, the vehicle position estimate x - t can be suitably calculated.

[0105] (Variation 3) Measurement z for the landmark with index k by Lidar 2 t k is the distance r t k ” and the scan angle “φ t kThe above explanation was given assuming that the vector value has elements r and r, but depending on the lidar product, the distance and angle to the target are converted into coordinate values ​​in three-dimensional space before being output. For example, in the case of a lidar that scans in two dimensions, r t k and φ t k Using this, x t k =r t k cosφ t k ,y t k =r t k sinφ t k The output is in the form of a rectangular coordinate. For lidar products that provide such output, for example, simply x t k ,y t k From the value of r t k ,φ t k After calculating the value of , the method described in this invention can be applied.

[0106] (Variation 4) Instead of storing the map DB 10 in the storage unit 12, the vehicle-mounted device 1 may have a server device (not shown) that has the map DB 10. In this case, the vehicle-mounted device 1 acquires necessary landmark information by communicating with the server device via a communication unit (not shown). [Explanation of symbols]

[0107] 1 Onboard device 2. Rider 3 Gyro sensor 4 Vehicle speed sensor 5. Orientation Sensor 10 Map DB

Claims

[Claim 1] An acquisition unit for acquiring map information; a first acquisition unit that acquires first information indicating a distance and an angle to an object present in a first range; a first estimation unit that estimates a position of the moving object based on position information of the object included in the map information and the first information; An estimation device comprising:

Citation Information

Patent Citations

  • Object identifying device

    JP2007187618A

  • Autonomous movement system

    WO2012086029A1

  • Vehicle lateral lane positioning control

    JP2014089691A

  • Display method, display device, display program and recording medium

    JP2015135695A