A self-driving car positioning system and method in a complex urban environment

Abstract

The present invention discloses a system and method for positioning an autonomous driving vehicle in a complex urban environment. The system comprises: a GPS unit, an IMU unit, a wheel odometer unit, a laser radar target detection unit, a temperature sensor unit, an extended Kalman filter unit and a system noise variance matrix update unit. The present invention enables an autonomous driving vehicle to adjust the system noise variance matrix of a filter according to different driving environments in a complex urban environment, thereby achieving stable and accurate positioning of the vehicle, and providing sufficient pre-positioned data for decision-making, planning and control of motion in its subsequent system.

Description

Technical Field

[0001] The present invention belongs to the technical field of vehicle positioning, and in particular relates to an automatic driving vehicle system and method adapted to complex urban environments. Background Art

[0002] Autonomous vehicles (Self-driving automobile), also known as driverless cars, computer-driven cars, or wheeled mobile robots, are intelligent cars that achieve unmanned driving through a computer system.

[0003] Compared with traditional cars, self-driving cars need to obtain their own position and posture in real time to provide the necessary positioning information for the planning and decision-making of the next moment's movement. At present, the positioning method widely used in self-driving cars is the fusion positioning of the global navigation satellite system (GNSS) and the inertial navigation system (INS). The positioning devices used are GPS, IMU and wheel odometer. The commonly used algorithms are Kalman filter algorithm, extended Kalman filter and its variants. This positioning method is widely used because of its simple devices and low cost. However, the noise of IMU and wheel odometer is not fixed. When the car is driving in a complex urban environment, due to the frequent acceleration and deceleration of the vehicle, coupled with the influence of sensor temperature changes, the noise of IMU and wheel odometer will change frequently. In addition, the complex urban environment is prone to the loss of GPS signal. Without the correction of GPS signal, the filter will diverge, and finally the positioning will fail.

[0004] Therefore, how to improve the positioning stability and accuracy of autonomous vehicles in complex urban traffic systems and ensure that autonomous vehicles can stably obtain their own positions when GPS signals are lost is an issue that urgently needs to be addressed in the field of autonomous driving. Summary of the invention

[0005] In view of the above-mentioned deficiencies in the prior art, the purpose of the present invention is to provide a system and method for positioning an autonomous vehicle in a complex urban environment, so as to solve the problem in the prior art that the changing IMU noise caused by the complex urban environment cannot be corrected due to the lack of vehicle-mounted GPS signals, thereby causing positioning failure. The present invention enables an autonomous vehicle to adjust the system noise variance matrix of the filter for different driving environments in a complex urban environment, thereby achieving stable and accurate self-positioning of the vehicle, and providing sufficient pre-data for its subsequent system for decision-making, planning and control of movement.

[0006] To achieve the above object, the technical solution adopted by the present invention is as follows:

[0007] The present invention discloses an automatic driving vehicle positioning system in a complex urban environment, comprising: a GPS unit, an IMU unit, a wheel odometer unit, a laser radar target detection unit, a temperature sensor unit, an extended Kalman filter unit and a system noise variance matrix update unit;

[0008] The GPS unit is installed outside the vehicle and is used to obtain real-time latitude and longitude positioning information and heading angle information of the vehicle, and send the obtained information to the extended Kalman filter unit in real time;

[0009] The IMU unit is installed inside the vehicle and is used to obtain the instantaneous acceleration information and instantaneous angular velocity information of the vehicle, and send the obtained information to the extended Kalman filter unit in real time;

[0010] The wheel odometer unit is installed on the wheel of the vehicle and is used to obtain the instantaneous speed information of the vehicle and send the obtained instantaneous speed information to the extended Kalman filter unit in real time;

[0011] The laser radar target detection unit is installed on the top of the vehicle and is used to obtain point cloud data around the vehicle in real time, calculate the number of moving targets around the vehicle and the distance information from the vehicle through the PV-RCNN algorithm, and transmit the calculated information to the system noise variance matrix update unit;

[0012] The temperature sensor unit is used to obtain the temperature information of the IMU and the wheel odometer in real time, and transmit the temperature information to the system noise variance matrix update unit in real time;

