Auxiliary positioning method based on lane line and template matching VO, storage medium and equipment

Abstract

The invention discloses an auxiliary positioning method based on a lane line and template matching VO, a storage medium and equipment, and relates to the technical field of autonomous unmanned systems, and the method comprises the following steps: S1, template matching auxiliary positioning, S2, lane line offset distance calculation, and S3, lane line auxiliary positioning. According to the invention, by fusing the visual information and the inertial information, stable decimeter-level positioning precision can be provided in a GNSS denial scene. A direct method visual odometer is combined with lane line detection, so that accumulation of IMU integral errors can be effectively inhibited, and the positioning stability is improved. And the direct method visual odometer does not depend on feature point extraction, can adapt to low texture features and dynamic scenes of roads, and has better robustness. And a point-to-point map matching strategy is adopted, so that the unmanned vehicle on the ground can be quickly positioned in a scene with a relatively high real-time requirement.

Description

Technical Field

[0001] The present invention relates to the technical field of autonomous unmanned systems, and in particular to an auxiliary positioning method based on lane lines and template matching (VO). Background Art

[0002] As an important branch of autonomous unmanned systems, ground unmanned vehicles (UGVs) are dedicated to giving robots humanoid intelligent control and have enormous research value. Ground unmanned vehicles are comprehensive systems that integrate multiple fields, including but not limited to environmental perception, high-precision positioning, decision-making and planning, and motion control. Perception and positioning are inseparable and are the key foundation for achieving autonomous navigation of ground unmanned vehicles. Currently, ground unmanned vehicles have been widely used in various fields. High-precision positioning technology requires sub-meter or centimeter-level positioning accuracy without human intervention, thereby ensuring that ground unmanned vehicles can complete specific tasks in complex environments. In addition, in order to achieve long-term stable and safe operation of ground unmanned vehicles, their positioning services need to have strong anti-interference and continuity. Ground unmanned vehicles need to always know their exact position.

[0003] For ground unmanned vehicles to operate stably and safely for extended periods, their positioning services must be both interference-resistant and continuous. As one of the most promising outdoor positioning solutions for ground unmanned vehicles, GNSS / INS combined positioning technology has been widely used in military, industrial, agricultural, and other fields.

[0004] Traditional GNSS / IMU combined positioning methods in existing technologies perform well when the GNSS signal is strong. However, when the GNSS signal is interrupted or blocked, IMU integration errors quickly accumulate, significantly reducing positioning accuracy. Although some studies have attempted to use visual odometry (VO) to assist positioning, these methods mostly rely on feature point extraction, have high requirements for road texture, and are prone to mismatching in dynamic scenes, leading to error accumulation. Summary of the Invention

[0005] The purpose of the present invention is to provide an auxiliary positioning method based on lane lines and template matching VO to solve the current market problems raised by the above background technology.

[0006] To achieve the above objectives, the present invention provides the following technical solutions: an assisted positioning method based on lane lines and template matching VO, including: S1 template matching assisted positioning, S2 lane line offset distance calculation and S3 lane line assisted positioning, specifically as follows:

[0007] S1 Template Matching Assisted Positioning: The monocular camera and the ground unmanned vehicle are positioned perpendicular to the bottom surface to capture a sequence of images containing road texture features. An initial template is predefined in the center of the i-th frame image. The rectangular area with the greatest similarity to the initial template is searched and recorded on the captured i+1-th frame image.

[0008] S2 Lane Offset Calculation: A motion camera parallel to the ground is used to capture an image of the road ahead of the unmanned vehicle, including the lane lines. Inverse perspective mapping is used to obtain a bird's-eye view of the road, thereby constructing a mathematical model of the left and right lane lines and calculating the lane offset distance.

[0009] S3 lane marking assisted positioning, including:

[0010] S31 Lane Map Matching Modeling: Using lane map key points as matching targets, the projected coordinates of the ground unmanned vehicle in the lane map are obtained using an iterative closest point method. The lateral offset distance between the ground unmanned vehicle and the lane line is calculated based on the vehicle's position.

[0011] S32 measurement model: Based on the S31 model, the lane line lateral offset distance vector for map matching is specified as:

[0012]

[0013] Where x and z represent the forward and vertical components of the lateral offset distance of the map matching.

[0014] Preferably, the width and height of the initial template for the S1 template matching assisted positioning are Tw and Th respectively.

[0015] Preferably, the pixel displacement increments Δu and Δv in the time interval from the i-th frame to the i+1-th frame are calculated based on the initial template T and the searched rectangular area S:

[0016] Δu=u0-u1

[0017] Δv=v0-v1

[0018] Among them, (u0, v0) is the coordinate of the upper left corner of the initial template T in the image coordinate system, (u1, v1) corresponds to the coordinate of the upper left corner of the matching rectangular area S in the image coordinate system; the camera installation height h and the focal length of the camera in the x and y directions f x and f y It is known that according to the relationship between the image coordinate system and the camera coordinate system, the corresponding actual displacement increment Δx is calculated c and Δy c :

[0019]

[0020] Considering the installation structure of the camera and the ground unmanned vehicle, the camera coordinate system is rotated 180° around the y-axis to coincide with the image coordinate system;

[0021] The corresponding ground unmanned vehicle displacement increment Δx v and Δy v for:

[0022] Δx v =-Δx c

[0023] Δy v =Δy c

[0024] If the position of the ground unmanned vehicle in the i-th frame is known, the yaw angle θ collected by the IMU in the i+1 frame is i+1 , calculate the position of the ground unmanned vehicle in the i+1th frame (x v,i+1 ,y v,i+1 ):

[0025]

[0026] Preferably, the operating condition is that the road is flat, and the movement of the ground unmanned vehicle in the direction perpendicular to the ground is ignored. The position coordinates (x v,i+1 ,y v,i+1 ) can be written as a three-dimensional vector form:

[0027]

[0028] According to the design of the error state Kalman filter, the position obtained by template matching VO is The estimated position of the inertial system with the error state The difference is used as the rough position observation of the filter; the corresponding observation Jacobian matrix is:

[0029] H vo =[I303030303].

[0030] Preferably, the coordinates of any point (u, v) in the camera coordinate system of the image in front of the ground unmanned vehicle containing the lane line taken by the motion camera for solving the lane line offset distance S2 are (x c y c z c ), corresponding to the point (x w y w z w ).

[0031] Preferably, the world coordinate system takes the optical center of the camera as its origin, and the z-axis points vertically upward to the ground; the operating condition is that the ground is flat and the Xc Always located by X w and Y w In the plane formed, there are pitch angles α and yaw angles β; the image coordinate system is projected onto the road plane in the following way:

[0032]

[0033] Where ξ=α+π / 2, the normalized focal length of the camera in the u and v directions is f u and f v and the optical center (c u ,c v ) is determined by camera calibration;

[0034] The key points related to each lane line are fitted by the least squares method to determine the equations of the left and right lane lines; and the distance from the center of the vehicle to the detection line is calculated.

[0035] Preferably, when pedestrians or vehicles temporarily block the lane, causing lane line detection failure and abnormal points, the Z-score method combined with the sliding window is used to remove abnormal values;

[0036] Set a threshold. If the Z value exceeds the threshold, the data point will be considered an outlier, and the distance value at this time will be replaced by the average value within the window at that moment;

[0037] The Z-score value for each data point is calculated as follows:

[0038]

[0039] in, represents the average distance within the window, and σ represents the variance.

[0040] Preferably, the lane line map matching modeling in S31 is specifically as follows:

[0041] The IMU is installed at the center point O of the rear wheel on the top of the ground unmanned vehicle, using the right front upper point as the carrier coordinate system; the camera is installed at point C on the top of the vehicle; let the lateral offset distance detected by the camera system be PN c ;

[0042] Among them, the observation point P(x0,0) is a point directly in front of the camera; considering the installation position of the sensor, the arm vector from the IMU center O to the observation point P is The lateral offset distance PN obtained by map matching m The calculation process is as follows:

[0043] S311: When the GNSS signal is interrupted, the IMU / VO combined positioning system provides a preliminary position prediction value, and then determines the position of the return point P in the ENU coordinate system through arm compensation.

[0044]

[0045] S312: Taking the left lane line as an example, traverse the key points of the left lane line map and calculate the distance between point P and the lane line map point set.

[0046] S313: Take the minimum value of the distance as the lateral offset distance PN obtained by map matching m .

[0047] Preferably, in the S32 measurement model, the one-dimensional offset distance is written as a distance vector; the PN is observed on the y-axis. m It can be expressed as observation point P and projection point N m The distance is as follows:

[0048]

[0049] The lateral offset distance of the lane line calculated by the camera system Also written in vector form:

[0050]

[0051] Then the lane line lateral offset distance observation Z l It is expressed as the difference between the observations of the camera system and the map matching system. The perturbation analysis of the observations yields the following formula:

[0052]

[0053] The Jacobian matrix H corresponding to the lane line lateral offset distance observation l As shown below:

[0054]

[0055] A storage medium readable by an electronic device stores a computer program / instruction thereon, which, when executed by a processor, executes the steps of the auxiliary positioning method based on lane lines and template matching VO as claimed in the claim.

[0056] An electronic device, comprising:

[0057] processor;

[0058] A memory having a computer program stored thereon, wherein the computer program is executed by the processor to perform the steps of the auxiliary positioning method based on lane lines and template matching VO as claimed in claim 1.

[0059] Compared with the prior art, the present invention has the following beneficial effects:

[0060] By fusing visual and inertial information, this invention can provide stable decimeter-level positioning accuracy in GNSS-denied scenarios. The combination of direct visual odometry and lane detection effectively suppresses the accumulation of IMU integration errors, improving positioning stability. Furthermore, direct visual odometry does not rely on feature point extraction, adapting to low-texture road conditions and dynamic scenes, and exhibiting improved robustness. Using a point-to-point map matching strategy, it can rapidly locate unmanned ground vehicles in scenarios with high real-time requirements. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] The accompanying drawings, as part of this disclosure, are intended to provide a further understanding of the disclosure. The exemplary embodiments of the disclosure and their descriptions are intended to explain the disclosure and do not constitute undue limitations thereon. Obviously, the drawings described below are merely examples, and those skilled in the art can derive other drawings based on these drawings without inventive effort.

[0062] Figure 1 Schematic diagram of the template matching process of S1 template matching assisted positioning of the present invention;

[0063] Figure 2 This is a schematic diagram of VO coordinate conversion for template matching in the present invention;

