High-precision positioning method, device, equipment and readable storage medium

By combining the extended Kalman filtering algorithm and visual identification information, the accuracy and stability of lane-level positioning are improved, and the problems of high-precision inertial navigation are solved, and the problems of high-precision inertial guidance in lane-level positioning are realized in weak signal areas high-precision positioning.

CN114168900BActive Publication Date: 2024-11-15DONGFENG COMML VEHICLE CO LTD
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
CN202111441906.3
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2021-11-30
Publication Date
2024-11-15
Estimated Expiration
2041-11-30

AI Technical Summary

Technical Problem

High-precision inertial navigation is costly and has low stability and accuracy when achieving lane-level positioning. It is difficult to achieve stable and high-precision positioning in long tunnels or under viaducts where the signal is weak.

Method used

By obtaining the observed state vector of the current moment of the bicycle and the fusion optimization state vector of the previous moment, using the extended Kalman filtering algorithm for state prediction and fusion, combining the global satellite navigation system and visual recognition of lane line information, improving the accuracy of lane-level positioning and stability.

Benefits of technology

The cost of lane-level positioning is reduced, the positioning accuracy and stability in weak signal areas are improved, and the problem of high-precision inertial guides is solved.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN114168900B_ABST
    Figure CN114168900B_ABST
Patent Text Reader

Abstract

The present invention provides a high-precision positioning method, device, equipment and readable storage medium. The method includes: obtaining the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle; obtaining the predicted state vector of the vehicle at the current moment according to the fused and optimized state vector of the vehicle at the previous moment; fusing the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The present invention solves the problem of high cost, low stability and low accuracy of lane-level positioning by high-precision inertial navigation.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of automotive electronic technology, and in particular to a high-precision positioning method, device, equipment and readable storage medium. Background Art

[0002] As the number of cars continues to increase, road safety issues are becoming more and more serious. Therefore, advanced driver assistance systems have become a research hotspot. Among many advanced driver assistance system applications, lane-level positioning of vehicles is very important.

[0003] In the existing technology, lane-level positioning can be achieved by using high-precision inertial navigation and carrier phase differential technology (RTK) equipment. However, high-precision inertial navigation is expensive, and it cannot achieve lane-level positioning in long tunnels, under viaducts, or in places with weak signals. Therefore, high-precision inertial navigation is also affected by road terrain, and has the defects of low stability and accuracy in achieving lane-level positioning. Summary of the invention

[0004] The main purpose of the present invention is to provide a high-precision positioning method, device, equipment and readable storage medium, aiming to solve the problem of high cost, low stability and accuracy of lane-level positioning achieved by high-precision inertial navigation.

[0005] In a first aspect, the present invention provides a high-precision positioning method, the high-precision positioning method comprising:

[0006] Obtaining the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle;

[0007] The predicted state vector of the vehicle at the current moment is obtained according to the fused and optimized state vector of the vehicle at the previous moment;

[0008] The observed state vector of the vehicle at the current moment is fused with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment.

[0009] Optionally, obtaining the observed state vector of the vehicle at the current moment includes:

[0010] Obtain the current observation state vector of the vehicle according to the global satellite navigation system GNSS;

[0011] Alternatively, the observation state vector of the vehicle at the current moment is obtained based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map.

[0012] Optionally, the step of obtaining the observation state vector of the vehicle at the current moment according to the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map includes:

[0013] Determine the observation coordinates of the vehicle at the current moment based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map;

[0014] After determining the observation coordinates of the vehicle at the current moment, obtain 5 points before and after the observation coordinates of the vehicle on the left and right lanes and the center line of the lane where the vehicle is located. Among them, the lane line position of the vehicle is the lane line position corresponding to the lane line information of the high-precision map;

[0015] Perform 5th-order polynomial fitting on the points on the left and right lane lines and the center line, and get 3 polynomials;

[0016] Calculate three lane line heading angles according to three polynomials respectively, and calculate the average of the three lane line heading angles, which is the heading angle of the lane line on the high-precision map;

[0017] The relative angle between the vehicle and the visual lane line is obtained through the visual controller;

[0018] Calculate the sum of the heading angle of the lane line on the high-precision map and the relative angle between the vehicle and the visual lane line to obtain the observed heading angle of the vehicle at the current moment;

[0019] The observed speed of the vehicle at the current moment is determined according to the fused and optimized speed of the vehicle at the previous moment.

[0020] Optionally, the step of obtaining the predicted state vector of the vehicle at the current moment according to the fused and optimized state vector of the vehicle at the previous moment includes:

[0021] According to the fused and optimized coordinates, heading angle and / or speed of the ego vehicle at the previous moment, the predicted state vector of the ego vehicle at the current moment is obtained by the first recursive formula of the extended Kalman filter algorithm. The first recursive formula is as follows:

[0022]

