Vehicle visual positioning method, system, device and readable storage medium

Abstract

The present invention discloses a vehicle visual positioning method, system, device and readable storage medium. The rotation matrix in the vehicle motion process is parameterized as an angle variation, and the translation vector in the vehicle motion process is parameterized as a translation component to be represented by graph theory concepts. A feature detection algorithm based on robust feature point AKAZE and a feature point matching algorithm combining zero-mean normalized cross-correlation and multi-strategy are used to ensure the accuracy of feature point extraction and feature point association. An improved RANSAC algorithm's external point removal algorithm removes noise points and erroneous matching points, ensuring the accuracy of motion calculation. The vehicle motion information is solved by a nonlinear weighted least square method and a filtering processing technology based on Sigma points is used to further optimize the motion result. The positioning accuracy is high. The improved RANSAC algorithm based on initial value estimation reduces the complexity of external point removal, ensures the real-time performance of the algorithm, reduces the number of feature points, thereby reducing the complexity of the algorithm and ensuring the real-time performance of the entire algorithm.

Description

Technical Field

[0001] The present invention belongs to the field of image processing and relates to a visual positioning method, and in particular to a vehicle visual positioning method, system, device and readable storage medium. Background Art

[0002] Existing vehicle positioning methods have many technical defects. For example, traditional odometers have large positioning errors on slippery surfaces, GNSS will drift significantly or fail to position directly in the absence of satellites, and IMU based on incremental positioning cannot overcome the defect of cumulative errors. LiDAR has high positioning accuracy, but poor real-time performance and expensive equipment. The fusion positioning method using multiple sensors has received high attention and research in recent years. The advantages and disadvantages of various sensors complement each other, but this positioning technology is relatively complex to implement and has a high cost.

[0003] With the development of image processing technology, high-precision positioning technology based on visual information, known as visual odometry, has been widely used in fields such as Mars exploration, unmanned vehicles, and micro-aerial vehicles. The mobile carrier is equipped with one or more cameras, and the environmental image information obtained by the camera is used to calculate the motion information of the carrier during movement. This is the basic task of the visual odometry. Long-term and long-distance positioning tasks in unstructured environments place strict requirements on the comprehensive performance of the visual odometry. However, the current visual positioning technology is greatly restricted by the surrounding environment of the carrier, and has not overcome the problem of error accumulation in the cumulative positioning method, resulting in the inability to meet the application requirements of some actual systems. Summary of the invention

[0004] The purpose of the present invention is to provide a vehicle visual positioning method, system, device and readable storage medium to overcome the drift error problem caused by the existing visual positioning method.

[0005] In order to achieve the above object, the present invention adopts the following technical scheme:

[0006] A vehicle visual positioning method comprises the following steps:

[0007] S1, using a calibrated binocular camera to collect photo images during the vehicle's motion, correcting the photo images using the internal and external parameters of the binocular camera to obtain a rotation matrix and a translation vector during the vehicle's motion;

[0008] S2, parameterize the rotation matrix during the vehicle motion into the angle change, and parameterize the translation vector during the vehicle motion into the translation component;

[0009] S3, preprocessing the original video image of the vehicle driving, extracting feature points in the preprocessed image using the AKAZE method and describing the feature points to obtain a feature vector set of the feature points of the preprocessed image;

[0010] S4, the feature vector set of the image feature points obtained in step S3 is subjected to box filtering, feature point matching, feature matching ratio test and feature ring matching test to obtain a correctly matched feature point pair set;

[0011] S5, using the RANSAC algorithm based on initial estimation to iterate the matched feature point pair set in step S4, removing the feature points on the target body in the original video image in step S3, and obtaining a correct matching point pair set;

[0012] S6, using a nonlinear weighted least square method to calculate the rotation matrix and translation vector value of the vehicle motion for the correct matching point pair set in step S5;

[0013] S7, based on the concept of graph theory, the rotation matrix and translation vector of the vehicle motion obtained in step S6 are represented by the angle change and the translation component to obtain a schematic diagram of the vehicle motion and complete the positioning of the vehicle.

[0014] Furthermore, the rotation matrix during the vehicle motion process is parameterized into an angle change as follows:

[0015]

[0016]

[0017] in, is the rotation matrix during vehicle motion, and the angle change is (θ x ,θ y ,θ z ), the rotation matrix roll angle θ x is the angle of rotation of the vehicle around the x-axis, the pitch angle θ y is the angle of rotation of the vehicle around the y-axis, the yaw angle θ z The vehicle is the angle of rotation around the z-axis, is the translation vector of the vehicle during motion, t x ,t y ,t z They are the translation components of the vehicle around the x-axis, y-axis, and z-axis respectively.

[0018] Furthermore, the initial estimate in the RANSAC algorithm based on initial estimate means that the geometric transformation relationship from image I(x,y) to another image I′(x′,y′) is determined, and for the feature point P(x,y) in image I(x,y) and the feature point P′(x′,y′) in the image to be matched I′(x′,y′), the following relationship is satisfied:

[0019] (1) The straight-line distance between P(x,y) and P′(x′,y′) Equal or close,

[0020] (2) The slope of the line formed by P(x,y) and P′(x′,y′) equal or close to;

[0021] The matching feature points that satisfy the above relationship are retained, otherwise the feature points are deleted, and the remaining matching points are used as the input values ​​of the RANSAC algorithm to remove the feature points on the target object.

[0022] Furthermore, the weight function determined by the nonlinear weighted least squares method is:

[0023] in, is the weight corresponding to the i-th observation value in the q-th step, is the error term ε i The covariance of