[0064] Figure 3 Schematic diagram of the image, camera, and world coordinate system for calculating the lane offset distance of S2 in the present invention;

[0065] Figure 4 Schematic diagram of the lane map matching model of the present invention;

[0066] Figure 5 This is a flow chart of an auxiliary positioning method based on lane lines and template matching VO according to the present invention;

[0067] Figure 6 This is a schematic diagram of the positioning results of the first group of VO experiments using template matching according to the first embodiment of the present invention;

[0068] Figure 7 This is a schematic diagram of the positioning results of the second group of VO experiments using template matching according to the first embodiment of the present invention;

[0069] Figure 8 This is a schematic diagram of the detection effect of the multi-task model under cloudy conditions in Example 1 of the present invention;

[0070] Figure 9 This is a schematic diagram of the detection effect of the multi-task model under sunny conditions in Example 1 of the present invention;

[0071] Figure 10 This is a schematic diagram of the detection effect of the multi-task model under foggy conditions in Example 1 of the present invention;

[0072] Figure 11 This is a schematic diagram of a static ranging result according to an embodiment of the present invention;

[0073] Figure 12 This is a schematic diagram of the dynamic ranging result of Example 1 of the present invention;

[0074] Figure 13 This is a top view of a set of experimental scenes and their trajectories according to an embodiment of the present invention;

[0075] Figure 14 This is a schematic diagram of GNSS / IMU combined positioning results under different GNSS disappearance times according to Example 1 of the present invention;

[0076] Figure 15 Schematic diagram of experimental results of the visually assisted positioning method according to Example 1 of the present invention. DETAILED DESCRIPTION

[0077] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments 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 making creative efforts are within the scope of protection of the present invention.

[0078] See also Figure 1-Figure 5 , an assisted positioning method based on lane lines and template matching VO, including: S1 template matching assisted positioning, S2 lane line offset distance solution and S3 lane line assisted positioning.

[0079] In specific implementation: Template matching is one of the commonly used methods in direct visual odometry, and its process includes locating a sub-image in a larger image, that is, matching the template T with the search area I. This method uses IMU to provide yaw information instead of the traditional Ackermann steering model. On the one hand, this method overcomes the dependence of the traditional template matching visual odometry on the output of the Ackermann model and expands its usability. On the other hand, the use of additional sensors to provide the yaw angle of the ground unmanned vehicle can suppress the cumulative error generated by the template matching VO during long-term operation to a certain extent. This method uses the improved template matching VO to solve the displacement increment of the ground unmanned vehicle between adjacent image frames. The specific steps are as follows:

[0080] S1 Template matching assisted positioning: The monocular camera and the ground unmanned vehicle are perpendicular to the bottom surface to shoot a sequence of pictures containing road texture features. Taking the i-th frame and the i+1-th frame as an example, the matching process of adjacent image sequences is as follows: Figure 1 As shown in Figure 1, an initial template is predefined in the center area of ​​the i-th frame image.

[0081] As a further explanation: Assume that the width and height of the initial template assisted by S1 template matching are Tw and Th respectively, and search for the rectangular area with the greatest similarity to the initial template on the captured i+1 frame image and record it.

[0082] According to the initial template T and the searched rectangular area S, the pixel displacement increments Δu and Δv in the time interval from the i-th frame to the i+1-th frame are calculated:

[0083] Δu=u0-u1

[0084] Δv=v0-v1

[0085] Among them, (u0, v0) is the coordinate of the upper left corner of the initial template T in the image coordinate system, (u1, v1) corresponds to the coordinate of the upper left corner of the matching rectangular area S in the image coordinate system; the camera installation height h and the focal length of the camera in the x and y directions f x and f y It is known that according to the relationship between the image coordinate system and the camera coordinate system, the corresponding actual displacement increment Δx is calculated c and Δy c :

[0086]

[0087] Considering the installation structure of the camera and the ground unmanned vehicle, the camera coordinate system is rotated 180° around the y-axis and coincides with the image coordinate system. Figure 2 The template matching VO shown is related to the coordinate system of the monocular camera.

[0088] Therefore, the corresponding ground unmanned vehicle displacement increment Δx v and Δy v for:

[0089] Δx v =-Δx c

[0090] Δy v =Δy c

[0091] If the position of the ground unmanned vehicle in the i-th frame is known, the yaw angle θ collected by the IMU in the i+1 frame is i+1 , calculate the position of the ground unmanned vehicle in the i+1th frame (x v,i+1 ,y v,i+1 ):

[0092]

[0093] In one embodiment, the operating condition is that the road is flat and the movement of the ground unmanned vehicle in the direction perpendicular to the ground is ignored. The position coordinates (x v,i+1,y v,i+1 ) can be written as a three-dimensional vector form:

[0094]

[0095] According to the design of the error state Kalman filter, the position obtained by template matching VO is and

[0096] Error state inertial system estimated position The difference is used as the rough position observation of the filter; the corresponding observation Jacobian matrix is:

[0097] H vo =[I303030303].

[0098] S2 lane offset calculation: A motion camera parallel to the ground is used to capture an image of the road ahead, including lane lines. Inverse perspective mapping (IPM) is used to obtain a bird's-eye view of the road, thereby constructing a mathematical model of the left and right lane lines and calculating the lane offset distance.

