Intelligent networked vehicle positioning method and system

Abstract

The application discloses a positioning method and system based on intelligent networked vehicles, comprising obtaining the positioning result of the time step k-1 of the vehicle to be positioned, updating and correcting the positioning result, and obtaining the correction result of the current time step k; judging whether the millimeter wave radar observation information corresponding to the time step k and the correction result are successfully fused, and whether the fusion result is successfully updated to obtain the first prediction state information of the time step k+1; if both are successful, the fusion result is taken as the positioning result of the time step k, the positioning result is updated and corrected, and the second prediction state information of the time step k+1 is obtained; the first prediction state information and the second prediction state information corresponding to the time step k+1 are fused to determine the correction result of the time step k+1; otherwise, the correction result of the time step k is taken as the positioning result of the time step k; the positioning result of the time step k is updated and corrected to obtain the correction result of the time step k+1. The application enhances the positioning effect of the vehicle.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned vehicles, in particular to a positioning method and system based on intelligent connected vehicles. BACKGROUND

[0002] Based on the development of 5G, Beidou, artificial intelligence, big data and other fields, autonomous driving has ushered in new development opportunities. The positioning accuracy directly affects the development and use of unmanned vehicles. Due to the complexity of traffic conditions, the positioning method proposed in the prior art may fail to connect each module due to various reasons, resulting in the problem of missing positioning points of unmanned vehicles. In the tunnel scene, satellite positioning signals are easily blocked, and IMU inertial navigation positioning has a long time and large cumulative error, resulting in inaccurate vehicle positioning accuracy. The present application proposes a positioning method and system based on intelligent connected vehicles to improve the positioning accuracy of intelligent connected vehicles. SUMMARY

[0003] The present application is proposed based on the above-mentioned needs of the prior art, and aims to solve the problems of missing positioning points and low positioning accuracy of intelligent connected vehicles.

[0004] To solve the above problems, the present application adopts the following technical solutions:

[0005] A positioning method based on intelligent connected vehicles, comprising:

[0006] Obtaining the positioning result of time step k-1 of the vehicle to be positioned, wherein the positioning result includes vehicle number, motion state information and estimated error covariance matrix;

[0007] Updating and correcting the positioning result of time step k-1 to obtain the correction result of current time step k, wherein the updating includes calculating the input information based on the kinematic formula; the correction includes calculating the updating result based on the Kalman filter using global navigation satellite system GNSS monitoring information and inertial measurement unit IMU monitoring information;

[0008] Judging whether the millimeter wave radar observation information corresponding to time step k and the correction result are successfully fused, and whether the fusion result is successfully updated to obtain the first predicted state information of time step k+1, determining the positioning result of time step k and the correction result of k+1 according to the judgment result, and increasing the time step to execute the step again;

[0009] If both are successful, the fusion result is taken as the positioning result of time step k; the positioning result of time step k is updated and corrected to obtain the second predicted state information of time step k+1; the first predicted state information of time step k+1 and the second predicted state information of time step k+1 are fused to determine the correction result of time step k+1;

[0010] Otherwise, the correction result of the current time step k is taken as the positioning result of time step k; the positioning result of time step k is updated and corrected to obtain the correction result of time step k+1.

[0011] Optionally, the input information is calculated based on a kinematic formula, and the calculation formula comprises:

[0012]

[0013]

[0014] wherein F is a state update matrix, Q is a process noise covariance matrix, T is an update interval, n represents a vehicle number, X is vehicle motion state information, and P is an estimation error covariance matrix.

[0015] Optionally, the update result is calculated using GNSS monitoring information to obtain first correction information, comprising:

[0016] The vehicle is monitored in real time using GNSS, and the formula is:

[0017]

[0018] wherein, is the observation value of vehicle n by GNSS, is a GNSS observation matrix, is the observation error of GNSS, which is subject to Gaussian white noise, n represents a vehicle number, X n,k is the motion state information of vehicle n at time step k;

[0019] The update result is calculated according to the GNSS monitoring value, and the formula is:

[0020]

[0021]

[0022]

[0023] wherein R G is a GNSS observation noise covariance matrix, F is a state update matrix, T is an update interval, and P is an estimation error covariance matrix.

[0024] Optionally, the first correction information is calculated according to the IMU monitoring information, including:

[0025] The vehicle is monitored in real time by using the IMU, and the formula is:

