An online estimation method for extrinsic parameters of vehicle-mounted cameras in natural scenes

Abstract

The present invention discloses an online estimation method for extrinsic parameters of a vehicle-mounted camera in a natural scene. The method comprises: firstly selecting two images as key frames, calculating the trifocal tensor based on the image taken by the camera at the current moment and the two key frame images and constructing an auxiliary tensor; standardizing the two tensors and using a reduced-order observer to estimate their derivatives online; calculating the camera angular velocity using the vehicle angular velocity, the standardized auxiliary tensor and its derivatives; designing a camera rotation extrinsic detector and estimating the camera rotation extrinsic parameters online; designing a camera translation extrinsic detector and estimating the camera translation extrinsic parameters online; using a key frame strategy to timely select a new key frame to replace the old key frame and switch until the camera extrinsic parameter estimation value converges to obtain the final extrinsic parameter. The present invention eliminates the reliance of the existing methods on artificial visual features and planar reference objects, and overcomes the constraints of the limited camera field of view on the vehicle workspace.

Description

Technical Field

[0001] The present invention belongs to the intersection field of computer vision and automation and is a method for estimating extrinsic parameters of a vehicle-mounted camera, and in particular relates to an online method for estimating extrinsic parameters of a vehicle-mounted camera in a natural scene. Background Art

[0002] The smart car obtains visual information of the environment through the camera and represents it in the camera's own coordinate system. However, subsequent modules such as path planning and trajectory tracking need to use external environment information represented in the vehicle coordinate system. The relative position of the camera coordinate system with respect to the vehicle coordinate system is the external parameter of the camera relative to the vehicle.

[0003] In vision-based tracking and stabilization problems, adaptive controllers can achieve online compensation for the influence of unknown camera extrinsics by offsetting the terms associated with unknown extrinsics in the time derivative of the Lyapunov function of the control task (J. Chen, B. Jia, and K. Zhang, “Trifocal tensor-based adaptive visual trajectory tracking control of mobile robots,” IEEE Transactions on Cybernetics, vol. 47, no. 11, pp. 3784–3798, Nov. 2017), and ultimately achieve the established control task. However, it does not obtain the true value of the camera extrinsics. When the task or scene changes, it is still necessary to re-analyze and process the influence of extrinsics.

[0004] Although there are many offline calibration methods to obtain camera external parameters, most of the related work relies on special artificial visual features, such as checkerboards or Aruco Markers, etc.; another part of the work requires the camera to only be able to move in a specific mode, such as circular motion, linear motion, or a combination of multiple motion modes (G. Yu, J. Chen, K. Zhang, and X. Zhang, "Camera external self-calibration for intelligent vehicles," IEEE 28th International Symposium on Industrial Electronics (ISIE), Vancouver, BC, Canada, 12–14 June 2019, pp. 1688-1693). In addition, most of these methods also require special sites or special technicians, which are too complicated for a smart car that needs to run outdoors for a long time and whose camera external parameters may change frequently due to bumps or even collisions. Although there are some online estimation methods for camera extrinsic parameters that do not rely on artificial visual features, most of these methods are based on homography technology (Kaixiang Zhang, Jian Chen, Guoqing Yu, Xinfang Zhang, and Zhaojian Li, "Visual Trajectory Tracking of Wheeled Mobile Robots With Uncalibrated Camera Extrinsic Parameters," IEEE Transactions on Systems, Man, and Cybernetics: Systems, vol. 51, no. 11, Nov. 2021), which requires the presence of static reference objects with planar features in the scene (such as traffic signs, regular building facades, etc.). However, such objects do not exist in many actual scenes, and real-time perception of them also consumes a lot of computing resources.

[0005] In summary, the existing work on unknown camera extrinsic parameters still has some shortcomings, so this work will consider an online estimation method for vehicle-mounted camera extrinsic parameters in natural scenes. Summary of the invention

[0006] In order to estimate the extrinsic parameters of a vehicle-mounted camera in a natural scene, the present invention proposes an online estimation method for the extrinsic parameters of a vehicle-mounted camera in a natural scene. The present invention uses a trifocal tensor to construct the relative pose relationship between multiple frames of images, decouples the rotational extrinsic parameters and the translational extrinsic parameters of the camera by introducing an auxiliary tensor, and uses a reduced-order observer to estimate the tensor derivatives after standardization online. Based on the above information, the angular velocity information of the camera is restored in real time, and the SO(3) observer is designed in combination with the angular velocity information of the vehicle to realize the online estimation of the rotational extrinsic parameters of the camera. Based on the estimated value of the rotational extrinsic parameters and the linear velocity information of the vehicle, a nonlinear observer is further designed to realize the online estimation of the translational extrinsic parameters of the camera.

[0007] The technical solution of the present invention is:

[0008] The method comprises the following steps:

[0009] 1) Select two frames of images with the same feature points taken by the vehicle-mounted camera as two key frame images, and preset the initial camera rotation and translation extrinsic parameter estimation values;