[0023] Among them, v t is the optimized speed of the vehicle at the last moment, (x t ,y t ) is the fusion optimized coordinate of the vehicle at the last moment, where x t is the x-coordinate after fusion optimization of the vehicle at the last moment, y t The heading is the optimized coordinate of the y direction of the vehicle at the last moment. t is the heading angle after the fusion optimization of the ego vehicle, d t is the time interval between the current moment and the previous moment, L is the wheelbase of the vehicle, steerangle is the steering wheel angle of the vehicle, v t+1is the predicted speed of the vehicle at the current moment, heading t+1 1 is the predicted heading angle of the vehicle at the current moment, (x t+1 ,y t+1 ) is the predicted coordinate of the vehicle at the current moment, where x t+1 is the predicted x-coordinate of the vehicle at the current moment, y t+1 It is the predicted y-direction coordinate of the vehicle at the current moment.

[0024] Optionally, the step of fusing the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain a fused and optimized state vector of the vehicle at the current moment includes:

[0025] At the last moment, the second recursive formula P of the extended Kalman filter algorithm is t+1 =J*P t *J T +Q recursively calculates the covariance matrix at the current moment, where P t+1 is the covariance matrix at the current moment, J is the Jacobian matrix, P t is the covariance matrix of the previous moment, T is the transposed matrix, and Q is the process noise;

[0026] Substitute the covariance matrix at the current moment into the first preset formula to calculate the extended Kalman gain. The first preset formula is as follows: K = P t+1 *H T (H*P t+1 *H T +R) -1 , where R is the observation noise, H is the observation matrix, and K is the extended Kalman gain;

[0027] Substitute the extended Kalman gain, the observed state vector of the vehicle at the current moment, and the predicted state vector of the vehicle at the current moment into the second preset formula to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The second preset formula is as follows: M3=M1+K(M2-H*M1), where M1 is the predicted state vector of the vehicle at the current moment, M2 is the observed state vector of the vehicle at the current moment, and M3 is the fused and optimized state vector of the vehicle at the current moment.

[0028] Optionally, after the step of fusing the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment, the method further includes:

[0029] Substitute the extended Kalman gain and the covariance matrix at the current moment into the third preset formula The covariance matrix at the current moment is updated, where: is the updated covariance matrix at the current moment, P t+1 is the covariance matrix at the current moment, I is the diagonal matrix, K is the extended Kalman gain, and H is the observation matrix.

[0030] In a second aspect, the present invention further provides a high-precision positioning device, the high-precision positioning device comprising:

[0031] An acquisition module is used to acquire the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle;

[0032] A prediction module is used to obtain the predicted state vector of the vehicle at the current moment according to the fused and optimized state vector of the vehicle at the previous moment;

[0033] The fusion module is used to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment.

[0034] Optionally, the fusion module is used to:

[0035] At the last moment, the second recursive formula P of the extended Kalman filter algorithm is t+1 =J*P t *J T +Q recursively calculates the covariance matrix at the current moment, where P t+1 is the covariance matrix at the current moment, J is the Jacobian matrix, P t is the covariance matrix of the previous moment, T is the transposed matrix, and Q is the process noise;

[0036] Substitute the covariance matrix at the current moment into the first preset formula to calculate the extended Kalman gain. The first preset formula is as follows: K = P t+1 *H T (H*P t+1 *H T +R) -1 , where R is the observation noise, H is the observation matrix, and K is the extended Kalman gain;

[0037] Substitute the extended Kalman gain, the observed state vector of the vehicle at the current moment, and the predicted state vector of the vehicle at the current moment into the second preset formula to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The second preset formula is as follows: M3=M1+K(M2-H*M1), where M1 is the predicted state vector of the vehicle at the current moment, M2 is the observed state vector of the vehicle at the current moment, and M3 is the fused and optimized state vector of the vehicle at the current moment.

[0038] In a third aspect, the present invention also provides a high-precision positioning device, comprising a processor, a memory, and a high-precision positioning program stored in the memory and executable by the processor, wherein when the high-precision positioning program is executed by the processor, the steps of the high-precision positioning method as described above are implemented.

[0039] In a fourth aspect, the present invention further provides a readable storage medium, on which a high-precision positioning program is stored, wherein when the high-precision positioning program is executed by a processor, the steps of the high-precision positioning method as described above are implemented.

[0040] In the present invention, the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment are obtained, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle; the predicted state vector of the vehicle at the current moment is obtained according to the fused and optimized state vector of the vehicle at the previous moment; the observed state vector of the vehicle at the current moment is fused with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. Through the present invention, the predicted coordinates, heading angle and / or speed of the vehicle at the current moment are obtained according to the fused and optimized coordinates, heading angle and / or speed of the vehicle at the previous moment, and the observed coordinates, heading angle and / or speed of the vehicle at the current moment are fused with the predicted coordinates, heading angle and / or speed of the vehicle at the current moment to obtain the fused and optimized coordinates, heading angle and / or speed of the vehicle at the current moment, thereby improving the accuracy of lane-level positioning. On the other hand, in special road sections where the observed state vector of the vehicle at the current moment cannot be obtained, the position of the vehicle is updated according to the predicted state vector of the vehicle at the current moment, thereby improving the stability of lane-level positioning. Based on the above description, the problem of high cost, low stability and low accuracy of lane-level positioning achieved by high-precision inertial navigation is solved. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] Figure 1 A schematic diagram of the hardware structure of a high-precision positioning device involved in an embodiment of the present invention;

[0042] Figure 2 It is a flowchart of the first embodiment of the high-precision positioning method of the present invention;