[0099] In one embodiment: Figure 3 The image, camera and world coordinate system shown in the figure, the coordinates of any point (u, v) in the camera coordinate system of the image in front of the ground unmanned vehicle including the lane line captured by the motion camera are (x c y c z c ), corresponding to the point (x w y w z w ).

[0100] The world coordinate system takes the optical center of the camera as its origin, and the z-axis points vertically upward to the ground; the operating condition is that the ground is flat and the X c Always located by X w and Y w In the plane formed, there are pitch angles α and yaw angles β; the image coordinate system is projected onto the road plane in the following way:

[0101]

[0102] Where ξ=α+π / 2, the normalized focal length of the camera in the u and v directions is f u and f v and the optical center (c u ,c v ) is determined through camera calibration.

[0103] The key points related to each lane line are fitted by the least squares method to determine the equations of the left and right lane lines; and the distance from the center of the vehicle to the detection line is calculated.

[0104] In practical applications, when pedestrians or vehicles temporarily block lane detection, causing lane line detection failure and outliers, the Z-score method combined with a sliding window is used to remove outliers. A threshold is set. If the Z value exceeds the threshold, the data point is considered an outlier, and the distance value at this time is replaced by the average value within the window at that moment. The Z-score value of each data point is calculated as follows:

[0105]

[0106] in, represents the average distance within the window, and σ represents the variance.

[0107] It should be noted that the threshold values ​​set in the above practical applications need to be set and selected by those skilled in the art according to the actual working conditions and the requirements of the practical applications, and will not be described or limited in detail here.

[0108] S3 lane marking assisted positioning, including:

[0109] S31 Lane Map Matching Modeling: Figure 4 The lane map matching model shown uses lane map key points as matching targets, uses an iterative closest point method to obtain the projection coordinates of the ground unmanned vehicle in the lane map, and combines the vehicle's position to calculate the lateral offset distance between the ground unmanned vehicle and the lane line.

[0110] The S31 lane map matching modeling is as follows:

[0111] The IMU is installed at the center point O of the rear wheel on the top of the ground unmanned vehicle, using the right front upper point as the carrier coordinate system; the camera is installed at point C on the top of the vehicle; let the lateral offset distance detected by the camera system be PN c ;

[0112] Among them, the observation point P(x0,0) is a point directly in front of the camera; considering the installation position of the sensor, the arm vector from the IMU center O to the observation point P is The lateral offset distance PN obtained by map matching m The calculation process is as follows:

[0113] S311: When the GNSS signal is interrupted, the IMU / VO combined positioning system provides a preliminary position prediction value, and then determines the position of the return point P in the ENU coordinate system through arm compensation.

[0114]

[0115] S312: Taking the left lane line as an example, traverse the key points of the left lane line map and calculate the distance between point P and the lane line map point set.

[0116] S313: Take the minimum value of the distance as the lateral offset distance PN obtained by map matching m .

[0117] S32 measurement model: Based on the S31 model, the lane line lateral offset distance vector for map matching is specified as:

[0118]

[0119] Where x and z represent the components of the lateral offset distance of the map matching in the forward and vertical directions, which can be ignored to simplify the calculation. The one-dimensional offset distance is written in the form of a distance vector to facilitate the calculation of the observation Jacobian matrix. m It can be expressed as observation point P and projection point N m The distance is as follows:

[0120]

[0121] The lateral offset distance of the lane line calculated by the camera system Also written in vector form:

[0122]

[0123] Then the lane line lateral offset distance observation Z l It is expressed as the difference between the observations of the camera system and the map matching system. The perturbation analysis of the observations yields the following formula:

[0124]

[0125] Therefore, the Jacobian matrix H corresponding to the lane line lateral offset distance observation l As shown below:

[0126]

[0127] A storage medium readable by an electronic device stores a computer program / instruction thereon, which, when executed by a processor, executes the steps of the auxiliary positioning method based on lane lines and template matching VO as claimed in the claim.

[0128] An electronic device, comprising:

[0129] processor;

[0130] A memory having a computer program stored thereon, wherein the computer program is executed by the processor to perform the steps of the auxiliary positioning method based on lane lines and template matching VO as claimed in claim 1.

[0131] Example 1

[0132] In this embodiment, the computer is configured with AMD Ryzen 7 4800H and NVIDIA GeForce GTX 1650 running Ubuntu 18.04 ROS Melodic system to collect sensor data in real time and run the algorithm. All sensors are connected to the computer via USB serial port. The sensor part includes: 480P USB industrial monocular camera, EZVIZ S3 sports camera, SANCHI100D4IMU, R6093UIMU and Unicore UM482. The actual vehicle verification platform adopts Ackerman chassis structure. The bottom layer is equipped with STM32 single-chip microcomputer as the main controller to control the movement of the vehicle body. The upper computer is equipped with ROS robot operating system for synchronous acquisition and data processing of multi-sensor data.