[0024] By minimizing the formula and formula The rotation matrix can be obtained and translation vectors value,

[0025] in

[0026]

[0027]

[0028] and They are and The centroid coordinates of and They are k q and k q+1 The three-dimensional coordinates of the world coordinate system at the moment, and It is the deviation relative to the centroid coordinates of each feature point set.

[0029] Furthermore, the camera is q to k q+1The single-step motion matrix at time for

[0030]

[0031] From the initial time k0 to the current time k q The motion matrix M q+1 It can be calculated by accumulating the single-step motion matrix:

[0032] Furthermore, box filtering: the preprocessed image is divided into multiple non-overlapping boxes of 50*50 pixels, and at most two feature points are retained in each box. After processing, an image to be matched containing uniformly sparse feature points is obtained;

[0033] Feature point matching: The uniformly sparse feature points between different frame images to be matched are matched using a zero-mean normalized cross-correlation function as an evaluation criterion for the similarity of the two feature points. After this step, an image containing initial matching feature points is obtained.

[0034] Furthermore, the zero-mean normalized cross-correlation function is as follows:

[0035]

[0036] Where I(·) and I′(·) are the two images to be matched, I(x,y) represents the pixel intensity value at (x,y) in image I(·), I′(x+u,y+v) represents the pixel intensity value at point (x,y)+(u,v) in image I′(·), u and v are the offsets in the x-axis and y-axis directions, respectively. and Respectively represent the mean pixel intensity within the window;

[0037] The larger the ZMNCC(u,v) value is, the higher the similarity between the two feature points is, and the two feature points are retained. The two feature points with the largest ZMNCC(u,v) values ​​are the optimal matching points, followed by the suboptimal matching points.

[0038] A vehicle visual positioning system includes an image acquisition and processing module, a data processing module and a result output module.

[0039] The image acquisition processing module is used to correct the photo images collected by the calibrated binocular camera during the vehicle movement process through the internal and external parameters of the binocular camera, obtain the rotation matrix and translation vector of the vehicle during the vehicle movement process, and parameterize the rotation matrix of the vehicle during the vehicle movement process into the angle change, and parameterize the translation vector of the vehicle during the vehicle movement process into the translation component to express it in graph theory concepts;

[0040] The data processing module is used for preprocessing the original video image of vehicle driving, using the AKAZE method to extract the feature points in the preprocessed image and describe the feature points to obtain the feature vector set of the preprocessed image feature points, and then the obtained feature vector set of the image feature points is subjected to box filtering, feature point matching, feature matching ratio test and feature ring matching test to obtain a correctly matched feature point pair set, and the matched feature point pair set is iterated by using the RANSAC algorithm based on initial estimation to remove the feature points on the target body in the original video image to obtain a correctly matched point pair set; the nonlinear weighted least square method is used to calculate the rotation matrix and translation vector value of the vehicle motion for the correctly matched point pair set; the angle change amount and translation component are used to represent the rotation matrix and translation vector of the vehicle motion, and the vehicle motion schematic diagram is obtained and output through the result output module.

[0041] A terminal device comprises 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 above-mentioned vehicle positioning method when executing the computer program.

[0042] A computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the above-mentioned vehicle positioning method are implemented.

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

[0044] The present invention discloses a vehicle visual positioning method. The rotation matrix in the vehicle motion process is parameterized as an angle variation, the translation vector in the vehicle motion process is parameterized as a translation component and expressed in graph theory concepts. The feature detection algorithm based on the robust feature point AKAZE and the feature point matching algorithm combining zero-mean normalized cross-correlation and multi-strategy ensure the accuracy of feature point extraction and feature point association. The external point removal algorithm of the improved RANSAC algorithm removes noise points and erroneous matching points, ensuring the accuracy of motion calculation. The vehicle motion information is solved by the nonlinear weighted least square method and the filtering processing technology based on the Sigma point is used to further optimize the motion result, and the positioning accuracy is high. The AKAZE feature algorithm can meet the real-time requirements, the improved RANSAC algorithm based on initial value estimation reduces the complexity of external point removal, ensures the real-time performance of the algorithm, and the box filter evens the distribution of feature points and reduces the number of feature points, thereby reducing the complexity of the algorithm and ensuring the real-time performance of the entire algorithm.

[0045] The present invention adopts the RANSAC algorithm to remove external points, so there is no need to consider whether there is a target object, that is, a dynamic person or object, in the field of view. The box filter is used to make the feature points evenly distributed on the entire image, ensuring the stability and reliability of the algorithm processing.

[0046] The positioning system of the present invention is simple, reduces the system development cost, and provides possibility for the promotion and application of large-scale vehicle positioning systems. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 It is a flow chart of the method in an embodiment of the present invention.

[0048] Figure 2 Schematic diagram of the installation of a binocular camera in an embodiment of the present invention.

[0049] Figure 3 It is a flow chart of a multi-strategy matching algorithm in an embodiment of the present invention.

[0050] Figure 4 It is a schematic diagram of characteristic ring matching in an embodiment of the present invention.

[0051] Figure 5 It is a flow chart of an outlier removal algorithm of an improved RANSAC algorithm based on initial estimation in an embodiment of the present invention.

[0052] Figure 6 It is a flow chart of a vehicle motion estimation algorithm based on nonlinear weighted least squares method in an embodiment of the present invention.

[0053] Figure 7 It is a schematic diagram of a vehicle motion representation based on a graph in an embodiment of the present invention.

[0054] Figure 8 It is an example of a campus environment image in an embodiment of the present invention.