[0043] Figure 3 It is a schematic diagram of functional modules of the first embodiment of the high-precision positioning device of the present invention.

[0044] The realization of the purpose, functional features and advantages of the present invention will be further explained in conjunction with embodiments and with reference to the accompanying drawings. DETAILED DESCRIPTION

[0045] It should be understood that the specific embodiments described herein are only used to explain the present invention, and are not used to limit the present invention.

[0046] In a first aspect, an embodiment of the present invention provides a high-precision positioning device.

[0047] Reference Figure 1 , Figure 1 Schematic diagram of the hardware structure of the high-precision positioning device involved in the embodiment of the present invention. In the embodiment of the present invention, the high-precision positioning device may include a processor 1001 (such as a central processing unit, CPU), a communication bus 1002, a user interface 1003, a network interface 1004, and a memory 1005. Among them, the communication bus 1002 is used to realize the connection and communication between these components; the user interface 1003 may include a display screen (Display), an input unit such as a keyboard (Keyboard); the network interface 1004 may optionally include a standard wired interface, a wireless interface (such as wireless fidelity WIreless-FIdelity, WI-FI interface); the memory 1005 may be a high-speed random access memory (random access memory, RAM), or a stable memory (non-volatile memory), such as a disk storage, and the memory 1005 may optionally be a storage device independent of the aforementioned processor 1001. Those skilled in the art will understand that Figure 1 The hardware structure shown in the figure does not constitute a limitation of the present invention, and may include more or less components than those shown in the figure, or combine certain components, or arrange the components differently.

[0048] Continue to refer to Figure 1 , Figure 1 The memory 1005 as a computer storage medium may include an operating system, a network communication module, a user interface module, and a high-precision positioning program. The processor 1001 may call the high-precision positioning program stored in the memory 1005 and execute the high-precision positioning method provided in the embodiment of the present invention.

[0049] In a second aspect, an embodiment of the present invention provides a high-precision positioning method.

[0050] In one embodiment, referring to Figure 2 , Figure 2 FIG. 1 is a flow chart of the first embodiment of the high-precision positioning method of the present invention. Figure 2 As shown, the high-precision positioning methods include:

[0051] Step S10, obtaining the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle;

[0052] In this embodiment, the state vector includes the coordinates, heading angle and / or speed of the vehicle, and the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment are obtained, that is, the observed coordinates, heading angle and / or speed of the vehicle at the current moment and the fused and optimized coordinates, heading angle and / or speed of the vehicle at the previous moment are obtained.

[0053] Furthermore, in one embodiment, the step of obtaining the observed state vector of the vehicle at the current moment includes:

[0054] Obtain the current observation state vector of the vehicle according to the global satellite navigation system GNSS;

[0055] Alternatively, the observation state vector of the vehicle at the current moment is obtained based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map.

[0056] In this embodiment, with the rise of many satellite positioning and navigation systems, the global satellite positioning and navigation system has a new name: GNSS (Global Navigation Satellite System). The current observation coordinates, heading angle and / or speed of the vehicle are obtained according to the global satellite navigation system GNSS.

[0057] Or, the observation coordinates, heading angle and / or speed of the vehicle at the current moment are obtained based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map. Among them, the visual recognition lane line information of the vehicle at the current moment is obtained according to the visual controller, and the high-precision map lane line information is obtained according to the observation state vector of the vehicle at the current moment combined with the high-precision map.

[0058] Furthermore, the lane line information of the high-precision map includes lane line point information, lane line type and lane line topology information. Among them, the high-precision map, that is, the high-precision electronic map, is also called the high-resolution map (HD Map, High Definition Map). High precision, on the one hand, means that the absolute coordinate accuracy of the high-precision electronic map is higher. The absolute coordinate accuracy refers to the accuracy between a certain target on the map and the real external world. On the other hand, the road traffic information elements contained in the high-precision map are richer and more detailed. In addition to the road (Road) level navigation information that the high-precision map can provide, it can also provide lane (Lane) level navigation information.

[0059] Furthermore, in one embodiment, the step of obtaining the observation state vector of the vehicle at the current moment according to the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map includes:

[0060] Determine the observation coordinates of the vehicle at the current moment based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map;

[0061] After determining the observation coordinates of the vehicle at the current moment, obtain 5 points before and after the observation coordinates of the vehicle on the left and right lanes and the center line of the lane where the vehicle is located. Among them, the lane line position of the vehicle is the lane line position corresponding to the lane line information of the high-precision map;

[0062] Perform 5th-order polynomial fitting on the points on the left and right lane lines and the center line, and get 3 polynomials;

[0063] Calculate three lane line heading angles according to three polynomials respectively, and calculate the average of the three lane line heading angles, which is the heading angle of the lane line on the high-precision map;

[0064] The relative angle between the vehicle and the visual lane line is obtained through the visual controller;

[0065] Calculate the sum of the heading angle of the lane line on the high-precision map and the relative angle between the vehicle and the visual lane line to obtain the observed heading angle of the vehicle at the current moment;

[0066] The observed speed of the vehicle at the current moment is determined according to the fused and optimized speed of the vehicle at the previous moment.