[0026]

[0027] wherein, is the observation value of the IMU on the vehicle n; is the observation matrix of the IMU, is the observation error of the IMU, subject to Gaussian white noise, n represents the vehicle number, X n,k is the motion state information of the vehicle numbered n at the time step k;

[0028] The first correction information is calculated according to the IMU monitoring value, and the formula is:

[0029]

[0030]

[0031]

[0032] wherein, R IMU is the observation noise covariance matrix of the IMU, T is the update interval, and P is the estimation error covariance matrix.

[0033] Optionally, the millimeter wave radar observation information corresponding to the time step k is fused with the correction result, including:

[0034] The vehicle information observed by the millimeter wave radar in real time is obtained, and the formula is:

[0035]

[0036] wherein, is the observation matrix of the radar; is the state information of the radar; is the observation error of the radar, subject to Gaussian white noise, X n,k is the motion state information of the vehicle numbered n at the time step k;

[0037] The millimeter wave radar observation information is converted into vehicle information in the GNSS positioning coordinate system; the converted vehicle information and the correction result are time-synchronized and data-associated and then fused.

[0038] Optionally, the millimeter wave radar observation information is converted into vehicle information in the GNSS positioning coordinate system; the converted vehicle information and the correction result are time-synchronized and data-associated and then fused, including:

[0039] Obtaining millimeter wave radar observation information, converting the millimeter wave radar observation information into standard observation information T in a GNSS positioning coordinate system N,k ;

[0040] The correction result and the standard observation information T N,k Time synchronization

[0041] The synchronized correction result and the standard observation information T n,k Data association

[0042] The associated correction result and the standard observation information T n,k Data fusion, and the calculation formula includes

[0043]

[0044]

[0045]

[0046] Wherein, R L is a millimeter wave radar observation noise covariance matrix, T is an update interval, n represents a vehicle number, X is vehicle motion state information, and P is an estimated error covariance matrix.

[0047] A positioning system based on an intelligent networked vehicle, comprising:

[0048] A vehicle computing unit configured to obtain a positioning result of a to-be-positioned vehicle at a time step k-1, update and correct the positioning result at the time step k-1 to obtain a correction result at a current time step k, receive a positioning result at the time step k and a correction result at a time step k+1 fed back by a judgment unit, increment the time step, and send the feedback result to a roadside computing unit;

[0049] The roadside computing unit is connected with the vehicle computing unit, fuses the millimeter wave radar observation information corresponding to the time step k with the correction result, and obtains first prediction state information at the time step k+1 through updating of the fusion result;

[0050] A judgment unit connected with the vehicle computing unit and the roadside computing unit judges whether the roadside computing unit feeds back, if yes, takes the fusion result as the positioning result of time step k; updates and corrects the positioning result of time step k to obtain the second prediction state information of time step k+1; fuses the first prediction state information of time step k+1 and the second prediction state information of time step k+1 to determine the correction result of time step k+1, and feeds back the positioning result of time step k and the correction result of time step k+1 to the vehicle computing unit; otherwise, takes the correction result of the current time step k as the positioning result of time step k; updates and corrects the positioning result of time step k to obtain the correction result of time step k+1, and feeds back the positioning result of time step k and the correction result of time step k+1 to the vehicle computing unit.

[0051] Compared with the prior art, the present application provides a smart connected vehicle positioning method and system, which fuses two roadside enabled positioning feedback methods, enhances the positioning effect of the vehicle, and does not weaken the positioning effect of the vehicle when losing millimeter wave radar monitoring information or failing data association. The judgment unit determines whether the roadside computing unit has feedback, thereby improving the stability of the system. BRIEF DESCRIPTION OF DRAWINGS

[0052] In order to more clearly illustrate the technical solutions in the embodiments of the present application or the prior art, the drawings needed in the embodiments or prior art description will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments described in the present application, and other drawings can also be obtained by those skilled in the art based on these drawings.

[0053] Figure 1 is a flowchart of a smart connected vehicle positioning method provided by the present application;

[0054] Figure 2 is a feedback mechanism diagram of a smart connected vehicle positioning method provided by the present application;

[0055] Figure 3 is a complete feedback mechanism diagram of a smart connected vehicle positioning method provided by the present application;

[0056] Figure 4 is a step-by-step feedback mechanism diagram of a smart connected vehicle positioning method provided by the present application;