[0055] Fig. 9 It is a schematic diagram of the vehicle positioning result based on the algorithm of the present invention in an outdoor environment in an embodiment of the present invention. DETAILED DESCRIPTION

[0056] The present invention is further described in detail below in conjunction with the accompanying drawings:

[0057] A vehicle visual positioning method comprises the following steps:

[0058] S1, using a calibrated binocular camera to collect photo images during the vehicle's motion, correcting the photo images using the internal and external parameters of the binocular camera to obtain a rotation matrix and a translation vector during the vehicle's motion;

[0059] The binocular camera is fixed on the roof facing the front of the vehicle, the distance between the optical center of the camera and the horizontal ground is 1.56 meters, and the camera is tilted 18° horizontally toward the ground.

[0060] S2, parameterize the rotation matrix during the vehicle motion into the angle change, and parameterize the translation vector during the vehicle motion into the translation component;

[0061] Specifically, the rotation matrix parameterized during the vehicle motion process is converted into an angle variation as follows:

[0062]

[0063]

[0064] in, is the rotation matrix during vehicle motion, and the angle change is (θ x ,θ y ,θ z ), the rotation matrix roll angle θ x is the angle of rotation of the vehicle around the x-axis, the pitch angle θ y is the angle of rotation of the vehicle around the y-axis, the yaw angle θ z The vehicle is the angle of rotation around the z-axis, is the translation vector of the vehicle during motion, t x ,t y ,t z They are the translation components of the vehicle around the x-axis, y-axis, and z-axis respectively.

[0065] S3, preprocessing the original video image of the vehicle driving, extracting feature points in the preprocessed image using the AKAZE method and describing the feature points to obtain a feature vector set of the feature points of the preprocessed image;

[0066] S4, the feature vector set of the image feature points obtained in step S3 is subjected to box filtering, feature point matching, feature matching ratio test and feature ring matching test to obtain a correctly matched feature point pair set;

[0067] Box filter:

[0068] The preprocessed image is divided into multiple non-overlapping boxes of 50*50 pixels, and at most two feature points are retained in each box. After processing, an image to be matched containing uniformly sparse feature points is obtained;

[0069] Feature point matching:

[0070] The uniformly sparse feature points between the different frame images to be matched are matched using a zero-mean normalized cross-correlation function as an evaluation criterion for the similarity of the two feature points. After this step, an image containing initial matching feature points is obtained;

[0071] The zero-mean normalized cross-correlation function is as follows:

[0072]

[0073] Where I(·) and I′(·) are the two images to be matched, I(x,y) represents the pixel intensity value at (x,y) in image I(·), I′(x+u,y+v) represents the pixel intensity value at point (x,y)+(u,v) in image I′(·), u and v are the offsets in the x-axis and y-axis directions, respectively. and Respectively represent the mean pixel intensity within the window;

[0074] The larger the ZMNCC(u,v) value is, the higher the similarity between the two feature points is, and the two feature points are retained. The two feature points with the largest ZMNCC(u,v) value are the optimal matching points, and the next ones are the suboptimal matching points.

[0075] Feature matching ratio test:

[0076] Take the match between the left and right frame images or the adjacent front and back frame images containing the initial matching feature points, calculate the Euclidean distance between the feature points in the left frame or the previous frame image and the optimal matching point and the suboptimal matching point in the right frame or the next frame image, and obtain the ratio of the Euclidean distance between the feature points in the left frame or the previous frame image and the optimal matching point and the Euclidean distance between the feature points in the left frame or the previous frame image and the suboptimal matching point. If the ratio is less than a fixed threshold, the matching is stable and the matching point pair is retained. Otherwise, the matching is unstable and the matching point pair is deleted. The fixed threshold λ=0.6. After this step, an image containing feature points after the ratio test is obtained.

[0077] Characteristic ring matching test:

[0078] From the left image I containing the feature points after the ratio test at the current k moment L,k A feature point P in L，k Start with the starting point, and search for the right image I containing the feature points after the ratio test at time k. R,k A feature point P in R，k , right image I at time k-1 R,k-1 A feature point P in R，k-1 , right image I at time k-2 R,k-2 A feature point P in R，k-2 , left image I at time k-2L,k-2 A feature point P in L，k-2 , left image I at time k-1 L,k-1 A feature point P in L，k-1 and the left image I at time k L,k A feature point P′ in L，k End is the end point;

[0079] If and only if the starting point P L，k and the end point P′ L，k When they are the same feature point, that is, P L，k =P′ L，k , then the ring matching is considered successful and the matching point pair is retained. Otherwise, the matching fails and the matching point pair is deleted. All feature points in the left image at the current k moment are traversed to find the corresponding matching point pair and obtain the set of correctly matched feature point pairs.

[0080] S5, using the RANSAC algorithm based on initial estimation to iterate the matched feature point pair set in step S4, removing the feature points on the target body in the original video image in step S3, and obtaining a correct matching point pair set;

[0081] Specifically, the initial estimate in the RANSAC algorithm based on initial estimate means that the geometric transformation relationship from image I(x,y) to another image I′(x′,y′) is determined, and for the feature point P(x,y) in image I(x,y) and the feature point P′(x′,y′) in the image to be matched I′(x′,y′), the following relationship is satisfied:

[0082] (1) The straight-line distance between P(x,y) and P′(x′,y′) Equal or close,

[0083] (2) The slope of the line formed by P(x,y) and P′(x′,y′) equal or close to;

[0084] The matching feature points that satisfy the above relationship are retained, otherwise the feature points are deleted, and the remaining matching points are used as the input values ​​of the RANSAC algorithm to remove the feature points on the target object.