[0133] This example experimentally verifies the positioning accuracy of the template matching visual odometry. The original image size collected on the asphalt road is 640×480, where the red frame represents the search area and the blue frame represents the template, and the template size is 240×240. During the experiment, the vehicle speed was kept within 1.03m / s. Two sets of experimental data were collected and the position of the ground unmanned vehicle was calculated according to the S1 template matching auxiliary positioning. The positioning results are shown in the figure. Figure 6 and Figure 7 Figure 1 shows a comparison of the odometer and RTK trajectories, with subfigure (a) comparing the odometer and RTK trajectories, and subfigure (b) showing the error curves for the odometry solution coordinates in the plane, x-axis, and y-axis. Since the RTK frequency is 10 Hz and the VO output frequency is 30 Hz, the interpolate module in the Python library is used to interpolate the RTK position sequence by a factor of 3 before calculating the error. Experimental results show that the trajectory error of the template matching VO increases slowly over time, and is particularly significant when the vehicle is cornering.

[0134] Table 1 shows the numerical analysis of planar position errors from the two sets of experiments. It is observed that template matching VO positioning suffers from cumulative errors. Taking the average error as an example, when the ground unmanned vehicle traveled 139.9590 meters, the average error was 1.2820 meters, with an average relative error of 0.9%. When the ground unmanned vehicle traveled 227.7674 meters, the average error was 3.7576 meters, with an average relative error of 1.6%. Relying solely on template matching VO positioning is clearly inadequate to meet the actual needs of ground unmanned vehicles.

[0135]

[0136] Table 1

[0137] This example conducts a practical test of multi-task road scene perception. The multi-task road scene perception network provides lane masks for lane line lateral offset distance calculation. The model's detection performance directly affects the stability and accuracy of the distance solution. The experimental scene selected for the actual test is an unstructured road with two-way lanes separated by a dotted line and a clear curb on both sides of the road. An EZVIZ S3 motion camera mounted on a red car captured three sets of continuous image frames under different weather conditions, including cloudy, sunny, and foggy scenes. The camera frequency was 25Hz, and each experimental set captured 50s of data. The three sets of data together constituted 3750 test images, all with an image size of 1280×720. The trained multi-task road scene perception model was tested on a computing platform configured with an AMD Ryzen 7 4800H and an NVIDIA GeForce GTX 1650. The total inference time for the 3750 images was 533.625s, or an average inference time of 0.1423s per image, approximately 7fps, which corresponds to the update frequency of the / yolop / lane topic in the time synchronization mechanism.

[0138] Figure 8 、 Figure 9 and Figure 10 Detailed display of the output of the multi-task network for a road test scenario. From left to right, the original image, the multi-task detection results, and the individually extracted lane mask (including the road edge).

[0139] Figure 8 The following are four sets of output results for cloudy conditions. We observed that the proposed multi-task model demonstrated excellent detection performance on open, unobstructed straight and curved roads, maintaining stable detection even in the presence of pedestrians or vehicles. Lane detection performance remained strong even on wet roads after rain, with no significant false or missed detections.

[0140] Figure 9 These are four sets of output results for a sunny day. In sunny scenes, buildings and trees on both sides of the road cast shadows under direct sunlight, significantly increasing detection difficulty. Observing the visualization results reveals that due to strong sunlight, the number of missed and falsely detected lane marking pixels increases compared to cloudy scenes. However, drivable area segmentation and traffic object detection are relatively less affected by lighting. The multi-task model is able to adapt to varying lighting and shadow environments, maintaining consistently low levels of false and missed detections across all three tasks.

[0141] Figure 10 These are four sets of output results for foggy conditions. Reduced visibility and poor image quality make detection more difficult, making lane marking misdetection more likely at long distances. However, detection performance remains stable at close range on straight roads, curves, and under obstructions.

[0142] In summary, the multi-task model proposed in this paper can achieve reliable drivable area segmentation, traffic target detection, and lane line detection under different weather conditions and road conditions. Its road scene perception performance has a certain degree of robustness, providing a stable and reliable lane line mask for the subsequent lane line lateral offset distance calculation.

[0143] This example evaluates the accuracy of lane line lateral offset distances. This verifies the accuracy of lane line lateral offset distance calculations within the camera system. This distance value is obtained using the distance calculation method described in S2 Lane Line Offset Distance Calculation. Two sets of experiments verify the accuracy of the distance detection system in static and dynamic scenarios, respectively. The experimental data is collected using actual sensors, and the experimental results validate the effectiveness of the lane line lateral offset distance estimation strategy proposed in this paper.

[0144] (1) Static scene distance accuracy verification and analysis

[0145] We selected a road scene with clear lane lines for the relevant experiments. The static scene distance verification experiment collected three sets of data, which were 1m, 2m, and 3m away from the center of the vehicle to the left lane line. The actual distance was measured by a tape measure with a resolution of 1mm. The experiment started from a distance of 1m and increased by 1m each time until 3m, recording 300 frames of data for each distance. Figure 11 The error is shown in Table 2. Figure 11 The lateral offset distance calculated by the camera system was found to exhibit some small fluctuations, but overall maintained a stable trend. In static scenes, fluctuations in ranging values ​​are generally caused by noise in the camera-captured image, changes in illumination, and image resolution limitations. The error calculations in Table 2 show that the average ranging error at various distances is within 10 cm. The maximum error is 0.1874 m when the vehicle is 2 m from the lane line, and the minimum error is 0.0008 m when the vehicle is 1 m from the lane line. The average error shows a slow upward trend as the distance from the lane line increases. In all three data sets, the standard deviation of ranging values ​​is less than 5 cm, indicating that the distance values ​​output by the camera system in static conditions are relatively stable.