[0067] In this embodiment, the visual recognition lane line information is matched with the high-precision map lane line information. If the visual recognition lane line information matches the high-precision map lane line information successfully, the lane line position corresponding to the high-precision map lane line information is used as the lane line position of the vehicle at the current moment. If the visual recognition lane line information does not match the high-precision map lane line information, the visual recognition lane line information and the high-precision map lane line information are re-acquired, and the new visual recognition lane line information is matched with the new high-precision map lane line information until the match is successful. Furthermore, the line distance between the vehicle and the left and right lines of the lane at the current moment is obtained from the visual lane line information, and the observation coordinates of the vehicle at the current moment are determined based on the visually recognized distance to the left and right lines of the lane line.

[0068] After determining the observation coordinates of the vehicle at the current moment, obtain 5 points before and after the observation coordinates of the vehicle on the left and right lanes and the center line of the lane where the vehicle is located. Among them, the lane line position of the vehicle is the lane line position corresponding to the lane line information of the high-precision map, that is, the lane line where the vehicle is located is the lane line of the high-precision map;

[0069] Perform a 5th-order polynomial fit on the points on the left side of the lane line to obtain the polynomial y1=a1x 5 +b1x 4 +c1x 3 +d1x 2 +e1x+f1, the points on the right side of the lane line are fitted with a 5th-order polynomial to obtain the polynomial y2=a2x 5 +b2x 4 +c2x 3 +d2x 2 +e2x+f2, the points taken on the lane center line are fitted with a 5th-order polynomial to obtain the polynomial y3=a3x 5 +b3x 4 +c3x 3 +d3x 2 +e3x+f3.

[0070] Find the polynomial y1=a1x 5 +b1x 4 +c1x 3 +d1x 2 +e1x+f1 derivative, we can get x=0 in, is the tangent direction, i.e. the heading angle, so tan(heading1)=e1, similarly, tan(heading2)=e2, tan(heading3)=e3, and further, the heading angles of the three lane lines heading1, heading2, and heading3 can be calculated. Then the average heading angle of the three lane lines heading4 is calculated, and the average heading angle of the three lane lines heading4 is the heading angle of the lane line in the high-precision map.

[0071] The relative angle θ between the ego vehicle and the visual lane line is obtained through the visual controller, and the heading angle of the lane line on the high-precision map and the sum of the relative angle between the ego vehicle and the visual lane line are calculated to obtain the observed heading angle of the ego vehicle at the current moment. t+ 12, where if counterclockwise is positive, then heading t+1 2=heading4+θ, if clockwise is positive, then heading t+1 2 = heading4 + (-θ).

[0072] It should be noted that, in this embodiment, the vehicle is assumed to travel at a constant speed by default, that is, the observed speed of the vehicle at the current moment is the same as the fused and optimized speed of the vehicle at the previous moment, so the observed speed of the vehicle at the current moment can be determined based on the fused and optimized speed of the vehicle at the previous moment.

[0073] Step S20, obtaining the predicted state vector of the vehicle at the current moment according to the fused and optimized state vector of the vehicle at the previous moment;

[0074] In this embodiment, the predicted state vector of the vehicle at the current moment is obtained through the extended Kalman filter algorithm based on the fusion optimized state vector of the vehicle at the previous moment. Kalman filtering is an algorithm that uses the linear system state equation and the system input and output observation data to optimally estimate the system state, that is, the Kalman filter uses the dynamic information of the target to obtain a good estimate of the target position. The extended Kalman filter (EKF) is an extended form of the standard Kalman filter in the nonlinear case, wherein the basic idea of ​​the extended Kalman filter EKF is to linearize the nonlinear system using the Taylor series expansion, and then filter the signal using the Kalman filter framework.

[0075] Furthermore, in one embodiment, step S20 includes:

[0076] The predicted state vector of the vehicle at the current moment is obtained by the first recursive formula of the extended Kalman filter algorithm according to the fused optimized coordinates, heading angle and / or speed of the vehicle at the previous moment. The first recursive formula is as follows:

[0077]

[0078] Among them, v t is the optimized speed of the vehicle at the last moment, (x t ,y t ) is the fusion optimized coordinate of the vehicle at the last moment, where x t is the x-coordinate after fusion optimization of the vehicle at the last moment, y t The heading is the optimized coordinate of the y direction of the vehicle at the last moment. t is the heading angle after the fusion optimization of the ego vehicle, d t is the time interval between the current moment and the previous moment, L is the wheelbase of the vehicle, steerangle is the steering wheel angle of the vehicle, v t+1 is the predicted speed of the vehicle at the current moment, heading t+1 1 is the predicted heading angle of the vehicle at the current moment, (x t+1 ,y t+1 ) is the predicted coordinate of the vehicle at the current moment, where x t+1 is the predicted x-coordinate of the vehicle at the current moment, y t+1 It is the predicted y-direction coordinate of the vehicle at the current moment.

[0079] In this embodiment, the fused and optimized coordinates, heading angle and / or speed of the vehicle at the previous moment are obtained, and the coordinates, heading angle and / or speed of the vehicle at the current moment are predicted based on the fused and optimized coordinates, heading angle and / or speed of the vehicle at the previous moment. Specifically, the predicted state vector of the vehicle at the current moment is obtained by the first recursive formula of the extended Kalman filter algorithm based on the fused and optimized coordinates, heading angle and / or speed of the vehicle at the previous moment. The first recursive formula is as follows:

[0080]

[0081] Among them, v t is the optimized speed of the vehicle at the last moment, (x t ,y t ) is the fusion optimized coordinate of the vehicle at the last moment, where x t is the x-coordinate after fusion optimization of the vehicle at the last moment, y t The heading is the optimized coordinate of the y direction of the vehicle at the last moment. t is the heading angle after the fusion optimization of the ego vehicle, d t is the time interval between the current moment and the previous moment, L is the wheelbase of the vehicle, steerangle is the steering wheel angle of the vehicle, v t+1 is the predicted speed of the vehicle at the current moment, heading t+1 1 is the predicted heading angle of the vehicle at the current moment, (x t+1 ,y t+1 ) is the predicted coordinate of the vehicle at the current moment, where x t+1 is the predicted x-coordinate of the vehicle at the current moment, y t+1 It is the predicted y-direction coordinate of the vehicle at the current moment.

[0082] Step S30, fusing the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain a fused and optimized state vector of the vehicle at the current moment.

[0083] In this embodiment, if the observed state vector of the vehicle at the current moment is obtained based on the global satellite navigation system GNSS, the observed state vector of the vehicle at the current moment obtained based on the global satellite navigation system GNSS is fused with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment.

[0084] If the observation state vector of the vehicle at the current moment is obtained based on the visual recognition lane line information and the high-precision map lane line information of the vehicle at the current moment, then the observation state vector of the vehicle at the current moment obtained based on the visual recognition lane line information and the high-precision map lane line information of the vehicle at the current moment is fused with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment.

[0085] Furthermore, in one embodiment, step S30 includes:

[0086] At the last moment, the second recursive formula P of the extended Kalman filter algorithm is t+1 =J*P t *J T +Q recursively calculates the covariance matrix at the current moment, where P t+1 is the covariance matrix at the current moment, J is the Jacobian matrix, P t is the covariance matrix of the previous moment, T is the transposed matrix, and Q is the process noise;

[0087] Substitute the covariance matrix at the current moment into the first preset formula to calculate the extended Kalman gain. The first preset formula is as follows: K = P t+1 *H T (H*P t+1 *H T +R) -1 , where R is the observation noise, H is the observation matrix, and K is the extended Kalman gain;

[0088] Substitute the extended Kalman gain, the observed state vector of the vehicle at the current moment, and the predicted state vector of the vehicle at the current moment into the second preset formula to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The second preset formula is as follows: M3=M1+K(M2-H*M1), where M1 is the predicted state vector of the vehicle at the current moment, M2 is the observed state vector of the vehicle at the current moment, and M3 is the fused and optimized state vector of the vehicle at the current moment.

[0089] In this embodiment, at the last moment, the second recursive formula P of the extended Kalman filter algorithm is used. t+1 =J*P t *J T +Q recursively calculates the covariance matrix at the current moment, where P t+1 is the covariance matrix at the current moment, J is the Jacobian matrix, P t is the covariance matrix of the previous moment, T is the transposed matrix, and Q is the process noise. In the extended Kalman filter algorithm, the covariance represents the expectation of the overall error of the two variables.

[0090] Substitute the covariance matrix at the current moment into the first preset formula to calculate the extended Kalman gain. The first preset formula is as follows: K = P t+1 *H T (H*P t+1 *H T +R) -1 , where R is the observation noise, H is the observation matrix, and K is the extended Kalman gain. The extended Kalman gain K is actually a trade-off between the predicted value and the observed value, that is, the trade-off between the observed coordinates of the ego vehicle at the current moment and the predicted coordinates of the ego vehicle at the current moment and / or the trade-off between the observed heading angle of the ego vehicle at the current moment and the predicted heading angle of the ego vehicle at the current moment and / or the trade-off between the observed speed of the ego vehicle at the current moment and the predicted speed of the ego vehicle at the current moment.

[0091] Substitute the extended Kalman gain, the observed state vector of the vehicle at the current moment, and the predicted state vector of the vehicle at the current moment into the second preset formula to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The second preset formula is as follows: M3=M1+K(M2-H*M1), wherein M1 is the predicted state vector of the vehicle at the current moment, M2 is the observed state vector of the vehicle at the current moment, and M3 is the fused and optimized state vector of the vehicle at the current moment, that is, when M1 is the predicted coordinates, heading angle and / or speed of the vehicle at the current moment, and M2 is the observed coordinates, heading angle and / or speed of the vehicle at the current moment, M3 is the fused and optimized coordinates, heading angle and / or speed of the vehicle at the current moment.

[0092] In this embodiment, the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment are obtained, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle; the predicted state vector of the vehicle at the current moment is obtained based on the fused and optimized state vector of the vehicle at the previous moment; the observed state vector of the vehicle at the current moment is fused with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. Through this embodiment, the predicted coordinates, heading angle and / or speed of the vehicle at the current moment are obtained based on the fused and optimized coordinates, heading angle and / or speed of the vehicle at the previous moment, and the observed coordinates, heading angle and / or speed of the vehicle at the current moment are fused with the predicted coordinates, heading angle and / or speed of the vehicle at the current moment to obtain the fused and optimized coordinates, heading angle and / or speed of the vehicle at the current moment, thereby improving the accuracy of lane-level positioning. On the other hand, in special road sections where the observed state vector of the vehicle at the current moment cannot be obtained, the position of the vehicle is updated according to the predicted state vector of the vehicle at the current moment, thereby improving the stability of lane-level positioning. Based on the above description, the problem of high cost, low stability and low accuracy of lane-level positioning achieved by high-precision inertial navigation is solved.

