Sensor-aided vehicle positioning system

• US 9,970,772 B2
• Filed: 06/08/2016
• Issued: 05/15/2018
• Est. Priority Date: 09/17/2013
• Status: Active Grant

• Current Assignee: GM Global Technology Operations LLC (General Motors Company)
• Original Assignee: GM Global Technology Operations LLC (General Motors Company)
• Inventors: Zeng, Shuqing, Salinger, Jeremy A., Litkouhi, Bakhtiar B., Pazhayampallil, Joel, O'Dea, Kevin A., Nickolaou, James N., Shields, Mark E.
• Primary Examiner(s): Troost, Aaron L
• Application Number:
  US15/176,419
• Publication Number:
  US 20160282128A1
• Time in Patent Office: 706 Days
• CPC Class Codes: B60W 30/0956 the prediction being responsive to traffic or environmental parameters
• CPC Class Codes: B60W 30/12 Lane keeping
• CPC Class Codes: G01C 21/30 Map- or contour-matching
• CPC Class Codes: G01C 21/3602 Input other than that of destination using image analysis, e.g. detection of road signs, lanes, buildings, real preceding vehicles using a camera
• CPC Class Codes: G01S 13/06 Systems determining position data of a target
• CPC Class Codes: G01S 13/931 of land vehicles
• CPC Class Codes: G01S 19/42 Determining position
• CPC Class Codes: G01S 19/485 whereby the further system is an optical system or imaging system
• CPC Class Codes: G01S 2013/9322 using additional data, e.g. driver condition, road state or weather data
• CPC Class Codes: G01S 5/16 using electromagnetic waves other than radio waves
• CPC Class Codes: G01S 7/412 based on a comparison between measured values and known or stored values
• CPC Class Codes: G05D 1/0212 with means for defining a desired trajectory involving a plurality of land vehicles G05D1/0287

Abstract

A method for localizing a vehicle in a digital map. GPS raw measurement data is retrieved from satellites. A digital map of a region traveled by the vehicle based on the raw measurement data is retrieved from a database. The digital map includes a geographic mapping of a traveled road and registered roadside objects. The registered roadside objects are positionally identified in the digital map by earth-fixed coordinates. Roadside objects are sensed in the region traveled by the vehicle using distance data and bearing angle data. The sensed roadside objects are matched on the digital map. A vehicle position is determined on the traveled road by fusing raw measurement data and sensor measurements of the identified roadside objects. The position of the vehicle is represented as a function of linearizing raw measurement data and the sensor measurement data as derived by a Jacobian matrix and normalized measurements, respectively.

Claims

1. A method for localizing a vehicle in a digital map comprising:

retrieving raw satellite measurement data from at least three satellites in a Global Navigation Satellite System (GNSS) satellite system;

retrieving from a database a digital map of a region traveled by the vehicle based on the raw measurement data, the digital map including a geographic mapping of a traveled road and registered roadside objects, the registered roadside objects being positionally identified in the digital map by earth-fixed coordinates;

sensing roadside objects in the region traveled by the vehicle using distance data and bearing angle data;

matching the sensed roadside objects on the digital map;

determining a position of the vehicle on the traveled road by fusing the raw satellite measurement data and sensor measurement data of the sensed roadside objects, wherein the position of the vehicle is represented as a function of linearizing the raw satellite measurement data and the sensor measurement data as derived by a Jacobian matrix and normalized measurements, respectively; and

updating the position of the vehicle in a vehicle positioning system utilizing the determined position of the vehicle;

wherein determining the position includes using the following equation;

min_x∥

H_GNSSx−

o_GNSS∥

²

where x=(x, y, v_h, φ

)^T, x y is a ground plane position of the vehicle, v_h is a ground plane longitudinal velocity of the vehicle, φ

is a ground plane orientation of the vehicle, H_GNSS is the derived Jacobian matrix, and o_GNSS are the normalized measurements.

2. The method of claim 1, wherein the raw satellite measurement data includes range measurements and Doppler measurements.

3. The method of claim 2, wherein the range measurements are determined from the following equations:

∥

P₁−

P_H∥

=R₁

∥

P₂−

P_H∥

=R₂

∥

P₃−

P_H∥

=R₃

where P₁, P₂, P₃ are positions of the at least three satellites, respectively, and P_H is a position of the vehicle.

