A real-time pose calculation method for a vehicle-mounted forward-facing camera

Abstract

The present invention provides a method for calculating the posture of a vehicle-mounted forward camera in real time. The method performs semantic segmentation on the image captured by the vehicle-mounted forward camera to perform preliminary classification on the image, judges the lane state in the current image, and calculates the camera posture when the lane line is a right-angle line or a curve if the left and right lane lines are detected. If the left and right lane lines cannot be detected, the camera posture at the current moment is calculated by screening out the correct optical flow points. Furthermore, the vehicle motion relationship between the two frames can be used to calculate the vehicle motion between the two frames, without the need to calculate the vehicle motion between the two frames with the help of vehicle body imu / GPS and other signals.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle-mounted camera trajectory lines, and in particular to a real-time calculation method for the position and posture of a vehicle-mounted forward-facing camera. Background Art

[0002] In the process of vehicle operation, the camera pose of the intelligent driving perception system will shift with the movement of the vehicle. If the offset of the camera is not accurately known, it will cause serious errors in the camera ranging. Therefore, it is crucial to calculate the camera pose in real time, that is, the process of accurate camera pose. The current common method is to calculate the camera pose by the vanishing point of the lane line, and the vanishing point method needs to rely entirely on the straight lane line scene. For example, if the lane line is blocked by other vehicles during driving, the vanishing point of the lane line cannot be calculated, and then the camera pose cannot be calculated. In addition, the vanishing point method cannot calculate the motion relationship between the vehicle and the two frames of images. It is necessary to use the vehicle body imu / GPS and other signals to calculate the vehicle's motion between the two frames of images. Summary of the invention

[0003] In response to the above problems, this patent proposes a real-time calculation method for the posture of a vehicle-mounted forward camera, which at least includes: This solution proposes a camera posture calculation solution that combines lane line vanishing points and optical flow tracking, expands the applicable scenarios for calculating camera posture, and improves the stability of the autonomous driving perception algorithm.

[0004] Specifically, a method for real-time calculation of the position and posture of a vehicle-mounted forward camera includes the following steps:

[0005] S1: Obtain an image captured by the vehicle's forward-facing camera, perform preliminary classification on the image through semantic segmentation, and determine the lane status in the current image;

[0006] S2: If the detected left and right lane lines are straight lines, determine whether they are straight lines; if they are straight lines, use the vanishing point method to calculate the pitch angle, and then calculate the camera pose; if they are curves, use the initial external parameters of rotation R and translation T to project the coordinates of the pixels on the curve with the same Y coordinate value to the world coordinate system, calculate the horizontal difference of the pixels on the left and right curves in the world coordinate system, and use it as the lane width value, and iteratively calculate the final pitch angle, and then calculate the camera pose;

[0007] S3: If the left or right lane lines cannot be detected, the optical flow points are adaptively selected to match the positions in the current image. The camera position at the current moment is calculated by screening out the correct optical flow points, and then the vehicle motion between the two frames is calculated based on the vehicle motion relationship between the two frames.

[0008] Wherein, the S1 further includes:

[0009] Set the lane area to 1, set the non-lane area to 0, set the grayscale value of any point on the lane to be higher than the grayscale value of the left and right lane lines, and use the binary calculation formula to determine the lane status in the current image:

[0010] ,

[0011] Where: is the pixel value of any point in the image, is the pixel value of the lane line on the left side of the current pixel, is the pixel value of the lane line to the right of the current pixel, The semantic segmentation result.

[0012] Furthermore, the step of determining whether the left and right lane lines are straight lines in S2 further includes:

[0013] The left lane line is marked as L1, and the right lane line is marked as L2. The curve equation is obtained by fitting each detected lane line using the least squares method:

[0014] ,

[0015] Where: is the average fitting coefficient, x is the vertical coordinate of the lane line pixel, and y is the horizontal coordinate of the lane line pixel;

[0016] Calculate the curvature of the fitting curve using the formula:

[0017]

[0018] If the curvature R ≤ threshold T, the current lane line is a straight line; otherwise, it is a curve.

[0019] Furthermore, the S2 also includes calculating the pitch angle using the vanishing point method: after perspective projection imaging of L1 and L2, the following is obtained:

[0020] ,

[0021] Calculate the image coordinates of the vanishing point V (X V , Y V );

[0022] The pitch angle is calculated as:

[0023] ,

[0024] Calculate the camera pose of the current straight lane line:

[0025] T=[pitch,yaw,roll,Tx,Ty,Tz],