[0013] The extended Kalman filter unit is installed inside the vehicle. When the GPS unit signal is available, the latitude and longitude positioning information and heading angle information sent by the GPS unit, the instantaneous angular velocity information and instantaneous acceleration information sent by the IMU unit, and the instantaneous speed information sent by the wheel odometer unit are obtained in real time, and then the extended Kalman filter in the extended Kalman filter unit is used to fuse the above information to finally obtain the real-time positioning information of the vehicle; when the GPS unit signal is unavailable, the system noise variance matrix sent by the system noise variance matrix update unit is received, and the system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit is updated by the system noise variance matrix, and the instantaneous angular velocity information and instantaneous acceleration information sent by the IMU unit and the instantaneous speed information sent by the wheel odometer unit are obtained in real time, and then the updated extended Kalman filter is used to fuse the information obtained from the IMU unit and the wheel odometer unit to finally obtain the real-time positioning information of the vehicle;

[0014] The system noise variance matrix update unit is installed inside the vehicle, receives the number of moving targets around the vehicle and the distance information from the vehicle sent by the laser radar target detection unit, and the temperature information of the wheel odometer and the IMU sent by the temperature sensor unit, and calculates the system noise variance matrix that makes the extended Kalman filter have the best filtering effect based on the above received information using the noise gain algorithm, and sends it to the extended Kalman filter unit to replace the current system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit.

[0015] Furthermore, the PV-RCNN algorithm is a 3D target detection framework, which uses a feature extraction method that uses 3Dvoxel convolution and point-based pointnet convolution to detect 3D targets from point cloud data and obtain the confidence, type and distance information of the 3D target from the vehicle.

[0016] The present invention also provides a method for positioning an autonomous driving vehicle in a complex urban environment. Based on the above system, the steps are as follows:

[0017] 1) Real-time acquisition of the vehicle's own latitude and longitude positioning information and heading angle information, the vehicle's own instantaneous acceleration information and instantaneous angular velocity information, the vehicle's instantaneous speed information, the confidence, type, and distance information of the 3D targets around the vehicle, and the temperature information of the IMU and wheel odometer;

[0018] 2) Determine whether the information received by the GPS unit is available: decode the received GPS raw data to obtain GPS status information. If the decoded status information is a non-negative number, the information received by the GPS unit is available, and proceed to step 3); if the decoded status information is a negative number, the information received by the GPS unit is unavailable, and proceed to step 4);

[0019] 3) The vehicle latitude and longitude positioning information and heading angle information, vehicle instantaneous acceleration information and instantaneous angular velocity information, and vehicle instantaneous velocity information obtained in step 1) are sent to an extended Kalman filter unit, and the extended Kalman filter in the unit fuses the above information to obtain real-time positioning information of the vehicle;

[0020] 4) The number and distance information of targets around the vehicle, as well as the temperature information of the IMU and the wheel odometer are sent to the system noise variance matrix update unit. The system noise variance matrix that makes the extended Kalman filter filtering effect best in this state is obtained through the noise gain algorithm, and it is sent to the extended Kalman filter unit to replace the current system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit; the instantaneous acceleration information and instantaneous angular velocity information sent by the IMU unit and the instantaneous velocity information sent by the wheel odometer unit are integrated with the updated extended Kalman filter to obtain the positioning result in the absence of GPS.

[0021] Furthermore, the GPS decoding method used in step 2) is: using the function package nmea_navsat_driver under the LINUX system to decode the GPS raw data.

[0022] Furthermore, the specific algorithm of the extended Kalman filter used in step 3) is as follows:

[0023] X n =f(X n-1 ,u n )

[0024] Z n =h(X n )

[0025] Where, X n-1 represents the state of the vehicle at time n-1, x n-1 Indicates the latitude value at time n-1, y n-1 Indicates the longitude value at time n-1, Indicates the heading angle at time n-1, X n represents the state of the vehicle at time n, x n Indicates the latitude value at time n, y n Indicates the longitude value at time n, represents the heading angle at time n, u n represents the input of the system at time n, Z n represents the observed value of the vehicle at time n, f() and h() represent the state transfer equation and observation equation respectively;

[0026]

[0027] In the formula, F n represents the state transfer matrix of the system at time n, G n It represents the mapping matrix from system input to system state at time n, H nRepresents the observation matrix of the system at time n; According to the above three matrix formulas, the process of describing the standard EKF algorithm is as follows: Given the initial value of the vehicle state and the initial value of the system probability covariance matrix Then, the state of the driverless car is estimated by iterating according to the following formula:

[0028]

[0029]