[0010] 2) The relative rotation matrix of the current two key frame images is obtained by using the epipolar geometry constraint. The on-board camera takes a frame image as the current frame image, and then the initial trifocal tensor is solved by using the current frame image and the two key frame images. Then, the initial auxiliary tensor is constructed according to the current initial trifocal tensor and the relative rotation matrix of the current two key frame images. Then, the current initial trifocal tensor and the initial auxiliary tensor are standardized respectively to obtain the standardized trifocal tensor and the standardized auxiliary tensor.

[0011] 3) constructing a reduced-order observer according to the current standardized trifocal tensor and the standardized auxiliary tensor, and using the reduced-order observer to observe the derivatives of the current standardized trifocal tensor and the standardized auxiliary tensor online to obtain the derivatives of the current standardized trifocal tensor and the standardized auxiliary tensor;

[0012] 4) The camera angular velocity is restored by using the geometric relationship between the vehicle angular velocity measured by the on-board velocity sensor, the current normalized auxiliary tensor, and the vector cross product of the normalized auxiliary tensor derivative;

[0013] 5) Estimate the actual angular velocity of the vehicle based on the recovered camera angular velocity and the current camera rotation extrinsic parameter estimation value and obtain the vehicle angular velocity estimation value; then construct a camera rotation extrinsic detector using the vehicle actual angular velocity at the historical moment and the vehicle angular velocity estimation value, and use the camera rotation extrinsic detector to continuously perform online correction on the camera rotation extrinsic parameter estimation value;

[0014] 6) constructing a camera translation external parameter detector using the actual linear velocity and angular velocity of the vehicle at the historical moment, the latest camera rotation extrinsic parameter estimation value in step 5), the camera angular velocity restored in step 4), the current standardized trifocal tensor, and the derivative of the standardized trifocal tensor, and using the camera translation external parameter detector to continuously perform online correction on the camera translation extrinsic parameter estimation value;

[0015] 7) extracting feature points of the current frame image and the two key frame images respectively, performing feature point matching on the feature points of the three images to obtain common feature points, calculating the ratio of the number of common feature points to the number of feature points in each of the three images and recording them as corresponding common matching ratios, and comparing the minimum common matching ratio with a preset matching threshold;

[0016] 8) When the minimum common matching ratio is greater than the preset matching threshold, repeat steps 2)-7); otherwise, replace the earliest key frame image of the two key frame images with the current frame image and update it to two new key frame images, and then repeat steps 2)-7); until the camera rotation and translation extrinsic parameter estimation values ​​tend to be stable.

[0017] In step 2), the initial auxiliary tensor is constructed according to the current initial trifocal tensor and the relative rotation matrix of the current two key frame images. The specific calculation formula is as follows:

[0018]

[0019] Among them, Q λ(j) refers to the initial auxiliary tensor Q λ The j-th matrix, T λ(j) is the initial trifocal tensor T λ The j-th matrix of is the relative rotation matrix The jth column of ; [·] × Represents a skew-symmetric matrix operation for finding a three-dimensional vector; j = 1, 2, 3.

[0020] In step 2), the current initial trifocal tensor and the initial auxiliary tensor are standardized respectively, and the specific calculation formula is as follows:

[0021]

[0022]

[0023]

[0024] Among them, Q λ represents the initial auxiliary tensor, T λ represents the initial trifocal tensor, Q represents the normalized auxiliary tensor, T represents the normalized trifocal tensor, λ represents the scale factor, Q λ(jkl)refers to the initial auxiliary tensor Q λ The j-th matrix Q λ(j) For the element in the lth column and kth row, j=1,2,3, k=1,2,3, l=1,2,3.

[0025] The formula of the reduced-order observer in step 3) is as follows:

[0026]

[0027]

[0028]

[0029]

[0030]

[0031]

[0032] in, represents the derivative of the normalized trifocal tensor T, is the normalized trifocal tensor derivative The estimated value of represents the derivative of the normalized auxiliary tensor Q, is the normalized auxiliary tensor derivative The estimated value of T With λ Q are the normalized triple focal tensor observation gain and the normalized auxiliary tensor observation gain, ξ T With ξ Q They are the standardized triple focal tensor observed auxiliary variables and the standardized auxiliary tensor observed auxiliary variables.

[0033] The step 4) is specifically as follows:

[0034] 4.1) Calculate the temporary rotation transformation matrix corresponding to the 9 column vectors contained in the standardized auxiliary tensor based on the current standardized auxiliary tensor and the standardized auxiliary tensor derivative The specific calculation is as follows:

[0035]

[0036] Among them, "×" represents the cross product operation of the vector, ||Q (jl) || represents the lth column vector Q of the jth matrix of the normalized auxiliary tensor Q (jl) The Euclidean norm of , Represents the derivative of the lth column vector of the jth matrix of the normalized auxiliary tensor Q The Euclidean norm of ;