[0146]

[0147] Table 2

[0148] (2) Dynamic scene distance accuracy verification and analysis

[0149] To verify the accuracy of the distance estimation algorithm in dynamic scenarios, the experimental site was selected to include clear lane markings and unobstructed roads. Lane map points were first collected using 10Hz RTK. The main GNSS antenna was mounted directly above the left rear wheel of a cart, which then tracked the lane markings to collect keypoint positions. The latitude, longitude, and altitude of the lane keypoints collected by RTK were then transformed to the ENU coordinate system using Gauss-Krüger, ignoring altitude. Next, the vehicle's current real-time position, point Pj, was recorded using RTK. Using the closest point iteration method, the closest lane map point, pi, to the vehicle was found within the set of lane map keypoints {p1, p2, ...pm}. The distance between point Pj and point pi was calculated as the reference ground truth of the lane lateral offset distance at the current moment. Both points Pj and pi were provided by RTK with a positioning accuracy of 1 cm. Without considering installation errors, the accuracy of this reference ground truth distance was assumed to be at the centimeter level.

[0150] Four sets of experimental data were collected. Figure 12 The results of 4 experiments are shown. Figure 12 As shown in (a)(c)(e)(g), red represents the distance reference truth value obtained by solution, black represents the original measured distance, green represents the distance after IOR filtering, and blue represents the distance after Z-Score filtering. Considering the computational efficiency and the distribution of the original distance data, the filter window width is set to 10, the IQR whiskers are 0.1, and the Z-Score threshold is set to 0.1. It is observed that the lateral offset distance calculated by the camera system is basically consistent with the trend of the reference value, but there are outliers in the original measurement data. The distance measurement value is obviously smoothed after filtering. In order to more intuitively observe the removal of distance outliers before and after filtering, Figure 12 (b)(d)(f)(h) visualize the corresponding distance measurement distributions in the form of boxplots. The filtered data distribution is more compact and the number of outliers is significantly reduced, indicating that filtering has a positive impact on data stability and consistency. Compared with the IQR filtering method, the Z-Score filtering method can remove more outliers.

[0151] Since the reference distance calculation frequency is 10 Hz, while the camera detection system frequency is 7 Hz, to calculate the error sequence between the reference distance sequence (groundtruth) and the distance sequence detected by the camera system, the camera system distance sequence is interpolated to ensure that the two sequences are of the same length, and then the error sequence is obtained by subtraction. Table 3 shows the distribution of distance measurement errors corresponding to the four experimental groups. It is observed that, whether using IQR filtering or Z-Score filtering, the filtered distances show a downward trend in mean absolute error, mean square error, root mean square error, and relative error. The average error of the camera system's raw distance output is 15.36 cm, which is reduced to 14.0 cm and 13.85 cm, respectively, after filtering to remove outliers. Based on the root mean square error, the IQR filtering method reduces the error by 4.13 cm, while the Z-Score filtering method reduces it by 4.62 cm. The Z-Score method reduces the relative error of the distance to below 7%, which is superior to the IQR filtering method. Therefore, the Z-Score method is used to handle outliers in the combined positioning system, which is essential for the subsequent stable operation of the combined positioning system.

[0152]

[0153]

[0154] Table 3

[0155] This example experimentally validates the combined positioning system. To verify the performance of the proposed combined positioning algorithm, a road scene dataset was collected on a two-way road. The outdoor scene selected included clear lane markings and an unobstructed, open road to ensure continuous RTK signal reception, which served as a ground truth comparison. Furthermore, to evaluate the positioning performance of the proposed GILV combined positioning algorithm in the presence of GNSS signal interruption, GNSS signal interruption was simulated by blocking the GNSS signal with code, and the results were compared with the ground truth obtained by RTK.

[0156] The experimental scene and trajectory top view in the map are as follows Figure 13 As shown in the figure, the red mark of the vehicle trajectory, the five-pointed star marks the starting point, and the triangle marks the end point. It includes curves and straights, with a total length of 200.4669m. During the GNSS simulation interruption, three positioning methods are involved: (1) pure IMU nominal state track calculation, (2) IMU / VO combined positioning, and (3) IMU / VO / Lane combined positioning. Error analysis is only performed on the GNSS signal interruption period. While the ROS system is running different positioning algorithms, its timestamp and predicted ground unmanned vehicle position coordinates are saved at the same time. Since the frequency of the combined positioning system output position is different from the RTK update frequency that provides the true value, the true value obtained by RTK solution is interpolated based on the recorded timestamp to calculate the error.

[0157] The GNSS / IMU combined positioning effect is as follows Figure 14 As shown in the figure, the red part represents the RTK trajectory, and the blue part represents the position state output by the GNSS / IMU combined positioning method based on the error state Kalman filter. Since the ground unmanned vehicle moves on the ground, the Z axis is not considered. Figure 14 In (a), (b), and (c), six intervals of 10 seconds, three intervals of 30 seconds, and two intervals of 60 seconds are set to simulate GNSS signal interruption. When the GNSS signal is interrupted, due to the lack of observations, the filter relies only on the nominal state of the inertial measurement unit for track calculation. It is observed that the cumulative position error of the ground unmanned vehicle gradually increases over time. It is worth noting that the direction and amplitude of this trajectory drift are random when the GNSS signal is interrupted for the same period of time, but the trajectory drift phenomenon is particularly significant in the curved driving part.

