Estimation device, control method, program, and storage medium

Abstract

Provided is an estimation device capable of accurately estimating a current position. 【Solution means】The driving support system includes an in-vehicle device 1 mounted on a vehicle and performing control related to driving support of the vehicle, a lidar 2, a gyro sensor 3, and a vehicle speed sensor 4. The in-vehicle device 1 obtains a measurement value z t k indicating the positional relationship between the reference landmark Lk of index k and the host vehicle from the lidar 2. The in-vehicle device 1 corrects a prior estimated value x k estimated from the moving speed measured by the vehicle speed sensor 4 and the angular velocity measured by the gyro sensor 3 based on the position vector m t k of the reference landmark Lk included in the map DB10 and the measurement value z - t to calculate a posterior estimated value x ^ t .

Description

[Technical Field]

[0001] This invention relates to a technique for estimating the current location with high accuracy. [Background technology]

[0002] Techniques for measuring the distance to surrounding objects have been known for some time. For example, Patent Document 1 discloses an example of a vehicle equipped with a lidar that detects a point cloud of object surfaces by scanning horizontally while intermittently emitting laser light and receiving the reflected light (scattered light). Patent Document 2 also discloses a location search device having a map database containing latitude and longitude information of multiple locations to be searched. [Prior art documents] [Patent Documents]

[0003] [Patent Document 1] Japanese Patent Publication No. 2014-089691 [Patent Document 2] Japanese Patent Publication No. 2015-135695 [Overview of the project] [Problems that the invention aims to solve]

[0004] In fields such as autonomous driving, it is necessary to estimate the current position with high accuracy, but conventional methods of estimating the current position, which mainly rely on the output of internal sensors that detect the state of the vehicle, are sometimes insufficient. Patent documents 1 and 2 do not disclose any method for calculating the absolute position of a vehicle with high accuracy.

[0005] This invention was made to solve the above-mentioned problems, and its main objective is to provide an estimation device capable of estimating the 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 for acquiring map information; a first acquisition unit for acquiring first information indicating the distance and angle to an object in a first range; and a first estimation unit for estimating the position of a moving object based on the position information of the object included in the map information and the first information.

[0007] Furthermore, the invention described in claim 9 is a control method performed by an estimation device, comprising: an acquisition step of acquiring map information; a first acquisition step of acquiring first information indicating the distance and angle to an object in a first range; and a first estimation step of estimating the position of the moving body based on the position information of the object included in the map information and the first information.

[0008] Furthermore, the invention described in claim 10 is a program executed by a computer, characterized in that the computer functions as an acquisition unit for acquiring map information, a first acquisition unit for acquiring first information indicating the distance and angle to an object in a first range, and a first estimation unit for estimating the position of a moving object based on the position information of the object included in the map information and the first information. [Brief explanation of the drawing]

[0009] [Figure 1] This is a schematic diagram of the driver assistance system according to the first embodiment. [Figure 2] This is a block diagram showing the functional configuration of an in-vehicle device. [Figure 3] This diagram shows the vehicle's position on a two-dimensional Cartesian coordinate system. [Figure 4] This diagram shows the general relationship between the prediction step and the measurement update step. [Figure 5] This is a block diagram showing the functional configuration of the vehicle position estimation unit in the first embodiment. [Figure 6] This diagram shows the position of the vehicle before and after movement, assuming circular motion, represented by various variables. [Figure 7] This diagram shows the relationship between the location of landmarks and the position of the vehicle. [Figure 8] This diagram shows the functional configuration of the landmark extraction block. [Figure 9] This diagram shows the relationship between the Rider's scan range and the landmark search range. [Figure 10] This flowchart shows the procedure for the processing performed by the vehicle position estimation unit in the first embodiment. [Figure 11] This flowchart shows the details of the landmark extraction process. [Figure 12] This is a schematic diagram of the driver assistance system according to the second embodiment. [Figure 13] This is a block diagram showing the functional configuration of the vehicle position estimation unit in the second embodiment. [Figure 14] This diagram shows the relationship between the location of landmarks and the position of the vehicle. [Modes for carrying out the invention]

[0010] According to a preferred embodiment of the present invention, the estimation device comprises: an acquisition unit for acquiring map information; a first acquisition unit for acquiring first information indicating the distance and angle to an object in a first range; and a first estimation unit for estimating the position of the moving object based on the position information of the object included in the map information and the first information.

[0011] The estimation device described above comprises 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 the distance and angle to an object in a first range (i.e., the positional relationship between the moving object and the features in the first range). The first estimation unit estimates the position of the moving object based on the positional information of the object included in the map information and the first information. In this embodiment, the estimation device can accurately estimate the position of the moving object by utilizing the positional information of features registered in the map information.