[0026] Among them, pitch is the pitch angle, yaw is the deflection angle, and roll is the rotation angle. is the horizontal distance from the camera to the center of the world, is the vertical distance from the camera to the center of the world, is the installation height of the camera; is the optical center coordinate of the camera in the vertical direction, H is the image height, is the vertical field of view of the camera.

[0027] The S2 also includes: if the current lane line is a curve, all pixel points are taken from L1 and L2, and the points in L1 and L2 are recorded as: and ;

[0028] Find the coordinates with the same Y value, recorded as:

[0029] ,

[0030] Using the rotation R of the initial external parameter of the camera's installation angle and the translation matrix T of the camera's installation position coordinates, the coordinates with the same Y value are projected into the world coordinate system, and we get:

[0031] ,

[0032] ,

[0033] ,

[0034] In the formula, is the rotation matrix of the roll angle, is the rotation matrix of the yaw angle, is the rotation matrix of the pitch angle;

[0035] Then the rotation matrix is:

[0036] ;

[0037] The translation matrix is:

[0038] ;

[0039] Get the world coordinate point: W = R*I +T, optimize to get:

[0040] .

[0041] Furthermore, the horizontal difference of each set of points in the world coordinate system is used as the width between L1 and L2:

[0042] ;

[0043] The width difference between the two sets of points is:

[0044] ,

[0045] The extrinsic parameters are iteratively updated using the average value of the width and the partial derivative of the rotation R matrix. The extrinsic parameter update process only uses pitch as a variable, and iterates until d = 0 to obtain the final pitch angle value.

[0046] Calculate the camera height of the current curved lane line:

[0047]

[0048] in, is the current camera height, is the camera height at the previous moment, is the lane width at the current moment, is the lane width at the previous moment.

[0049] Wherein, the S3 further includes: the process of setting the optical flow point is:

[0050] Within 5m on the left and right sides of the front of the vehicle, a light flow point is set every 1m, with 10 light flow points in each row. Within 30m in front of the vehicle, a row of light flow points is set every 1m forward, for a total of 30 rows, and a total of 300 points are set as light flow points, recorded as: , mapped to the previous frame image, the optical flow image point at the previous moment is recorded as: ; Match out The position in the current image is recorded as: , the matching process is:

[0051] First establish the motion relationship:

[0052] ;

[0053] ;

[0054] In the above formula, is the window weight function: .

[0055] Furthermore, the S3 also includes screening out correct optical flow points:

[0056] Will Use the rotation matrix R and translation matrix T of the previous moment to map to the current moment, recorded as:

[0057] ,

[0058] ,

[0059] Combine the vehicle speed information to estimate the movement amount at the previous and next moments: in, is the estimated amount of motion, v is the vehicle speed, is the time interval between the upper and lower images;

[0060] when and Points with an error of more than 0.5 m are considered to be mismatched points.

[0061] Furthermore, the S3 also includes calculating the camera pose at the current moment: using the filtered optical flow matching points at the current moment and the world coordinate point at the previous moment , solve the camera pose: .

[0062] Furthermore, the S3 also includes calculating the vehicle motion between two frames of images: according to the translation matrix of the camera position , given the camera position at the previous moment ,but It is the lateral and longitudinal movement of the vehicle between two frames of images.

[0063] In summary, the present invention provides a method for calculating the posture of a vehicle-mounted forward camera in real time. The method performs semantic segmentation on the image captured by the vehicle-mounted forward camera to perform preliminary classification on the image, and determines the lane status in the current image. If the left and right lane lines are detected, the camera posture is calculated when the lane lines are right-angled lines or curves. If the left and right lane lines cannot be detected, the camera posture at the current moment is calculated by screening out the correct optical flow points. Furthermore, the vehicle motion relationship between the two frames can be used to calculate the vehicle motion between the two frames, without the need to calculate the vehicle motion between the two frames with the help of vehicle body imu / GPS and other signals. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 This is a schematic diagram of the real-time calculation method of the position and posture of the vehicle-mounted forward camera described in the present invention. DETAILED DESCRIPTION

[0065] In order to enable those skilled in the art to better understand the scheme of the present invention, the technical scheme in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the 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 creative work are within the scope of protection of the present invention.

[0066] like Figure 1 As shown, the real-time calculation method of the posture of the vehicle-mounted forward camera proposed by the present invention can be used to track and calibrate the posture of the camera in the case of two straight lane lines, or in the case of two curved lane lines, or in the case of a single lane line or no lane line, and specifically includes the following steps:

[0067] S1: Obtain an image captured by the vehicle's forward-facing camera, perform preliminary classification on the image through semantic segmentation, and determine the lane status in the current image;

[0068] Specific:

[0069] Set the lane area to 1, set the non-lane area to 0, set the grayscale value of any point on the lane to be higher than the grayscale value of the left and right lane lines, and use the binary calculation formula to determine the lane status in the current image:

[0070] ,

[0071] Where: is the pixel value of any point in the image, is the pixel value of the lane line on the left side of the current pixel, is the pixel value of the lane line to the right of the current pixel, The semantic segmentation result.

[0072] S2: If the detected left and right lane lines are straight lines, determine whether they are straight lines; if they are straight lines, use the vanishing point method to calculate the pitch angle, and then calculate the camera pose; if they are curves, use the initial external parameters of rotation R and translation T to project the coordinates of the pixels on the curve with the same Y coordinate value to the world coordinate system, calculate the horizontal difference of the pixels on the left and right curves in the world coordinate system, and use it as the lane width value, and iteratively calculate the final pitch angle, and then calculate the camera pose;

[0073] Specific:

[0074] Generally, the two lane lines closest to the vehicle body are selected to calculate the pitch angle. The left lane line is preferably marked as L1 and the right lane line is marked as L2. First, determine whether the current lane line is a straight line. The judgment basis is the curvature radius. The calculation process is:

[0075] The curve equation is obtained by fitting each detected lane line using the least squares method:

[0076] ,

[0077] Where: is the average fitting coefficient, x is the vertical coordinate of the lane line pixel, and y is the horizontal coordinate of the lane line pixel;

[0078] Calculate the curvature of the fitting curve using the formula:

[0079]

[0080] If the curvature R ≤ threshold T, the current lane line is a straight line; otherwise, it is a curve.

[0081] Furthermore, if it is determined to be a straight line, the vanishing point method is directly used to calculate the pitch angle. The definition of the vanishing point is that after two parallel straight lines are imaged in the image through perspective projection, the intersection point in the image is the vanishing point. Specifically, the vanishing point method is used to calculate the pitch angle: After the L1 and L2 are imaged through perspective projection, the following is obtained:

[0082] ,

[0083] Calculate the image coordinates (XV, YV) of the vanishing point V;

[0084] The pitch angle is calculated as:

[0085] ,

[0086] Calculate the camera pose of the current straight lane line:

[0087] T=[pitch,yaw,roll,Tx,Ty,Tz],

[0088] Among them, pitch is the pitch angle, yaw is the deflection angle, and roll is the rotation angle. is the horizontal distance from the camera to the center of the world, is the vertical distance from the camera to the center of the world, is the installation height of the camera; is the optical center coordinate of the camera in the vertical direction, H is the image height, is the vertical field of view of the camera.

[0089] If it is determined that the two lane lines are not straight lines, the pitch angle calculation process is based on the fact that the width of the lane lines at different positions in the world coordinate system is the same. Then all the pixels are taken from L1 and L2, and the points in L1 and L2 are recorded as: and ;

[0090] Find the coordinates with the same Y value, recorded as:

[0091] ,

[0092] Using the rotation R of the initial external parameter of the camera's installation angle and the translation matrix T of the camera's installation position coordinates, the coordinates with the same Y value are projected into the world coordinate system, and we get:

[0093] ,

[0094] ,

[0095] ,

[0096] In the formula, is the rotation matrix of the roll angle, is the rotation matrix of the yaw angle, is the rotation matrix of the pitch angle;

[0097] Then the rotation matrix is:

[0098] ;

[0099] The translation matrix is:

[0100] ;

[0101] Get the world coordinate point: W = R*I +T, optimize to get:

[0102] .

[0103] Furthermore, the horizontal difference of each set of points in the world coordinate system is used as the width between L1 and L2:

[0104] ;

[0105] The width difference between the two sets of points is:

[0106] ,

[0107] The extrinsic parameters are iteratively updated using the average value of the width and the partial derivative of the rotation R matrix. The extrinsic parameter update process only uses pitch as a variable, and iterates until d = 0 to obtain the final pitch angle value.

[0108] Calculate the camera height of the current curved lane line:

[0109]

[0110] in, is the current camera height, is the camera height at the previous moment, is the lane width at the current moment, is the lane width at the previous moment.

[0111] S3: If the left or right lane lines cannot be detected, the optical flow points are adaptively selected to match the positions in the current image. The camera position at the current moment is calculated by screening out the correct optical flow points, and then the vehicle motion between the two frames is calculated based on the vehicle motion relationship between the two frames.

[0112] Specific:

[0113] The process of setting the optical flow points is as follows: within the 5m range on the left and right sides of the front of the vehicle, set an optical flow point every 1m, with 10 optical flow points in each row; within the 30m range in front of the vehicle, set a row of optical flow points every 1m forward, for a total of 30 rows, and a total of 300 points are set as optical flow points, recorded as: , mapped to the previous frame image, the optical flow image point at the previous moment is recorded as: ; Match out The position in the current image is recorded as: , the matching process is:

[0114] First establish the motion relationship:

[0115] ;

[0116] ;

[0117] In the above formula, is the window weight function: .

[0118] Furthermore, the S3 also includes screening out correct optical flow points:

[0119] Will Use the rotation matrix R and translation matrix T of the previous moment to map to the current moment, recorded as:

[0120] ,

[0121] ,

[0122] Combine the vehicle speed information to estimate the movement amount at the previous and next moments: in, is the estimated amount of motion, v is the vehicle speed, is the time interval between the upper and lower images;

[0123] when and Points with an error of more than 0.5 m are considered to be mismatched points.

[0124] Furthermore, the S3 also includes calculating the camera pose at the current moment: using the filtered optical flow matching points at the current moment and the world coordinate point at the previous moment , solve the camera pose: .

[0125] Furthermore, the S3 also includes calculating the vehicle motion between two frames of images: according to the translation matrix of the camera position , given the camera position at the previous moment ,but It is the lateral and longitudinal movement of the vehicle between two frames of images.

[0126] The above-mentioned embodiments only express several implementation methods of the present invention, and the description thereof is relatively specific and detailed, but it cannot be understood as limiting the scope of the patent of the present invention. It should be pointed out that, for ordinary technicians in this field, several variations and improvements can be made without departing from the concept of the present invention, which all belong to the protection scope of the present invention. Therefore, the protection scope of the patent of the present invention shall be subject to the attached claims.

Claims

1. A method for real-time calculation of the position and posture of a vehicle-mounted forward camera, characterized in that: The following steps are involved: S1: Obtain an image captured by the vehicle's forward-facing camera, perform preliminary classification on the image through semantic segmentation, and determine the lane status in the current image; S2: If the detected left and right lane lines are straight lines, determine whether the left and right lane lines are straight lines; if they are straight lines, calculate the pitch angle using the vanishing point method, and then calculate the camera posture; If it is a curve, the initial external parameters of rotation R and translation T are used to project the coordinates of the pixels with the same Y coordinate value on the curve to the world coordinate system, and the horizontal difference between the pixels of the left and right curves in the world coordinate system is calculated and used as the lane width. The final pitch angle is calculated through iteration, and then the camera pose is calculated; S3: If the left and right lane lines cannot be detected, the optical flow points are adaptively selected to match the positions in the current image. The camera position at the current moment is calculated by selecting the correct optical flow points, and then the vehicle motion between the two frames is calculated based on the vehicle motion relationship between the two frames. Among them, the left lane line is marked as L1, and the right lane line is marked as L2; The final pitch angle is calculated iteratively, and then the camera pose is calculated, specifically: The vanishing point method is used to calculate the pitch angle: the image coordinates of the vanishing point V are calculated after the perspective projection of L1 and L2; according to the image coordinates of the vanishing point V (X V , Y V ) calculates the pitch angle pitch, and calculates the camera pose of the current straight lane line based on the pitch angle pitch; The S3 also includes screening out the correct optical flow points: Will [I x_flow1 I y_flow1 ] Use the rotation matrix R and translation matrix T of the previous moment to map to the current moment, recorded as: [IN x_flow1 IN y_flow1 IN z_flow1 ], Combine the vehicle speed information to estimate the movement amount at the upper and lower moments: dist_p = v*dt, where dist_p is the estimated movement amount, v is the vehicle speed, and dt is the time interval between the upper and lower moments; When the error between dist and dist_p exceeds 0.5m, the point is considered as a mismatched point; The S3 also includes calculating the camera position at the current moment: using the filtered optical flow matching points at the current moment [I x_flow1 , I y_flow1 ] and the world coordinate point [W x_flow0 , W y_flow0 , W z_flow0 ], solve the camera pose: W T =K*R*M T +T.

2. The method for real-time calculation of the position and posture of a vehicle-mounted forward camera according to claim 1, characterized in that: The S1 further comprises: Set the lane area to 1, set the non-lane area to 0, set the grayscale value of any point on the lane to be higher than the grayscale value of the left and right lane lines, and use the binary calculation formula to determine the lane status in the current image: Where: I is the pixel value of any point in the image, I l is the pixel value of the lane line on the left side of the current pixel, I r is the pixel value of the lane line to the right of the current pixel, I s The semantic segmentation result.

