This application is a U.S. National Stage entry of PCT Application No: PCT/JP2018/019134 filed May 17, 2018, which claims priority to Japanese Patent Application No. 2017-100124, filed May 19, 2017, the contents of which are incorporated herein by reference.
The present invention relates to a technique of estimating a position of a movable body based on positions of features.
In an automatic driving vehicle, it is necessary to match feature positions measured by a sensor such as a LiDAR (Light Detection and Ranging) and feature positions in map information for automatic driving to precisely estimate an own vehicle position. Examples of the features used here include white lines, road signs and signboards. Patent Reference 1 discloses an example of a method of estimating an own vehicle position by using the feature positions detected by a LiDAR and the feature positions in map information. Patent Reference 2 discloses a technique of transmitting an electromagnetic wave to a road surface and detecting white lines based on its reflectivity.
Patent Reference 1: Japanese Patent Application Laid-Open under No. 2017-72422
Patent Reference 2: Japanese Patent Application Laid-Open under No. 2015-222223
When an own vehicle position is estimated using white lines, an amount of data measured by a LiDAR becomes different dependently upon kinds (e.g., continuous line or broken line) of the white lines and deterioration of painting. Therefore, when an own vehicle position is estimated using white lines, detection accuracy of the white lines is changed between the cases in which the amount of LiDAR data used for detecting the while lines is small and large. As a result, accuracy of estimating an own vehicle position is changed.
The above is an example of the problem to be solved by the present invention. It is an object of the present invention to appropriately adjust a range in which white lines are detected according to situation, and to prevent deterioration of accuracy in estimating an own vehicle position.
An invention described in claims is a measurement device comprising: an acquisition unit configured to acquire output data from a sensor unit for detecting surrounding feature; an extraction unit configured to extract, from the output data, data corresponding to detection result in a predetermined range in a predetermined positional relation with an own position; and a processing unit configured to execute predetermined processing based on the extracted data, wherein the predetermined range is determined in accordance with accuracy of the own position.
Another invention described in claims is a measurement method executed by a measurement device comprising: an acquisition process configured to acquire output data from a sensor unit for detecting surrounding feature; an extraction process configured to extract, from the output data, data corresponding to detection result in a predetermined range in a predetermined positional relation with an own position; and a processing process configured to execute predetermined processing based on the extracted data, wherein the predetermined range is determined in accordance with accuracy of the own position.
Still another invention described in claims is a program executed by a measurement device including a computer, the program causes the computer to function as: an acquisition unit configured to acquire output data from a sensor unit for detecting surrounding feature; an extraction unit configured to extract, from the output data, data corresponding to detection result in a predetermined range in a predetermined positional relation with an own position; and a processing unit configured to execute predetermined processing based on the extracted data, wherein the predetermined range is determined in accordance with accuracy of the own position.
According to one aspect of the present invention, there is provided a measurement device comprising: an acquisition unit configured to acquire output data from a sensor unit for detecting surrounding feature; an extraction unit configured to extract, from the output data, data corresponding to detection result in a predetermined range in a predetermined positional relation with an own position; and a processing unit configured to execute predetermined processing based on the extracted data, wherein the predetermined range is determined in accordance with accuracy of the own position.
The above measurement device acquires output data from a sensor unit for detecting surrounding feature, and extracts, from the output data, data corresponding to detection result in a predetermined range in a predetermined positional relation with an own position. Then, the measurement device executes predetermined processing based on the extracted data. The predetermined range is determined in accordance with accuracy of the own position. Therefore, the predetermined range is appropriately determined in accordance with the accuracy of the own position, and the predetermined processing is executed based on the data extracted from the predetermined range.
In one mode of the above measurement device, the feature is a road surface line painted on a road surface, and the predetermined range is determined in accordance with the accuracy of the own position in a first direction crossing a longitudinal direction of the road surface line. In this mode, the predetermined range is determined in accordance with the accuracy of the own position in the first direction crossing the longitudinal direction of the road surface line. In this specification, “the road surface line” includes lane dividing lines such as white lines and yellow lines, and linear road signs such as stop-lines and pedestrian crossing.
In a preferred example of the above measurement device, the extraction unit changes a length of the predetermined range in the first direction in accordance with the accuracy of the own position in the first direction. In another preferred example, the extraction unit changes the length of the predetermined range in the first direction in accordance with the length of the road surface line in the first direction and the accuracy of the own position. In still another preferred example, the extraction unit makes the length in the first direction longer as the accuracy of the own position becomes lower.
In another mode of the above measurement device, the measurement device is loaded on a movable body, and the extraction unit sets the predetermined range at four positions including a right-front position, a right-rear position, a left-front position and a left-rear position with respect to a position of the movable body. In this mode, the data is extracted in the four positions around the movable body, and the predetermined processing is executed based on them. Preferably, the processing unit executes processing of detecting a position of the feature and estimating a position of the measurement device based on the position of the feature.
According to another aspect of the present invention, there is provided a measurement method executed by a measurement device comprising: an acquisition process configured to acquire output data from a sensor unit for detecting surrounding feature; an extraction process configured to extract, from the output data, data corresponding to detection result in a predetermined range in a predetermined positional relation with an own position; and processing process configured to execute predetermined processing based on the extracted data, wherein the predetermined range is determined in accordance with accuracy of the own position. According to this method, the predetermined range is appropriately determined in accordance with the accuracy of the own position, and the predetermined processing is executed based on the data extracted from the predetermined range.
According to still another aspect of the present invention, there is provided a program executed by a measurement device including a computer, the program causes the computer to function as: an acquisition unit configured to acquire output data from a sensor unit for detecting surrounding feature; an extraction unit configured to extract, from the output data, data corresponding to detection result in a predetermined range in a predetermined positional relation with an own position; and a processing unit configured to execute predetermined processing based on the extracted data, wherein the predetermined range is determined in accordance with accuracy of the own position. By executing the program by a computer, the above measurement device may be realized. This program may be handled in a manner stored in a storage medium.
A preferred embodiment of the present invention will be described below with reference to the attached drawings. [White Line Extraction Method]
(Calculation of White Line Predicted Position)
As illustrated, a vehicle 5 exists on a map coordinate system (Xm, Ym), and a vehicle coordinate system (Xv, Yv) is defined based on the position of the vehicle 5. Specifically, the traveling direction of the vehicle 5 is defined as Xv-axis of the vehicle coordinate system, and its perpendicular direction is defined as Yv-axis of the vehicle coordinate system.
On the left and right sides of the vehicle 5, there are white lines serving as lane boundaries. The positions of the white lines on the map coordinate system, i.e., the white line map positions are included in high-density map managed by a server, and are acquired from the server. In this embodiment, the data of the white line is stored in the high-density map as coordinate point sequence. The LiDAR loaded on the vehicle 5 measures scan data along the scan lines 2. The scan lines 2 indicate loci of the scan by the LiDAR.
In
Here, the white line predicted positions WLPP(l′xv, l′yv) indicating predicted positions of the white lines are given by the following equation (1) using the white line map position WLMP(mxm, mym), the predicted own vehicle position PVP (x′m, y′m) and the predicted own vehicle azimuth angle Ψ′m.
Therefore, by the equation (1), the white line predicted positions WLPP1(l′xv1, l′yv1) are calculated for the white line WL1, and the white line predicted positions WLPP2(l′xv2, l′yv2) are calculated for the white line WL2. Thus, for each of the white lines WL1 and WL2, plural white line predicted positions WLPP1 and WLPP2 are obtained.
(Determination of White Line Predicted Range)
Next, based on the white line predicted positions WLPP, white line predicted ranges WLPR are determined. The white line predicted range WLPR indicates a range in which the white line is supposed to exist, based on the predicted own vehicle position PVP. The white line predicted ranges WLPR are set at four positions at most, i.e., at the right-front position, the right-rear position, the left-front position and the left-rear position of the vehicle 5.
D1=√{square root over ((l′xv1−αv)2+(l′yv1)2)} (2)
Similarly, as to the white line WL2, a distance D2 is calculated by the following equation (3) based on the forward reference point (αv, 0v) and plural white line predicted positions WLPP2(l′xv2, l′yv2) forming the white line WL2, and the white line predicted position WLPP2 having the minimum distance D2 is determined as a predicted range reference point Pref2.
D2=√{square root over ((l′xv2−αv)2+(l′yv2)2)} (3)
Then, as illustrated in
[Calculation of White Line Center Position]
Next, a white line center position WLCP is calculated using the white line predicted positions WLPP.
Also, as shown in
(Correction of White Line Predicted Range)
Next, correction of the white line predicted range will be described. As described above, the white line predicted range WLPR is determined based on the white line predicted positions WLPP. However, when the estimation accuracy of the white line predicted positions WLPP is low, the accuracy of the white line predicted range WLPR becomes low, and the white line may possibly deviate from the white line predicted range WLPR.
In order to solve this problem, in this embodiment, the width of the white line predicted range WLPR is corrected based on the estimation accuracy of the predicted own vehicle position PVP of the vehicle 5 as shown in
For example, when the estimation accuracy of the predicted own vehicle position PVP in the Yv-axis direction is 1 m, the correction is made to increase the width of the white line predicted range WLPR, set by the method described with reference to
As a specific example, when the own vehicle position is estimated using extended Kalman filter, the accuracy of the current predicted own vehicle position PVP may be calculated from the values of the covariance matrix sequentially calculated by the expanded Kalman filter and the white line predicted range WLPR may be corrected. The predicted range for a general feature (landmark) is given by the following equation.
Here, “DM”, “WM”, “HM” are sizes of the feature stored in the high-density map. “√P11(t)” and “√P22(t)” are square roots of the elements of the covariance matrix and indicate the estimation accuracy in the x-direction and the y-direction, respectively. “Cz” is a constant indicating the z-direction (vertical direction) component, and “c” is a coefficient. According to this equation, the prediction range becomes small when the estimation accuracy of the predicted own vehicle position PVP is high, and the predicted range becomes large when the estimation accuracy of the predicted own vehicle position PVP is low. Since the white line is used as the feature in this embodiment, the width (correction amount) of the white line predicted range WLPR may be determined based on the Yv-axis direction component of the size S(t) of the predicted range calculated by the equation (5).
[Device Configuration]
The own vehicle position estimation device 10 includes internal sensors 11, external sensors 12, an own vehicle position prediction unit 13, a communication unit 14, a white line map information acquisition unit 15, a white line position prediction unit 16, a scan data extraction unit 17, a white line center position calculation unit 18 and an own vehicle position estimation unit 19. The own vehicle position prediction unit 13, the white line map information acquisition unit 15, the white line position prediction unit 16, the scan data extraction unit 17, the white line center posit ion calculation unit 18 and the own vehicle position estimation unit 19 are realized by a computer such as a CPU which executes a program prepared in advance.
The internal sensors 11 measure the own vehicle position of the vehicle as a GNSS (Global Navigation Satellite System)/IMU (Inertia Measurement Unit) composite navigation system, and include a satellite measurement sensor (GPS), a gyro sensor and a vehicle speed sensor. The own vehicle position prediction unit 13 predicts the own vehicle position by the GNSS/IMU composite navigation system based on the outputs of the internal sensors 11, and supplies the predicted own vehicle position PVP to the white line position prediction unit 16.
The external sensors 12 are the sensors for detecting objects around the vehicle, and include a stereo camera and a LiDAR. The external sensors 12 supply the scan data SD acquired by the measurement to the scan data extraction unit 17.
The communication unit 14 is used for wireless communication with the server 7. The white line map information acquisition unit 15 receives the white line map information associated with the white lines existing around the vehicle from the server 7 via the communication unit 14, and supplies the white line map positions WLMP included in the white line map information to the white line position prediction unit 16.
The white line position prediction unit 16 calculates the white line prediction positions WLPP by the aforementioned equation (1) based on the white line map positions WLMP and the predicted own vehicle position PVP acquired from the own vehicle position prediction unit 13. Also, the white line position prediction unit 16 determines the white line prediction ranges WLPR by the aforementioned equations (2) and (3) based on the white line prediction positions WLPP, and corrects the white line prediction ranges WLPR in accordance with the estimation accuracy of the predicted own vehicle position PVP as described above. Then, the white line position prediction unit 16 supplies the corrected white line prediction ranges WLPR to the scan data extraction unit 17.
The scan data extraction unit 17 extracts the white line scan data WLSD based on the white line prediction ranges WLPR supplied from the white line position prediction unit 16 and the scan data SD acquired from the external sensors 12. Specifically, from the scan data SD, the scan data extraction unit 17 extracts the scan data which are included in the white line prediction ranges WLPR, which are on the road, and whose reflection intensity is larger than a predetermined value, as the white line scan data WLSD, and supplies them to the white line center position calculation unit 18.
The white line center position calculation unit 18 calculates the white line center positions WLCP from the white line scan data WLSD by the equation (4), as described with reference to
The own vehicle position estimation unit 19 estimates the own vehicle position and the own vehicle azimuth angle based on the white line map positions WLMP in the high-density map and the white line center positions WLCP which are the measurement data of the white lines by the external sensor 12. Japanese patent application laid-open under No 2017-72422 discloses an example of a method for estimating an own vehicle position by matching the landmark position information in the high-density map and the measured position information of the landmark by external sensors.
In the above configuration, the external sensor 12 is an example of a sensor unit according to the present invention, the scan data extraction unit 17 is an example of an acquisition unit and an extraction unit of the present invention, and the own vehicle position estimation unit 19 is an example of a processing unit of the present invention.
[Own Vehicle Position Estimation Processing]
Next, the own vehicle position estimation processing by the own vehicle position estimation device 10 will be described.
First, the own vehicle position predict ion unit 13 acquires the predicted own vehicle position PVP based on the outputs from the internal sensors 11 (step S11). Next, the white line map information acquisition unit 15 connects to the server 7 via the communication unit 14, and acquires the white line map information from the high-density map stored in the database 8 (step S12). It is noted that step S11 and S12 may be exchanged.
Next, the white line position prediction unit 16 calculates the white line prediction positions WLPP based on the white line map positions WLMP included in the white line position information acquired in step S12 and the predicted own vehicle position PVP acquired in step S11 (step S13). Also, the white line position prediction unit 16 determines the white line predicted ranges WLPR based on the white line predicted positions WLPP, and further corrects the white line predicted ranges WLPR based on the estimation accuracy of the predicted own vehicle position PVP to supply the corrected white line prediction ranges WLPR to the scan data extraction unit 17 (step S14).
Next, from the scan data SD acquired from the LiDAR serving as the external sensor 12, the scan data extraction unit 17 extracts the scan data which are included in the white line prediction ranges WLPR, which are on the road, and whose reflection intensity is larger than the predetermined value, as the white line scan data WLSD, and supplies them to the white line center position calculation unit 18 (step S15).
Next, the white line center position calculation unit 18 calculates the white line center positions WLCP based on the white line prediction ranges WLPR and the white line scan data WLSD, and supplies them to the own vehicle position estimation unit 19 (step S16). Then, the own vehicle position estimation unit 19 executes own vehicle position estimation using the white line center positions WLCP (step S17), and outputs the own vehicle position and the own vehicle azimuth angle (step S18). Then, the own vehicle position estimation processing ends.
While the above embodiment uses the white lines which are the lane boundaries indicating the lanes, the application of the present invention is not limited to this example, and linear road signs such as pedestrian crossings and stop lines may be used. Also, instead of white lines, yellow lines may be used. The lane dividing lines such as white and yellow lines and the road signs mentioned above are examples of road surface lines according to the present invention.
Number | Date | Country | Kind |
---|---|---|---|
JP2017-100124 | May 2017 | JP | national |
Filing Document | Filing Date | Country | Kind |
---|---|---|---|
PCT/JP2018/019134 | 5/17/2018 | WO |
Publishing Document | Publishing Date | Country | Kind |
---|---|---|---|
WO2018/212280 | 11/22/2018 | WO | A |
Number | Name | Date | Kind |
---|---|---|---|
6813370 | Arai | Nov 2004 | B1 |
20100061591 | Okada | Mar 2010 | A1 |
20180172455 | Yamaguchi et al. | Jun 2018 | A1 |
Number | Date | Country |
---|---|---|
2019288 | Jan 2009 | EP |
3599421 | Dec 2004 | JP |
2010023721 | Feb 2010 | JP |
2015206733 | Nov 2015 | JP |
2015222223 | Dec 2015 | JP |
2017072422 | Apr 2017 | JP |
2016189732 | Dec 2016 | WO |
Entry |
---|
Machine Translation of JP-3599421-B2 (Year: 2004). |
Extended European Search Report dated Feb. 3, 2021 in counterpart EP Patent Application No. 18801784.2, 11 pages. |
International Search Report for related International Application No. PCT/JP2018/019134, dated Aug. 21, 2018; English translation provided (2 pages). |
Number | Date | Country | |
---|---|---|---|
20200208977 A1 | Jul 2020 | US |