[0012] In one embodiment of the estimation device described above, the estimation device further comprises a second estimation unit that calculates a first estimated position, which is the estimated current position of the moving object. The first estimation unit estimates the position of the moving object based on the difference between second information, which indicates the positional relationship between the object and the first estimated position, and the first estimated position, and the first estimated position. In this embodiment, the estimation device can perform highly accurate position estimation 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 embodiment of the estimation device described above, the second estimation unit calculates the first estimated position based on the estimated position of the moving object at least a predetermined time prior. This allows the estimation device to suitably estimate the current position of the moving object by taking into account the estimated position of the moving object a predetermined time prior.

[0014] In another embodiment of the estimation device described above, the estimation device further comprises a second acquisition unit for acquiring 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. In this embodiment, the estimation device can calculate the first estimated position from the estimated position of the moving body a predetermined time ago with high accuracy and low computational load.

[0015] In another embodiment of the estimation device described above, the second estimation unit alternately performs a prediction step in which it calculates the first estimated position, and the first estimation unit alternately performs an update step in which it corrects the first estimated position calculated in the previous prediction step based on the difference between the first information and the second information. 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 preceding the prediction step. In this embodiment, the estimation device can calculate the first estimated position corresponding to the current time with high accuracy and low computational load while correcting the previously calculated first estimated position by alternately performing the update step and the prediction step.

[0016] In another embodiment of the estimation device described above, the first acquisition unit acquires the first information from a measuring device having an irradiation unit that irradiates laser light while changing the irradiation direction, a light receiving unit that receives the laser light reflected by the object, and an output unit that outputs the first information based on the received signal output by the light receiving unit, the irradiation direction corresponding to the laser light received by the light receiving unit, and the response delay time of the laser light. In this embodiment, the first acquisition unit can suitably generate and output first information indicating the distance and angle to an object in a first range. Furthermore, in this embodiment, since measurements are performed on three-dimensional geographical features registered in map information, the distance and direction to a reference geographical feature can be suitably 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 snow accumulation or nighttime.

[0017] In another embodiment of the estimation device described above, the feature is an artificial object. This allows the first acquisition unit to generate the first information more stably compared to when natural objects are the target.

[0018] In another embodiment of the estimation device described above, the features are man-made objects arranged periodically. This allows 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 performed by an estimation device comprises: an acquisition step of acquiring map information; a first acquisition step of acquiring first information indicating the distance and angle to an object in a first range; and a first estimation step of estimating the position of the moving object based on the position information of the object included in the map information and the first information. By performing this control method, the estimation device can accurately estimate the position of the moving object using the position information of features registered in the map information.

[0020] According to another preferred embodiment of the present invention, a computer program is provided that causes the computer to function as an acquisition unit for acquiring map information, a first acquisition unit for acquiring first information indicating the distance and angle to an object in a first range, and a first estimation unit for estimating the position of a moving object based on the 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 a moving object using the position information of features registered in the map information. Preferably, the program is stored in a storage medium. [Examples]

[0021] Hereinafter, preferred embodiments of the present invention will be described with reference to the drawings.

[0022] <First Example> (1) Schematic configuration Figure 1 is a schematic diagram of the driver assistance system according to the first embodiment. The driver assistance system shown in Figure 1 includes an on-board unit 1 mounted on the vehicle that performs control related to driver assistance, 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 on-board unit 1 is electrically connected to the lidar 2, gyro sensor 3, and vehicle speed sensor 4, and estimates the position of the vehicle on which the on-board unit 1 is mounted (also called "vehicle position") based on their outputs. Based on the estimated vehicle position, the on-board unit 1 performs automatic driving control of the vehicle so that it travels along a set route. The on-board unit 1 stores a map database (DB:DataBase) 10 that stores road data and information about landmarks that serve as markers near the road (also called "landmark information"). Landmark information is information that associates at least an index assigned to each landmark with the location information of the landmark. Based on this landmark information, the on-board unit 1 limits the search range for landmarks by the lidar 2 and estimates the vehicle position by comparing it with the output of the lidar 2, etc. Hereafter, the landmark that the on-board unit 1 uses as a reference for estimating the vehicle position will be called the "reference landmark Lk," and the index of the reference landmark Lk will be "k." The reference landmark Lk is an example of an "object" in this invention.

[0024] Candidate landmarks for the reference landmark Lk include, for example, kilometer posts, 100m posts, delineators, traffic infrastructure facilities (e.g., signs, directional signs, traffic lights), utility poles, and streetlights that are periodically arranged along the roadside. Preferably, the aforementioned landmarks are man-made objects to enable stable measurement, and more preferably, they are periodically placed to allow for periodic correction of the vehicle's position. The intervals between them do not need to be strictly constant; they just need to exist with a certain degree of periodicity (e.g., utility poles and streetlights). The intervals may also differ depending on the driving area.