[0085] S6, using a nonlinear weighted least square method to calculate the rotation matrix and translation vector value of the vehicle motion for the correct matching point pair set in step S5;

[0086] Specifically, the weight function determined by the nonlinear weighted least squares method is:

[0087] in, is the weight corresponding to the i-th observation value in the q-th step, is the error term εi The covariance of

[0088] By minimizing the formula and formula The rotation matrix can be obtained and translation vectors value,

[0089] in

[0090]

[0091]

[0092] and They are and The centroid coordinates of and They are k q and k q+1 The three-dimensional coordinates of the world coordinate system at the moment, and It is the deviation relative to the centroid coordinates of each feature point set.

[0093] S7, based on the concept of graph theory, the rotation matrix and translation vector of the vehicle motion obtained in step S6 are represented by the angle change and the translation component to obtain a schematic diagram of the vehicle motion and complete the positioning of the vehicle.

[0094] Specifically, the camera is in k q to k q+1 The single-step motion matrix at time for

[0095]

[0096] From the initial time k0 to the current time k q The motion matrix M q+1 It can be calculated by accumulating the single-step motion matrix:

[0097] In one embodiment of the present invention, a terminal device is provided, the terminal device includes a processor and a memory, the memory is used to store a computer program, the computer program includes program instructions, and the processor is used to execute the program instructions stored in the computer storage medium. The processor adopts a central processing unit (CPU), or adopts other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), off-the-shelf programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is suitable for implementing one or more instructions, and is specifically suitable for loading and executing one or more instructions to implement the corresponding method flow or corresponding functions; the processor described in the embodiment of the present invention can be used for the operation of the vehicle positioning method.

[0098] A vehicle visual positioning system, comprising:

[0099] It includes image acquisition and processing module, data processing module and result output module.

[0100] The image acquisition processing module is used to correct the photo images collected by the calibrated binocular camera during the vehicle movement process through the internal and external parameters of the binocular camera, obtain the rotation matrix and translation vector of the vehicle during the vehicle movement process, and parameterize the rotation matrix of the vehicle during the vehicle movement process into the angle change, and parameterize the translation vector of the vehicle during the vehicle movement process into the translation component to express it in graph theory concepts;

[0101] The data processing module is used for preprocessing the original video image of vehicle driving, using the AKAZE method to extract the feature points in the preprocessed image and describe the feature points to obtain the feature vector set of the preprocessed image feature points, and then the obtained feature vector set of the image feature points is subjected to box filtering, feature point matching, feature matching ratio test and feature ring matching test to obtain a correctly matched feature point pair set, and the matched feature point pair set is iterated by using the RANSAC algorithm based on initial estimation to remove the feature points on the target body in the original video image to obtain a correctly matched point pair set; the nonlinear weighted least square method is used to calculate the rotation matrix and translation vector value of the vehicle motion for the correctly matched point pair set; the angle change amount and translation component are used to represent the rotation matrix and translation vector of the vehicle motion, and the vehicle motion schematic diagram is obtained and output through the result output module.

[0102] In another embodiment of the present invention, the present invention also provides a storage medium, specifically a computer-readable storage medium (Memory), which is a memory device in a terminal device for storing programs and data. The computer-readable storage medium includes a built-in storage medium in the terminal device, which provides a storage space and stores the operating system of the terminal, and may also include an extended storage medium supported by the terminal device. In addition, one or more instructions suitable for being loaded and executed by a processor are also stored in the storage space, and these instructions may be one or more computer programs (including program codes). It should be noted that the computer-readable storage medium here may be a high-speed RAM memory, or a non-volatile memory (Non-volatile memory), such as at least one disk memory. The processor may load and execute one or more instructions stored in the computer-readable storage medium to implement the corresponding steps of the vehicle positioning method in the above embodiment.

[0103] Embodiment 1:

[0104] Following the above technical solution, Figures 1 to 9 As shown, this embodiment provides a vehicle visual positioning device, including a high-definition binocular camera installed at the rearview mirror inside the vehicle to collect images with a resolution of 1600*1200, the main optical axis of the camera is parallel to the vehicle base and faces forward, an RJ-45 network data cable is used to connect the camera with a LAN interface to a gigabit network switch, data is transmitted to a server for processing and a positioning result is obtained, a vehicle-mounted power supply combined with an inverter is used as a power supply for the equipment, and the high-precision positioning information processed by the present invention is used for later decision-making generation and state control of intelligent vehicles, a Basler aca1600-60gc high-definition camera ensures the amount of image information, a Cisco WS-C3560X-24T-L gigabit network switch ensures the data transmission speed, an AMAX PSC-6S server ensures the data processing speed, and a network switch and an industrial computer are placed in the trunk of the vehicle.

[0105] like Figure 1 As shown, the vehicle positioning method of the present invention specifically includes the following steps:

[0106] Step 1, calibrate the binocular camera using Zhang Zhengyou plane calibration method; the camera in the present invention adopts a pinhole imaging model, and then uses Zhang Zhengyou plane calibration method to calibrate the internal and external parameter values ​​of the binocular camera.

[0107] In the imaging geometry of the camera, four coordinate systems are involved: the world coordinate system, the camera coordinate system, the image physical coordinate system, and the image pixel coordinate system. In general, the camera imaging model can be regarded as a linear pinhole model that conforms to the central projection. Under the central perspective projection model, the world coordinate system (X w ,Y w ,Z w ,1) and the image pixel coordinates (u, v) of the coordinate point j and its projection point j' are:

[0108]

[0109] in

[0110]

