A method for autonomous vehicle trajectory and navigation image projection calibration

Abstract

This invention belongs to the field of autonomous driving positioning and navigation technology, specifically a method for autonomous vehicle trajectory and navigation image projection calibration. The main steps include: acquiring map data of the actual scene via satellite to establish a high-precision navigation map and image; controlling the vehicle to drive on the road and obtaining the vehicle's driving trajectory via GPS; extracting the pixel corner trajectory shape of the actual road in the navigation image, calculating the GPS corner trajectory shape based on the trajectory recorded in GPS, and generating point-pair data by connecting the discrete points of the GPS trajectory with the image pixels; performing coordinate system transformation on the point-pair data through similarity transformation, and projecting the discrete points of the GPS trajectory onto the high-precision navigation image through calibration transformation to complete the projection calibration. This method solves the problems of geometric distortion and computational efficiency in existing affine transformation models, reduces the dependence on high-precision sensors, and adapts to the navigation needs of autonomous vehicles in road scenarios.

Description

Technical Field

[0001] This invention mainly relates to the fields of vehicle control and autonomous driving, specifically a method for calibrating the trajectory of an autonomous vehicle and navigation image projection. Background Technology

[0002] In the process of autonomous driving, GPS provides positioning and navigation for the vehicle, while high-precision navigation maps extract road information (traffic areas, lane lines, traffic signs, and other relevant information) from navigation images to support planning decisions and provide visualization for monitoring. GPS and high-precision navigation maps use different coordinate systems, requiring strict alignment during autonomous driving; otherwise, they become unusable, leading to significant errors and even collision risks. For example, in complex road networks or narrow road scenarios, errors in map and vehicle trajectory calibration can cause lane deviation, path planning failure, and even collision risks.

[0003] Existing technologies often employ affine transformation models for calibrating vehicle trajectories and map coordinates. This method collects vehicle GPS trajectory points and uses the six degrees of freedom (scaling, rotation, translation, and shearing) of affine transformation to establish a mapping relationship between the map and the vehicle coordinate system. Its main steps include: collecting GPS data, constructing the affine transformation matrix, solving for parameters through linear optimization, and projecting the trajectory onto the map. However, this method suffers from the following problems: First, the non-uniform scaling and shearing of the affine transformation can distort road angles and parallelism in the map, affecting the accuracy of path planning; second, the linear optimization relying on least squares is sensitive to initial parameters, and accumulated errors are easily introduced when GPS data is noisy or map resolution is insufficient; finally, the computational efficiency is low, as the six-degree-of-freedom model requires solving complex matrices, making it difficult to meet the real-time requirements of in-vehicle systems. Summary of the Invention

[0004] To address the shortcomings of current technologies, this invention combines existing technologies and, based on practical applications, provides a method for autonomous vehicle trajectory and navigation image projection calibration. By matching the geometric features of satellite maps and GPS trajectories, it reduces the reliance on high-precision sensors and adapts to the navigation needs of autonomous vehicles in road scenarios.

[0005] The technical solution of the present invention is as follows: A method for autonomous vehicle trajectory and navigation image projection calibration includes the following steps: S1. Collect map data of the actual scene through satellites to create high-precision navigation maps and images; S2. Based on the actual road conditions, control the vehicle to drive on the road and obtain the vehicle's driving trajectory through GPS; S3. Extract the pixel corner trajectory shape of the actual road in the navigation image, calculate the GPS corner trajectory shape based on the trajectory recorded in GPS, and generate point pair data by combining the discrete points of the GPS trajectory with the pixel points of the image. S4. The calibration model equations are used to iteratively optimize and calibrate the point pair data through similarity transformation, and the calibration results are obtained. The discrete points of the GPS trajectory are transformed and projected onto the high-precision navigation image through the calibration results to complete the calibration projection.

[0006] Furthermore, in step S4, the similarity transformation adopts the order of translation, rotation, and scaling. The reprojection error is solved by constructing an iterative method for the calibration model through similarity transformation and Gauss-Newton method theory, and the Jacobian matrix is ​​combined to accelerate parameter convergence.

[0007] Furthermore, in step S4, the model equations are calibrated as follows: ; The iterative formula expression is as follows: ; In the formula, For actual image pixels, For scaling matrices, For rotation matrix, It is a translation matrix. These are discrete points on the GPS-recorded trajectory. This is the initial estimate. Let Jacobian matrix be the model function. Let be the residual matrix.