[0037] 4.2) According to the geometric relationship between the vehicle angular velocity measured by the on-board velocity sensor, the current standardized auxiliary tensor and the vector cross product of the standardized auxiliary tensor derivative, the sine value and two cosine values ​​corresponding to the 9 angles between the restored camera angular velocity and the 9 column vectors contained in the standardized auxiliary tensor are calculated. The calculation formula for the sine value and cosine value corresponding to each angle θ is as follows:

[0038]

[0039]

[0040] Among them, ω v Indicates the vehicle angular velocity measured by the on-board speed sensor;

[0041] 4.3) According to the restored camera angular velocity and the 9 column vectors contained in the standardized auxiliary tensor, the corresponding sine value and two cosine values ​​and the corresponding temporary rotation transformation matrix The pre-selected camera angular velocity set corresponding to the 9 angles is calculated using the following formula:

[0042]

[0043] Among them, ω c is the camera angular velocity;

[0044] 4.4) Combine the camera angular velocity sets corresponding to the 9 angles to form 2 9 Combined camera angular velocity sets are obtained, and the geometric center of each combined camera angular velocity set is calculated. Then, the distance from each camera angular velocity in each combined camera angular velocity set to the geometric center of the current combined camera angular velocity set is calculated and the distances are summed. The geometric center of the combined camera angular velocity vector set with the smallest distance sum is taken as the recovered camera angular velocity.

[0045] The formula of the camera rotation extrinsic parameter estimation observer in step 5) is as follows:

[0046]

[0047]

[0048]

[0049] in, represents the estimated value of the camera rotation external parameter, ω represents the angular velocity of the camera rotation external parameter correction, ω v (i) represents the actual angular velocity of the vehicle measured at the i-th moment, ω c (i) represents the camera angular velocity recovered at the i-th moment, n ≥ 1, kω Rotate the exterior viewing gain for the camera.

[0050] The step 6) is specifically as follows:

[0051] 6.1) Calculate the camera translation extrinsic parameter estimation auxiliary variable s using the latest camera rotation extrinsic parameter estimation value in step 5), the camera angular velocity recovered in step 4), the current standardized trifocal tensor, and the derivative of the standardized trifocal tensor. The calculation formula is as follows:

[0052]

[0053] in, Represents the relative rotation matrix The element in row l and column j, represents the estimated value of the camera rotation extrinsic parameter, T (jl) represents the lth column vector of the jth matrix of the normalized trifocal tensor T, ω c Indicates the recovered camera angular velocity;

[0054] 6.2) Based on the camera translation extrinsic auxiliary variables and the actual angular velocity of the vehicle before and at the current moment, the motion excitation matrix A at each moment is constructed. Based on the camera translation extrinsic auxiliary variables and the actual linear velocity of the vehicle, the motion excitation vector h at each moment is constructed. The calculation formula is as follows:

[0055]

[0056]

[0057] Among them, s (1) ,s (2) ,s (3) They represent the first, second, and third components of the auxiliary variable s for camera translation extrinsic estimation, ω v(1) ,ω v(2) ,ω v(3) They represent the actual angular velocity of the vehicle ω v The first, second, and third elements of v(1) , v v(2) , v v(3) Represent the actual linear speed of the vehicle v v the 1st, 2nd, and 3rd elements of ;

[0058] 6.3) According to the motion excitation matrix A and motion excitation vector h at each moment, the camera translation and external detection device is constructed, and the formula is as follows:

[0059]

[0060] Where A(i) represents the motion excitation matrix A at the i-th moment, h(i) represents the motion excitation vector h at the i-th moment, 1≤i≤n, represents the camera translation extrinsic parameter estimation value, K represents the translation extrinsic parameter estimation gain matrix, and T represents the matrix transpose;

[0061] The camera translation extrinsic parameter estimation is continuously corrected online using the camera translation extrinsic detector.

[0062] The beneficial effects of the present invention are:

[0063] The present invention uses trifocal tensors to perform visual and geometric modeling of feature points in the scene, eliminating the reliance on artificial visual features or plane scene assumptions, and can directly extract visual feature points from natural scenes, avoiding the reliance of traditional camera extrinsic calibration methods on fixed sites, special equipment, and professionals. It also avoids the reliance of homography-based online calibration methods on plane reference objects, greatly improving the flexibility of extrinsic parameter estimation. In addition, the present invention also introduces a key frame strategy to continuously select and update key frames, overcoming the constraints of the camera's limited field of view, and vehicles equipped with cameras can travel continuously in a large space without paying attention to specific reference objects. At the same time, the extrinsic parameter estimation values ​​obtained by the present invention are more accurate than those of the extended Kalman filter. BRIEF DESCRIPTION OF THE DRAWINGS

[0064] Figure 1 It is a schematic diagram of the relative relationship between the vehicle and camera coordinate systems.

[0065] Figure 2 It is a schematic diagram of constructing the trifocal tensor using three views.