[0093] Furthermore, in one embodiment, after step S30, the method further includes:

[0094] Substitute the extended Kalman gain and the covariance matrix at the current moment into the third preset formula The covariance matrix at the current moment is updated, where: is the updated covariance matrix at the current moment, P t+1 is the covariance matrix at the current moment, I is the diagonal matrix, K is the extended Kalman gain, and H is the observation matrix.

[0095] In this embodiment, the covariance matrix P is an iteratively updated quantity. After each round of prediction and update, P will update a new value, that is, the covariance matrix of the current moment predicted by the extended Kalman filter algorithm at the previous moment is updated to obtain the updated covariance matrix of the current moment. Specifically, the extended Kalman gain and the covariance matrix of the current moment are substituted into the third preset formula Update the covariance matrix at the current moment, where is the updated covariance matrix at the current moment, P t+1 is the covariance matrix at the current moment, I is the diagonal matrix, K is the extended Kalman gain, H is the observation matrix. Furthermore, with several rounds of iterations, the true value will become increasingly tighter, so it can be set according to the estimate during initialization.

[0096] In a third aspect, an embodiment of the present invention further provides a high-precision positioning device.

[0097] In one embodiment, referring to Figure 3 , Figure 3 FIG. 1 is a schematic diagram of the functional modules of the first embodiment of the high-precision positioning device of the present invention. Figure 3 As shown, the high-precision positioning device includes:

[0098] An acquisition module 10 is used to acquire the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle;

[0099] A prediction module 20, used to obtain a predicted state vector of the vehicle at the current moment according to the fused and optimized state vector of the vehicle at the previous moment;

[0100] The fusion module 30 is used to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain a fused and optimized state vector of the vehicle at the current moment.

[0101] Furthermore, in one embodiment, the acquisition module 10 is used to:

[0102] Obtain the current observation state vector of the vehicle according to the global satellite navigation system GNSS;

[0103] Alternatively, the observation state vector of the vehicle at the current moment is obtained based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map.

[0104] Furthermore, in one embodiment, the acquisition module 10 is further used to:

[0105] Determine the observation coordinates of the vehicle at the current moment based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map;

[0106] After determining the observation coordinates of the vehicle at the current moment, obtain 5 points before and after the observation coordinates of the vehicle on the left and right lanes and the center line of the lane where the vehicle is located. Among them, the lane line position of the vehicle is the lane line position corresponding to the lane line information of the high-precision map;

[0107] Perform 5th-order polynomial fitting on the points on the left and right lane lines and the center line, and get 3 polynomials;

[0108] Calculate three lane line heading angles according to three polynomials respectively, and calculate the average of the three lane line heading angles, which is the heading angle of the lane line on the high-precision map;

[0109] The relative angle between the vehicle and the visual lane line is obtained through the visual controller;

[0110] Calculate the sum of the heading angle of the lane line on the high-precision map and the relative angle between the vehicle and the visual lane line to obtain the observed heading angle of the vehicle at the current moment;

[0111] The observed speed of the vehicle at the current moment is determined according to the fused and optimized speed of the vehicle at the previous moment.

[0112] Furthermore, in one embodiment, the prediction module 20 is used to:

[0113] The predicted state vector of the vehicle at the current moment is obtained by the first recursive formula of the extended Kalman filter algorithm according to the fused optimized coordinates, heading angle and / or speed of the vehicle at the previous moment. The first recursive formula is as follows:

[0114]

[0115] Among them, v t is the optimized speed of the vehicle at the last moment, (x t ,y t ) is the fusion optimized coordinate of the vehicle at the last moment, where x t is the x-coordinate after fusion optimization of the vehicle at the last moment, yt The heading is the optimized coordinate of the y direction of the vehicle at the last moment. t is the heading angle after the fusion optimization of the ego vehicle, d t is the time interval between the current moment and the previous moment, L is the wheelbase of the vehicle, steerangle is the steering wheel angle of the vehicle, v t+1 is the predicted speed of the vehicle at the current moment, heading t+1 1 is the predicted heading angle of the vehicle at the current moment, (x t+1 ,y t+1 ) is the predicted coordinate of the vehicle at the current moment, where x t+1 is the predicted x-coordinate of the vehicle at the current moment, y t+1 It is the predicted y-direction coordinate of the vehicle at the current moment.

[0116] Furthermore, in one embodiment, the fusion module 30 is used to:

[0117] At the last moment, the second recursive formula P of the extended Kalman filter algorithm is t+1 =J*P t *J T +Q recursively calculates the covariance matrix at the current moment, where P t+1 is the covariance matrix at the current moment, J is the Jacobian matrix, P t is the covariance matrix of the previous moment, T is the transposed matrix, and Q is the process noise;