4. The method of claim 2 wherein the Doppler measurements are determined from the following equations:

5. The method of claim 1, wherein determining the position utilizes wireless router data, wherein the linearized measurements for determining the position is further represented by the following equation:

min_x∥

H_WRx−

o_WR∥

²

where H_WR is another derived Jacobian matrix, and o_WR are normalized measurements derived from the wireless data.

6. The method of claim 5, wherein the wireless router data includes a distance measurement between the vehicle and a Wi-Fi access point, and a distance measurement between the vehicle and an RFID tag.

7. The method of claim 6, wherein the distance measurement between the vehicle and the Wi-Fi access point is represented by the following equation:

∥

P_WIFI−

P_H∥

=R_WIFI

where P_WIFI is a position of the Wi-Fi router, and P_H is a position of the vehicle.

8. The method of claim 6, wherein the distance measurement between the vehicle and the Wi-Fi access point is represented by the following equation:

∥

P_RFID−

P_H∥

=R_RFID

where P_RFID is a position of the RFID tag, and P_H is a position of the vehicle.

9. The method of claim 1, wherein determining the vehicle position utilizes sensor data, wherein the linearized measurements for determining the position is further represented by the following equation:

min_x∥

H_SENx−

o_SEN∥

²

where H_SEN is another derived Jacobian matrix, and o_SEN are normalized measurements derived from the sensor data.

10. The method of claim 9, wherein the sensor data includes Doppler measurements between the vehicle and a tree trunk, between the vehicle and a light pole, and between the vehicle and a traffic sign.

11. The method of claim 10, wherein the Doppler measurements are represented by the following equations:

12. The method of claim 1 wherein determining the vehicle position utilizes lane marker data, wherein the minimization of the position error is represented by the following equation:

min_x∥

H_LINx−

o_LIN∥

²

where H_LIN is another derived Jacobian matrix, and o_LIN are normalized measurements derived from the lane marker data.

13. The method of claim 12, wherein the lane marker data includes lateral offsets between the vehicle and a left lane marker, between the vehicle and a right lane marker, and between the vehicle and a guard rail wherein the line marker data is represented by the following equations:

x cos η

_L−

y sin η

_L+D_L=d_L

x cos η

_R−

y sin η

_R+D_R=d_R

x cos η

_G−

y sin η

_G+D_G=d_G

where η

_L, η

_R, and η

_G are the measured relative orientation angle of the left lane marker, the right lane marker, and the guard rail, respectively, D_L, D_R, and D_G are distance between the vehicle and the left lane marker, the right lane marker, and the guard rail, respectively.

14. The method of claim 1, wherein localizing the position of the vehicle utilizes the raw satellite measurement data, wireless router data, sensor data, and lane marker data, and wherein the linearized measurements for determining the position are represented by the following equation:

min_x∥

H_GNSSx−

o_GNSS∥

²+∥

H_WRx−

o_WR∥

²+∥

H_SENx−

o_SEN∥

²+∥

H_LINx−

o_LIN∥

²

where each of H_GSNN, H_WR, H_SEN, and H_LIN is a derived Jacobian matrix related to a respective measurement device, and o_GSNN, o_WR, o_SEN and o_LIN are normalized measurements respectively derived from the raw satellite measurement data, the wireless router data, the sensor data, and the lane marker data.

15. The method of claim 1, further comprising determining the ground plane heading of the vehicle and a curvature of the traveled road as a function of Global Positioning Satellite (GPS) coordinates in the raw satellite measurement data and the determined vehicle position utilizing coordinates of the sensed roadside objects.

16. The method of claim 15, wherein the ground plane heading of the vehicle and the curvature of the traveled road is provided to a curvature estimator, the method further comprising:

enabling curvature speed control of the vehicle via the curvature estimator.

17. The method of claim 15, wherein the ground plane heading of the vehicle and the curvature of the traveled road is provided to a curvature estimator for enabling lane centering control of the vehicle, the method further comprising:

enabling curvature speed control of the vehicle via the curvature estimator.

18. The method of claim 15, further comprising localizing a position of the vehicle in a curved road as a function of the ground plane heading of the vehicle and the curvature of the traveled road.

19. The method of claim 1, wherein determining the position of the vehicle on the road of travel further includes determining a relative position of the vehicle utilizing pseudolite signals.