[0025] The lidar 2 discretely measures the distance to an object in the external environment by emitting a pulsed laser within 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 laser light while changing the irradiation direction, a light receiving unit that receives reflected light (scattered light) of the irradiated laser light, and an output unit that outputs scan data based on the received signal output by the light receiving unit. 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 received signal described above. In this embodiment, the lidar 2 is assumed to be installed facing the direction of travel of the vehicle so as to scan at least the area in front of the vehicle. The lidar 2, gyro sensor 3, and vehicle speed sensor 4 each supply output data to the in-vehicle unit 1. The in-vehicle unit 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] Figure 2 is a block diagram showing the functional configuration of the in-vehicle unit 2. The in-vehicle unit 2 mainly consists of an interface 11, a storage unit 12, an input unit 14, a control unit 15, and an output unit 16. Each of these elements is interconnected via a bus line.

[0027] Interface 11 acquires output data from sensors such as the rider 2, gyro sensor 3, and vehicle speed sensor 4, and supplies it to the control unit 15.

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

[0029] The input unit 14 includes buttons, a touch panel, a remote controller, a voice input device, etc., for user operation, and accepts inputs such as specifying a destination for route searching and specifying whether to turn autonomous driving on or off. The output unit 16 includes, for example, a display or speaker that outputs based on the control of the control unit 15.

[0030] The control unit 15 includes a CPU for executing programs and controls the entire in-vehicle unit 1. In this embodiment, the control unit 15 includes a vehicle position estimation unit 17 and an automatic driving control unit 18.

[0031] The vehicle position estimation unit 17 corrects the vehicle position estimated from the output data of the gyro sensor 3 and vehicle speed sensor 4 based on the distance and angle measurements taken by the rider 2 with respect to a 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 in which it estimates the vehicle position from the output data of the gyro sensor 3 and vehicle speed sensor 4 based on a state estimation method based on Bayesian estimation, and a measurement update step in which it corrects the estimated value of the vehicle position calculated in the previous prediction step. In the first embodiment, an example using an extended Kalman filter as an example of a state estimation method based on Bayesian estimation will be described later. 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 the 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's position estimated by the vehicle position estimation unit 17. Based on the set route, the automatic driving control unit 18 sets a target trajectory and controls the vehicle's position by sending a guide signal to the vehicle so that the vehicle's position estimated by the vehicle position estimation unit 17 is within a predetermined width of the target trajectory.

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

[0034] (2-1) Basic Explanation The following describes the basic matters that are the premise of the processes executed by the host vehicle position estimation unit 17. In the subsequent descriptions, the host vehicle position is represented by the state variable vector “x = (x, y, θ)”. Also, for the provisional estimated value estimated in the prediction step, “ - ” is attached above the character representing the estimated value, and for the more accurate estimated value updated in the measurement update step, “ ^ ” is attached above the character representing the value. Note that, for the convenience in this specification, the character with “^” or “-” attached above an arbitrary symbol is represented as “A ^ ” or “A - ” (“A” is an arbitrary character).

[0035] FIG. 3 is a diagram showing the state variable vector x in two-dimensional orthogonal coordinates. As shown in FIG. 3, the host vehicle position on the plane defined on the two-dimensional orthogonal coordinates of xy is represented by the coordinates “(x, y)” and the orientation “θ” of the host vehicle. Here, the orientation θ 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 corresponding 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 the 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 executed by a two-step process of alternately executing the prediction step and the measurement update step. Therefore, in this embodiment, the calculation and update of the estimated value of the state variable vector x are sequentially executed 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 represented as “x - t ” or “x ^ t ”.

[0037] In the prediction step, the host vehicle position estimation unit 17 uses the state variable vector x at time t - 1 calculated in the immediately preceding measurement update step ^ t-1On the other hand, by applying the vehicle's moving speed "v" and angular velocity "ω" (collectively referred to as "control value u" t =(v t , ω t ), T an estimated value of the own vehicle position at time t (also referred to as "pre-estimated value") x - t is calculated. The control value u t is an example of the "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 own vehicle position estimation unit 17 calculates a covariance matrix (also referred to as "pre-covariance matrix") "Σ - t " corresponding to the error distribution of the pre-estimated value x - t from the covariance matrix "Σ ^ t-1 " at time t-1 calculated in the immediately preceding measurement update step.

