Lane line auxiliary positioning method, device, electronic device and storage medium

Abstract

The present invention provides a lane line auxiliary positioning method, device, electronic device and storage medium, the method comprising: estimating the coordinate position of the IMU during vehicle movement based on the GNSS / INS combined navigation system; calculating the position of the lane line detection point in the camera system by the lever arm compensation method according to the IMU coordinate position; calculating the projection point position of the lane line detection point on the corresponding lane line by map matching based on the high-precision map; calculating the relative distance between the lane line detection point and the projection point on the lane line, and using the relative distance as the matched lane line distance; using the difference between the lane line distance identified by the camera and the matched lane line distance as observation information, and combining the GNSS / INS fusion positioning method to construct a lane line auxiliary positioning model. Thereby, the GNSS / INS combined positioning accuracy can be improved, reliable auxiliary positioning can be provided for the GNSS / INS combined positioning system, and the application requirements of complex scenes can be met.

Description

Technical Field

[0001] The present invention belongs to the field of auxiliary positioning technology, and in particular relates to a lane line auxiliary positioning method, device, electronic equipment and storage medium. Background Art

[0002] GNSS / INS integrated navigation algorithms are often used in vehicle positioning systems, but in areas where GNSS signals are blocked, the positioning accuracy of the integrated navigation system cannot be guaranteed. In order to solve the positioning error caused by occlusion and the system defect of error divergence over time, it is usually effective to use odometer, NHC (non-holonomic constraint) and other information to assist GNSS / INS combined positioning, but its positioning accuracy is still difficult to improve significantly. Summary of the invention

[0003] In view of this, an embodiment of the present invention provides a lane line auxiliary positioning method, device, electronic device and storage medium, which are used to solve the problem of low positioning accuracy of existing auxiliary positioning methods when the GNSS / INS combined navigation signal is blocked.

[0004] In a first aspect of an embodiment of the present invention, a lane line auxiliary positioning method is provided, comprising:

[0005] Estimate the coordinate position of the IMU during vehicle motion based on the GNSS / INS integrated navigation system;

[0006] The position of the lane detection point in the camera system is calculated using the lever arm compensation method based on the IMU coordinate position;

[0007] Based on the high-precision map, the projection point position of the lane line detection point on the corresponding lane line is calculated through map matching;

[0008] Calculate the relative distance between the lane line detection point and the projection point on the lane line, and use the relative distance as the matched lane line distance;

[0009] The difference between the lane line distance recognized by the camera and the matched lane line distance is used as observation information, and the lane line auxiliary positioning model is constructed in combination with the GNSS / INS fusion positioning method.

[0010] In a second aspect of an embodiment of the present invention, a lane line auxiliary positioning device is provided, comprising:

[0011] IMU position estimation module, used to estimate the coordinate position of the IMU during vehicle motion based on the GNSS / INS integrated navigation system;

[0012] The detection point position calculation module is used to calculate the lane line detection point position in the camera system through the lever arm compensation method according to the IMU coordinate position;

[0013] The projection point position calculation module is used to calculate the projection point position of the lane line detection point on the corresponding lane line through map matching based on the high-precision map;

[0014] A relative distance calculation module is used to calculate the relative distance between the lane line detection point and the projection point on the lane line, and use the relative distance as the matched lane line distance;

[0015] The positioning model building module is used to take the difference between the lane line distance recognized by the camera and the matched lane line distance as observation information, and combine it with the GNSS / INS fusion positioning method to build a lane line auxiliary positioning model.

[0016] In a third aspect of an embodiment of the present invention, an electronic device is provided, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the method described in the first aspect of the embodiment of the present invention when executing the computer program.

[0017] In a fourth aspect of an embodiment of the present invention, a computer-readable storage medium is provided, wherein the computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method provided in the first aspect of the embodiment of the present invention are implemented.

[0018] According to a fifth aspect of an embodiment of the present invention, a computer program product is provided. The computer program product includes a computer program. When the computer program is executed by one or more processors, the steps of the method provided in the first aspect of the embodiment of the present invention are implemented.

[0019] In an embodiment of the present invention, by calculating the relative distance between the lane line detection point and the projection point, the difference between the lane line distance recognized by the camera and the matched lane line distance is combined with the GNSS / INS fusion positioning method to construct a lane line auxiliary positioning model, thereby providing auxiliary positioning for the GNSS / INS combined positioning system based on the lane line, and improving the positioning accuracy of the GNSS / INS combined positioning system in complex scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0020] In order to more clearly illustrate the technical solutions in the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without creative work.