[0066] Figure 3 It is a schematic diagram of the geometric relationship between the normalized trifocal tensor and the vector cross product of the camera angular velocity.

[0067] Figure 4 It is the result of estimating the normalized trifocal tensor derivative and the normalized auxiliary tensor derivative using a reduced-order observer.

[0068] Figure 5 It is the result of online recovery of the camera angular velocity.

[0069] Figure 6 It is the online recovery result of the camera external parameters.

[0070] Figure 7 It is the comparison result between the recovery accuracy of camera extrinsic parameters and the extended Kalman filter.

[0071] Figure 8 It is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0072] The present invention will be further described below in conjunction with the embodiments and drawings.

[0073] The vehicle is equipped with a speed sensor, which is an inertial measurement unit or an integrated navigation system, used to measure the speed of the three axes expressed in its own coordinate system. With three-axis angular velocity

[0074] The vehicle coordinate system coincides with the speed sensor coordinate system, both denoted as F v , are right-handed coordinate systems;

[0075] The coordinate system of the vehicle-mounted camera takes the optical center of the camera as its origin and is denoted by F c , is a right-handed coordinate system;

[0076] The on-board camera is rigidly connected to the vehicle. The on-board camera has 6 external parameters relative to the vehicle, all of which are unknown. They are the first to third translation external parameters and the first-third rotation extrinsic parameters Where a, b, and c are the position components of the optical center of the vehicle camera in the vehicle coordinate system along the x, y, and z axes of the vehicle coordinate system, respectively. They are the roll angle (roll), pitch angle (pitch) and yaw angle (yaw) of the onboard camera relative to the vehicle coordinate system. Based on these six parameters, the relative position between the onboard camera and the vehicle coordinate system can be expressed as a translation vector and the rotation matrix

[0077]

[0078] Among them, SO(3) represents the special orthogonal group, S r , S p , S y , C r , C p and C y They are the first to sixth rotation external parameter auxiliary variables respectively.

[0079] like Figure 8 As shown, the present invention comprises the following steps:

[0080] 1) Select two frames of images with the same feature points taken by the vehicle-mounted camera as two key frame images, and preset the initial camera rotation and translation extrinsic parameter estimation values;

[0081] 2) Use epipolar geometry constraints to obtain the relative rotation matrix of the current two key frame images SO(3) represents a special orthogonal group. The vehicle-mounted camera takes a frame of image as the current frame image. In the specific implementation, the moment when the vehicle-mounted camera takes the image for the first time in step 2) is taken as the initial moment. The relationship between the current frame image and the two key frame images is as follows: Figure 1 As shown, the initial trifocal tensor is then solved using the current frame image and two key frame images, as Figure 2 As shown, taking a point in space as an example, it is in the current two key frame images F1, F2 and the current frame image F c The normalized Euclidean coordinates in are denoted as m1, m2 and m c , the initial trifocal tensor can be calculated based on the following relationship

[0082]

[0083] in, It means T λ The j-th matrix of It means based on the vector m c The constructed skew-symmetric matrix is It refers to the skew-symmetric matrix constructed based on the vector m2. refers to the jth element of vector m2, refers to a zero matrix of dimension 3×3;

[0084] Then, the initial auxiliary tensor is constructed according to the current initial trifocal tensor and the relative rotation matrix of the current two key frame images, and then the current initial trifocal tensor and the initial auxiliary tensor are standardized (i.e., normalized) respectively to obtain a standardized trifocal tensor and a standardized auxiliary tensor;

[0085] In step 2), the initial auxiliary tensor is constructed according to the current initial trifocal tensor and the relative rotation matrix of the current two key frame images. The specific calculation formula is as follows:

[0086]

[0087] Among them, Q λ(j) refers to the initial auxiliary tensor Q λ The j-th matrix, T λ(j) is the initial trifocal tensor T λ The j-th matrix of is the relative rotation matrix The jth column of ; [·] × Represents the skew-symmetric matrix operation for finding a three-dimensional vector; j=1, 2, 3.

[0088] In step 2), the current initial trifocal tensor and the initial auxiliary tensor are standardized respectively, and the specific calculation formula is as follows:

[0089]

[0090]

[0091]

[0092] Among them, Q λ represents the initial auxiliary tensor, T λ represents the initial trifocal tensor, Q represents the normalized auxiliary tensor, T represents the normalized trifocal tensor, λ represents the scale factor, Q λ(jkl) refers to the initial auxiliary tensor Q λ The j-th matrix Q λ(j) For the element in the lth column and kth row, j=1,2,3, k=1,2,3, l=1,2,3.

[0093] 3) constructing a reduced-order observer according to the current standardized trifocal tensor and the standardized auxiliary tensor, and using the reduced-order observer to observe the derivatives of the current standardized trifocal tensor and the standardized auxiliary tensor online to obtain the derivatives of the current standardized trifocal tensor and the standardized auxiliary tensor;