[0038] Also, in the measurement update step, the own vehicle position estimation unit 17 obtains the measurement value "z t " of the reference landmark Lk by the lidar 2 and the measurement estimated value "z - t " of the reference landmark Lk obtained by modeling the measurement process by the lidar 2 from the pre-estimated value x ^ t . The measurement value z t is a two-dimensional vector representing the distance and scan angle of the reference landmark Lk measured by the lidar 2 at time t, as will be described later. Then, the own vehicle position estimation unit 17 multiplies the difference between the measurement value z t and the measurement estimated value z ^ t by a separately obtained Kalman gain "K t ", and adds this to the pre-estimated value x - t to calculate an updated state variable vector (also referred to as "post-estimated value") x ^ t .

[0039] [Number] Also, in the measurement update step, the host vehicle position estimation unit 17, similar to the prediction step, calculates the covariance matrix (also referred to as the "posterior covariance matrix") Σ corresponding to the error distribution of the posteriorestimation value x ^ from the prior covariance matrix Σ ^ t - t .

[0040] (2-2) Processing Outline FIG. 5 is a block diagram showing the functional configuration of the host vehicle position estimation unit 17. The host 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, arithmetic blocks 31 to 33, and unit delay blocks 34 and 35.

[0041] The state transition model (speed operation model) block 20 calculates the prior estimation value x ^ t-1 from the posterior estimation value x t at time t - 1 based on the control value u t =(v t ) T =(x - t , y - t , θ - t ) - t . FIG. 6 shows the prior estimation values "x T ", "y - t ", "θ - t " at the reference time t and the posterior estimation values "x - t ", "y ^ t-1 ", "θ ^ t-1 " at time t - 1 ^ t-1 ​It is a diagram showing the relationship with "". According to the geometric relationship shown in FIG. 6, the prior estimated value x - t =(x - t 、y - t 、θ - t ) T is represented by the following formula (2). Note that "Δt" represents the time difference between time t and time t-1.

[0042]

Number

[0043] The covariance calculation block 21 calculates the prior covariance matrix Σ t using the matrix "Rt" showing the error distribution obtained by converting the error distribution of the control value u ^ t-1 into the three-dimensional space of the state variable vector (x, y, θ) and the Jacobian matrix "Gt" obtained by linearizing the state transition model shown in formula (2) around x - t based on formula (3).

[0044]

Number

[0045]

Number

[0046] The measurement model block 23 is the position vector m of the reference landmark Lk at index k. k and prior estimate x - t From, the measured value z t k Estimated value of "z ^ t k =(r ^ t k , φ ^ t k ) T The following is calculated and supplied to the calculation block 31. Here, "r ^ t k " is the prior estimate x - t This indicates the distance of index k to the landmark when relative to φ ^ t k" is the prior estimate x - t This shows the scan angle of index k relative to the landmark, with respect to the reference point Lk. Figure 7 shows the relationship between the position of the reference landmark Lk and the position of the vehicle. Based on the geometric relationship shown in Figure 7, the distance r ^ t k It can be expressed as shown in equation (5) below.

[0047]

number

[0048]

number

[0049]

number

[0050] Furthermore, the measurement model block 23 linearizes the measurement models shown in equations (5) and (7) around the prior estimate value x - t and calculates the Jacobian matrix "H t k ". Here, the Jacobian matrix H t k is represented by the following equation (8).

[0051]

Equation

[0052] The covariance calculation block 24 calculates the covariance matrix "S t k " necessary for calculating the Kalman gain K t k based on the following equation (9).

[0053]

Equation

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

[0055]

Equation

[0056]

number

[0057]

number

[0058] (2-3) Details of the Landmark Extraction Block Figure 8 shows the functional configuration of the landmark extraction block 22. As shown in Figure 8, 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.

[0059] The search candidate selection block 41 uses the prior estimate x - t Based 、 The lidar 2 recognizes the scan range (also called "scan range Rsc"), and selects landmarks located within the recognized scan range Rsc from the map DB 10. In this case, the search candidate selection block 41 is determined by the direction θ. - t It is ±90° from, and the position (x - t , y - t The area within the range measurable distance of lidar 2 from ) is set as the scan range Rsc. Then, the search candidate selection block 41 sets the position vector m corresponding to index k. k =(m k,x , m k,y ) is extracted from map DB10. The scan range Rsc is an example of the "first range" in this invention.

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