[0158] Table 4 shows the error statistics of the IMU nominal state track calculation during the GNSS simulation interruption period. It is observed that when the GNSS disappears for only 10 seconds, the maximum error in the X direction reaches 2.7395m, and the maximum error in the Y direction reaches 3.1922m. The positioning performance is severely degraded and cannot be applied to the positioning of actual moving vehicles. Moreover, the error increases exponentially with time. When the GNSS disappears for 60 seconds, the maximum errors in the X and Y directions exceed 100 meters. It is worth noting that the position error and error change rate in the Y direction are significantly greater than those in the X direction. The Y direction is approximately perpendicular to the lane in this dataset. The IMU nominal state track calculation is more sensitive to changes in the vehicle's lateral position.

[0159]

[0160] Table 4

[0161] The combined positioning effects of GNSS / IMU / VO and GNSS / IMU / VO / Lane (GILV) are as follows: Figure 15As shown. In order to verify the positioning performance of this combined positioning method under the condition of long-term GNSS signal interruption, a 100s GNSS signal simulation interruption interval is set. The introduction of template matching VO position constraint has greatly suppressed the IMU position divergence problem. In the initial stage of the simulation interval, the VO trajectory is highly consistent with the true trajectory. However, as time goes by, the position observation solved by VO gradually deviates from the reference true value, and the position error slowly increases. However, after adding the lateral constraint of the lane line, the cumulative error of the VO solution is constrained by the lateral offset distance of the lane line, and the predicted trajectory of the car is closer to the RTK true trajectory. In the 100s GNSS simulation interruption interval, including curves and straight roads, the total length is 100.5904m, of which 84.4612m is in the X direction and 52.2386m is in the Y direction. Observation Figure 15 (b) The GILV algorithm shows greater error stability on straight roads, but exhibits greater position error volatility on curved roads. Compared to GNSS / IMU / VO, GILV provides redundant lateral position observations that do not contain cumulative errors, further improving vehicle positioning performance.

[0162] Tables 5, 6, and 7 compare the positioning errors of IMU / VO and IMU / VO / Lane in the plane, X, and Y directions, respectively, during GNSS outages. Within 100 seconds of GNSS signal loss, the average plane position error of IMU / VO was 0.2943m, and the maximum plane position error was 1.2251m, still failing to meet lane-level positioning requirements. The plane position error of IMU / VO / Lane remained within 45cm, with an average error of 0.0815m, achieving decimeter-level (<50cm) positioning performance. Based on root mean square error, IMU / VO / Lane achieved a 76.1% reduction in X-axis error and a 68.3% reduction in Y-axis error compared to IMU / VO. Based on mean error, IMU / VO / Lane achieved a 75.8% reduction in X-axis error and a 71.3% reduction in Y-axis error compared to IMU / VO. It is worth noting that by comparing the vehicle's position errors in the X and Y directions, it is found that IMU / VO / Lane also relies on the positioning accuracy of VO. The error of IMU / VO in the Y direction is slightly greater than the error in the X direction, and IMU / VO / Lane also meets this requirement. This is because in the lane map matching strategy in this algorithm, the predicted position of the vehicle is given by IMU / VO and then matched with the lane map. In summary, the combined positioning method proposed in this paper can effectively solve the problem of positioning error drift in the event of GNSS interruption, ensure that the ground unmanned vehicle receives stable and continuous positioning signals, and improve the stability of the ground unmanned vehicle's autonomous operation in road scenarios.

[0163]

[0164] Table 5

[0165]

[0166] Table 6

[0167]

[0168] Table 7

[0169] Although the embodiments of the present invention have been shown and described above, it will be understood that the above embodiments are illustrative and are not to be construed as limitations on the present invention. A person skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention.

Claims

1. An auxiliary positioning method based on lane lines and template matching VO, characterized in that: include: S1 template matching assisted positioning, S2 lane line offset distance calculation and S3 lane line assisted positioning are as follows: S1 Template Matching Assisted Positioning: The monocular camera and the ground unmanned vehicle are positioned perpendicular to the bottom surface to capture a sequence of images containing road texture features. An initial template is predefined in the center of the i-th frame image. The rectangular area with the greatest similarity to the initial template is searched and recorded on the captured i+1-th frame image. S2 Lane Offset Calculation: A motion camera parallel to the ground is used to capture an image of the road ahead of the unmanned vehicle, including the lane lines. Inverse perspective mapping is used to obtain a bird's-eye view of the road, thereby constructing a mathematical model of the left and right lane lines and calculating the lane offset distance. S3 lane marking assisted positioning, including: S31 Lane Map Matching Modeling: Using lane map key points as matching targets, the projected coordinates of the ground unmanned vehicle in the lane map are obtained using an iterative closest point method. The lateral offset distance between the ground unmanned vehicle and the lane line is calculated based on the vehicle's position. S32 measurement model: Based on the S31 model, the lane line lateral offset distance vector for map matching is specified as: Where x and z represent the forward and vertical components of the lateral offset distance of the map matching.

2. The auxiliary positioning method based on lane lines and template matching VO according to claim 1, characterized in that: The width and height of the initial template for the S1 template matching assisted positioning are Tw and Th respectively.