[0057] Figure 5 is a millimeter wave radar installation position diagram of a smart connected vehicle positioning method provided by the present application;

[0058] Figure 6 is a coordinate transformation schematic diagram based on the intelligent networked vehicle positioning method provided by the embodiment of the present application. DETAILED DESCRIPTION

[0059] To make the objects, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some but not all of the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0060] To make the objects, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are some but not all of the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative work fall within the protection scope of the present application.

[0061] Embodiment 1

[0062] The autonomous vehicle has the functional requirement of high efficiency and low delay information interaction, and the intelligent roadside device is an important component of the roadside end of the vehicle-road cooperation. The main function is to collect the current road conditions, traffic conditions and other information, communicate with the roadside perception device, traffic signal, electronic sign and other terminals through the communication network, realize the functions of vehicle-road interconnection and real-time interaction of traffic signals, and assist the driver to drive. However, the real traffic condition is very complex. When the multi-vehicle environment and various vehicle network scenes are triggered at the same time, the data packets between the beacons are easy to collide, resulting in connection failure, so that the global navigation satellite system (GNSS) and the inertial measurement unit (IMU) observation information and the radar observation information are not necessarily fused by the roadside computing unit, and finally the vehicle positioning point is missing or not accurately positioned. Therefore, the embodiment of the present application provides a positioning method based on intelligent networked vehicle, as shown in the figure, which comprises: Figures 1-2

[0063] obtaining the positioning result of the vehicle to be positioned at time step k-1, wherein the positioning result comprises vehicle number, motion state information and estimated error covariance matrix;

[0064] updating and correcting the positioning result at time step k-1 to obtain the correction result at current time step k, wherein the updating comprises: calculating the input information based on the kinematic formula; and the correction comprises: calculating the updated result based on the Kalman filter and using the global navigation satellite system (GNSS) monitoring information and the inertial measurement unit (IMU) monitoring information; ​

[0065] determine the positioning result of the time step k and the correction result of the time step k+1 according to the determination result, and the time step is incremented to execute the step again;

[0066] If both are successful, the fusion result is taken as the positioning result of the time step k; the positioning result of the time step k is updated and corrected to obtain the second prediction state information of the time step k+1; the first prediction state information of the time step k+1 and the second prediction state information of the time step k+1 are fused to determine the correction result of the time step k+1;

[0067] Otherwise, the correction result of the current time step k is taken as the positioning result of the time step k; the positioning result of the time step k is updated and corrected to obtain the correction result of the time step k+1.

[0068] In the embodiment, the positioning result is the final output value of the vehicle positioning, and the value range of k is k>2, and k is a positive integer.

[0069] The embodiment of the application fuses two feedback mechanisms, which are a complete feedback mechanism and a step-by-step feedback mechanism, and specifically includes:

[0070] The complete feedback mechanism, as shown in Figure 3 , includes: obtaining the positioning result of a vehicle to be positioned at a time step k-1 updating the positioning result at the time step k-1 to obtain {X n,K|K-1 , P n,|k-1}, correcting by GNSS to obtain correcting the GNSS correction result by IMU to obtain obtaining the correction result of the current time step k {X k , P k}, determining whether the millimeter wave radar observation information corresponding to the time step k is successfully fused with the correction result, if successfully fused, taking the fusion result as the positioning result of the time step k; otherwise, taking the correction result of the time step k {X k , P k} as the positioning result of the time step k.

[0071] The intelligent networked vehicle can obtain more accurate vehicle positioning information through the complete feedback mechanism, and can directly obtain the vehicle positioning information of the current time step. Compared with the traditional feedback method, the feedback mechanism solves the problem of data lag feedback when the positioning data is fed back, that is, the error caused by providing the vehicle positioning information obtained by the kinematic equation to the next time step to the client is solved.

[0072] The step-by-step feedback mechanism comprises Figure 4 as shown in the formula (1) : obtaining the positioning result of the vehicle to be positioned at time step k-1 updating the positioning result at time step k-1 to obtain {X n,k|k-1 , P N,|K-1} through GNSS correction re-correcting the GNSS correction result through IMU to obtain obtaining the correction result {X k , P k} of the current time step k, taking the correction result of the time step k as the positioning result of the time step k; updating and correcting the positioning result of the time step k to obtain the second predicted state information of the time step k+1; judging whether the millimeter wave radar observation information corresponding to the time step k and the correction result are successfully fused; if yes, updating the fusion result of the time step k , and fusing the updated result with the second predicted state information of the time step k+1 to obtain the positioning result of the time step k+1; otherwise, taking the correction result {X k , P k} as the positioning result of the time step k+1.