[0030] In the formula, represents the prior state estimate of the vehicle at time n, represents the posterior state estimate of the vehicle at time n, represents the posterior probability covariance matrix of the vehicle's system state at time n-1, represents the transpose of the state transfer matrix of the system at time n, Represents the transpose of the mapping matrix from system input to system state at time n, represents the transpose of the observation matrix of the system at time n, K n represents the Kalman gain at time n, represents the prior probability covariance matrix of the vehicle's system state at time n, represents the posterior probability covariance matrix of the vehicle's system state at time n, R n Represents the covariance matrix corresponding to the uncertainty of the observations, R n Determined by the sensor used, Q n represents the system noise variance matrix at time n, e v,n represents the speed measurement noise of the wheel odometer at time n, e a,n represents the acceleration measurement noise of IMU at time n, e ω,n represents the angular velocity measurement noise of IMU at time n, e v,n , e a,n , e ω,n The initial value of is obtained by calibration test;

[0031] The extended Kalman filter fusion algorithm is completed, which can complete the fusion of the vehicle's latitude and longitude positioning information and heading angle information, the vehicle's instantaneous acceleration information and instantaneous angular velocity information, and the vehicle's instantaneous speed information, and finally output the vehicle's posterior state estimate at time n That is, the vehicle's position and heading angle information.

[0032] Furthermore, the noise gain algorithm in step 4) is specifically as follows:

[0033]

[0034]

[0035] In the formula, Δ k-1,k represents the traffic gain from time k-1 to time k, α k represents the vehicle impact weight coefficient at time k, α k-1 represents the vehicle impact weight coefficient at time k-1, β k represents the pedestrian influence weight coefficient at time k, β k-1 represents the pedestrian influence weight coefficient at time k-1, n1 represents the number of surrounding vehicles obtained by the laser radar target detection unit at time k, n2 represents the number of surrounding pedestrians obtained by the laser radar target detection unit at time k, n3 represents the number of surrounding vehicles obtained by the laser radar target detection unit at time k-1, n4 represents the number of surrounding pedestrians obtained by the laser radar target detection unit at time k-1, represents the confidence of the i-th vehicle target obtained by the lidar target detection unit at time k, represents the confidence of the j-th pedestrian target obtained by the lidar target detection unit at time k, represents the confidence of the i-th vehicle target obtained by the laser radar target detection unit at time k-1, represents the confidence of the j-th pedestrian target obtained by the laser radar target detection unit at time k-1, represents the distance between the i-th vehicle target and the vehicle itself obtained by the laser radar target detection unit at time k, represents the distance from the jth pedestrian target to the vehicle obtained by the laser radar target detection unit at time k, represents the distance between the i-th vehicle target and the vehicle itself obtained by the laser radar target detection unit at time k-1, represents the distance from the jth pedestrian target to the vehicle obtained by the laser radar target detection unit at time k-1, represents the IMU temperature gain from time k-1 to time k, Indicates that the temperature sensor unit obtains the temperature of the IMU at time k, in degrees Fahrenheit. Indicates that the temperature sensor unit obtains the temperature of the IMU at time k-1, in degrees Fahrenheit. represents the wheel odometer temperature gain from time k-1 to time k, Indicates that the temperature sensor unit obtains the temperature of the wheel odometer at time k, in degrees Fahrenheit. Indicates that the temperature sensor unit obtains the temperature of the wheel odometer at time k-1, in degrees Fahrenheit. represents the IMU gain coefficient from time k-1 to time k, represents the wheel odometer gain coefficient from time k-1 to time k, θ E represents the weight of environmental factors, with a value of 0.7, θ I Indicates the IMU temperature influence factor, the value is 0.3, θ W Indicates the weight of the factors affecting the temperature of the wheel odometer, and its value is 0.3;

[0036] Assume that the system noise variance matrix at time k-1 is Q k-1 :

[0037]

[0038] In the formula, e v,k-1 is the speed noise of the wheel odometer at time k-1, e a,k-1 is the acceleration noise of IMU at time k-1, e ω,k-1 is the angular velocity noise of IMU at time k-1;

[0039] Then the system noise variance matrix at time k is Q k :

[0040]

[0041] The system noise variance matrix Q k Replace the original matrix of the extended Kalman filter, and then use the updated extended Kalman filter for fusion positioning.

[0042] Beneficial effects of the present invention:

[0043] 1. The present invention ensures the stability of the positioning signal output of the self-driving car when the GPS signal is missing, thereby improving the safety of the self-driving car driving on urban roads;

[0044] 2. The present invention can automatically adjust the system noise variance matrix of the filter according to different urban environments to ensure the stability of the filter, thereby stably outputting more accurate positioning information. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] Figure 1 This is a schematic diagram of the positioning method of the present invention.