[0118] Substitute the covariance matrix at the current moment into the first preset formula to calculate the extended Kalman gain. The first preset formula is as follows: K = P t+1 *H T (H*P t+1 *H T +R) -1 , where R is the observation noise, H is the observation matrix, and K is the extended Kalman gain;

[0119] Substitute the extended Kalman gain, the observed state vector of the vehicle at the current moment, and the predicted state vector of the vehicle at the current moment into the second preset formula to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The second preset formula is as follows: M3=M1+K(M2-H*M1), where M1 is the predicted state vector of the vehicle at the current moment, M2 is the observed state vector of the vehicle at the current moment, and M3 is the fused and optimized state vector of the vehicle at the current moment.

[0120] Furthermore, in one embodiment, the high-precision positioning device further includes an updating module, which is used to:

[0121] Substitute the extended Kalman gain and the covariance matrix at the current moment into the third preset formula The covariance matrix at the current moment is updated, where: is the updated covariance matrix at the current moment, P t+1 is the covariance matrix at the current moment, I is the diagonal matrix, K is the extended Kalman gain, and H is the observation matrix.

[0122] Among them, the functional implementation of each module in the above-mentioned high-precision positioning device corresponds to the various steps in the above-mentioned high-precision positioning method embodiment, and its functions and implementation processes will not be repeated here one by one.

[0123] In a fourth aspect, an embodiment of the present invention further provides a readable storage medium.

[0124] The readable storage medium of the present invention stores a high-precision positioning program, wherein when the high-precision positioning program is executed by a processor, the steps of the high-precision positioning method as described above are implemented.

[0125] Among them, the method implemented when the high-precision positioning program is executed can refer to the various embodiments of the high-precision positioning method of the present invention, and will not be repeated here.

[0126] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or system including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or system. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or system including the element.

[0127] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0128] Through the description of the above implementation methods, those skilled in the art can clearly understand that the above-mentioned embodiment methods can be implemented by means of software plus a necessary general hardware platform, and of course by hardware, but in many cases the former is a better implementation method. Based on such an understanding, the technical solution of the present invention is essentially or the part that contributes to the prior art can be embodied in the form of a software product, which is stored in a storage medium (such as ROM / RAM, magnetic disk, optical disk) as described above, and includes a number of instructions for a terminal device to execute the methods described in each embodiment of the present invention.

[0129] The above are only preferred embodiments of the present invention, and are not intended to limit the patent scope of the present invention. Any equivalent structure or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A high-precision positioning method, It is characterized in that The high-precision positioning method comprises: Obtaining the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle; The predicted state vector of the vehicle at the current moment is obtained according to the fused and optimized state vector of the vehicle at the previous moment; The observed state vector of the vehicle at the current moment is fused with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment; The step of obtaining the predicted state vector of the vehicle at the current moment according to the fused and optimized state vector of the vehicle at the previous moment comprises: The predicted state vector of the vehicle at the current moment is obtained by the first recursive formula of the extended Kalman filter algorithm according to the fused optimized coordinates, heading angle and / or speed of the vehicle at the previous moment. The first recursive formula is as follows: in, is the optimized fusion speed of the vehicle at the last moment, ( , ) is the fusion optimized coordinate of the vehicle at the last moment, where For the last moment of your car x The coordinates after direction fusion optimization, For the last moment of your car y The coordinates after direction fusion optimization, It is the heading angle after the fusion optimization of the vehicle. is the interval between the current moment and the previous moment, is the wheelbase of the vehicle, is the steering wheel angle of the vehicle, is the predicted speed of the vehicle at the current moment, is the predicted heading angle of the vehicle at the current moment, ( , ) is the predicted coordinate of the vehicle at the current moment, where is the current time of the vehicle x Direction prediction coordinates, is the current time of the vehicle y Direction prediction coordinates; The step of fusing the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment includes: At the last moment, the second recursive formula of the extended Kalman filter algorithm is Recursively derive the covariance matrix at the current moment, where is the covariance matrix at the current moment, J is the Jacobian matrix, is the covariance matrix of the previous moment, T is the transposed matrix, and Q is the process noise; Substitute the covariance matrix at the current moment into the first preset formula to calculate the extended Kalman gain. The first preset formula is as follows: , where R is the observation noise, H is the observation matrix, and K is the extended Kalman gain; Substitute the extended Kalman gain, the observed state vector of the vehicle at the current moment, and the predicted state vector of the vehicle at the current moment into the second preset formula to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The second preset formula is as follows: ,in, is the predicted state vector of the vehicle at the current moment, is the observed state vector of the vehicle at the current moment, It is the fusion optimized state vector of the vehicle at the current moment.

2. The high-precision positioning method according to claim 1, It is characterized in that The obtaining of the observed state vector of the vehicle at the current moment includes: Obtain the current observation state vector of the vehicle according to the global satellite navigation system GNSS; Alternatively, the observation state vector of the vehicle at the current moment is obtained based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map.