[0094] The formula of the reduced-order observer in step 3) is as follows:

[0095]

[0096]

[0097]

[0098]

[0099]

[0100]

[0101] in, represents the derivative of the normalized trifocal tensor T, is the normalized trifocal tensor derivative The estimated value of represents the derivative of the normalized auxiliary tensor Q, is the normalized auxiliary tensor derivative The estimated value of T With λ Q are the normalized triple focal tensor observation gain and the normalized auxiliary tensor observation gain, ξ T With ξ Q They are the standardized triple focal tensor observation auxiliary variables and the standardized auxiliary tensor observation auxiliary variables,

[0102] 4) The camera angular velocity is restored by using the vector cross product geometric relationship between the vehicle angular velocity measured by the vehicle velocity sensor, the current standardized auxiliary tensor, and the standardized auxiliary tensor derivative. The vector cross product geometric relationship satisfies like Figure 3 As shown;

[0103] Step 4) is specifically as follows:

[0104] 4.1) Calculate the temporary rotation transformation matrix corresponding to the 9 column vectors contained in the standardized auxiliary tensor based on the current standardized auxiliary tensor and the standardized auxiliary tensor derivative The specific calculation is as follows:

[0105]

[0106] Among them, "×" represents the cross product (outer product) operation of the vector, ||Q (jl) || represents the lth column vector Q of the jth matrix of the normalized auxiliary tensor Q (jl) The Euclidean norm of , Represents the derivative of the lth column vector of the th matrix of the normalized auxiliary tensor Q The Euclidean norm of ;

[0107] 4.2) According to the geometric relationship between the vehicle angular velocity measured by the vehicle speed sensor, the current standardized auxiliary tensor and the vector cross product of the standardized auxiliary tensor derivative, a sine value and two cosine values ​​corresponding to the 9 angles between the restored camera angular velocity and the 9 column vectors contained in the standardized auxiliary tensor are calculated. Specifically, the restored camera angular velocity and the lth column vector Q of the jth matrix of the standardized auxiliary tensor Q (jl) There is an angle, and each angle corresponds to a set of cosine values ​​that are two numbers that are opposite to each other. The calculation formula for the sine and cosine values ​​corresponding to each angle θ is as follows:

[0108]

[0109]

[0110] Among them, ω v Indicates the vehicle angular velocity measured by the on-board speed sensor;

[0111] 4.3) According to the restored camera angular velocity and the 9 column vectors contained in the standardized auxiliary tensor, the corresponding sine value and two cosine values ​​and the corresponding temporary rotation transformation matrix The following formula is used to calculate the corresponding pre-selected camera angular velocity sets under 9 angles. The pre-selected camera angular velocity set under each angle contains two camera angular velocities ω c :

[0112]

[0113] Among them, ω c is the camera angular velocity,

[0114] 4.4) Combine the camera angular velocities of the pre-selected camera angular velocity sets corresponding to the 9 included angles, that is, select one camera angular velocity from each camera pre-selected angular velocity set and combine it with other selected camera angular velocities to form 2 9 Combined camera angular velocity sets are obtained, and the geometric center of each combined camera angular velocity set is calculated. Then, the distance from each camera angular velocity in each combined camera angular velocity set to the geometric center of the current combined camera angular velocity set is calculated and the distances are summed. The geometric center of the combined camera angular velocity vector set with the smallest distance sum is taken as the recovered camera angular velocity.

[0115] 5) Estimate the actual angular velocity of the vehicle based on the recovered camera angular velocity and the current camera rotation extrinsic parameter estimate and obtain the vehicle angular velocity estimate. Specifically, the camera rotation extrinsic parameter estimate is multiplied by the camera angular velocity to obtain the vehicle angular velocity information estimate. Then, the actual angular velocity of the vehicle at historical moments and the vehicle angular velocity estimate are used to construct a camera rotation extrinsic detector, and the camera rotation extrinsic parameter estimate is continuously corrected online using the camera rotation extrinsic detector. The historical moments are all moments before the current frame moment.

[0116] The formula of the camera rotation extrinsic parameter estimation observer in step 5) is as follows:

[0117]

[0118]

[0119]

[0120] in, represents the estimated value of the camera rotation external parameter, ω represents the angular velocity of the camera rotation external parameter correction, ω v (i) represents the actual angular velocity of the vehicle measured at the i-th moment, ω c (i) represents the camera angular velocity recovered at the i-th moment, n ≥ 1, k ω Rotate the camera to see the gain.

[0121] The camera rotation extrinsic parameter estimate is iteratively updated until the next recovered camera angular velocity arrives.

[0122] The first to third rotation external parameters of the camera are calculated based on the estimated value of the camera rotation external parameter. The formula is as follows:

[0123]

[0124] Case 1:

[0125]

[0126]

[0127]

[0128] Case 2:

[0129]

[0130]

[0131]