[0061] The search range narrowing block 43 is defined as a scan angle φ within the scan range Rsc. ^ t k From a predetermined search angle width "Δ φ The difference is within the range of distance r. ^ t k Search range width "Δ r Set the range that will result in " (also called the "search range Rtag"). Search angle width Δ φ and search distance width Δ r These are the expected measured values ​​z t k and estimated value z ^ t k The error is predetermined based on experiments and other factors, taking into account the error. ^ t k =(r ^ t k , φ ^ t k ) T Prior estimate x is required to calculate (see Equations 5 and 7). - t =(x - t , y - t θ - t ) T The estimation accuracy and the measured value z t k It depends on the accuracy of the lider 2 that outputs scan data corresponding to Δ. Therefore, preferably, the search angle width Δ φ and search distance width Δ r This is the prior estimate x - t It is set taking into account at least one of the estimation accuracy of and the accuracy of LIDA2. Specifically, the search angle width Δ φ and search distance width Δr This is the prior estimate x - t The higher the estimation accuracy of [the first method], the shorter the time, and the higher the accuracy of [the second method], the shorter the time.

[0062] The extraction block 44 extracts the measured value z, which corresponds to the point cloud of the reference landmark Lk, based on the search range Rtag set by the search range refinement block 43. t k This is extracted from the total scan data at time t of rider 2. Specifically, the extracted block 44 is the measured value z that satisfies the following equations (13) and (14). t i Determine whether or not a beam exists (where "i" is the index of each beam emitted by Lida 2 in a single scan).

[0063]

number

[0064]

number

[0065] In this case, preferably, the extraction block 44 further performs a process to select scan data corresponding to the reference landmark Lk from the scan data within the search range Rtag, and then supplies the measured value z to the calculation block 31. t k It would be good to make a decision.