[0073] The intelligent networked vehicle solves the problem that the traditional feedback method cannot use the alternative scheme when the steps are incomplete, resulting in missing positioning points, through the step-by-step feedback mechanism. Compared with the intelligent networked vehicle for single vehicle positioning, the intelligent networked vehicle enabled by the roadside can receive a correction value provided by the roadside, which can further improve the positioning effect of the vehicle. When the intelligent networked vehicle does not receive the correction information sent by the roadside, it can still realize vehicle positioning through GNSS+IMU, solving the problem that the traditional feedback method cannot use the alternative scheme when the steps are incomplete, resulting in missing positioning points.

[0074] The positioning accuracy of the intelligent networked vehicle is improved by fusing the above two mechanisms. In the future smart city and under the background of mature automatic driving technology, the ranging accuracy of the roadside is high and the time delay is low in the tunnel scene. The kinematic equation of the automatic driving vehicle is known, and the GNSS signal is easily blocked in the tunnel. The IMU has the disadvantage of long time and large cumulative error. Therefore, the precision of the automatic driving vehicle position observed by the roadside is extremely high. Therefore, the fusion feedback mechanism is adopted to increase the weight of the roadside observation correction of the automatic driving vehicle positioning, that is, not only the positioning information after the roadside empowerment is taken as the positioning result of the current time step k, but also the positioning information after the roadside empowerment is predicted through the kinematic equation and taken as the third correction result of the next time step k+1. The proportion of roadside empowerment is increased, which greatly enhances the positioning accuracy of the automatic driving vehicle in this background and scene.

[0075] The millimeter wave radar has the advantages of strong anti-interference capability, significant price advantage, strong adaptability, strong anti-infection capability in bad weather, etc., although the amount of data obtained compared with the laser radar is small, but the millimeter wave radar of the embodiment of the application can already meet the data demand, therefore, the embodiment of the application uses the millimeter wave radar to obtain the road test observation information.

[0076] In the embodiment of the application, a model is established for the intelligent connected vehicle, and at time step k, the motion state information of the intelligent connected vehicle numbered n is:

[0077] X n,k = [p x,n,k ,v x,n,k ,a x,n,k ; p y,n,k ,v y,n,k ,a y,n,k ] T

[0078] Wherein, p x,n,k , v x,n,k , a x,n,k respectively represent the position, speed and acceleration in the x direction; p y,n,k , v y,n,k , a y,n,k respectively represent the position, speed and acceleration in the y direction.

[0079] Suppose that the intelligent connected vehicle n obeys the uniform acceleration model in a very short time interval, then at time step k+1, the motion state information of the intelligent connected vehicle n is:

[0080] X n,k+1 = FX n,k +q n,k

[0081] Wherein, F is a state update matrix, and the expression is: Wherein, T is an update interval, q n,k is a process excitation noise, which is assumed to obey a Gaussian white noise, that is is a Gaussian distribution, wherein the process noise covariance matrix is:

[0082] Wherein, Should be between 0.5 and 1 times the maximum acceleration increment in the x direction within the time interval T, Should be between 0.5 and 1 times the maximum acceleration increment in the y direction within the time interval T.

[0083] Suppose that the state information of the intelligent connected vehicle n at the previous time step is recorded as

[0084] The input information is calculated based on a kinematic formula, and the calculation formula comprises:

[0085]

[0086]

[0087] Wherein, F is a state update matrix, Q is a process noise covariance matrix, T is an update interval, n represents a vehicle number, X is vehicle motion state information, and P is an estimated error covariance matrix. The estimated value of the state x k is also called a posterior state, The state x predicted according to the state estimated value of the time step k-1 is also called a prior state, and P k|k-1 is a covariance matrix of the prior state estimated error, The covariance matrix of the time step k-1 is represented.

[0088] Optionally, the update result is calculated by using GNSS monitoring information to obtain first correction information, comprising:

[0089] The vehicle is monitored in real time by using GNSS, and the formula is:

[0090]