[0046] Figure 2 These are the five GPS missing areas specifically selected for the verification experiment of the present invention.

[0047] Figure 3 This is a comparison chart of positioning effects of a specific path in the verification experiment of the present invention.

[0048] Figure 4 This is a comparison chart of the maximum error values ​​of five GPS missing areas in the verification experiment of the present invention.

[0049] Figure 5 This is a comparison chart of the root mean square error values ​​of five GPS missing areas in the verification experiment of the present invention. DETAILED DESCRIPTION

[0050] In order to facilitate the understanding of those skilled in the art, the present invention is further described below in conjunction with embodiments and drawings. The contents mentioned in the implementation modes are not intended to limit the present invention.

[0051] The present invention discloses an automatic driving vehicle positioning system in a complex urban environment, comprising: a GPS unit, an IMU unit, a wheel odometer unit, a laser radar target detection unit, a temperature sensor unit, an extended Kalman filter unit and a system noise variance matrix update unit;

[0052] The GPS unit is installed outside the vehicle and is used to obtain real-time latitude and longitude positioning information and heading angle information of the vehicle, and send the obtained information to the extended Kalman filter unit in real time;

[0053] The IMU unit is installed inside the vehicle and is used to obtain the instantaneous acceleration information and instantaneous angular velocity information of the vehicle, and send the obtained information to the extended Kalman filter unit in real time;

[0054] The wheel odometer unit is installed on the wheel of the vehicle and is used to obtain the instantaneous speed information of the vehicle and send the obtained instantaneous speed information to the extended Kalman filter unit in real time;

[0055] The laser radar target detection unit is installed on the top of the vehicle and is used to obtain point cloud data around the vehicle in real time, calculate the number of moving targets around the vehicle and the distance information from the vehicle through the PV-RCNN algorithm, and transmit the calculated information to the system noise variance matrix update unit;

[0056] Among them, the PV-RCNN algorithm is a 3D target detection framework. The feature extraction method adopted is to use 3Dvoxel convolution and point-based pointnet convolution to detect 3D targets from point cloud data and obtain the confidence, type and distance information of the 3D target from the vehicle.

[0057] The temperature sensor unit includes two temperature sensors, which are respectively installed on the IMU and the wheel odometer, and are used to obtain the temperature information of the IMU and the wheel odometer in real time, and transmit the temperature information to the system noise variance matrix update unit in real time;

[0058] The extended Kalman filter unit is installed inside the vehicle. When the GPS unit signal is available, the latitude and longitude positioning information and heading angle information sent by the GPS unit, the instantaneous angular velocity information and instantaneous acceleration information sent by the IMU unit, and the instantaneous speed information sent by the wheel odometer unit are obtained in real time, and then the extended Kalman filter in the extended Kalman filter unit is used to fuse the above information to finally obtain the real-time positioning information of the vehicle; when the GPS unit signal is unavailable, the system noise variance matrix sent by the system noise variance matrix update unit is received, and the system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit is updated by the system noise variance matrix, and the instantaneous angular velocity information and instantaneous acceleration information sent by the IMU unit and the instantaneous speed information sent by the wheel odometer unit are obtained in real time, and then the updated extended Kalman filter is used to fuse the information obtained from the IMU unit and the wheel odometer unit to finally obtain the real-time positioning information of the vehicle;

[0059] The system noise variance matrix update unit is installed inside the vehicle, receives the number of moving targets around the vehicle and the distance information from the vehicle sent by the laser radar target detection unit, and the temperature information of the wheel odometer and the IMU sent by the temperature sensor unit, and calculates the system noise variance matrix that makes the extended Kalman filter have the best filtering effect based on the above received information using the noise gain algorithm, and sends it to the extended Kalman filter unit to replace the current system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit.

[0060] Reference Figure 1 As shown, the present invention also provides a method for positioning an autonomous driving vehicle in a complex urban environment. Based on the above system, the steps are as follows:

[0061] 1) Real-time acquisition of the vehicle's own latitude and longitude positioning information and heading angle information, the vehicle's own instantaneous acceleration information and instantaneous angular velocity information, the vehicle's instantaneous speed information, the confidence, type, and distance information of the 3D targets around the vehicle, and the temperature information of the IMU and wheel odometer;