[0008] Furthermore, in step S4, the Jacobian matrix expression is as follows: ; In the formula, This represents the scaling ratio in the scaling matrix. , Let be the translation vector in the translation matrix. Let be the rotation angle in the rotation matrix. , These are the Gaussian coordinates corresponding to the GPS points; Initial settings , , , Then, the Jacobian matrix and residual matrix are substituted into the calibration model formula for solution, and the parameters are iterated simultaneously. , , , When the parameter increment is less than the threshold, the result is considered to have converged, and the calibration result is output.

[0009] The beneficial effects of this invention are: A 4-DOF model (scaling, rotation, translation) based on similarity transformations is used to strictly constrain the road shape and parallelism to remain unchanged, eliminating the impact of affine distortion on navigation accuracy. An iterative method for the calibration model is constructed using similarity transformations and the Gauss-Newton method to solve the reprojection error. Combined with the Jacobian matrix to accelerate parameter convergence, lightweight and fast calibration is achieved. Geometric feature matching between satellite maps and GPS trajectories is integrated to improve calibration accuracy and reduce dependence on high-precision sensors. The calibration model equations solve the problems of geometric distortion and computational efficiency in existing affine transformation models, adapting to the high-precision navigation and monitoring visualization needs of autonomous vehicles in complex road scenarios. Attached Figure Description

[0010] Figure 1 This is a detailed flowchart of the present invention.

[0011] Figure 2 This is a schematic diagram of the high-precision navigation image that has been established.

[0012] Figure 3 This is a schematic diagram of the vehicle's driving trajectory obtained via GPS.

[0013] Figure 4 for Figure 2 A schematic diagram of the shape of the corner point trajectory of the corresponding image pixels.

[0014] Figure 5 for Figure 3 A schematic diagram of the corresponding GPS corner trajectory shape.

[0015] Figure 6 This is a visualization of the calibrated projection. Detailed Implementation

[0016] The present invention will be further described in conjunction with the accompanying drawings and specific embodiments. It should be understood that these embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. Furthermore, it should be understood that after reading the teachings of this invention, those skilled in the art can make various alterations or modifications to the invention, and these equivalent forms also fall within the scope defined in this application.

[0017] This embodiment proposes an autonomous vehicle trajectory and navigation image projection calibration method, which is mainly a high-precision map calibration method based on similarity transformation. It reduces the dependence on high-precision sensors by matching the geometric features of satellite maps and GPS trajectories, and adapts to the navigation needs of autonomous vehicles in road scenarios.

[0018] refer to Figure 1 The following is the main flowchart of this implementation.

[0019] 1. Use satellites to collect map data of actual scenes to create high-precision navigation maps and images, such as... Figure 2 The image shown is a schematic diagram of the high-precision navigation map that has been created.

[0020] 2. Based on the actual road conditions, control the vehicle to drive on the road, and obtain the vehicle's driving trajectory via GPS for reference. Figure 3 The image shows a schematic diagram of the vehicle's driving trajectory.

[0021] 3. Extract the pixel corner trajectory shapes of the actual road in the high-precision navigation image based on image processing. Simultaneously, calculate the corner trajectory shapes based on the trajectory recorded in the GPS. By determining whether the corner trajectory shapes of the two match consistently, map the discrete points of the GPS trajectory to the image pixels one-to-one. For example... Figure 4 The image shown is a schematic diagram of the pixel corner trajectory shape obtained after extracting the navigation image. Figure 5 The image shown is a schematic diagram of the corner point trajectory shape calculated based on the trajectory recorded by GPS.

[0022] 4. The calibration of vehicle trajectory and navigation image is essentially a transformation between two two-dimensional coordinate systems. There are generally four types of coordinate system transformations: Euclidean transformation, similarity transformation, affine transformation, and homography transformation. In this coordinate system transformation, parallelism and path angles must remain unchanged. Therefore, in this embodiment, the mathematical model for the transformation adopts a similarity transformation, as detailed below.