[0111] Among them, z c is the coordinate value on the z-axis in the camera coordinate system; α and β are the effective focal lengths of the u-axis and v-axis respectively; in a camera model with higher accuracy, a correction parameter γ needs to be introduced, which is a measure of the degree of inclination of the coordinate axis in pixels in the image, γ = αtanθ, θ represents the degree of inclination of the v-axis of the CCD array; M1 is the camera intrinsic parameter, M2 is the extrinsic parameter; R(r) and T represent the rotation matrix and translation vector respectively, r i (i=1,2,...,9) are the 9 components of the rotation matrix R(r), is the translation vector T = (t x ,t y ,t z ) is a skew-symmetric matrix representation of .

[0112] Step 2, parameterizing the rotation matrix during the vehicle motion process into an angle change, and parameterizing the translation vector during the vehicle motion process into a translation component;

[0113] The purpose of vehicle motion parameterization is to calculate the angle change (θ x ,θ y ,θ z ), translation component (t x ,t y ,t z ) is linked to R(r) and T. To locate the vehicle, the algorithm is used to calculate the angular change of the vehicle's motion and the translation component. The calculated information is then parameterized into the rotation matrix R(r) and translation vector T during the vehicle's motion.

[0114] Rotation Matrix Contains three angle information: roll angle θ x is the angle of rotation of the vehicle around the x-axis, the pitch angle θ yis the angle of rotation of the vehicle around the y-axis, the yaw angle θ z The vehicle is the angle of rotation around the z-axis, and the matrix is ​​expressed as:

[0115]

[0116]

[0117] Translation Vector Contains three components of information: T = (t x ,t y ,t z ) T .

[0118] Among them, t x ,t y ,t z They are the translation components of the vehicle around the x-axis, y-axis, and z-axis respectively.

[0119] Step 3, collecting the original video image, preprocessing the original video image, using the AKAZE algorithm to extract feature points in the preprocessed image and describe the feature points to obtain a feature vector set of the feature points of the preprocessed image;

[0120] In the present invention, two Basler aca1600-60gc cameras with a resolution of 1600*1200 are used to form a binocular camera device. The high resolution ensures that the collected image information is rich and the positioning result is accurate. The installation position and angle of the camera play a key role in the content of the collected video image. Figure 2 As shown in the figure, the binocular camera is fixed on the roof facing the front of the vehicle, and the distance between the optical center of the camera and the horizontal ground is 1.56 meters; in order to limit the environmental information that is too far away in the camera's field of view and ensure the accuracy of feature point detection, the camera is set to tilt at an angle of 18° toward the ground.

[0121] Image preprocessing is to perform extreme correction on the collected stereo images to make the image planes parallel and reduce the cost of feature point search in subsequent steps.

[0122] Robust feature point AKAZE feature detection: AKAZE is a multi-scale 2D feature detection algorithm and description algorithm in nonlinear space, which includes three main steps: nonlinear scale space construction, feature point detection and feature point description. The present invention selects the AKAZE feature algorithm to extract feature points in the original video image and describes the feature points to obtain a feature vector set of image feature points. Each feature point on the image corresponds to a feature vector, and all feature point vectors constitute a feature vector set of image feature points.

[0123] Step 4, the feature vector set of the image feature points obtained in step 3 is subjected to box filtering, feature point matching, feature matching ratio test and feature ring matching test to obtain a correctly matched feature point pair set;

[0124] The purpose of multi-strategy matching is to find the correct correspondence between the feature vector sets of image feature points obtained in step 3 and obtain a set of correct matching point pairs. It can be divided into four steps: box filter algorithm processing, ZMNCC feature point matching, feature matching ratio test and feature ring matching test.

[0125] Step 41, box filter

[0126] Divide the image preprocessed in step 3 into multiple non-overlapping boxes of 50*50 pixels, and retain at most two feature points in each box;

[0127] The benefits of using a box filter algorithm to control the number and distribution of feature points are: reducing the number of feature points and the complexity of subsequent calculations; the feature points are evenly distributed throughout the image, with feature points distributed on both dynamic objects and static backgrounds, alleviating the problem of motion estimation drift caused by most of the feature points in the image falling on dynamic objects; based on the distance of the feature points from the camera, feature points closer to the camera are retained, thereby improving the accuracy of vehicle self-motion estimation.

[0128] Step 42: Feature point matching

[0129] The feature point matching is specifically ZMNCC feature point matching. The present invention uses a zero mean normalized cross-correlation function as an evaluation criterion for the similarity of two feature points to perform feature matching on the feature points between different frame images to be matched;

[0130] The zero-mean normalized cross-correlation function is as follows:

[0131]

[0132] Where I(·) and I′(·) are the two images to be matched, I(x,y) represents the pixel intensity value at (x,y) in image I(·), I′(x+u,y+v) represents the pixel intensity value at point (x,y)+(u,v) in image I′(·), u and v are the offsets in the x-axis and y-axis directions, respectively. and Respectively represent the mean pixel intensity in the window; in the present invention, the window N is set w The size is 5×5 pixels.

[0133] The larger the ZMNCC(u,v) value is, the higher the similarity between the two feature points is, and the two feature points are retained. The two feature points with the largest ZMNCC(u,v) values ​​are the optimal matching points, followed by the suboptimal matching points.

[0134] Step 43: Feature matching ratio test

[0135] Select the match between the left and right frame images or the adjacent front and rear frame images, calculate the Euclidean distance between the feature point in the left frame or the previous frame image and the optimal matching point and the suboptimal matching point in the right frame or the next frame image, and obtain the ratio of the Euclidean distance between the feature point in the left frame or the previous frame image and the optimal matching point and the ratio of the Euclidean distance between the feature point in the left frame or the previous frame image and the suboptimal matching point.