[0062] 2) Determine whether the information received by the GPS unit is available: decode the received GPS raw data to obtain GPS status information. If the decoded status information is a non-negative number, the information received by the GPS unit is available, and proceed to step 3); if the decoded status information is a negative number, the information received by the GPS unit is unavailable, and proceed to step 4);

[0063] Specifically, the GPS decoding method used in step 2) is: using the function package nmea_navsat_driver under the LINUX system to decode the GPS raw data.

[0064] 3) The vehicle latitude and longitude positioning information and heading angle information, vehicle instantaneous acceleration information and instantaneous angular velocity information, and vehicle instantaneous velocity information obtained in step 1) are sent to an extended Kalman filter unit, and the extended Kalman filter in the unit fuses the above information to obtain real-time positioning information of the vehicle;

[0065] The specific algorithm of the extended Kalman filter used is as follows:

[0066] X n =f(X n-1 ,u n )

[0067] Z n =h(X n )

[0068] Where, X n-1 represents the state of the vehicle at time n-1, x n-1 Indicates the latitude value at time n-1, y n-1 Indicates the longitude value at time n-1, Indicates the heading angle at time n-1, X n represents the state of the vehicle at time n, x n Indicates the latitude value at time n, y n Indicates the longitude value at time n, represents the heading angle at time n, u n represents the input of the system at time n, Z n represents the observed value of the vehicle at time n, f() and h() represent the state transfer equation and observation equation respectively;

[0069]

[0070] In the formula, F n represents the state transfer matrix of the system at time n, G n It represents the mapping matrix from system input to system state at time n, H n Represents the observation matrix of the system at time n; According to the above three matrix formulas, the process of describing the standard EKF algorithm is as follows: Given the initial value of the vehicle state quantity X0 - and the initial value of the system probability covariance matrix Then, the state of the driverless car is estimated by iterating according to the following formula:

[0071]

[0072] In the formula, represents the prior state estimate of the vehicle at time n, represents the posterior state estimate of the vehicle at time n, represents the posterior probability covariance matrix of the vehicle's system state at time n-1, represents the transpose of the state transfer matrix of the system at time n, Represents the transpose of the mapping matrix from system input to system state at time n, represents the transpose of the observation matrix of the system at time n, K n represents the Kalman gain at time n, represents the prior probability covariance matrix of the vehicle's system state at time n, represents the posterior probability covariance matrix of the vehicle's system state at time n, R n Represents the covariance matrix corresponding to the uncertainty of the observations, R n Determined by the sensor used, Q n represents the system noise variance matrix at time n, e v,n represents the speed measurement noise of the wheel odometer at time n, e a,n represents the acceleration measurement noise of IMU at time n, e ω,n represents the angular velocity measurement noise of IMU at time n, e v,n , e a,n , e ω,n The initial value of is obtained by calibration test;

[0073] The extended Kalman filter fusion algorithm is completed, which can complete the fusion of the vehicle's latitude and longitude positioning information and heading angle information, the vehicle's instantaneous acceleration information and instantaneous angular velocity information, and the vehicle's instantaneous speed information, and finally output the vehicle's posterior state estimate at time n That is, the vehicle's position and heading angle information.

[0074] 4) The number and distance information of the targets around the vehicle, and the temperature information of the IMU and the wheel odometer are sent to the system noise variance matrix update unit, and the system noise variance matrix that makes the extended Kalman filter filtering effect the best in this state is obtained through the noise gain algorithm, and it is sent to the extended Kalman filter unit to replace the current system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit; the updated extended Kalman filter receives the instantaneous acceleration information and instantaneous angular velocity information sent by the IMU unit and the instantaneous velocity information sent by the wheel odometer unit, and the above information is integrated to obtain the positioning result in the absence of GPS;

[0075] The noise gain algorithm is as follows:

[0076]