[0091] Wherein, is an observation value of the vehicle n by the GNSS, is a GNSS observation matrix, is an observation error of the GNSS, and is subject to Gaussian white noise, n represents a vehicle number, and X n,k is motion state information of the vehicle numbered n at the time step k.

[0092] In the embodiment of the application, according to the intelligent network connected vehicle motion state model and the state matrix, it is known that the GNSS can only provide position and speed information.

[0093] The update result is calculated according to the GNSS monitoring value, and the formula is:

[0094]

[0095]

[0096]

[0097] Wherein, R G is a GNSS observation noise covariance matrix, F is a state update matrix, T is an update interval, and P is an estimated error covariance matrix. kis the gain at time k, also known as the mixing factor.

[0098] Optionally, the first correction information is calculated according to the IMU monitoring information, including:

[0099] The vehicle is monitored in real time by using the IMU, and the formula is:

[0100]

[0101] wherein, is the observation value of the vehicle n by the IMU; is the observation matrix of the IMU, is the observation error of the IMU, subject to Gaussian white noise, n represents the vehicle number, X n,k is the motion state information of the vehicle numbered n at time step k.

[0102] In the embodiment of the application, according to the intelligent networked vehicle motion state model and the state matrix, it can be known that the GNSS can only provide acceleration information.

[0103] The first correction information is calculated according to the IMU monitoring value, and the formula is:

[0104]

[0105]

[0106]

[0107] wherein, R IMU is the observation noise covariance matrix of the IMU, T is the update interval, and P is the estimation error covariance matrix.

[0108] The update result is calculated by using the GNSS monitoring information and the IMU monitoring information, and the second correction value L n,k ={n,X k ,P k}.

[0109] Optionally, the millimeter wave radar observation information corresponding to the time step k is fused with the correction result, including:

[0110] Step one: obtaining the vehicle information observed by the millimeter wave radar in real time, and the formula is:

[0111]

[0112] wherein, is the radar observation matrix; is the state information of the radar; is the observation error of the radar, subject to Gaussian white noise, X n,kThis provides the motion state information of vehicle number n at time step k.

[0113] In this embodiment of the invention, the installation location of the millimeter-wave radar is as follows: Figure 5 As shown, the information about the intelligent connected vehicle detected by the millimeter-wave radar m is denoted as T. n,m,k T n,m,k =[p x,n,m,k ,v x,n,m,k ;p y,n,m,k ,v y,n,m,k ] T , where p x,n,m,k Let m be the relative position of the millimeter-wave radar and the intelligent connected vehicle in the x-direction at time step k; v x,n,m,k Let m be the relative velocity of the millimeter-wave radar and the intelligent connected vehicle in the x-direction at time step k; p y,n,m,k v represents the relative position of the millimeter-wave radar m and the intelligent connected vehicle in the y-direction at time step k; y,n,m,k Let L be the relative velocity of the millimeter-wave radar m and the intelligent connected vehicle in the y-direction at time step k; let L be the set of relevant information about the intelligent connected vehicle detected by the millimeter-wave radar m at time step k. m,k ={T n,m,k}, where (X, Y) represents the information of the roadside observation target in a coordinate system with the millimeter-wave radar coordinates as the origin; (p x +X,p y +Y) represents standard observation information in the GNSS coordinate system.

[0114] Step 2: Convert millimeter-wave radar observation information into vehicle information in GNSS positioning coordinate system; synchronize the converted vehicle information and correction results in time and then fuse them together.

[0115] Optionally, this step specifically includes:

[0116] S10: Acquire millimeter-wave radar monitoring information and convert the millimeter-wave radar detection information into standard observation information T in the GNSS positioning coordinate system. n,k .

[0117] In this embodiment of the invention, millimeter-wave radar monitoring information T is acquired through millimeter-wave radar m. n,m,k Transmit millimeter-wave radar monitoring information T n,m,k The coordinates and velocities of intelligent connected vehicles, with millimeter-wave radar as the origin, are converted to the vehicle's velocity and coordinates in the GNSS positioning coordinate system. An example of this conversion is shown below. Figure 6 As shown, this is called standard observation information, denoted as:

[0118] T n,k =[p x,n,k ,v x,n,k ;py,n,k ,v y,n,k ] T