[0132] 6) constructing a camera translation external parameter detector using the actual linear velocity and angular velocity of the vehicle before and at the current moment, the latest camera rotation extrinsic parameter estimation value in step 5), the camera angular velocity recovered in step 4), the current standardized trifocal tensor, and the derivative of the standardized trifocal tensor, and using the camera translation external parameter detector to continuously perform online correction on the camera translation extrinsic parameter estimation value;

[0133] Step 6) is specifically as follows:

[0134] 6.1) Calculate the camera translation extrinsic parameter estimation auxiliary variable s using the latest camera rotation extrinsic parameter estimation value in step 5), the camera angular velocity recovered in step 4), the current standardized trifocal tensor, and the derivative of the standardized trifocal tensor. The calculation formula is as follows:

[0135]

[0136] in, Represents the relative rotation matrix The element in row l and column j, represents the estimated value of the camera rotation extrinsic parameter, T (jl) represents the lth column vector of the jth matrix of the normalized trifocal tensor T, ω c Indicates the recovered camera angular velocity;

[0137] 6.2) Based on the camera translation extrinsic auxiliary variables and the actual angular velocity of the vehicle before and at the current moment, the motion excitation matrix A at each moment is constructed; based on the camera translation extrinsic auxiliary variables and the actual linear velocity of the vehicle, the motion excitation vector h at each moment is constructed. The calculation formula is as follows:

[0138]

[0139]

[0140] Among them, s (1) ,s (2) ,s (3) They represent the first, second, and third components of the auxiliary variable s for camera translation extrinsic estimation, ω v(1) ,ω v(2) ,ω v(3) They represent the actual angular velocity of the vehicle ω v The first, second, and third elements of v(1) , v v(2) , v v(3) Represent the actual linear speed of the vehicle v v The first, second, and third elements of

[0141] 6.3) According to the motion excitation matrix A and motion excitation vector h at each moment, the camera translation and external detection device is constructed, and the formula is as follows:

[0142]

[0143] Where A(i) represents the motion excitation matrix A at the i-th moment, h(i) represents the motion excitation vector h at the i-th moment, 1≤i≤n, represents the estimated value of the camera translation extrinsic parameter, K represents the translation extrinsic parameter estimation gain matrix, which is a preset positive definite matrix in the specific implementation; T represents the matrix transpose, n≥1,

[0144] In specific implementation,

[0145] Using this observer, we continuously monitor the actual angular velocity and linear velocity of the vehicle and the angular velocity of the camera before obtaining the next frame. The value of is iteratively updated. In the specific implementation, this update law is used to The value is updated 200 times and waits for the next frame of new image and vehicle speed information.

[0146] 7) extracting feature points of the current frame image and the two key frame images respectively, performing feature point matching on the feature points of the three images to obtain common feature points, calculating the ratio of the number of common feature points to the number of feature points in each of the three images and recording them as the first, second and third common matching ratios respectively, and comparing the minimum common matching ratio with a preset matching threshold;

[0147] 8) When the minimum common matching ratio is greater than the preset matching threshold, in the specific implementation, the preset matching threshold is recorded as σ=0.6, Then repeat steps 2)-7) without updating the two key frame images; otherwise, replace the earliest key frame image of the two key frame images with the current frame image and update it to the new two key frame images, and then repeat steps 2)-7); until the camera rotation and translation extrinsic parameter estimation values ​​tend to be stable, and use the final camera rotation and translation extrinsic parameter estimation values ​​as the vehicle-mounted camera extrinsic parameters.

[0148] Every time according to and Calculate the first-third translation extrinsic parameters and the first-third rotation extrinsic parameters of the camera respectively:

[0149]

[0150]

[0151]

[0152] Case 1:

[0153]

[0154]

[0155]

[0156] Case 2:

[0157]

[0158]

[0159]

[0160] Until these camera extrinsic parameter estimates become stable, they are taken as the final values ​​of the camera extrinsic parameters.

[0161] by and Taking as an example, the estimation results of the normalized auxiliary tensor derivative and the normalized trifocal tensor derivative are as follows Figure 4 (a) and Figure 4 As shown in (b), it can be seen that the estimated value will converge to the true value in a very short time after each key frame switching.

[0162] The camera angular velocity ω c The estimated results are as follows Figure 5 As shown in the figure, it can be seen that each key frame switch will cause the convergence process to restart, but the estimated value will always converge to the true value in a very short time.

[0163] The estimation results of the first-third rotation extrinsic parameters and the first-third translation extrinsic parameters of the camera are as follows: Figure 6 (a) with Figure 5 As shown in (b), it can be seen that the estimated value of the rotation extrinsic parameter converges to the true value quickly after 5 seconds, and the estimated value of the translation extrinsic parameter converges to the true value quickly after about 9 seconds.

[0164] Comparative analysis with the Extended Kalman Filter (EKF):