[0077] In the formula, Δ k-1,krepresents the traffic gain from time k-1 to time k, α k represents the vehicle impact weight coefficient at time k, α k-1 represents the vehicle impact weight coefficient at time k-1, β k represents the pedestrian influence weight coefficient at time k, β k-1 represents the pedestrian influence weight coefficient at time k-1, n1 represents the number of surrounding vehicles obtained by the laser radar target detection unit at time k, n2 represents the number of surrounding pedestrians obtained by the laser radar target detection unit at time k, n3 represents the number of surrounding vehicles obtained by the laser radar target detection unit at time k-1, n4 represents the number of surrounding pedestrians obtained by the laser radar target detection unit at time k-1, represents the confidence of the i-th vehicle target obtained by the lidar target detection unit at time k, represents the confidence of the j-th pedestrian target obtained by the lidar target detection unit at time k, represents the confidence of the i-th vehicle target obtained by the laser radar target detection unit at time k-1, represents the confidence of the j-th pedestrian target obtained by the laser radar target detection unit at time k-1, represents the distance between the i-th vehicle target and the vehicle itself obtained by the laser radar target detection unit at time k, represents the distance from the jth pedestrian target to the vehicle obtained by the laser radar target detection unit at time k, represents the distance between the i-th vehicle target and the vehicle itself obtained by the laser radar target detection unit at time k-1, represents the distance from the jth pedestrian target to the vehicle obtained by the laser radar target detection unit at time k-1, represents the IMU temperature gain from time k-1 to time k, Indicates that the temperature sensor unit obtains the temperature of the IMU at time k, in degrees Fahrenheit. Indicates that the temperature sensor unit obtains the temperature of the IMU at time k-1, in degrees Fahrenheit. represents the wheel odometer temperature gain from time k-1 to time k, Indicates that the temperature sensor unit obtains the temperature of the wheel odometer at time k, in degrees Fahrenheit. Indicates that the temperature sensor unit obtains the temperature of the wheel odometer at time k-1, in degrees Fahrenheit. represents the IMU gain coefficient from time k-1 to time k, represents the wheel odometer gain coefficient from time k-1 to time k, θ E represents the weight of environmental factors, with a value of 0.7, θ IIndicates the IMU temperature influence factor, the value is 0.3, θ W Indicates the weight of the factors affecting the temperature of the wheel odometer, and its value is 0.3;

[0078] Assume that the system noise variance matrix at time k-1 is Q k-1 :

[0079]

[0080] In the formula, e v,k-1 is the speed noise of the wheel odometer at time k-1, e a,k-1 is the acceleration noise of IMU at time k-1, e ω,k-1 is the angular velocity noise of IMU at time k-1;

[0081] Then the system noise variance matrix at time k is Q k :

[0082]

[0083] The system noise variance matrix Q k Replace the original matrix of the extended Kalman filter, and then use the updated extended Kalman filter for fusion positioning.

[0084] To further verify the positioning accuracy of the designed positioning method, a road is selected for experimental verification. The specific path is as follows: Figure 2 As shown in the figure, the circled part is the part where the GPS signal is missing. Select one of the paths when the GPS signal is missing. Figure 3 As shown in the figure, it can be seen that the positioning effect after optimization is closer to the reference path than before optimization. The positioning data of the five GPS missing times are analyzed: the maximum error value is as follows Figure 4 As shown in Figure 2, it can be seen that the maximum positioning error values ​​after optimization are smaller than the maximum positioning error values ​​before optimization, and the maximum error values ​​are reduced by 45.32% on average; the root mean square error values ​​are as follows Figure 5 As shown, it can be seen that the root mean square error values ​​of the optimized positioning data are all smaller than the root mean square error values ​​of the positioning data before optimization, and the root mean square error values ​​are reduced by 32.58% on average.

[0085] The present invention has many specific application paths. The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements can be made without departing from the principle of the present invention. These improvements should also be regarded as the protection scope of the present invention.

Claims