[0119] p x,n,k is the position of the nth vehicle in the x direction measured by the millimeter wave radar in the global coordinate system at time step k;v x,n,k is the velocity of the nth vehicle in the x direction measured by the millimeter wave radar in the global coordinate system at time step k; p y,n,k is the position of the nth vehicle in the y direction measured by the millimeter wave radar in the global coordinate system at time step k;v y,n,k is the velocity of the nth vehicle in the y direction measured by the millimeter wave radar in the global coordinate system at time step k.

[0120] S11: the correction result and the standard observation information T n,k Time synchronization.

[0121] In the embodiment of the application, time synchronization means down-conversion of a high-frequency data set or data padding of a low-frequency data.

[0122] S12: the synchronized correction result and the standard observation information T n,k Data association.

[0123] The embodiment of the application can associate the two information through an explicit data association method, and preferably selects a global nearest neighbor algorithm to associate the correction result L n,k and the standard observation information T n,k .

[0124] S13: the associated correction result and the standard observation information T n,k Data fusion, and the calculation formula includes:

[0125]

[0126]

[0127]

[0128] wherein, R L is a millimeter wave radar observation noise covariance matrix, T is an update interval, n represents a vehicle number, X is vehicle motion state information, and P is an estimated error covariance matrix.

[0129] Embodiment 2

[0130] The embodiment of the application provides a positioning system based on an intelligent connected vehicle, which comprises:

[0131] The vehicle calculation unit is used to obtain the positioning result of the vehicle to be located at time step k-1; update and correct the positioning result of time step k-1 to obtain the correction result of the current time step k; receive the positioning result of time step k and the correction result of k+1 fed back by the judgment unit, increment the time step, and send the feedback result to the roadside calculation unit.

[0132] The roadside computing unit is connected to the vehicle computing unit, and fuses the millimeter-wave radar observation information corresponding to time step k with the correction result, and updates the fusion result to obtain the first predicted state information of time step k+1.

[0133] The judgment unit, connected to the vehicle calculation unit and the roadside calculation unit, determines whether the roadside calculation unit provides feedback. If so, it uses the fusion result as the positioning result for time step k; updates and corrects the positioning result for time step k to obtain the second predicted state information for time step k+1; fuses the first predicted state information and the second predicted state information for time step k+1 to determine the correction result for time step k+1, and feeds back the positioning result for time step k and the correction result for time step k+1 to the vehicle calculation unit; otherwise, it uses the correction result for the current time step k as the positioning result for time step k; updates and corrects the positioning result for time step k to obtain the correction result for time step k+1, and feeds back the positioning result for time step k and the correction result for time step k+1 to the vehicle calculation unit.

[0134] Specifically, it includes:

[0135] The vehicle computing unit obtains the positioning result from the previous time step k-1. The update result {X} for the current time step k was obtained through kinematic equation prediction. n,k|k-1 P n,k|k-1}, the update result {X} of the current time step K using GNSS at the current time step K. n,k|k-1 P n,k|k-1 Perform the first correction to obtain the first correction information at the current time step k. The first correction information for the current time step k is obtained from the IMU at the current time step k. Perform a second correction to obtain the correction result at the current time step k. The intelligent connected vehicle will use the correction result of the current time step k. As the single-vehicle positioning result L n,k Uploaded to the roadside computing unit.

[0136] The millimeter-wave radar will take the roadside observation information T observed at the current time step k. n,m,k The data is uploaded to the roadside computing unit, which then processes the vehicle positioning results (L). n,n With roadside observation information Tn,m,k Time synchronization, data association, data fusion, and obtaining the fusion result with roadside empowerment The fusion result with roadside empowerment The first predicted state information of the next time step k+1 is obtained by the roadside computing unit according to the kinematic equation The roadside computing unit feeds back the fusion result with roadside empowerment The first predicted state information of the next time step k+1 is obtained by the roadside computing unit according to the kinematic equation Feedback to the vehicle.