[0021] Figure 1 A schematic diagram of a process flow of a lane line auxiliary positioning method provided by an embodiment of the present invention;

[0022] Figure 2A schematic diagram of a hierarchical fusion positioning structure provided by an embodiment of the present invention;

[0023] Figure 3 A schematic structural diagram of a lane line auxiliary positioning device provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0024] In order to make the purpose, features and advantages of the present invention more obvious and easy to understand, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described below are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0025] It should be understood that the term "including" and other similar expressions in the specification or claims of the present invention and the above-mentioned drawings are intended to cover non-exclusive inclusions, such as a process, method, system, or device including a series of steps or units is not limited to the listed steps or units. In addition, "first" and "second" are used to distinguish different objects, not to describe a specific order.

[0026] See also Figure 1 , a schematic flow chart of a lane line auxiliary positioning method provided by an embodiment of the present invention includes:

[0027] S101, estimating the coordinate position of the IMU during vehicle motion based on the GNSS / INS integrated navigation system;

[0028] The GNSS / INS integrated navigation system uses an extended Kalman filter to fuse the GNSS positioning information and the inertial navigation system. The Kalman filter state vector to be estimated is:

[0029] x=[δr INS δv INS φ b g b a s g s a ] T ;

[0030] Among them, δr INS ,δv INS ,φ,b g 、b a 、s g 、s aThey represent the position error, velocity error, attitude error and residual errors of inertial navigation devices such as gyro bias, table bias, gyro scale factor, table scale factor, etc. in the inertial navigation system calculation process. At the same time, according to the inertial navigation error differential equation, the extended Kalman filter system equation can be derived to determine the state transfer matrix Φ k,k-1 and the system noise matrix G k-1 , we can get: x k =Φ k,k-1 x k-1 +G k-1 w k-1 .

[0031] It can be understood that IMU (Inertial measurement unit) is a device for measuring the three-axis attitude angle and acceleration of an object in the GNSS / INS integrated navigation system. The coordinate position of the IMU center point can be estimated based on the integrated navigation algorithm.

[0032] S102, calculating the position of the lane line detection point in the camera system by using the lever arm compensation method according to the IMU coordinate position;

[0033] Combined with the distance vector (i.e., the lever arm) from the center point of the IMU to the lane detection point in the camera, the lever arm compensation method is used to calculate the position information of the lane detection point under the camera system.

[0034] Specifically, the position of the lane line detection point in the camera system is calculated according to formula (1):

[0035]

[0036] in, is the position of the lane line detection point, is the center point of IMU, represents the unit transformation matrix, represents the direction cosine matrix of b system relative to n system, Indicates the front right lower direction lever arm value from the IMU point to the camera detection point, R M Represents the meridian curvature radius, R N represents the radius of curvature of the y-axis circle, h represents the elevation, Indicates the local latitude in radians.

[0037] It should be noted that, in the embodiment of the present invention, the inertial coordinate system is set as the i system, the earth coordinate system is set as the e system, the "north-east-ground" navigation coordinate system is set as the n system, the "front-right-down" carrier coordinate system is set as the v system, and the IMU coordinate system is set as the b system.

[0038] S103, based on the high-precision map, calculating the projection point position of the lane line detection point on the corresponding lane line through map matching;

[0039] The high-precision map is a pre-collected high-precision map, which contains road network data such as numerous lanes and lane lines. By matching the camera detection points on the map, the projection point and its corresponding coordinate position can be determined. The map matching refers to matching the sampling sequence of the driving trajectory with the digital map road network.

[0040] Among them, let the matched lane line distance vector be for

[0041] X, Y, and Z represent the estimated distance in the lower right direction of the vehicle coordinate system. For the lane line, only the y value in the right direction is concerned, that is, y=PN2.

[0042] Then the lane line distance vector in the vehicle coordinate system is expressed as:

[0043]

[0044] In the formula, Represents the distance vector from the lane line detection point to the projection point, is the position of the lane line detection point, is the projection point position, represents the unit transformation matrix, Represents the direction cosine matrix of the IMU coordinate system relative to the carrier coordinate system, Represents the direction cosine matrix of the IMU coordinate system relative to the navigation coordinate system.

[0045] S104, calculating the relative distance between the lane line detection point and the projection point on the lane line, and using the relative distance as the matched lane line distance;