1. A self-driving car positioning system in a complex urban environment, characterized in that: include: GPS unit, IMU unit, wheel odometer unit, lidar target detection unit, temperature sensor unit, extended Kalman filter unit and system noise variance matrix update unit; The GPS unit is installed outside the vehicle and is used to obtain real-time latitude and longitude positioning information and heading angle information of the vehicle, and send the obtained information to the extended Kalman filter unit in real time; The IMU unit is installed inside the vehicle and is used to obtain the instantaneous acceleration information and instantaneous angular velocity information of the vehicle, and send the obtained information to the extended Kalman filter unit in real time; The wheel odometer unit is installed on the wheel of the vehicle and is used to obtain the instantaneous speed information of the vehicle and send the obtained instantaneous speed information to the extended Kalman filter unit in real time; The laser radar target detection unit is installed on the top of the vehicle and is used to obtain point cloud data around the vehicle in real time, calculate the number of moving targets around the vehicle and the distance information from the vehicle through the PV-RCNN algorithm, and transmit the calculated information to the system noise variance matrix update unit; The temperature sensor unit is used to obtain the temperature information of the IMU and the wheel odometer in real time, and transmit the temperature information to the system noise variance matrix update unit in real time; The extended Kalman filter unit is installed inside the vehicle. When the GPS unit signal is available, the latitude and longitude positioning information and heading angle information sent by the GPS unit, the instantaneous angular velocity information and instantaneous acceleration information sent by the IMU unit, and the instantaneous speed information sent by the wheel odometer unit are obtained in real time, and then the extended Kalman filter in the extended Kalman filter unit is used to fuse the above information to finally obtain the real-time positioning information of the vehicle; when the GPS unit signal is unavailable, the system noise variance matrix sent by the system noise variance matrix update unit is received, and the system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit is updated by the system noise variance matrix, and the instantaneous angular velocity information and instantaneous acceleration information sent by the IMU unit and the instantaneous speed information sent by the wheel odometer unit are obtained in real time, and then the updated extended Kalman filter is used to fuse the information obtained from the IMU unit and the wheel odometer unit to finally obtain the real-time positioning information of the vehicle; The system noise variance matrix update unit is installed inside the vehicle, receives the number of moving targets around the vehicle and the distance information from the vehicle sent by the laser radar target detection unit, and the temperature information of the wheel odometer and the IMU sent by the temperature sensor unit, and calculates the system noise variance matrix that makes the extended Kalman filter have the best filtering effect based on the above received information using the noise gain algorithm, and sends it to the extended Kalman filter unit to replace the current system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit.

2. The automatic driving vehicle positioning system in a complex urban environment according to claim 1, characterized in that: The PV-RCNN algorithm is a 3D target detection framework that uses a feature extraction method that uses 3D voxel convolution and point-based pointnet convolution to detect 3D targets from point cloud data and obtain the confidence, type, and distance information of the 3D target from the vehicle.

3. A method for positioning an autonomous driving vehicle in a complex urban environment, based on the system described in any one of claims 1-2, characterized in that: Here are the steps: 1) Real-time acquisition of the vehicle's own latitude and longitude positioning information and heading angle information, the vehicle's own instantaneous acceleration information and instantaneous angular velocity information, the vehicle's instantaneous speed information, the confidence, type, and distance information of the 3D targets around the vehicle, and the temperature information of the IMU and wheel odometer; 2) Determine whether the information received by the GPS unit is available: decode the received GPS raw data to obtain GPS status information. If the decoded status information is a non-negative number, the information received by the GPS unit is available, and proceed to step 3); if the decoded status information is a negative number, the information received by the GPS unit is unavailable, and proceed to step 4); 3) The vehicle latitude and longitude positioning information and heading angle information, vehicle instantaneous acceleration information and instantaneous angular velocity information, and vehicle instantaneous velocity information obtained in step 1) are sent to an extended Kalman filter unit, and the extended Kalman filter in the unit fuses the above information to obtain real-time positioning information of the vehicle; 4) The number and distance information of targets around the vehicle, as well as the temperature information of the IMU and the wheel odometer are sent to the system noise variance matrix update unit. The system noise variance matrix that makes the extended Kalman filter filtering effect best in this state is obtained through the noise gain algorithm, and it is sent to the extended Kalman filter unit to replace the current system noise variance matrix of the extended Kalman filter in the extended Kalman filter unit; the instantaneous acceleration information and instantaneous angular velocity information sent by the IMU unit and the instantaneous velocity information sent by the wheel odometer unit are integrated with the updated extended Kalman filter to obtain the positioning result in the absence of GPS.

4. The method for positioning an autonomous driving vehicle in a complex urban environment according to claim 3, characterized in that: The GPS decoding method used in step 2) is: using the function package nmea_navsat_driver under the LINUX system to decode the GPS raw data.