[0137] The judging unit judges whether the roadside computing unit has feedback. If there is feedback, the feedback fusion result with roadside empowerment is taken as the positioning result of the current time step k The positioning result of the current time step k And based on the positioning result of the current time step k The update result of the next time step k+1 is obtained by the kinematic equation n,k+1|k , P n,k+1|k The first correction of the update result of the next time step k+1 is performed by the GNSS of the next time step k+1 n,k+1|k , P n,k+1|k The first correction information of the next time step k+1 is obtained The second correction of the first correction information of the next time step k+1 is performed by the IMU of the next time step k+1 The second predicted state information of the next time step k+1 is obtained After experiencing two corrections, the first predicted state information of the next time step k+1 is obtained by the roadside computing unit according to the kinematic equation The third correction value of the vehicle positioning of the next time step k+1 is taken as the second predicted state information of the next time step The third correction of the second predicted state information of the next time step k+1 is performed, and the third correction information of the next time step k+1, i.e., the correction result, is obtained The judging unit takes the positioning result of the current time step k and the correction result of the next time step k+1 As the single vehicle positioning result L n,k+1 Feedback to the vehicle computing unit, the vehicle computing unit increments the time step, and sends the feedback result to the roadside computing unit.

[0138] If there is no feedback, the single vehicle positioning result L n,k The positioning result of the current time step k k , P k} is taken as the positioning result of the current time step k k , P k} and the next time step k+1 update result {X n,k+1|k} is obtained by the GNSS of the next time step k+1 n,k+1|k} is obtained by the GNSS of the next time step k+1 n,k+1|k} is obtained by the GNSS of the next time step k+1 n,+1|k} is obtained by the GNSS of the next time step k+1 } is obtained by the GNSS of the next time step k+1 } is obtained by the GNSS of the next time step k+1 The positioning result of the current time step k and the next time step k+1 correction result as a single vehicle positioning result L n,k+1 is fed back to the vehicle computing unit, the vehicle computing unit increments the time step, and the feedback result is sent to the roadside computing unit.

[0139] Further, the roadside computing unit can also be connected with a cellular vehicle to everything (C-V2X) communication unit, receive traffic data collected by the fisheye detector and the video detector, and be fused again with the fusion result calculated by the roadside computing unit; the data fused again is transmitted to the first roadside communication unit through the switch, the first roadside communication unit can be transmitted to the adjacent second roadside communication unit in a direct link road communication mode; the first roadside communication unit or the second roadside communication unit can be transmitted to the C-V2X platform in a cellular communication mode of 4G / 5G technology, and the C-V2X platform again interacts with the vehicle through the roadside communication unit RSU in a direct link short-range communication mode, and provides traffic participant intention recognition and trajectory prediction, vehicle collision, and vehicle collision recognition and warning for the vehicle.

[0140] Compared with the prior art, the embodiment of the present application provides a positioning method and system based on an intelligent networked vehicle, which fuses two roadside enabled positioning feedback methods, enhances the positioning effect of the vehicle, and does not weaken the positioning effect of the vehicle when losing millimeter wave radar monitoring information or failing data association; the judgment unit determines whether the roadside computing unit has feedback, and improves the stability of the system.

[0141] The above specific embodiments further illustrate the purpose, technical solutions and advantages of the present application, and it should be understood that the above description is only a specific embodiment of the present application and does not limit the protection scope of the present application, and any modification, equivalent replacement, improvement, etc. within the spirit and principles of the present application should be included in the protection scope of the present application.

Claims