[0165] The estimation accuracy of the camera extrinsic parameters of the present invention is compared with that of the extended Kalman filter (A Kalman filter-based algorithm for IMU-camera calibration: Observability analysis and performance evaluation," IEEE Transactions on Robotics, vol. 24, no. 5, pp. 1143-1156, Oct. 2008), and the results are shown in the form of box plots. Figure 7 Among them Figure 7 (a) Figure 7 (b) with Figure 7 (c) are the comparison results of the estimation accuracy of the first-third rotation external parameters of the camera, Figure 7 (d) Figure 7 (e) with Figure 7 (f) are the comparison results of the estimation accuracy of the first to third translation extrinsic parameters of the camera, respectively. It can be seen that the camera extrinsic parameter estimation accuracy obtained by the present invention is significantly better than that of the extended Kalman filter.

Claims

1. A method for online estimation of extrinsic parameters of a vehicle-mounted camera in a natural scene, characterized in that: The following steps are involved: 1) Select two frames of images with the same feature points taken by the vehicle-mounted camera as two key frame images, and preset the initial camera rotation and translation extrinsic parameter estimation values; 2) The relative rotation matrix of the current two key frame images is obtained by using the epipolar geometry constraint. The on-board camera takes a frame image as the current frame image, and then the initial trifocal tensor is solved by using the current frame image and the two key frame images. Then, the initial auxiliary tensor is constructed according to the current initial trifocal tensor and the relative rotation matrix of the current two key frame images. Then, the current initial trifocal tensor and the initial auxiliary tensor are standardized respectively to obtain the standardized trifocal tensor and the standardized auxiliary tensor. 3) constructing a reduced-order observer according to the current standardized trifocal tensor and the standardized auxiliary tensor, and using the reduced-order observer to observe the derivatives of the current standardized trifocal tensor and the standardized auxiliary tensor online to obtain the derivatives of the current standardized trifocal tensor and the standardized auxiliary tensor; 4) The camera angular velocity is restored by using the geometric relationship between the vehicle angular velocity measured by the on-board velocity sensor, the current normalized auxiliary tensor, and the vector cross product of the normalized auxiliary tensor derivative; 5) Estimate the actual angular velocity of the vehicle based on the recovered camera angular velocity and the current camera rotation extrinsic parameter estimation value and obtain the vehicle angular velocity estimation value; then construct a camera rotation extrinsic detector using the vehicle actual angular velocity at the historical moment and the vehicle angular velocity estimation value, and use the camera rotation extrinsic detector to continuously perform online correction on the camera rotation extrinsic parameter estimation value; 6) constructing a camera translation external parameter detector using the actual linear velocity and angular velocity of the vehicle at the historical moment, the latest camera rotation extrinsic parameter estimation value in step 5), the camera angular velocity restored in step 4), the current standardized trifocal tensor, and the derivative of the standardized trifocal tensor, and using the camera translation external parameter detector to continuously perform online correction on the camera translation extrinsic parameter estimation value; 7) extracting feature points of the current frame image and the two key frame images respectively, performing feature point matching on the feature points of the three images to obtain common feature points, calculating the ratio of the number of common feature points to the number of feature points in each of the three images and recording them as corresponding common matching ratios, and comparing the minimum common matching ratio with a preset matching threshold; 8) When the minimum common matching ratio is greater than the preset matching threshold, repeat steps 2)-7); otherwise, replace the earliest key frame image of the two key frame images with the current frame image and update it to two new key frame images, and then repeat steps 2)-7); until the camera rotation and translation extrinsic parameter estimation values ​​tend to be stable.

2. According to the method for online estimation of extrinsic parameters of a vehicle-mounted camera in a natural scene in claim 1, it is characterized in that: In step 2), the initial auxiliary tensor is constructed according to the current initial trifocal tensor and the relative rotation matrix of the current two key frame images. The specific calculation formula is as follows: Among them, Q λ(j) refers to the initial auxiliary tensor Q λ The j-th matrix, T λ(j) is the initial trifocal tensor T λ The j-th matrix of is the relative rotation matrix The jth column of ; [·] × Represents a skew-symmetric matrix operation for finding a three-dimensional vector; j = 1, 2, 3.

3. The method for online estimation of extrinsic parameters of a vehicle-mounted camera in a natural scene according to claim 1, characterized in that: In step 2), the current initial trifocal tensor and the initial auxiliary tensor are standardized respectively, and the specific calculation formula is as follows: Among them, Q λ represents the initial auxiliary tensor, T λ represents the initial trifocal tensor, Q represents the normalized auxiliary tensor, T represents the normalized trifocal tensor, λ represents the scale factor, Q λ(jkl) refers to the initial auxiliary tensor Q λ The j-th matrix Q λ(j) For the element in the lth column and kth row, j=1, 2, 3, k=1, 2, 3, l=1, 2, 3.