3. The method for real-time calculation of the position and posture of a vehicle-mounted forward camera according to claim 1, characterized in that: The step S2 of determining whether the left and right lane lines are straight lines further includes: The curve equation is obtained by fitting each detected lane line using the least squares method: y=a0+a1x+a2x 2 +a3x 3 , Where: a0, a1, a2, a3 are the average fitting coefficients, x is the vertical coordinate of the lane line pixel, and y is the horizontal coordinate of the lane line pixel; Calculate the curvature of the fitting curve using the formula: If the curvature R ≤ threshold T, the current lane line is a straight line; otherwise, it is a curve.

4. The method for real-time calculation of the position and posture of a vehicle-mounted forward camera according to claim 3, characterized in that: The S2 also includes calculating the pitch angle using the vanishing point method: after perspective projection imaging of L1 and L2, we get: the L1 =a L1 + b L1 x0 and L2 =a L2 +b L2 x1, Calculate the image coordinates of the vanishing point V (X V , Y V ); The pitch angle is calculated as: Calculate the camera pose of the current straight lane line: T=[pitch,yaw,roll,Tx,Ty,Tz], Where pitch is the pitch angle, yaw is the deflection angle, roll is the rotation angle, Tx is the horizontal distance between the camera and the world center, Ty is the vertical distance between the camera and the world center, and Tz is the installation height of the camera; C y is the optical center coordinate of the camera in the vertical direction, H is the image height, V FoV is the vertical field of view of the camera.

5. The method for real-time calculation of the position and posture of a vehicle-mounted forward camera according to claim 4, characterized in that: The S2 also includes: if the current lane line is a curve, all pixel points are taken from L1 and L2, and the points in L1 and L2 are recorded as: (X L10 , Y L10 ), (X L11 , Y L11 ),…(X L1m ,Y L1m ) and (X L20 , Y L20 ), (X L21 , Y L21 ),…(X L2n , Y L2n ); Find the coordinates with the same Y value, recorded as: Using the rotation R of the initial external parameter of the camera's installation angle and the translation matrix T of the camera's installation position coordinates, the coordinates with the same Y value are projected into the world coordinate system, and we get: Where R0 is the rotation matrix of the roll angle, R1 is the rotation matrix of the yaw angle, and R2 is the rotation matrix of the pitch angle; Then the rotation matrix is: R = R0*R1*R2; The translation matrix is: T=[T x T y T z ] T ; Get the world coordinate point: W = R * I + T, optimize to get: In=[In x IN y IN z ]。 6. The method for real-time calculation of the position and posture of a vehicle-mounted forward camera according to claim 5, characterized in that: Also includes: The horizontal difference in the world coordinate system of each set of points is used as the width between L1 and L2: lane_width=|W xl1 -W xl2 |; The width difference between the two sets of points is: d=|lane_width1-lane_width0|, The extrinsic parameters are iteratively updated using the average value of the width and the partial derivative of the rotation R matrix. The extrinsic parameter update process only uses pitch as a variable, and iterates until d = 0 to obtain the final pitch angle value. Calculate the camera height of the current curved lane line: Among them, T z_f1 is the camera height at the current moment, T z_f0 is the camera height at the previous moment, lane_width_f1 is the lane width at the current moment, and lane_width_f0 is the lane line width at the previous moment.

7. The method for real-time calculation of the position and posture of a vehicle-mounted forward camera according to claim 1, characterized in that: The step S3 also includes: the process of setting the optical flow point is: Within 5m on the left and right sides of the front of the vehicle, a light flow point is set every 1m, and each row has 10 light flow points. Within 30m forward of the vehicle, a row of light flow points is set every 1m forward, for a total of 30 rows, and a total of 300 points are set as light flow points, recorded as: [W x_flow0 , W y_flow0 ], mapped to the previous frame image, the optical flow image point at the previous moment is recorded as: [I x_flow0 , I y_flow0 ]; Matches [I x_flow0 , I y_flow0 ] in the current image, denoted as: [Ix flow1 I yflow1 ], the matching process is: First establish the motion relationship: I(x,y,t)=I(x+dx,y+dt,t+dt); In the above formula, W 2 (x) is the window weight function: V = (A T W 2 A) -1 A T W 2 b.

8. The method and system for real-time calculation of the position and posture of a vehicle-mounted forward camera according to claim 7, characterized in that: The S3 also includes calculating the vehicle motion between two frames of images: according to the translation matrix T1 of the camera position = [T x1 T y1 T z1 ] T , it is known that the camera position at the last moment T0 = [T x0 T y0 T z0 ] T , then T x1 -T x0 and T y1 -T y0 It is the lateral and longitudinal movement of the vehicle between two frames of images.

Images