[0023] The 3D similarity transformation has 4 degrees of freedom, which are the scaling parameters of the scaling matrix. Angular parameters in the rotation matrix Translation parameters in the translation matrix , The transformations should be performed as follows: translation before scaling (otherwise scaling will affect translation); rotation before scaling (otherwise non-uniform scaling will affect rotation); and translation before rotation (otherwise rotation will affect the position of the coordinate system). Matrix multiplication is performed from left to right. Let the points on the image be... , The image pixel coordinates are; the discrete points recorded by GPS are... , Here are the Gaussian coordinates corresponding to the GPS points; the scaling matrix is... , For scaling ratio; rotation matrix , For rotation angle; translation matrix , Let be the translation vector; then we have: ; ; .

[0024] For the above equation, let the actual image point be... , The point obtained after calculation is: Then There is a reprojection error when reprojecting onto the image. For reprojection error In this embodiment, an optimization algorithm is used to find the minimum error.

[0025] The minimum reprojection error can be solved using the Gauss-Newton method in the minimum error optimization algorithm, and the calibration model equation is: ; The iterative formula for solving is: ; in The first estimate is a preliminary estimate. Let Jacobian matrix be the model function. Let be the residual vector. Let the parameter increment be... Then we have: ; set up The two components are , , The two components are , , The two components are , .according to The residual vector can be obtained. : ; .

[0026] For Jacobian matrix There are two components , Four parameters , , , Then we have the Jacobian matrix: ; The Jacobian matrix The first line represents Components, the second row represents Components; the first column represents the parameters. The second column represents the parameters. The third column represents the parameters. The fourth column represents the parameters. The first row and first column represent right Find the partial derivative, and so on. , Component pairs , , , Take the partial derivatives one by one.

[0027] Set initial values , , , Then, the Jacobian matrix and residual matrix are substituted into the calibration model formula to solve the problem, while iterating the parameters. , , , After each iteration , , , Substitute the above , , , Then continue iterating, when If the result is less than 1e-7, the result is considered converged, and the parameters at this point (i.e., the calibration result) are recorded. , , , .calculate Then, the GPS track points are calibrated and projected onto a high-precision navigation map image for plotting. The visualization effect is then observed, and the results are as follows: Figure 6 As shown.

Claims

1. A method for calibrating the trajectory of an autonomous vehicle against a navigation image projection, characterized in that, Includes the following steps: S1. Collect map data of the actual scene through satellites to create high-precision navigation maps and images; S2. Based on the actual road conditions, control the vehicle to drive on the road and obtain the vehicle's driving trajectory through GPS; S3. Extract the pixel corner trajectory shape of the actual road in the navigation image, calculate the GPS corner trajectory shape based on the trajectory recorded in GPS, and generate point pair data by combining the discrete points of the GPS trajectory with the pixel points of the image. S4. The calibration model equations are used to iteratively optimize and calibrate the point pair data through similarity transformation, and the calibration results are obtained. The discrete points of the GPS trajectory are transformed and projected onto the high-precision navigation image through the calibration results to complete the calibration projection.

2. The autonomous vehicle trajectory and navigation image projection calibration method according to claim 1, characterized in that, In step S4, the similarity transformation adopts the order of translation, rotation, and scaling. The calibration model iterative method is constructed through similarity transformation and Gauss-Newton method to solve the reprojection error, and the Jacobian matrix is ​​combined to accelerate parameter convergence.

3. The autonomous vehicle trajectory and navigation image projection calibration method according to claim 2, characterized in that, In step S4, the model equations are calibrated as follows: ； The iterative formula expression is as follows: ； In the formula, For actual image pixels, For scaling matrices, For rotation matrix, It is a translation matrix. These are discrete points on the GPS-recorded trajectory. This is the initial estimate. Let Jacobian matrix be the model function. Let be the residual matrix.

4. The autonomous vehicle trajectory and navigation image projection calibration method according to claim 3, characterized in that, In step S4, the Jacobian matrix expression is as follows: ； In the formula, This represents the scaling ratio in the scaling matrix. , Let be the translation vector in the translation matrix. Let be the rotation angle in the rotation matrix. , These are the Gaussian coordinates corresponding to the GPS points; Set initial values , , , Then, the Jacobian matrix and residual matrix are substituted into the calibration model formula to solve the problem, while iterating the parameters. , , , When the parameter increment is less than the threshold, the result is considered to have converged, and the calibration result is output.

Images