4. The method for online estimation of extrinsic parameters of a vehicle-mounted camera in a natural scene according to claim 1, characterized in that: The formula of the reduced-order observer in step 3) is as follows: in, represents the derivative of the normalized trifocal tensor T, is the normalized trifocal tensor derivative The estimated value of represents the derivative of the normalized auxiliary tensor Q, is the normalized auxiliary tensor derivative The estimated value of T With λ Q are the normalized triple focal tensor observation gain and the normalized auxiliary tensor observation gain, ξ T With ξ Q They are the standardized triple focal tensor observed auxiliary variables and the standardized auxiliary tensor observed auxiliary variables.

5. The method for online estimation of extrinsic parameters of a vehicle-mounted camera in a natural scene according to claim 1, characterized in that: The step 4) is specifically as follows: 4.1) Calculate the temporary rotation transformation matrix corresponding to the 9 column vectors contained in the standardized auxiliary tensor based on the current standardized auxiliary tensor and the standardized auxiliary tensor derivative The specific calculation is as follows: Among them, "×" represents the cross product operation of the vector, ||Q (jl) || represents the lth column vector Q of the jth matrix of the normalized auxiliary tensor Q (jl) The Euclidean norm of , Represents the derivative of the lth column vector of the jth matrix of the normalized auxiliary tensor Q The Euclidean norm of ; 4.2) According to the geometric relationship between the vehicle angular velocity measured by the on-board velocity sensor, the current standardized auxiliary tensor and the vector cross product of the standardized auxiliary tensor derivative, the sine value and two cosine values ​​corresponding to the 9 angles between the restored camera angular velocity and the 9 column vectors contained in the standardized auxiliary tensor are calculated. The calculation formula for the sine value and cosine value corresponding to each angle θ is as follows: Among them, ω v Indicates the vehicle angular velocity measured by the on-board speed sensor; 4.3) According to the restored camera angular velocity and the 9 column vectors contained in the standardized auxiliary tensor, the corresponding sine value and two cosine values ​​and the corresponding temporary rotation transformation matrix The pre-selected camera angular velocity set corresponding to the 9 angles is calculated using the following formula: Among them, ω c is the camera angular velocity; 4.4) Combine the camera angular velocity sets corresponding to the 9 angles to form 2 9 Combined camera angular velocity sets are obtained, and the geometric center of each combined camera angular velocity set is calculated. Then, the distance from each camera angular velocity in each combined camera angular velocity set to the geometric center of the current combined camera angular velocity set is calculated and the distances are summed. The geometric center of the combined camera angular velocity vector set with the smallest distance sum is taken as the recovered camera angular velocity.

6. The method for online estimation of extrinsic parameters of a vehicle-mounted camera in a natural scene according to claim 1, characterized in that: The formula of the camera rotation extrinsic parameter estimation observer in step 5) is as follows: in, represents the estimated value of the camera rotation external parameter, ω represents the angular velocity of the camera rotation external parameter correction, ω v (i) represents the actual angular velocity of the vehicle measured at the i-th moment, ω c (i) represents the camera angular velocity recovered at the i-th moment, n ≥ 1, k ω Rotate the exterior viewing gain for the camera.

7. The method for online estimation of extrinsic parameters of a vehicle-mounted camera in a natural scene according to claim 1, characterized in that: The step 6) is specifically as follows: 6.1) Calculate the camera translation extrinsic parameter estimation auxiliary variable s using the latest camera rotation extrinsic parameter estimation value in step 5), the camera angular velocity recovered in step 4), the current standardized trifocal tensor, and the derivative of the standardized trifocal tensor. The calculation formula is as follows: in, Represents the relative rotation matrix The element in row l and column j, represents the estimated value of the camera rotation extrinsic parameter, T (jl) represents the lth column vector of the jth matrix of the normalized trifocal tensor T, ω c Indicates the recovered camera angular velocity; 6.2) Based on the camera translation extrinsic auxiliary variables and the actual angular velocity of the vehicle before and at the current moment, the motion excitation matrix A at each moment is constructed. Based on the camera translation extrinsic auxiliary variables and the actual linear velocity of the vehicle, the motion excitation vector h at each moment is constructed. The calculation formula is as follows: Among them, s (1) ,s (2) ,s (3) They represent the first, second, and third components of the auxiliary variable s for camera translation extrinsic estimation, ω v(1) ,ω v(2) ,ω v(3) They represent the actual angular velocity of the vehicle ω v The first, second, and third elements of v(1) , v v(2) , v v(3) Represent the actual linear speed of the vehicle v v the 1st, 2nd, and 3rd elements of ; 6.3) According to the motion excitation matrix A and motion excitation vector h at each moment, the camera translation and external detection device is constructed, and the formula is as follows: Where A(i) represents the motion excitation matrix A at the i-th moment, h(i) represents the motion excitation vector h at the i-th moment, 1≤i≤n, represents the camera translation extrinsic parameter estimation value, K represents the translation extrinsic parameter estimation gain matrix, and T represents the matrix transpose; The camera translation extrinsic detector is used to continuously perform online correction on the camera translation extrinsic parameter estimation.

Images