3. The auxiliary positioning method based on lane lines and template matching VO according to claim 2, characterized in that: According to the initial template T and the searched rectangular area S, the pixel displacement increments Δu and Δv in the time interval from the i-th frame to the i+1-th frame are calculated: Δu=u0-u1 Δv=v0-v1 Among them, (u0, v0) is the coordinate of the upper left corner of the initial template T in the image coordinate system, (u1, v1) corresponds to the coordinate of the upper left corner of the matching rectangular area S in the image coordinate system; the camera installation height h and the focal length of the camera in the x and y directions f x and f y It is known that according to the relationship between the image coordinate system and the camera coordinate system, the corresponding actual displacement increment Δx is calculated c and Δy c : Considering the installation structure of the camera and the ground unmanned vehicle, the camera coordinate system is rotated 180° around the y-axis to coincide with the image coordinate system; The corresponding ground unmanned vehicle displacement increment Δx v and Δy v for: Δx v =-Δx c Δy v =Δy c If the position of the ground unmanned vehicle in the i-th frame is known, the yaw angle θ collected by the IMU in the i+1 frame is i+1 , calculate the position of the ground unmanned vehicle in the i+1th frame (x v,i+1 ,y v,i+1 ):

4. The auxiliary positioning method based on lane markings and template matching VO according to claim 3 is characterized in that: The operating condition is that the road is flat and the movement of the ground unmanned vehicle in the direction perpendicular to the ground is ignored. The position coordinates (x v,i+1 ,y v,i+1 ) can be written as a three-dimensional vector form: According to the design of the error state Kalman filter, the position obtained by template matching VO is The estimated position of the inertial system with the error state The difference is used as the rough position observation of the filter; the corresponding observation Jacobian matrix is: H vo =[I303030303]。 5. The auxiliary positioning method based on lane lines and template matching VO according to claim 1, characterized in that: The coordinates of any point (u, v) in the camera coordinate system of the image in front of the ground unmanned vehicle containing the lane line captured by the motion camera for solving the S2 lane line offset distance are (x c y c z c ), corresponding to the point (x w y w z w ).

6. The auxiliary positioning method based on lane lines and template matching VO according to claim 5, characterized in that: The world coordinate system takes the optical center of the camera as its origin, and the z-axis points vertically upward to the ground; the operating condition is that the ground is flat and the X c Always located by X w and Y w In the plane formed, there are pitch angles α and yaw angles β; the image coordinate system is projected onto the road plane in the following way: Where ξ=α+π / 2, the normalized focal length of the camera in the u and v directions is f u and f v and the optical center (c u ,c v ) is determined by camera calibration; The key points related to each lane line are fitted by the least squares method to determine the equations of the left and right lane lines; and the distance from the center of the vehicle to the detection line is calculated.

7. The auxiliary positioning method based on lane marking and template matching VO according to claim 5, characterized in that: When pedestrians or vehicles temporarily block lane detection, causing lane line detection failure and abnormal points, the Z-score method combined with the sliding window is used to remove abnormal values; Set a threshold. If the Z value exceeds the threshold, the data point will be considered an outlier, and the distance value at this time will be replaced by the average value within the window at that moment; The Z-score value for each data point is calculated as follows: in, represents the average distance within the window, and σ represents the variance.

8. The auxiliary positioning method based on lane lines and template matching VO according to claim 1, characterized in that: The S31 lane map matching modeling is specifically as follows: The IMU is installed at the center point O of the rear wheel on the top of the ground unmanned vehicle, using the right front upper point as the carrier coordinate system; the camera is installed at point C on the top of the vehicle; let the lateral offset distance detected by the camera system be PN c ; Among them, the observation point P(x0,0) is a point directly in front of the camera; considering the installation position of the sensor, the arm vector from the IMU center O to the observation point P is The lateral offset distance PN obtained by map matching m The calculation process is as follows: S311: When the GNSS signal is interrupted, the IMU / VO combined positioning system provides a preliminary position prediction value, and then determines the position of the return point P in the ENU coordinate system through arm compensation. S312: Taking the left lane line as an example, traverse the key points of the left lane line map and calculate the distance between point P and the lane line map point set. S313: Take the minimum value of the distance as the lateral offset distance PN obtained by map matching m .

9. The auxiliary positioning method based on lane lines and template matching VO according to claim 1, characterized in that: In the S32 measurement model, the one-dimensional offset distance is written as a distance vector; the PN is observed on the y-axis. m It can be expressed as observation point P and projection point N m The distance is as follows: The lateral offset distance of the lane line calculated by the camera system Also written in vector form: Then the lane line lateral offset distance observation Z l It is expressed as the difference between the observations of the camera system and the map matching system. The perturbation analysis of the observations yields the following formula: The Jacobian matrix H corresponding to the lane line lateral offset distance observation l As shown below:

10. An electronic device readable storage medium having a computer program / instruction stored thereon, characterized in that: When the computer program / instruction is executed by a processor, the steps of the auxiliary positioning method based on lane lines and template matching VO are executed.

11. An electronic device, characterized in that: The electronic device comprises: processor; A memory having a computer program stored thereon, wherein the computer program, when executed by the processor, performs the steps of the auxiliary positioning method based on lane lines and template matching VO according to any one of claims 1 to 9.

Images