[0046] Preferably, an error disturbance analysis is performed on the matched lane line distance vector, and the following can be obtained:

[0047]

[0048] Substituting the estimated value, we can get the following formula:

[0049]

[0050] In the formula, is the projection point position, is the center point of IMU, D R represents the unit transformation matrix, is an orthogonal matrix, represents the direction cosine matrix of b system relative to v system, represents the direction cosine matrix of system n relative to system b, It represents the front right lower direction lever arm value from the IMU point to the camera detection point, φ represents the equivalent rotation vector of the attitude, and δ represents the gyro residual scale coefficient error.

[0051] The estimated amount, namely the error estimated amount, includes small errors such as gyro observation error and gyro device error caused by the rotation of the earth.

[0052] S105. Using the difference between the lane line distance recognized by the camera and the matched lane line distance as observation information, and combining the GNSS / INS fusion positioning method to construct a lane line auxiliary positioning model.

[0053] If the lane line distance recognized by the camera is PN, the recognized lane line distance vector is:

[0054]

[0055] In the formula, represents the theoretical true value of the lane line distance, e c Indicates the lane line error detected by the camera.

[0056] The extended Kalman filter observation equation for lane line distance assisted positioning can be written as:

[0057]

[0058] Substitute the expressions for the matching lane line distance and the observed lane line distance value and expand them to get

[0059]

[0060] According to the observation equation, the observation matrix can be obtained:

[0061]

[0062] In the formula, Represents the matched lane line distance vector, represents the observed lane line distance vector, z represents the new observation value, Represents the direction cosine matrix of the IMU coordinate system relative to the carrier coordinate system, Represents the direction cosine matrix of the navigation coordinate system relative to the IMU coordinate system, D R represents the unit transformation matrix, is the center point of IMU, is the projection point position.

[0063] According to the extended Kalman filter system equation and observation equation, the Kalman filter solution can be performed in combination with the GNSS / INS integrated navigation system, which is the lane line assisted GNSS / INS positioning model. Based on the assisted positioning model, the lane lines can be collected through the camera and combined with the high-precision map to achieve assisted positioning of the vehicle.

[0064] It is understood that, in one embodiment, Figure 2 As shown in the figure, in general multi-sensor fusion positioning, the positioning information provided by the traditional GNSS, the attitude information provided by the IMU, and the speed information provided by the odometer are combined with the Kalman filter for combined positioning, that is, the inertial navigation is used as the main working module. When the lane line distance difference information is observable, the lane line observation information can be used to assist the inertial navigation positioning. Specifically, the estimated distance from the vehicle to the lane line obtained based on map matching and the distance from the vehicle to the lane line observed based on camera recognition can be used to obtain the distance difference between the vehicle and the lane line, and then combined with the Kalman filter, the position estimation error caused by the inertial navigation mechanical arrangement algorithm is constrained to achieve lane line-based auxiliary positioning. The multi-sensor information does not interfere with each other, and it can be used immediately. This hierarchical fusion positioning structure can fully guarantee the implementation and performance inspection of the lane line auxiliary positioning algorithm.

[0065] In this embodiment, the auxiliary positioning block based on the lane line can improve the positioning accuracy of the GNSS / INS combined positioning, and can still maintain a high positioning accuracy when the GNSS signal is blocked.

[0066] It should be understood that the serial numbers of the steps in the above embodiments do not imply a sequence of execution, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiments of the present invention.

[0067] Figure 3 A schematic diagram of the structure of a lane line auxiliary positioning device provided by an embodiment of the present invention, the device includes:

[0068] An IMU position estimation module 310 is used to estimate the coordinate position of the IMU during vehicle motion based on a GNSS / INS integrated navigation system;

[0069] The detection point position calculation module 320 is used to calculate the lane line detection point position in the camera system through the lever arm compensation method according to the IMU coordinate position;

[0070] The method of calculating the position of the lane detection point in the camera system by the lever arm compensation method according to the IMU coordinate position is specifically as follows:

[0071] The position of the lane line detection point in the camera system is calculated according to formula (1):

[0072]

[0073] in, is the position of the lane line detection point, is the center point of IMU, represents the unit transformation matrix, Represents the direction cosine matrix of the IMU coordinate system relative to the navigation coordinate system, Indicates the front right lower direction lever arm value from the IMU point to the camera detection point, R M Represents the meridian curvature radius, R N represents the radius of curvature of the y-axis circle, h represents the elevation, Indicates the local latitude.