[0136] The specific steps are: for a feature point in one image, find two candidate matching points in another image, one of which is the best matching point and the other is the second best matching point, calculate the Euclidean distance between the feature point and the other two candidate matching points, and then calculate the ratio of the distance between the feature point and the best matching point and the distance between the feature point and the second best matching point. If the ratio is less than a fixed threshold, the match is stable and the matching point pair is retained. Otherwise, the match is unstable and the matching point pair is deleted. In the present invention, the fixed threshold λ=0.6 is taken.

[0137] Step 44: Feature ring matching test

[0138] The feature ring matching test is performed between multiple images. Considering the relative relationship between algorithm execution efficiency and algorithm accuracy, the present invention selects to implement ring matching between the left and right images of the previous frame, the left and right images of the current frame, and the left and right images of the next frame. L,k A feature point P in L，k Start as the starting point and search for the right image I at time k in sequence R,k A feature point P in R，k , right image I at time k-1 R,k-1 A feature point P in R，k-1 , right image I at time k-2 R,k-2 A feature point P in R，k-2 , left image I at time k-2 L,k-2 A feature point P in L，k-2 , left image I at time k-1 L,k-1 A feature point P in L，k-1 and the left image I at time k L,k A feature point P′ in L，k End is the end point;

[0139] If and only if the starting point P L，k and the end point P′ L，k When they are the same feature point, that is, P L，k =P′ L，k, then the ring matching is considered successful and the matching point pair is retained. Otherwise, the matching fails and the matching point pair is deleted. All feature points in the left image at the current k moment are traversed to find the corresponding matching point pair and obtain the matching point pair set.

[0140] Step 5, using the RANSAC algorithm based on initial estimation to iterate the matched feature point pair set in step 4, remove the feature points on the target body in the original video image in step 3, and obtain a correct matching point pair set;

[0141] In the traditional RANSAC algorithm, the model is first estimated by randomly selecting sample points, and then the model estimation is optimized through multiple iterations. When the number of outliers is large, iterative convergence is a relatively time-consuming process, and the estimated model error is large. The improved RANSAC algorithm based on initial estimation proposed in the present invention is based on the assumption that if the input data of the RANSAC algorithm contains fewer outliers, the model estimation process will converge faster and the estimation will be more accurate.

[0142] The initial estimate in the RANSAC algorithm based on initial estimate means that the geometric transformation relationship from image I(x,y) to another image I′(x′,y′) is determined. For the feature point P(x,y) in image I(x,y) and the feature point P′(x′,y′) in the image to be matched I′(x′,y′), they satisfy the following relationship:

[0143] (1) The straight-line distance between P(x,y) and P′(x′,y′) Equal or close,

[0144] (2) The slope of the line formed by P(x,y) and P′(x′,y′) equal or close to;

[0145] The correspondence between the above feature points is reasonable for the screening of internal points. The matching points that satisfy this relationship are retained, otherwise the feature points are deleted, that is, some singular matching points are deleted during the feature matching process, and the remaining matching points are used as the input value of the RANSAC algorithm. The RANSAC algorithm is used to remove the feature points on the target object to ensure that the algorithm is not affected by dynamic objects in the scene. The calculation formula for the number of RANSAC iterations is:

[0146]

[0147] Where n represents the number of algorithm iterations; p represents the probability that all randomly selected points from the data set are inliers during the iteration; ε represents the percentage of outliers in the data set; and s represents the number of points required for the estimation model. In the present invention, n is 1600 and s is 5.

[0148] Step 6: Use the nonlinear weighted least squares method to calculate the rotation matrix of the vehicle motion for the correct matching point pair set in step 5. and translation vectors value;

[0149] The ordinary least squares method assumes that the error term in the calculation process is a constant. Strictly speaking, this assumption is not valid in the process of intelligent vehicle motion estimation. For feature points at different distances from the camera in the acquired image, the error is different.

[0150] The present invention proposes to estimate vehicle self-motion based on a nonlinear weighted least squares method.

[0151] The purpose of vehicle self-motion estimation is to calculate the q to k q+1 The rotation matrix and translation vectors

[0152]

[0153] in, and They are k q+1 and k q The three-dimensional coordinates of the moment.

[0154] (1) Nonlinear weighted least squares

[0155] For an overdetermined equation with m equations and n variables:

[0156] y i =f i (x j )+ε i (i=1,...,m;j=1,...,n)

[0157] Among them, ε i is the error term and satisfies the normal distribution step f i (·) is a nonlinear function.

[0158] Corresponding to the error function, the motion estimation task is to minimize the nonlinear weighted least squares function shown in formula (1):

[0159]

[0160] Among them, w i >0 is the weight corresponding to the i-th observation. If the variable x(x j |j=1,...,n) i This problem is called heteroskedasticity problem.

[0161] According to Taylor expansion formula, we have:

[0162]

[0163] in, is the gradient expression. Substituting formula (2) into formula (1), ignoring the higher-order term o(|Δx|) 2 ,get:

[0164]

[0165] Where W = diag(w1,...,w m ),

[0166] In the present invention, an iterative process is designed, and the iterative process is used to calculate formula (2) to obtain formula (3):

[0167] Δx=(J T W (k) J) -1 J T W (k) P (3)

[0168] Among them, W (q) is the diagonal matrix of the qth step, each element of which is expressed as:

[0169]

[0170] The vehicle motion estimation algorithm flow chart based on nonlinear weighted least squares method combines the vehicle motion parameterization in step 2 and the nonlinear weighted least squares principle to estimate the vehicle motion:

[0171]

[0172] Estimating the vehicle's motion information is to estimate the k q and k q+1 The rotation matrix between and translation vectors

[0173] The rotation matrix is ​​obtained by minimizing formula (6):

[0174]

[0175] in, and It is the deviation relative to the centroid coordinates of each feature point set.

[0176] Calculate the translation vector from formula (7):

[0177]

[0178] in, and They are and The centroid coordinates of .

[0179] The rotation matrix obtained in step 6 and translation vectors The value is optimized by iterative Sigma point filtering results algorithm to minimize and Error covariance matrix.

[0180] Kalman filtering is used to estimate the current state of the car, assuming that the measured value and the state value are linearly related. Standard Kalman filtering is a linear filter that uses the state space model of signal and noise to update the state variable at the current moment using the estimated value at the previous moment and the observed value at the current moment to obtain the optimal state value. Motion estimation in high-precision positioning of intelligent vehicles is a nonlinear estimation problem. Extended Kalman filtering provides a nonlinear solution, but this nonlinear process reduces the estimation accuracy. The present invention adopts an iterative Sigma point filtering result optimization algorithm to minimize the error covariance matrix of the vehicle motion state obtained in the above steps for further optimization. Iterative Sigma point filtering has a fast convergence speed and high estimation accuracy.

[0181] The rotation matrix obtained in step 6 and translation vectors The processing object is optimized based on iterative Sigma point filtering to obtain more accurate vehicle motion information. The Sigma point in the present invention is selected by the method shown in formula (8):

[0182]

[0183] Among them, γ is the scale parameter that determines the state space, s is the mean matrix, P is the vehicle motion state error covariance calculated in the above steps, and i is the index representation of the Sigma point. In the present invention, set

[0184] System state mean The variance cov(Z,Z) and covariance cov(Z,Z) are calculated using a linear weighted regression process:

[0185]

[0186] in, and are the weights of the mean sequence and variance sequence of the calculated states corresponding to each sampling point.

[0187]

[0188] Where K is the Kalman gain. P and R are the state and measurement error covariance matrices, respectively.

[0189] Step 7, based on the concept of graph theory, the rotation matrix and translation vector of the vehicle motion obtained in step 6 are represented to obtain a schematic diagram of the vehicle motion and complete the positioning of the vehicle.

[0190] Camera in k q to k q+1 The single-step motion matrix at time for

[0191]

[0192] From the initial time k0 to the current time k q The motion matrix M q+1 It can be calculated by accumulating the single-step motion matrix:

[0193] Assume that the camera position at the initial moment k0 is Current camera position It can be obtained by multiplying the camera position coordinates at the initial moment with the motion matrix, that is From k s to k q+1 The motion matrix at the moment can be calculated by the following formula:

[0194] Example 2: Algorithm verification in real environment

[0195] In order to illustrate the high accuracy of the visual positioning method in the present invention, an experimental verification was carried out in a real environment, and a high-precision DGPS was used for comparison. The model is NovAtel OEM6TM GNSS, and its dual antennas are respectively attached to the front and rear ends of the central axis of the top of the vehicle. The original video images collected by the binocular camera are transmitted to the Gigabit network switch through the network cable, and then transmitted from the Gigabit network switch to the server for data processing; while the DGPS data is directly transmitted to the server via the serial port. Fig. 9 The experimental verification results are shown, where VISO2 represents the positioning result obtained using the LIBVISO2 visual odometry method, and DGPS represents the high-precision vehicle motion trajectory obtained using differential GPS.

[0196] In such Fig. 9 The algorithm is verified in the complex campus environment shown in the figure. It can be seen that there are moving targets such as pedestrians and vehicles in the collected images, and they are also affected by shade from trees. Figure 2 The binocular camera stereo vision platform shown in the figure calculates the vehicle motion trajectory according to the execution order of the algorithm of the present invention. Analysis shows that in an environment with dense trees, the DGPS data drift phenomenon is more serious, the data point distribution is more discrete and deviates from the actual road trajectory, but the method of the present invention can well suppress the drift of the measurement results. Compared with the VISO2 method, the method of the present invention is closer to the actual motion trajectory of the vehicle, especially at turns, where the VISO2 method has a large cumulative error.

Claims

1. A vehicle visual positioning method, characterized in that: The following steps are involved: S1, using a calibrated binocular camera to collect photo images during the vehicle's motion, correcting the photo images using the internal and external parameters of the binocular camera to obtain a rotation matrix and a translation vector during the vehicle's motion; S2, parameterize the rotation matrix during the vehicle motion into the angle change, and parameterize the translation vector during the vehicle motion into the translation component; S3, preprocessing the original video image of the vehicle driving, extracting feature points in the preprocessed image using the AKAZE method and describing the feature points to obtain a feature vector set of the feature points of the preprocessed image; S4, the feature vector set of the image feature points obtained in step S3 is subjected to box filtering, feature point matching, feature matching ratio test and feature ring matching test to obtain a correctly matched feature point pair set; S5, using the RANSAC algorithm based on initial estimation to iterate the matched feature point pair set in step S4, removing the feature points on the target body in the original video image in step S3, and obtaining a correct matching point pair set; S6, using a nonlinear weighted least square method to calculate the rotation matrix and translation vector value of the vehicle motion for the correct matching point pair set in step S5; S7, based on the concept of graph theory, the rotation matrix and translation vector of the vehicle motion obtained in step S6 are represented by using the angle change and the translation component to obtain a schematic diagram of the vehicle motion, thereby completing the positioning of the vehicle; The weight function determined by the nonlinear weighted least squares method is: in, is the weight corresponding to the i-th observation value in the q-th step, is the error term ε i The covariance of By minimizing the formula and formula The rotation matrix can be obtained and translation vectors value, in q P i 3 =( q x i , q y i , q z i ) T , q+1 P i 3 =( q+1 x i , q+1 y i , q+1 z i ) T , and They are q P i 3 (i=1,2,...,n) and q+1 P i 3 The centroid coordinates of (i=1,2,...,n), q P i 3 and q+1 P i 3 They are k q and k q+1 The three-dimensional coordinates of the world coordinate system at the moment, and It is the deviation relative to the centroid coordinates of each feature point set.