5. The method for positioning an autonomous driving vehicle in a complex urban environment according to claim 3, characterized in that: The specific algorithm of the extended Kalman filter used in step 3) is as follows: X n =f(X n-1 ,u n ) Z n =h(X n ) Where, X n-1 represents the state of the vehicle at time n-1, x n-1 Indicates the latitude value at time n-1, y n-1 Indicates the longitude value at time n-1, Indicates the heading angle at time n-1; X n represents the state of the vehicle at time n, x n Indicates the latitude value at time n, y n Indicates the longitude value at time n, represents the heading angle at time n; u n Represents the input of the system at time n; Z n represents the observed value of the vehicle at time n; f() and h() represent the state transfer equation and the observation equation respectively; In the formula, F n represents the state transfer matrix of the system at time n, G n It represents the mapping matrix from system input to system state at time n, H n Represents the observation matrix of the system at time n; According to the above three matrix formulas, the process of describing the standard EKF algorithm is as follows: Given the initial value of the vehicle state and the initial value of the system probability covariance matrix Then, the state of the driverless car is estimated by iterating according to the following formula: In the formula, represents the prior state estimate of the vehicle at time n, represents the posterior state estimate of the vehicle at time n, represents the posterior probability covariance matrix of the vehicle's system state at time n-1, represents the transpose of the state transfer matrix of the system at time n, Represents the transpose of the mapping matrix from system input to system state at time n, represents the transpose of the observation matrix of the system at time n, K n represents the Kalman gain at time n, represents the prior probability covariance matrix of the vehicle's system state at time n, represents the posterior probability covariance matrix of the vehicle's system state at time n, R n Represents the covariance matrix corresponding to the uncertainty of the observations, R n Determined by the sensor used, Q n represents the system noise variance matrix at time n, e v,n represents the speed measurement noise of the wheel odometer at time n, e a,n represents the acceleration measurement noise of IMU at time n, e ω,n represents the angular velocity measurement noise of IMU at time n, e v,n , e a,n , e ω,n The initial value of is obtained by calibration test; The extended Kalman filter fusion algorithm is completed, which can complete the fusion of the vehicle's latitude and longitude positioning information and heading angle information, the vehicle's instantaneous acceleration information and instantaneous angular velocity information, and the vehicle's instantaneous speed information, and finally output the vehicle's posterior state estimate at time n That is, the vehicle's position and heading angle information.

6. The method for positioning an autonomous driving vehicle in a complex urban environment according to claim 5, characterized in that: The noise gain algorithm in step 4) is specifically as follows: In the formula, Δ k-1,k represents the traffic gain from time k-1 to time k, α k represents the vehicle impact weight coefficient at time k, α k-1 represents the vehicle impact weight coefficient at time k-1, β k represents the pedestrian influence weight coefficient at time k, β k-1 represents the pedestrian influence weight coefficient at time k-1, n1 represents the number of surrounding vehicles obtained by the laser radar target detection unit at time k, n2 represents the number of surrounding pedestrians obtained by the laser radar target detection unit at time k, n3 represents the number of surrounding vehicles obtained by the laser radar target detection unit at time k-1, n4 represents the number of surrounding pedestrians obtained by the laser radar target detection unit at time k-1, represents the confidence of the i-th vehicle target obtained by the lidar target detection unit at time k, represents the confidence of the j-th pedestrian target obtained by the lidar target detection unit at time k, represents the confidence of the i-th vehicle target obtained by the laser radar target detection unit at time k-1, represents the confidence of the j-th pedestrian target obtained by the laser radar target detection unit at time k-1, represents the distance between the i-th vehicle target and the vehicle itself obtained by the laser radar target detection unit at time k, represents the distance from the jth pedestrian target to the vehicle obtained by the laser radar target detection unit at time k, represents the distance between the i-th vehicle target and the vehicle itself obtained by the laser radar target detection unit at time k-1, represents the distance from the jth pedestrian target to the vehicle obtained by the laser radar target detection unit at time k-1, represents the IMU temperature gain from time k-1 to time k, Indicates that the temperature sensor unit obtains the temperature of the IMU at time k, in degrees Fahrenheit. Indicates that the temperature sensor unit obtains the temperature of the IMU at time k-1, in degrees Fahrenheit. represents the wheel odometer temperature gain from time k-1 to time k, Indicates that the temperature sensor unit obtains the temperature of the wheel odometer at time k, in degrees Fahrenheit. Indicates that the temperature sensor unit obtains the temperature of the wheel odometer at time k-1, in degrees Fahrenheit. represents the IMU gain coefficient from time k-1 to time k, represents the wheel odometer gain coefficient from time k-1 to time k, θ E represents the weight of environmental factors, with a value of 0.7, θ I Indicates the IMU temperature influence factor, the value is 0.3, θ W Indicates the weight of the factors affecting the temperature of the wheel odometer, and its value is 0.3; Assume that the system noise variance matrix at time k-1 is Q k-1 : In the formula, e v,k-1 is the speed noise of the wheel odometer at time k-1, e a,k-1 is the acceleration noise of IMU at time k-1, e ω,k-1 is the angular velocity noise of IMU at time k-1; Then the system noise variance matrix at time k is Q k : The system noise variance matrix Q k Replace the original matrix of the extended Kalman filter, and then use the updated extended Kalman filter for fusion positioning.

Images