[0074] The projection point position calculation module 330 is used to calculate the projection point position of the lane line detection point on the corresponding lane line through map matching based on the high-precision map;

[0075] A relative distance calculation module 340 is used to calculate the relative distance between the lane line detection point and the projection point on the lane line, and use the relative distance as the matched lane line distance;

[0076] The step of calculating the relative distance between the lane line detection point and the projection point on the lane line includes:

[0077] Assume the lane line distance vector is Then the lane line distance vector is expressed as:

[0078]

[0079] In the formula, Represents the distance vector from the lane line detection point to the projection point, is the position of the lane line detection point, is the projection point position, D R represents the unit transformation matrix, Represents the direction cosine matrix of the IMU coordinate system relative to the carrier coordinate system, Represents the direction cosine matrix of the navigation coordinate system relative to the IMU coordinate system.

[0080] The positioning model building module 350 is used to use the difference between the lane line distance recognized by the camera and the matched lane line distance as observation information, and to build a lane line auxiliary positioning model in combination with the GNSS / INS fusion positioning method.

[0081] By performing error perturbation analysis on the matched lane line distance vector, we can obtain:

[0082]

[0083] Substitute the estimated amount and expand the formula to get

[0084]

[0085] In the formula, is the projection point position, is the position of point O, D R represents the unit transformation matrix, represents the direction cosine matrix of b system relative to v system, represents the direction cosine matrix of system n relative to system b, Indicates the front right lower direction lever arm value from the IMU point to the camera detection point.

[0086] Furthermore, the observed lane line distance vector is expressed as:

[0087]

[0088] Then the extended Kalman filter observation equation for lane line distance assisted positioning is:

[0089]

[0090] Substitute the matched lane line distance value and the observed lane line distance value into the observation equation:

[0091]

[0092] According to the observation equation, the observation matrix H can be obtained as:

[0093]

[0094] In the formula, represents the theoretical true value of the lane line distance, e c represents the lane line error detected by the camera, z represents the new observation value, Represents the matched lane line distance vector, represents the observed lane line distance vector, Represents the direction cosine matrix of the IMU coordinate system relative to the carrier coordinate system, Represents the direction cosine matrix of the navigation coordinate system relative to the IMU coordinate system, D R represents the unit transformation matrix, is the center point of IMU, is the projection point position.

[0095] Those skilled in the art can clearly understand that for the convenience and brevity of description, the specific working processes of the above-described devices and modules can refer to the corresponding processes in the aforementioned system embodiments / method embodiments, and will not be repeated here.

[0096] It can be understood that, in one embodiment, the electronic device includes a memory, a processor, and a computer program stored in the memory and executable on the processor, and the computer program executes steps S101 to S105 in Example 1, and the processor implements auxiliary positioning of the lane line when executing the computer program.

[0097] A person skilled in the art can understand that all or part of the steps in the above-mentioned embodiment method can be completed by instructing related hardware through a program. The program can be stored in a computer-readable storage medium. When the program is executed, it includes steps S101 to S105. The storage medium includes, for example, ROM / RAM, a disk, an optical disk, etc.

[0098] In the above embodiments, the description of each embodiment has its own emphasis. For parts that are not described or recorded in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0099] As described above, the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A lane line auxiliary positioning method, characterized in that: Estimate the coordinate position of the IMU during vehicle motion based on the GNSS / INS integrated navigation system; The position of the lane detection point in the camera system is calculated using the lever arm compensation method based on the IMU coordinate position; Based on the high-precision map, the projection point position of the lane line detection point on the corresponding lane line is calculated through map matching; Calculate the relative distance between the lane line detection point and the projection point on the lane line, and use the relative distance as the matched lane line distance; The difference between the lane line distance recognized by the camera and the matched lane line distance is used as observation information, and the lane line auxiliary positioning model is constructed in combination with the GNSS / INS fusion positioning method.

2. The method according to claim 1, characterized in that The position of the lane line detection point in the camera system is calculated by the lever arm compensation method according to the IMU coordinate position as follows: The position of the lane line detection point in the camera system is calculated according to formula (1): in, is the position of the lane line detection point, is the center point of IMU, represents the unit transformation matrix, Represents the direction cosine matrix of the IMU coordinate system relative to the navigation coordinate system, Indicates the front right lower direction lever arm value from the IMU point to the camera detection point, R M Represents the meridian curvature radius, R N represents the radius of curvature of the y-axis circle, h represents the elevation, Indicates the local latitude.