2. A vehicle visual positioning method according to claim 1, characterized in that: The rotation matrix parameterized during the vehicle motion process is specifically: in, is the rotation matrix during vehicle motion, and the angle change is (θ x ,θ y ,θ z ), the rotation matrix roll angle θ x is the angle of rotation of the vehicle around the x-axis, the pitch angle θ y is the angle of rotation of the vehicle around the y-axis, the yaw angle θ z Vehicles around z The angle of rotation of the axis, is the translation vector of the vehicle during motion, t x ,t y ,t z The vehicle rotates around the x-axis, y-axis, z The translation component in the direction of the axis.

3. A vehicle visual positioning method according to claim 1, characterized in that: The initial estimate in the RANSAC algorithm based on initial estimate means that the geometric transformation relationship from image I(x,y) to another image I′(x′,y′) is determined. For the feature point P(x,y) in image I(x,y) and the feature point P′(x′,y′) in the image to be matched I′(x′,y′), the following relationship is satisfied: (1) The straight-line distance between P(x,y) and P′(x′,y′) equal, (2) The slope of the line formed by P(x,y) and P′(x′,y′) equal; The matching feature points that satisfy the above relationship are retained, otherwise the feature points are deleted, and the remaining matching points are used as the input values ​​of the RANSAC algorithm to remove the feature points on the target object.

4. A vehicle visual positioning method according to claim 1, characterized in that: Camera in k q to k q+1 The single-step motion matrix at time for From the initial time k0 to the current time k q The motion matrix M q+1 It can be calculated by accumulating the single-step motion matrix:

5. A vehicle visual positioning method according to claim 1, characterized in that: Box filtering: The preprocessed image is divided into multiple non-overlapping boxes of 50*50 pixels in size. A maximum of two feature points are retained in each box. After processing, an image to be matched containing uniformly sparse feature points is obtained. Feature point matching: The uniformly sparse feature points between different frame images to be matched are matched using a zero-mean normalized cross-correlation function as an evaluation criterion for the similarity of the two feature points. After this step, an image containing initial matching feature points is obtained.

6. A vehicle visual positioning method according to claim 5, characterized in that: The zero-mean normalized cross-correlation function is as follows: Where I(·) and I'(·) are the two frames of images to be matched, I(x,y) represents the pixel intensity value at (x,y) in image I(·), I'(x+u,y+v) represents the pixel intensity value at point (x,y)+(u,v) in image I'(·), u and v are the offsets in the x-axis and y-axis directions, respectively. and Respectively represent the mean pixel intensity within the window; The larger the ZMNCC(u,v) value is, the higher the similarity between the two feature points is, and the two feature points are retained. The two feature points with the largest ZMNCC(u,v) values ​​are the optimal matching points, followed by the suboptimal matching points.

7. A vehicle visual positioning system, characterized in that: It includes image acquisition and processing module, data processing module and result output module. The image acquisition processing module is used to correct the photo images collected by the calibrated binocular camera during the vehicle movement process through the internal and external parameters of the binocular camera, obtain the rotation matrix and translation vector of the vehicle during the vehicle movement process, and parameterize the rotation matrix of the vehicle during the vehicle movement process into the angle change, and parameterize the translation vector of the vehicle during the vehicle movement process into the translation component to express it in graph theory concepts; The data processing module is used to preprocess the original video image of the vehicle driving, extract the feature points in the preprocessed image by the AKAZE method and describe the feature points to obtain the feature vector set of the preprocessed image feature points, and then obtain the correctly matched feature point pair set by passing the obtained feature vector set of the image feature points through box filtering, feature point matching, feature matching ratio test and feature ring matching test, and then iterate the matched feature point pair set by using the RANSAC algorithm based on initial estimation to remove the feature points on the target body in the original video image to obtain the correctly matched point pair set; the nonlinear weighted least square method is used to calculate the rotation matrix and translation vector value of the vehicle motion for the correctly matched point pair set; The rotation matrix and translation vector of the vehicle motion are represented by the angle variation and the translation component, and the vehicle motion schematic diagram is obtained and outputted through the result output module; The weight function determined by the nonlinear weighted least squares method is: in, is the weight corresponding to the i-th observation value in the q-th step, is the error term ε i The covariance of By minimizing the formula and formula The rotation matrix can be obtained and translation vectors value, in q P i 3 =( q x i , q y i , q z i ) T , q+1 P i 3 =( q+1 x i , q+1 y i , q+1 z i ) T , and They are q P i 3 (i=1,2,...,n) and q+1 P i 3 The centroid coordinates of (i=1,2,...,n), q P i 3 and q+1 P i 3 They are k q and k q+1 The three-dimensional coordinates of the world coordinate system at the moment, and It is the deviation relative to the centroid coordinates of each feature point set.

8. A terminal 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 the method according to any one of claims 1 to 6 are implemented.

9. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the method according to any one of claims 1 to 6 are implemented.

Images