[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 a matching process with the 3D shape formed by the point cloud of scan data within the search range Rtag, thereby obtaining the measured value z corresponding to the point cloud of the reference landmark Lk. t k 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 pre-set threshold information to select the measured value z, which corresponds to the point cloud of the reference landmark Lk. t k Select the selected measurement value z. t k If multiple values ​​exist, any one measurement z t k These measured values ​​z may be supplied to the calculation block 31. t k Representative values ​​calculated or selected through statistical processing may be supplied to the calculation block 31.

[0067] Figure 9 shows the relationship between the scan range Rsc and the search range Rtag. In the example in Figure 9, the scan range Rsc is a semicircular area that is within a 90° range to the left and right of the front of the vehicle and within the distance that can be measured from the vehicle. Arrow 70 in Figure 9 indicates the scan direction.

[0068] In the example in Figure 9, first, the search candidate selection block 41 is determined by the prior estimate x - t Based on this, the scan range Rsc is identified, and landmarks within the scan range Rsc are considered as reference landmarks Lk, and the corresponding position vector m is used. kSelect the following. Then, the measurement estimate calculation block 42 calculates the position vector m of the reference landmark Lk. k and prior estimate x - t From, the measured value z t k Estimated value of z ^ t k =(r ^ t k , φ ^ t k ) T The search range refinement block 43 sets the search range Rtag based on equations (13) and (14). The extraction block 44 calculates the position vector m if scan data exists within the set search range Rtag. k The measurement model block 23 is supplied with the measured value z based on the scan data within the search range Rtag. t k The result is supplied to the calculation block 31. If no scan data is found 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 sets the pre-estimated value x - t The posterior estimated value x ^ t Set to the prior covariance matrix Σ - t The posterior covariance matrix Σ ^ t Set to this.

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

[0070] First, the vehicle position estimation unit 17 calculates the posterior estimated value x at time t-1. ^ t-1 and the posterior covariance matrix Σ ^ t-1In addition to capturing the data, the control value u at time t is obtained from the gyro sensor 3 and the vehicle speed sensor 4. t The value is obtained (step S101). Next, the state transition model block 20 of the vehicle position estimation unit 17 calculates the prior estimated value x based on equation (2). - t The covariance calculation block 21 of the vehicle position estimation unit 17 calculates the prior covariance matrix Σ based on equation (3). ー 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 performs the landmark extraction process shown in Figure 11, which will be described later (step S104). Then, the vehicle position estimation unit 17 extracts the position vector m of the landmark registered in the map DB 10. k The system determines whether or not a correspondence has been established between the data and the scan data of the Rider 2 (step S105). In this case, the vehicle position estimation unit 17 determines whether or not a predetermined flag indicating that the above-mentioned correspondence has been established has been set, based on the landmark extraction process shown in Figure 11.

[0072] Then, if the above correspondence is established (step S105; Yes), the covariance calculation block 24 calculates the covariance matrix S based on equation (9) above. t k The calculation is performed (step S106). Next, the Kalman gain calculation block 25 calculates the Kalman gain K t k This is calculated based on the above equation (10) (step S107). Then, the calculation block 33 calculates the prior estimated value x as shown in equation (12). - t In contrast, the measured value z supplied from landmark extraction block 22 t k and the estimated value z supplied from the measurement model block 23 ^ t k The difference from (i.e., "z t k -z ^ t k) to Kalman Gain K t k By adding the value obtained by multiplying by the given value, the posterior estimate x ^ t The following is calculated (step S108). The covariance update block 26 is supplied with the prior covariance matrix Σ from the covariance calculation block 21. - t And the Jacobian matrix H supplied from the measurement model block 23 t k And the Kalman gain K supplied from the Kalman gain calculation block 25 t k Based on this, the posterior covariance matrix Σ ^ t This is calculated based on the above equation (11) (step S109). Then, the unit delay block 34 is calculated based on the posterior estimate x ^ t The posterior estimated value x ^ t-1 The state transition model block 20 is supplied with the unit delay block 35, and the posterior covariance matrix Σ ^ t The posterior covariance matrix Σ ^ t-1 This is supplied to the covariance calculation block 21.

[0073] On the other hand, if the above correspondence could not be made (step S105; No), the vehicle position estimation unit 17 calculates the prior estimated value x - t The posterior estimated value x ^ t Set to the prior covariance matrix Σ - t The posterior covariance matrix Σ ^ t Set to (step S110).

[0074] (3-2) Landmark extraction process Figure 11 is a flowchart detailing the landmark extraction process in step S104 of Figure 10.

[0075] First, the search range narrowing block 43 of the landmark extraction block 22 acquires scan data from lidar 2 (step S201). Here, lidar 2 is assumed to emit "n" beams with gradually changing emission angles over one scan cycle, and the received light intensity and response time of the reflected light of each beam are measured to obtain the measured values ​​z t 1 ~z t n The output should be as follows.

[0076] Next, the search candidate selection block 41 is determined by the position vector m of the landmark located within the scan range Rsc of lider 2. k Select from map DB10 (step S202). In this case, the search candidate selection block 41 has a scan range Rsc and a azimuth θ. - t The position is ±90° from (x - t , y - t The system identifies an area within the range-measuring range of the Lida 2, and then uses a position vector m indicating the location within that area. k This is extracted from map DB10.

[0077] Subsequently, the measurement estimate calculation block 42 calculates the prior estimate x - t From position vector m k Measured value z when measured t k Estimated value of z ^ t k =(r ^ t k , φ ^ t k ) T The measurement estimate calculation block 42 calculates the estimated value z based on equations (5) and (7) described above. ^ t k =(r ^ t k , φ ^t k ) T Calculate.

[0078] Next, the search range narrowing block 43 has a scan angle φ that satisfies equation (13). t i Narrow down the range (i=1~n) (step S204). Extraction block 44 is the scan angle φ that satisfies equation (13). t i The measured value z is t i Of these, distance r ^ t i It is determined whether there exists a scan angle φ that falls within the range shown in equation (14) (step S205). Then, the scan angle φ that satisfies equation (13) is determined. t i The measured value z is t i Of these, distance r t i If there is a range that falls within the range shown in equation (14) (step S205; Yes), the extraction block 44 extracts the measured value z included in this search range. t i From among these, a process is performed to identify the measurement value corresponding to the reference landmark Lk (such as known shape / feature extraction processes and the use of light reception intensity), and the finally selected measurement value is z t k It is then extracted as such (step S206). Then, the extraction block 44 is the extracted position vector m k and the position vector m k The corresponding scan data is the measured value z t k The output is generated, and a flag indicating that the correspondence has been established is set (step S207). This flag is referenced in the determination process in step S105 of Figure 10 described above.

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

[0080] <Second Example> Figure 12 is a block diagram of the driver assistance system according to the second embodiment. In the example in Figure 12, the in-vehicle unit 1 is electrically connected to the direction sensor 5. The direction sensor 5 is, for example, a geomagnetic sensor, a compass, or a GPS compass, and supplies direction information corresponding to the direction of travel of the vehicle to the in-vehicle unit 1. The in-vehicle unit 1 has the same configuration as shown in Figure 2 as in the first embodiment, and estimates the vehicle's position based on landmark information registered in the map DB 10 and the output data of the lidar 2, gyro sensor 3, vehicle speed sensor 4, and direction sensor 5. Hereafter, components that are the same as in the first embodiment will be denoted by the same reference numerals as appropriate, and their descriptions will be omitted.

[0081] Figure 13 shows a schematic configuration of the vehicle position estimation unit 17A of the in-vehicle device 1 in the second embodiment. In the example of Figure 13, the vehicle position estimation unit 17A has a landmark extraction block 22 and a position estimation block 28. Thereafter, the vehicle's orientation at a reference time t output by the orientation sensor 5 is "θ". t The position of the vehicle at the reference time t output by position estimation block 28 is written as "x - t =(x - t , y - t It is written as ")".

[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, and the position vector m of the reference landmark Lk. k And the measured value z is the scan data of rider 2 relative to the reference landmark Lk. t k It outputs the following.

[0083] Specifically, in the first embodiment, the search candidate selection block 41 receives a prior estimate x supplied from the state transition model block 20. - t The scan range Rsc was determined based on this. Instead, in the second embodiment, the search candidate selection block 41 first calculates a provisional estimate of the x and y coordinates of the vehicle's position based on the geometric relationship shown in Figure 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's position that the position estimation block 28 estimated one time step earlier. - t-1 and the bearing θ measured one hour earlier t-1 And the control value u t =(v t , ω t ) T Based on this, a provisional estimate of the x and y coordinates of the vehicle's position is calculated according to equation (2). Then, the search candidate selection block 41 uses the calculated provisional estimate of the x and y coordinates of the vehicle's position and the direction θ t Based on this, the scan range Rsc is identified. Specifically, the search candidate selection block 41 is determined by the azimuth θ. t The scan range Rsc is defined as an area within ±90° of the calculated x and y coordinates of the vehicle's position that is within the range-measuring distance of the lidar 2. Then, the search candidate selection block 41 identifies the position vector m of a landmark indicating the location within the identified area. k This is extracted from map DB10.

[0084] The measurement estimate calculation block 42 calculates a provisional estimate of the x and y coordinates of the vehicle's position (preliminary estimate x in the first embodiment) calculated by the search candidate selection block 41. - t (equivalent to) and the position vector m extracted from map DB10 k Based on this, refer to equations (5) and (7), and the measured value z t k Estimated value of z ^ t k =(r ^ t k , φ ^ t k ) TThe following is calculated. Note that when the measurement estimate calculation block 42 refers to equations (5) and (7), the prior estimate x - t =(x - t , y - t Instead of using the search candidate selection block 41, use the provisional estimate of the x and y coordinates of the vehicle's position, and the direction θ - t Instead of azimuth θ t The search range narrowing block 43 sets the search range Rtag shown in equations (13) and (14), and the extraction block 44 satisfies equations (13) and (14) (r t i , φ t i ) The set of measured values ​​z t i (here z t k If such a thing exists, the measured value and the position vector m k The measured value z is supplied to the position estimation block 28. Preferably, the extraction block 44 further performs the process of selecting scan data corresponding to the reference landmark Lk from the scan data within the search range Rtag, similar to the first embodiment, and supplies the measured value z t k You may decide that.

[0085] The position estimation block 28 receives the position vector m supplied from the extraction block 44. k and measured value z t k =(r t k , φ t k ) T And the direction θ output by the direction sensor 5 t Therefore, the estimated value of the vehicle's position x - t =(x - t , y - t ) is calculated. Specifically, position estimation block 28 is the absolute value |θ t +φ t kIf | is less than 90°, the estimated value x is calculated based on equations (15) and (16) below. - t =(x - t , y - t Calculate ).

[0086]

number

[0087]

number

[0088]

number

[0089]

number

[0090] First, distance r t k and scan angle φt k is the distance r ^ t k and scan angle φ ^ t k Similarly, since it has the geometric relationship shown in Figure 7 above, the estimated value x - t =(x - t , y - t ) and azimuth θ t The same relationships as those in equations (5) and (6) above hold between them. Here, equation (19) below corresponds to a modified equation (6), and equation (20) corresponds to a modified equation (5).

[0091]

number

[0092]

number

[0093]

number

[0094] As shown in Figure 14(A), "θ t +φ t k If the angle is less than 90°, then "x - t <m k、x The following holds true. Also, as shown in Figure 14(B), "θ t +φ t k If " is 90°, then "x - t =m k、x The following holds true. Furthermore, as shown in Figure 14(C), "θ t +φ t k If " is greater than 90°, then "x - t >m k、x This statement is valid.

[0095] Therefore, "θ" t +φ t k If we include the case where "" is 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 This is the result. And in the case of (a) above, the left side of equation (21) is positive, so the sign of the right side is "+", and thus equation (15) above is derived. Also, in the case of (b) above, the relationship "x" shown in equation (18) - t =m k、x The following equation holds true. Furthermore, in the case of (c) above, the left side of equation (21) is a negative value, so the sign of the right side becomes "-", and thus equation (17) above is derived. Also, the estimated value x - t The y coordinate value "y - t This can be expressed as equation (16) by rearranging equation (19).

[0096] As described above, according to the second embodiment, the vehicle position estimation unit 17A uses the direction θ obtained from the direction sensor 5. t By using this, the position vector m of the landmarks registered in map DB10 k and measured value z t k =(r ^ t k , φ ^ t k ) T From this, the estimated value of the vehicle's position x - t =(x - t , y - t This allows for the appropriate calculation of ).

[0097] Note that the location vector m of the landmark registered in map DB10 kIf a correspondence cannot be established between the scan data of Rider 2 and the vehicle position estimation unit 17A, the provisional estimate of the x and y coordinates of the vehicle position calculated by the search candidate selection block 41 is used as the estimated value of the vehicle position x - t =(x - t , y - t Set as ). That is, in this case, the estimated value of the vehicle's position x - t =(x - t , y - t ) is the estimated value x of the vehicle's position estimated by position estimation block 28 one time step earlier. - t-1 The vehicle speed sensor 4 and gyro sensor 5 measure the control value u at the reference time t. t And the bearing θ measured one hour earlier. t-1 This is an estimated value calculated based on the above.

[0098] <Variation> The following describes suitable modifications of the first and second embodiments.

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

[0100] Generally, when a vehicle is in motion, obstacles such as other vehicles are present in front of or to the sides of the vehicle, resulting in situations where scan data for the reference landmark Lk selected from the map DB10 cannot be obtained from the rider 2 (i.e., occlusion). Taking this into consideration, in this modified example, the search candidate selection block 41 extracts the position vectors of multiple landmarks present within the scan range Rsc from the map DB10. This allows the vehicle's position to be estimated based on the landmarks that are not occluded, even if occlusion occurs for any of the landmarks. This makes it possible to detect the reference landmark Lk necessary 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 pre-estimated value can be corrected using multiple landmarks). In this case, the accuracy of vehicle position estimation can be statistically improved.

[0101] The processing of this modified example will be further explained with reference to Figure 11. In step S202, the landmark extraction block 22 refers to the map DB 10 and, if it determines that multiple landmarks exist within the scan range Rsc, selects the position vectors of at least two landmarks from the map DB 10. Then, in S203, the landmark extraction block 22 calculates measurement estimates for the selected position vectors. Then, in steps S204 and S205, the landmark extraction block 22 sets the search range Rtag shown in equations (13) and (14) for each calculated measurement estimate and attempts to extract scan data corresponding to the previously selected landmarks within each search range. If this correspondence is successful, the measurement values ​​and position vectors corresponding to the scan data are output in step S207.

[0102] Furthermore, the process when multiple landmarks are extracted and all of them are available will be explained in more detail with reference to Figure 10. In this case, multiple landmarks have been extracted in the landmark extraction process shown in S104, and step S105 is judged as Yes. The series of processes from S106 to S109 that follow are the processes to be performed for each extracted landmark, and these processes are looped through for the number of extracted landmarks, with the data corresponding to each landmark (corresponding to the output of step S207) as input.

[0103] (Modification 2) In the second embodiment, the vehicle position estimation unit 17A determines the direction θ based on the output of the direction sensor 5. t Instead of specifying the direction, the direction θ is determined based on the output of the gyro sensor 3. t It is also possible to estimate this.

[0104] In this case, the vehicle position estimation unit 17A calculates the bearing θ one time step earlier, similar to Figure 6 and equation (2). t-1 For this, angular velocity ω t The value obtained by multiplying by the time interval Δt between time t-1 and time t (ω t By adding Δt, the azimuth θ at the reference time is obtained. t The estimated value of is calculated. In this example as in the second example, the estimated value of the vehicle's position x is calculated without using Bayesian estimation such as an extended Kalman filter. - t This allows for the appropriate calculation of the result.

[0105] (Variation 3) Measurements of index k relative to landmark z by LIDA2 t k is the distance "r t k " and the scan angle of index k relative to the landmark when the front direction of the vehicle is set to 0 degrees "φ t kThe above explanation assumes that the output is a vector value with r as its elements, but some lidar products convert the distance and angle to the target into coordinate values ​​in 3D space before outputting. For example, in the case of a lidar that performs scanning in 2D, r t k and φ t k Using x t k =r t k cosφ t k ,y t k =r t k sinus t k The output is in the form of orthogonal coordinates. For Rider products that produce this kind of 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] (Modification 4) Instead of storing the map DB10 in the storage unit 12, the in-vehicle unit 1 may have a server device (not shown) that holds the map DB10. In this case, the in-vehicle unit 1 obtains necessary landmark information by communicating with the server device using a communication unit (not shown). [Explanation of symbols]

[0107] 1 On-vehicle device 2 Riders 3. Gyroscope sensor 4. Vehicle speed sensor 5 Directional Sensor 10 Map Database

Claims

[Claim 1] An acquisition unit that acquires map information, A first acquisition unit that acquires first information indicating the distance and angle to an object within a first range, A first estimation unit estimates the position of the moving body based on the position information of the object included in the map information and the first information, An estimation device equipped with the following features.

Images