3. The method according to claim 1, characterized in that The calculation of the relative distance between the lane line detection point and the projection point on the lane line includes: Assume the lane line distance vector is Then the lane line distance vector is expressed as: In the formula, Represents the distance vector from the lane line detection point to the projection point, is the position of the lane line detection point, is the projection point position, D R represents the unit transformation matrix, Represents the direction cosine matrix of the IMU coordinate system relative to the carrier coordinate system, Represents the direction cosine matrix of the navigation coordinate system relative to the IMU coordinate system.

4. The method according to claim 1, characterized in that: The difference between the lane line distance recognized by the camera and the matched lane line distance is used as observation information, and the lane line auxiliary positioning model is constructed in combination with the GNSS / INS fusion positioning method, including: The observed lane line distance vector is expressed as: Then the extended Kalman filter observation equation for lane line distance assisted positioning is: Substitute the matched lane line distance value and the observed lane line distance value into the observation equation. According to the observation equation, the observation matrix H can be obtained as: In the formula, represents the theoretical true value of the lane line distance, e c Indicates the lane line error detected by the camera. Represents the matched lane line distance vector, represents the observed lane line distance vector, z represents the new observation value, Represents the direction cosine matrix of the IMU coordinate system relative to the carrier coordinate system, Represents the direction cosine matrix of the navigation coordinate system relative to the IMU coordinate system, D R represents the unit transformation matrix, is the center point of IMU, is the projection point position.

5. A lane line auxiliary positioning device, characterized in that: include: IMU position estimation module, used to estimate the coordinate position of the IMU during vehicle motion based on the GNSS / INS integrated navigation system; The detection point position calculation module is used to calculate the lane line detection point position in the camera system through the lever arm compensation method according to the IMU coordinate position; The projection point position calculation module is used to calculate the projection point position of the lane line detection point on the corresponding lane line through map matching based on the high-precision map; A relative distance calculation module is used to calculate the relative distance between the lane line detection point and the projection point on the lane line, and use the relative distance as the matched lane line distance; The positioning model building module is used to take the difference between the lane line distance recognized by the camera and the matched lane line distance as observation information, and combine it with the GNSS / INS fusion positioning method to build a lane line auxiliary positioning model.

6. The device according to claim 5, characterized in that The position of the lane line detection point in the camera system is calculated by the lever arm compensation method according to the IMU coordinate position as follows: The position of the lane line detection point in the camera system is calculated according to formula (1): in, is the position of the lane line detection point, is the center point of IMU, represents the unit transformation matrix, Represents the direction cosine matrix of the IMU coordinate system relative to the navigation coordinate system, Indicates the front right lower direction lever arm value from the IMU point to the camera detection point, R M Represents the meridian curvature radius, R N represents the radius of curvature of the y-axis circle, h represents the elevation, Indicates latitude.

7. The device according to claim 5, characterized in that The calculation of the relative distance between the lane line detection point and the projection point on the lane line includes: Assume the lane line distance vector is Then the lane line distance vector is expressed as: In the formula, Represents the distance vector from the lane line detection point to the projection point, is the position of the lane line detection point, is the projection point position, D R represents the unit transformation matrix, Represents the direction cosine matrix of the IMU coordinate system relative to the carrier coordinate system, Represents the direction cosine matrix of the navigation coordinate system relative to the IMU coordinate system.

8. The device according to claim 5, characterized in that The difference between the lane line distance recognized by the camera and the matched lane line distance is used as observation information, and the lane line auxiliary positioning model is constructed in combination with the GNSS / INS fusion positioning method, including: The observed lane line distance vector is expressed as: Then the extended Kalman filter observation equation for lane line distance assisted positioning is: Substitute the matched lane line distance value and the observed lane line distance value into the observation equation. According to the observation equation, the observation matrix H can be obtained as: In the formula, represents the theoretical true value of the lane line distance, e c Indicates the lane line error detected by the camera. Represents the matched lane line distance vector, represents the observed lane line distance vector, z represents the new observation value, Represents the direction cosine matrix of the IMU coordinate system relative to the carrier coordinate system, Represents the direction cosine matrix of the navigation coordinate system relative to the IMU coordinate system, D R represents the unit transformation matrix, is the center point of IMU, is the projection point position.

9. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the computer program, the steps of a lane line auxiliary positioning method as described in any one of claims 1 to 4 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed, the steps of a lane line auxiliary positioning method as described in any one of claims 1 to 4 are implemented.

Images