1. A method for positioning based on intelligent connected vehicles, characterized in that, The method comprises the following steps: acquiring a positioning result of a vehicle to be positioned at a time step k-1, wherein the positioning result comprises a vehicle number, motion state information and an estimated error covariance matrix; updating and correcting the positioning result at the time step k-1 to obtain a correction result at a current time step k, wherein the updating comprises: calculating input information based on a kinematic formula; and the correcting comprises: calculating the updated result based on a Kalman filter and using global navigation satellite system (GNSS) monitoring information and inertial measurement unit (IMU) monitoring information; judging whether the millimeter wave radar observation information corresponding to the time step k and the correction result are successfully fused and whether the fused result is successfully updated to obtain first predicted state information at a time step k+1, and determining the positioning result at the time step k and the correction result at the time step k+1 according to a judgment result, and the time step is incremented to execute the step again; if both are successful, taking the fused result as the positioning result at the time step k; updating and correcting the positioning result at the time step k to obtain second predicted state information at the time step k+1; and fusing the first predicted state information at the time step k+1 and the second predicted state information at the time step k+1 to determine the correction result at the time step k+1; otherwise, taking the correction result at the current time step k as the positioning result at the time step k; updating and correcting the positioning result at the time step k to obtain the correction result at the time step k+1. 2.The intelligent vehicle positioning method of claim 1, wherein, The calculating input information based on the kinematic formula comprises the following formula: wherein F is a state updating matrix, Q is a process noise covariance matrix, T is an updating interval, n represents a vehicle number, X is vehicle motion state information, and P is an estimated error covariance matrix. 3.The intelligent vehicle positioning method of claim 1, wherein, The calculating the updated result based on the GNSS monitoring information comprises the following steps: real-time monitoring of the vehicle by the GNSS, and the formula is as follows: wherein, is the GNSS observation for vehicle n, is the GNSS observation matrix, is the observation error for GNSS, obeying Gaussian white noise, n represents the vehicle number, X n,k is the motion state information of the vehicle numbered n at time step k; calculating the updated result according to the GNSS monitoring value, and the formula is as follows: wherein R G is the GNSS observation noise covariance matrix, F is the state update matrix, T is the update interval, and P is the estimation error covariance matrix. 4.The intelligent vehicle positioning method of claim 3, wherein, calculating the first correction information according to the IMU monitoring information, and the formula is as follows: real-time monitoring of the vehicle by the IMU, and the formula is as follows: wherein, is the observation of the IMU for vehicle n; is the IMU observation matrix, is the observation error of the IMU, is Gaussian white noise, n denotes the vehicle number, X n,k is the motion state information of the vehicle numbered n at time step k; calculating the first correction information according to the IMU monitoring value, and the formula is as follows: where R IMU is the IMU observation noise covariance matrix, T is the update interval, and P is the estimation error covariance matrix.

5. The intelligent vehicle positioning method according to claim 1, wherein, fusing the millimeter wave radar observation information corresponding to the time step k and the correction result, which comprises the following steps: acquiring vehicle information observed by the millimeter wave radar in real time, and the formula is as follows: wherein, is a radar observation matrix; is state information of the radar; is an observation error of the radar, subject to Gaussian white noise, X n,k is motion state information of the vehicle numbered n at time step k; converting the millimeter wave radar observation information into vehicle information in a GNSS positioning coordinate system; and performing time synchronization and data association on the converted vehicle information and the correction result and then fusing them. 6.The intelligent vehicle positioning method of claim 5, wherein, The converting the millimeter wave radar observation information into vehicle information in the GNSS positioning coordinate system; the time synchronization and data association on the converted vehicle information and the correction result and then fusing them, which comprises the following steps: Obtaining millimeter wave radar observation information, converting the millimeter wave radar observation information into standard observation information T in a GNSS positioning coordinate system N,K ; The correction result and the standard observation information T N,K Time synchronization; The corrected results and the standard observation information T after synchronization are processed by a global nearest neighbor algorithm N,k Data association; The correction result of the association success and the standard observation information T n,k Data fusion, whose calculation formula includes: where R L is the millimeter-wave radar observation noise covariance matrix, T is the update interval, n represents the vehicle number, X is the vehicle motion state information, and P is the estimation error covariance matrix.

7. An intelligent vehicle connectivity-based positioning system, comprising: The method comprises the following steps: a vehicle calculation unit, configured to acquire a positioning result of a vehicle to be positioned at a time step k-1; updating and correcting the positioning result at the time step k-1 to obtain a correction result at a current time step k; receiving positioning result at the time step k and correction result at the time step k+1 fed back by a judging unit, incrementing the time step, and sending the feedback result to a roadside calculation unit; The roadside computing unit is connected with the vehicle computing unit, fuses the millimeter wave radar observation information corresponding to the time step k and the correction result, and obtains first prediction state information of a time step k+1 through updating of the fusion result; The judgment unit is connected with the vehicle computing unit and the roadside computing unit, judges whether the roadside computing unit feeds back, if yes, takes the fusion result as a positioning result of the time step k; updates and corrects the positioning result of the time step k to obtain second prediction state information of the time step k+1; fuses the first prediction state information of the time step k+1 and the second prediction state information of the time step k+1, determines a correction result of the time step k+1, and feeds back the positioning result of the time step k and the correction result of the time step k+1 to the vehicle computing unit; otherwise, takes the correction result of the current time step k as the positioning result of the time step k; updates and corrects the positioning result of the time step k to obtain the correction result of the time step k+1, and feeds back the positioning result of the time step k and the correction result of the time step k+1 to the vehicle computing unit.

Images