3. The high-precision positioning method according to claim 2, It is characterized in that The method of obtaining the observation state vector of the vehicle at the current moment according to the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map includes: Determine the observation coordinates of the vehicle at the current moment based on the visual recognition lane line information of the vehicle at the current moment and the lane line information of the high-precision map; After determining the observation coordinates of the vehicle at the current moment, obtain 5 points before and after the vehicle positioning coordinates on the left and right lanes and the center line of the lane where the vehicle is located. Among them, the lane line position of the vehicle is the lane line position corresponding to the lane line information of the high-precision map; Perform 5th-order polynomial fitting on the points on the left and right lane lines and the center line, and get 3 polynomials; Calculate three lane line heading angles according to three polynomials respectively, and calculate the average of the three lane line heading angles, which is the heading angle of the lane line on the high-precision map; The relative angle between the vehicle and the visual lane line is obtained through the visual controller; Calculate the sum of the heading angle of the lane line on the high-precision map and the relative angle between the vehicle and the visual lane line to obtain the observed heading angle of the vehicle at the current moment; The observed speed of the vehicle at the current moment is determined according to the fused and optimized speed of the vehicle at the previous moment.

4. The high-precision positioning method according to claim 1, It is characterized in that After the step of fusing the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment, the method further includes: Substitute the extended Kalman gain and the covariance matrix at the current moment into the third preset formula The covariance matrix at the current moment is updated, where: is the updated covariance matrix at the current moment, is the covariance matrix at the current moment, I is the diagonal matrix, K is the extended Kalman gain, and H is the observation matrix.

5. A high-precision positioning device, It is characterized in that The high-precision positioning device comprises: An acquisition module, used to acquire the observed state vector of the vehicle at the current moment and the fused and optimized state vector of the vehicle at the previous moment, wherein the state vector includes the coordinates, heading angle and / or speed of the vehicle; A prediction module is used to obtain the predicted state vector of the vehicle at the current moment according to the fused and optimized state vector of the vehicle at the previous moment; A fusion module is used to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain a fused and optimized state vector of the vehicle at the current moment; The prediction module is used to obtain the predicted state vector of the vehicle at the current moment through the first recursive formula of the extended Kalman filter algorithm according to the fused and optimized coordinates, heading angle and / or speed of the vehicle at the previous moment. The first recursive formula is as follows: in, is the optimized fusion speed of the vehicle at the last moment, ( , ) is the fusion optimized coordinate of the vehicle at the last moment, where For the last moment of your car x The coordinates after direction fusion optimization, For the last moment of your car y The coordinates after direction fusion optimization, It is the heading angle after the fusion optimization of the vehicle. is the interval between the current moment and the previous moment, is the wheelbase of the vehicle, is the steering wheel angle of the vehicle, is the predicted speed of the vehicle at the current moment, is the predicted heading angle of the vehicle at the current moment, ( , ) is the predicted coordinate of the vehicle at the current moment, where is the current time of the vehicle x Direction prediction coordinates, is the current time of the vehicle y Direction prediction coordinates; The fusion module is used to use the second recursive formula of the extended Kalman filter algorithm at the previous moment. Recursively derive the covariance matrix at the current moment, where is the covariance matrix at the current moment, J is the Jacobian matrix, is the covariance matrix of the previous moment, T is the transposed matrix, and Q is the process noise; Substitute the covariance matrix at the current moment into the first preset formula to calculate the extended Kalman gain. The first preset formula is as follows: , where R is the observation noise, H is the observation matrix, and K is the extended Kalman gain; Substitute the extended Kalman gain, the observed state vector of the vehicle at the current moment, and the predicted state vector of the vehicle at the current moment into the second preset formula to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The second preset formula is as follows: ,in, is the predicted state vector of the vehicle at the current moment, is the observed state vector of the vehicle at the current moment, It is the fusion optimized state vector of the vehicle at the current moment.

6. The high-precision positioning device according to claim 5, It is characterized in that The fusion module is used to: At the last moment, the second recursive formula of the extended Kalman filter algorithm is Recursively derive the covariance matrix at the current moment, where is the covariance matrix at the current moment, J is the Jacobian matrix, is the covariance matrix of the previous moment, T is the transposed matrix, and Q is the process noise; Substitute the covariance matrix at the current moment into the first preset formula to calculate the extended Kalman gain. The first preset formula is as follows: , where R is the observation noise, H is the observation matrix, and K is the extended Kalman gain; Substitute the extended Kalman gain, the observed state vector of the vehicle at the current moment, and the predicted state vector of the vehicle at the current moment into the second preset formula to fuse the observed state vector of the vehicle at the current moment with the predicted state vector of the vehicle at the current moment to obtain the fused and optimized state vector of the vehicle at the current moment. The second preset formula is as follows: ,in, is the predicted state vector of the vehicle at the current moment, is the observed state vector of the vehicle at the current moment, It is the fusion optimized state vector of the vehicle at the current moment.

7. A high-precision positioning device, It is characterized in that The high-precision positioning device includes a processor, a memory, and a high-precision positioning program stored in the memory and executable by the processor, wherein when the high-precision positioning program is executed by the processor, the steps of the high-precision positioning method as described in any one of claims 1 to 4 are implemented.

8. A readable storage medium, It is characterized in that The readable storage medium stores a high-precision positioning program, wherein when the high-precision positioning program is executed by a processor, the steps of the high-precision positioning method according to any one of claims 1 to 4 are implemented.