Motion-based self-calibration method for extrinsic parameters of vehicle-mounted depth camera

Abstract

The application belongs to the technical field of sensor calibration, and discloses a motion-based self-calibration method for vehicle-mounted depth camera external parameters, which comprises the following steps: collecting color images and depth images by using a depth camera on a vehicle, and simultaneously collecting wheel rotation angle data by using a wheel speed meter; inputting the color images and the depth image sequence into a SLAM algorithm to obtain the camera pose of the depth camera at each time; obtaining ground point cloud by using a flooding filling algorithm; obtaining a ground plane equation and part of the camera external parameters, obtaining the relative motion of the depth camera according to the camera pose, and projecting the relative motion onto the ground plane to obtain the camera motion sequence in a two-dimensional space; obtaining the motion sequence of the vehicle chassis; and solving the kinematic parameters of the vehicle chassis and the remaining camera external parameters by minimizing the matching error function according to the motion sequence of the vehicle chassis and the motion sequence of the depth camera. The application solves the equation degeneration problem caused by the planar motion characteristics of the vehicle in a three-dimensional space, and the calibration is more accurate.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of sensor calibration, and more particularly to a motion-based self-calibration method for vehicle-mounted depth camera external parameters. BACKGROUND

[0002] A depth camera can capture color images and measure the distance (depth) of each pixel to the camera. Compared with a laser sensor, it has low cost, rich information acquisition and flexible installation method, and is often used as a sensor for autonomous navigation system of a mobile vehicle. In order to control the motion of the mobile vehicle, the camera pose estimated by the sensing signal needs to be transformed into the vehicle pose. Solving this pose transformation is the problem of camera external parameter calibration.

[0003] At present, the methods for solving the problem of sensor external parameter calibration can be divided into two categories:

[0004] (1) The calibration method based on mode uses prior information of the environment, including scene features or known road markers, to estimate the pose of the sensor in the world coordinate system.

[0005] (2) The calibration method based on motion uses the motion sequence of the sensor to solve the pose transformation between sensors by estimating the pose of the sensor itself.

[0006] The calibration method based on mode is simple and direct, and the higher the measurement accuracy, the smaller the error of the calibration result. However, it requires the intervention of the operator, such as placing a calibration board for preprocessing, and cannot realize the automatic calibration of the external parameters of the sensor. The calibration method based on motion does not require preprocessing of the environment, and can automatically recalibrate the external parameters when the sensor position changes due to vehicle collision, vibration or human operation. However, by minimizing the matching error to solve the pose transformation, the noise collected by the sensor and the error of the pose estimation can greatly affect the calibration result. However, the intermediate calculation process of the algorithm is complex, and it is difficult to model the transmission of errors. In addition, since the vehicle can only move in a plane, the equation for solving the pose transformation in three-dimensional space is degenerate, and a stable solution cannot be obtained. Therefore, this method is usually used for external parameter calibration of sensors in a two-dimensional plane, and therefore there is an urgent need to design a more accurate external parameter calibration method for vehicle-mounted depth cameras. SUMMARY

[0007] In view of the above defects or improvement needs of the prior art, the application provides a motion-based self-calibration method for vehicle-mounted depth camera external parameters, which can reduce the external parameter calibration problem of the depth camera from three dimensions to two dimensions, solve the equation degeneration problem in three-dimensional space due to the planar motion characteristics of the vehicle, and calibrate more accurately.

[0008] To achieve the above object, according to one aspect of the present application, a motion-based self-calibration method for vehicle-mounted depth camera external parameters is provided, which comprises: S1: using a depth camera on a vehicle to collect color images and depth images, and using a wheel speed meter to collect wheel rotation angle data; S2: inputting the color images and depth image sequence into a SLAM algorithm to obtain camera poses of the depth camera at each time; S3: using a flood fill algorithm to find pixel blocks corresponding to the ground in the color images, and calculating local spatial coordinates of each pixel point, sequentially processing each color image and using the camera poses to transform the local spatial coordinates into global camera coordinates to form a ground point cloud; S4: using the spatial coordinates of each point of the ground point cloud, using a least square method to obtain a ground plane equation in the camera coordinate system, and obtaining part of the camera external parameters according to the coordinate relationship between the ground plane normal vector in the camera coordinate system and the world coordinate system, the part of the camera external parameters including depth camera z-axis translation t z , roll angle a, pitch angle b; S5: obtaining the relative motion of the camera in each time period according to the camera poses, and projecting the relative motion to the ground plane to obtain a camera motion sequence in a two-dimensional space; S6: establishing a relationship between the vehicle chassis motion and the wheel rotation angle according to a differential wheel kinematics model, and using the wheel rotation angle data to obtain a vehicle chassis motion sequence; S7: constructing a matching error function according to the relationship between the relative motion of the vehicle chassis and the depth camera, using the vehicle chassis motion sequence and the depth camera motion sequence, and solving the vehicle chassis kinematics parameters and the remaining camera external parameters by minimizing the matching error function, the vehicle chassis kinematics parameters including the distance between the left wheel and the right wheel b, the left wheel radius r L , the right wheel radius r R , and the remaining camera external parameters including depth camera x-axis translation t x , y-axis translation t y , and yaw angle g, thereby realizing self-calibration of the depth camera external parameters.

[0009] Preferably, step S3 specifically comprises: S31: judging the approximate area of the ground pixel block in the color image according to the installation position of the depth camera; S32: sequentially taking each pixel point in the area in the color image as a seed for flood fill, selecting the ground pixel block with a size exceeding a preset threshold, and solving the local spatial coordinates of each pixel point, sequentially executing the above operation on all color images and using the camera poses at each time to transform the local spatial coordinates into global spatial coordinates, and constructing a ground point cloud.

[0010] Preferably, step S4 specifically comprises:

[0011] S41: using the spatial coordinates of the ground point cloud as data, using a least square method to solve a ground plane equation, and the expression of the ground plane equation is:

[0012] ax+by+cz+d=0

[0013] Using the closed-form solution of linear least squares problem, where x, y, z are the spatial coordinates of the points in the ground point cloud, a 2 +b 2 +c 2 =1 and b<0, the ground plane equation parameters a, b, c, d are obtained, and d is the distance from the origin of the camera coordinate system to the ground plane along the normal vector direction of the ground plane;

[0014] S42: Establish the depth camera z-axis translation t z and the ground plane equation parameters, and the relationship is:

[0015] t z =d

[0016] Substitute the parameter d to obtain the depth camera z-axis translation t z ;

[0017] S43: According to the coordinate definition, the relationship between the coordinates of the normal vector of the ground plane in the world coordinate system and the camera coordinate system is established, and the relationship is:

[0018]

[0019] Where R x (α) is the rotation matrix of rotating α angle around the x-axis of the world coordinate system, R y (β) is the rotation matrix of rotating β angle around the y-axis of the world coordinate system, and R z (γ) is the rotation matrix of rotating γ angle around the z-axis of the world coordinate system; Simplify the relationship to establish the relationship between the camera roll angle α, pitch angle β and the ground plane equation parameters, and the relationship is:

[0020] β=arcsin(-c)

[0021] Substitute the parameters a, b, c to obtain the camera roll angle α and pitch angle β.

[0022] Preferably, step S5 specifically comprises:

[0023] S51: Obtain the relative motion of the camera in each time period using the camera pose, and the relative motion formula is:

[0024]

[0025] Where q k is the relative motion of the depth camera in the time period [t i , t i+1 ], T i and T i+1 are the time t iand time t i+1 of the camera;

[0026] S52: Projecting the relative motion of the camera in each time period to the ground plane to obtain a camera motion sequence. Convert the projection transformation to the ground plane to a pose transformation, which refers to transforming the initial camera pose to a preset camera pose with roll angle and pitch angle being zero and making the directions of the coordinate axes of the camera coordinate system consistent with the directions of the coordinate axes of the world coordinate system, pose transformation T proj is:

[0027]

[0028] wherein R proj represents the rotation matrix in T proj , R x (-α) is a rotation matrix rotating by -α around the x axis of the world coordinate system, and R y (-β) is a rotation matrix rotating by -β around the y axis of the world coordinate system.

[0029] The camera motion after projection to the ground plane is:

[0030]

[0031] Extracting the displacement amount in the preset camera coordinate system x and y axis direction and the rotation amount around the preset camera coordinate system z axis in the kth camera relative motion in the camera motion sequence

[0032] Preferably, in step S6, the relationship between the vehicle chassis motion and the wheel rotation angle is established according to the differential wheel kinematics model, and the relationship is:

[0033]

[0034]

[0035]

[0036] wherein, is the displacement amount in the forward direction of the vehicle, is the displacement amount perpendicular to the forward direction of the vehicle, is the rotation amount of the vehicle chassis, are the left and right wheel rotation angles in the time periods [t i , t i+1 ], respectively, r L , r​R denoted as , where is the radius of the left and right wheels respectively, and b is the distance between the left and right wheels;

[0037] Based on the pose transformation from the vehicle chassis to the depth camera in two-dimensional space, it can be known that the rotation of the vehicle chassis and the depth camera are equal. Therefore, the rotation and the left and right wheel angles in the depth camera motion sequence are used as data, and the parameter J is obtained by the least squares method. 21 J 22 The value. Introducing intermediate values. Solve according to the given relationship. The k-th relative motion of the vehicle chassis in the vehicle chassis motion sequence

[0038] Preferably, in step S7, the optimization problem of minimizing the matching error is:

[0039]

[0040] in, b is the distance between the left and right wheels, r L r is the radius of the left wheel. R Let t be the radius of the right wheel. x and t y The depth camera is translated along the x and y axes, and γ is the yaw angle.

[0041] Preferably, step S7 further includes transforming the constrained optimization problem into an unconstrained optimization problem using Lagrange multipliers:

[0042]

[0043] in, Use L to represent the function L is minimized when the partial derivative is zero, yielding information about The equation:

[0044]

[0045] Solve the equation to obtain Then, the kinematic parameters of the vehicle chassis and the remaining camera extrinsic parameters are obtained.

[0046] In summary, compared with the prior art, the motion-based extrinsic parameter calibration method for vehicle-mounted depth cameras provided by this invention has the following beneficial effects:

[0047] 1.The application uses color information of color images to segment ground point cloud and estimate plane equation, projects pose estimated by depth camera to the two-dimensional plane, so that depth camera extrinsic calibration problem is reduced from three-dimensional to two-dimensional, solves equation degeneration problem caused by planar motion characteristics of vehicle in three-dimensional space, in addition, extrinsic calibration problem is decomposed into multiple least square problems in solving process, reduces matrix size and reduces calculation cost.

[0048] 2.The vehicle-mounted depth camera extrinsic self-calibration method provided by the application does not need to place any calibration object and has no special requirements for the scene, can be autonomously executed by the vehicle control system, has good universality and high automation degree. BRIEF DESCRIPTION OF DRAWINGS

[0049] Figure 1 is a step diagram of the motion-based vehicle-mounted depth camera extrinsic self-calibration method of the application;

[0050] Figure 2A is a front view of a depth camera trolley;

[0051] Figure 2B is a left view of a depth camera trolley;

[0052] Figure 3A is a schematic diagram of a camera coordinate system;

[0053] Figure 3B is a schematic diagram of a vehicle coordinate system and a world coordinate system;

[0054] Figure 4 is a flowchart of the motion-based vehicle-mounted depth camera extrinsic self-calibration method of the application. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical scheme and advantages of the application clearer, the application is further described in detail below in combination with the drawings and examples. It should be understood that the specific examples described herein are only used to explain the application and do not limit the application. In addition, the technical features involved in each embodiment of the application described below can be combined with each other as long as they do not conflict with each other.

[0056] The application provides a motion-based vehicle-mounted depth camera extrinsic self-calibration method, as shown in Figure 1 and Figure 4 mainly includes the following steps S1-S7.

[0057] S1: color images and depth images are collected by using a depth camera on a vehicle, and wheel rotation angle data are collected by using a wheel speed meter.

[0058] In order to align the motion sequence of the depth camera and the vehicle chassis, i.e. to obtain the motion of the depth camera and the vehicle chassis in the same time period, the timestamp information of the collected data needs to be preserved during the collection process.

[0059] In a specific embodiment, in order to obtain experimental data, an experimental platform is built based on a Dashgo D1 mobile trolley, as shown in Figure 2A and Figure 2B An Intel RealSense D455 depth camera is mounted, and a software system is run on an x86 architecture notebook computer. The Dashgo D1 mobile trolley adopts a two-wheel differential structure and is equipped with a 600-line incremental photoelectric encoder as a wheel speed meter. The Intel RealSense D455 depth camera has an RGB resolution of 1280x800, a field of view of 90°x65°, a depth measurement resolution of 1280x720, a field of view of 87°x58°, an accuracy of <2% (at 4m), and an ideal working range of 0.6m-6m. The notebook computer communicates with the Arduino lower computer through a serial port and sends speed commands to control the motor.

[0060] As shown in Figure 3A and Figure 3B The midpoint of the vehicle axle in the initial state is taken as the origin of the vehicle coordinate system, and the x and y axes are established along the forward direction of the vehicle and the direction of the axle axis, respectively, and the z axis is established according to the right-hand rule; the projection point of the origin of the vehicle coordinate system onto the ground plane is taken as the origin of the world coordinate system, and the x, y and z coordinate axes of the world coordinate system are established in the same direction; the optical center of the depth camera in the initial state is taken as the origin of the camera coordinate system, and the z axis is established in the direction opposite to the perpendicular incidence imaging plane, and the x and y axes are established along the length and width directions of the imaging plane, respectively. In addition, in order to meet the observability requirement for the calibration parameters, the mobile trolley is controlled to move in an "8" shape trajectory.

[0061] S2: input the color image and depth image sequence into the SLAM algorithm to obtain the camera pose of the depth camera at each time.

[0062] In this embodiment, the ORB-SLAM2 algorithm is used to complete the positioning of the depth camera. ORB-SLAM2 adopts a three-thread parallel structure of tracking, local mapping and loop detection, can output camera pose in real time, and can provide better robustness, stability and real-time performance by using feature points for image matching and loop detection. In addition, in order to reduce the calculation scale, part of the continuous picture sequence is extracted as a key frame for global optimization. The key frame is selected according to the number of common observed feature points, which represents that the mobile trolley has traveled a short distance and the surrounding landscape has changed to a certain extent. The key frame is taken as the image sequence required for calibration, and the key frame timestamp is t i , where i = 1, 2,..., n.

[0063] S3: find the pixel blocks corresponding to the ground in the color images by using the flooding filling algorithm, and calculate the local spatial coordinates of each pixel point, sequentially process each color image and use the camera pose to unify the local spatial coordinates to the global camera coordinate system to form the ground point cloud.

[0064] Step S3 specifically comprises the following steps:

[0065] S31: according to the installation position of the depth camera, predict the approximate area of the ground pixel block in the color image.

[0066] According to the installation position of the depth camera, it is predicted that the ground pixel block is located in the lower half of the color image.

[0067] S32: sequentially flood fill each pixel point in the area of the color image as a seed, select the pixel block with a size exceeding a preset threshold as the pixel block corresponding to the ground, and solve the local spatial coordinates of each pixel point, sequentially execute the above operations on all color images and use the camera pose at each time to transform the local spatial coordinates into global spatial coordinates, and construct the ground point cloud.

[0068] In this embodiment, the pixel points in the lower half of the color image are sequentially executed as seeds for flooding filling, and the pixel blocks with a width greater than 1 / 2 of the image width and a height greater than 1 / 5 of the image height are selected as ground pixel blocks. The local spatial coordinates of each pixel point are calculated, and the above operations are sequentially executed on all color images and the local spatial coordinates are transformed into global spatial coordinates by using the camera pose at each time to construct the ground point cloud.

[0069] S4: using the spatial coordinates of each point of the ground point cloud, using the least square method to obtain the ground plane equation in the camera coordinate system, and according to the coordinate relationship between the ground plane normal vector in the camera coordinate system and the world coordinate system, obtaining part of the camera extrinsic parameters, including the depth camera z axis translation t z , roll angle α, pitch angle β.

[0070] Step S4 specifically comprises the following steps:

[0071] S41: using the spatial coordinates of the ground point cloud as data, using the least square method to solve the ground plane equation, the expression of the ground plane equation is:

[0072] ax+by+cz+d=0

[0073] using the closed-form solution of the linear least square problem, wherein x, y, z are the spatial coordinates of the points in the ground point cloud, a 2 +b 2 +c 2= 1 and b < 0, the ground plane equation parameters a, b, c, d are obtained, d is the distance from the origin of the camera coordinate system to the ground plane along the normal direction of the ground plane.

[0074] In this embodiment, the RANSAC method is used to remove outliers in the ground point cloud, and the error threshold is set to 0.02 (m). The closed-form solution of the linear least squares problem is used to obtain the ground plane equation parameters, and the scale and sign of the parameters are determined according to a 2 + b 2 + c 2 = 1 and b < 0.

[0075] S42: Establish the relationship between the depth camera z-axis translation t z and the ground plane equation parameter d, the relationship is:

[0076] t z = d

[0077] Substitute the parameter d to obtain the depth camera z-axis translation t z .

[0078] S43: According to the coordinate definition, establish the relationship between the normal vector of the ground plane in the world coordinate system and the camera coordinate system.

[0079] Specifically, the normal vector of the ground plane in the world coordinate system and the camera coordinate system are [0, 0, 1] T and [a, b, c] T respectively, according to the mathematical definition of coordinates:

[0080]

[0081] where, is the base vector of the world coordinate system, [1, 0, 0] T , [0, 1, 0] T , [0, 0, 1] T ; is the base vector of the camera coordinate system; ideally, the depth camera is installed without inclination, so according to the definition of the depth camera optical center coordinate system, [0, -1, 0] T , [0, 0, -1] T , [1, 0, 0] T , and in fact, the installation has an inclination, and rotates around the x, y, z axes by α, β, γ angles respectively, so, it is necessary to left multiply R x (α), R y (β) and R z (γ) in turn:

[0082]

[0083] wherein,

[0084] Substituting formula (2) into formula (1) can obtain:

[0085]

[0086] Multiplying R on both sides of the equation z (γ) -1 , and can obtain:

[0087]

[0088] Simplifying, the relationship between the roll angle a and the pitch angle b of the depth camera and the ground plane equation parameters is obtained, and the relationship is:

[0089]

[0090] β=arcsin(-c)

[0091] Substituting the parameters a, b, and c obtains the roll angle a and the pitch angle b of the camera.

[0092] S5: Obtain the relative motion of the camera in each time period according to the camera pose, and project the relative motion to the ground plane to obtain the camera motion sequence in the two-dimensional space.

[0093] Step S5 specifically includes:

[0094] S51: Obtain the relative motion of the camera in each time period by using the camera pose, and the relative motion formula is:

[0095]

[0096] wherein, q k is the relative motion of the depth camera in the time period [t i , t i+1 ], T i and T i+1 are the camera poses at time t i and time t i+1 , and R k and p k respectively represent the rotation matrix and the translation vector in q k .

[0097] S52: Project the relative motion of the camera in each time period to the ground plane to obtain a camera motion sequence. Convert the projection transformation to a pose transformation, which refers to transforming the initial camera pose to a preset camera pose with a roll angle and a pitch angle of zero and making the directions of the coordinate axes of the camera coordinate system consistent with the directions of the coordinate axes of the world coordinate system, pose transformation T proj is:

[0098]

[0099] where R proj represents the rotation matrix in T proj , The camera motion after projection to the ground plane is:

[0100]

[0101] Using represents the displacement vector in , Using represents the rotation matrix in , Then the camera relative motion q k The expression s k in two-dimensional space is:

[0102]

[0103] where, are the displacement amounts in the preset camera coordinate system x and y axis directions respectively, is the rotation amount around the preset camera coordinate system z axis, s k constitute the kth camera relative motion in the camera motion sequence.

[0104] According to the camera roll angle α and the pitch angle β, the transformation matrix T proj is calculated by substituting the camera pose of the key frame into formulas (5), (6), and (7) to calculate the camera motion sequence S = {s k} in two-dimensional space, where k = 1, 2,..., n-1.

[0105] S6: Establish the relationship between the vehicle chassis motion and the wheel rotation angle according to the differential wheel kinematics model, and obtain the vehicle chassis motion sequence using the wheel rotation angle data.

[0106] The differential wheel kinematics model is:

[0107]

[0108] where v and w are the velocity and angular velocity of the vehicle chassis motion, respectively, r L and r R are the left and right wheel radii, b is the distance between the left and right wheels, w L and w R are the angular velocities of the left and right wheels, respectively, and is used to represent the parameter matrix By integrating the kinematic model and assuming that the angular velocities are constant in the time period [t i , t i+1 ], the relationship between the vehicle chassis motion and the wheel angles is established, and the relationship is as follows:

[0109]

[0110]

[0111]

[0112] where, is the displacement in the forward direction of the vehicle, is the displacement perpendicular to the forward direction of the vehicle, is the rotation of the vehicle chassis, are the left and right wheel angles in the time period [t i , t i+1 ], respectively.

[0113] According to the pose transformation of the vehicle chassis to the depth camera in two-dimensional space, the rotation of the vehicle chassis and the depth camera is equal, so the rotation of the depth camera motion sequence and the left and right wheel angles are taken as data, and a linear least squares problem is established according to formula (8), and the parameters J 21 and J 22 are determined using the closed form of linear least squares. An intermediate variable is introduced, and the relative motion of the kth vehicle chassis in the vehicle chassis motion sequence is solved according to formulas (8), (9), and (10).

[0114] In this embodiment, the wheel angle data timestamp collected by the wheel speed sensor is t j , where j = 1, 2,..., m, which is not the same as the key frame timestamp t i , and the wheel angle at time t i is obtained by linear interpolation. Let the adjacent wheel angle data timestamps of t i be t j and t j+1 (t j < t i < t j+1 ), and the left and right wheel angles are Then the left and right wheel rotation angles at time t i

[0115]

[0116] The left and right wheel rotation angles in the time period [t i , t i+1 ] are respectively and The rotation amount of the depth camera around the z-axis of the world coordinate system and the left and right wheel rotation angles are taken as data, the parameters J 21 and J 22 are determined by using the closed form of linear least squares, and then substituted into formulas (8), (9) and (10) to calculate the chassis motion sequence C = {c k}, where k = 1, 2,..., n-1.

[0117] S7: A matching error function is constructed according to the relationship between the relative motion of the vehicle chassis and the depth camera, and vehicle chassis motion sequence and depth camera motion sequence are used to solve the vehicle chassis kinematic parameters and the remaining camera extrinsic parameters by minimizing the matching error function, the vehicle chassis kinematic parameters include the distance between the left wheel and the right wheel b, the left wheel radius r L and the right wheel radius r R , the remaining camera extrinsic parameters include the depth camera x, y axis translation t x and t y , and the yaw angle γ, thereby realizing the extrinsic self-calibration of the depth camera.

[0118] The relationship between the relative motion of the depth camera and the vehicle chassis in two-dimensional space is:

[0119]

[0120] Wherein, l = [t x , t y , γ] T is the pose change of the vehicle chassis to the depth camera, which is composed of the depth camera x, y axis translation t x , t y and the yaw angle γ, and the definition of the operator is The definition of the operator is

[0121] The optimization problem of minimizing the matching error is:

[0122]

[0123] ​where b is the distance between the left and right wheels, r L is the radius of the left wheel, r R is the radius of the right wheel, M is the middle quantity,

[0124] Equation (12) is converted to an unconstrained least squares problem using Lagrange multipliers:

[0125]

[0126] where Let L denote the function L is minimized when the partial derivative is zero, resulting in an equation for :

[0127]

[0128] Solving det(M + λW) = 0 for λ, and solving the equation for then applying r L = -J 21 b, r R = J 22 b, completes the solution for all calibration parameters.

[0129] In this example, the RANSAC method is applied to reject outliers in the motion sequence. The random sampling ratio of the camera motion sequence S and the chassis motion sequence C is 1 / 4, the iteration number is 20, the solving number is 10, and the error threshold is 0.02 (m). The error of the data point is defined as the displacement in . The matrix M in equation (12) is solved using the sampling points or inliers (camera motion, chassis motion sequence pairs within the error threshold). According to the numerical properties of M and W, det(M + λW) = 0 is a quadratic equation for λ, and two roots λ1, λ2 are obtained. The corresponding solutions are substituted into equation (13), and the solution of the least squares problem is finally determined according to the size of L. According to cos 2 γ + sin γ 2 = 1 and b > 0, the scale and sign of the parameters are determined, and the wheel distance b, the camera x, y axis translation t x , t y and the yaw angle γ are obtained. Then r L = -J 21 b, r R = J 22 b are used to obtain the left and right wheel radii r L and r R .

[0130] All calibration parameters can be obtained by the above steps: depth camera extrinsic parameter t x , t y , t z , α, β, γ, trolley kinematics parameter r L , r R , b, the homogeneous transformation matrix from the camera coordinate system to the vehicle coordinate system is:

[0131]

[0132] wherein, R rc is used to represent the rotation matrix in T rc , t rc is used to represent the displacement vector in T rc .

[0133]

[0134]

[0135] The visual SLAM algorithm outputs the depth camera pose T c in the camera coordinate system, then the vehicle chassis pose T r in the vehicle coordinate system is:

[0136]

[0137] Those skilled in the art will readily understand that the above description is only the preferred embodiment of the present application, and is not intended to limit the present application, and any modification, equivalent replacement and improvement made within the spirit and principle of the present application shall be included in the protection scope of the present application.

Claims

1. A motion-based self-calibration method for extrinsic parameters of a vehicle-mounted depth camera, characterized in that, The method comprises: S1: using a depth camera on a vehicle to collect color images and depth images, and using a wheel speed meter to collect wheel rotation angle data; S2: inputting the color image and depth image sequence into a SLAM algorithm to obtain the camera pose of the depth camera at each time; S3: using a flood fill algorithm to find the pixel blocks corresponding to the ground in the color image, and calculating the local spatial coordinates of each pixel point, sequentially processing each color image and using the camera pose to unify the local spatial coordinates into a global camera coordinate system to form a ground point cloud; S4: using the spatial coordinates of each point of the ground point cloud, a least square method is used to obtain a ground plane equation in a camera coordinate system, and according to the coordinate relationship of the ground plane normal vector in the camera coordinate system and the world coordinate system, part of camera extrinsic parameters are obtained, the part of camera extrinsic parameters including depth camera z-axis translation , roll angle , pitch angle ; S5: obtaining the relative motion of the camera in each time period according to the camera pose, and projecting the relative motion to the ground plane to obtain a camera motion sequence in two-dimensional space; S6: establishing the relationship between the vehicle chassis motion and the wheel rotation angle according to the differential wheel kinematics model, and obtaining the vehicle chassis motion sequence using the wheel rotation angle data; S7: constructing a matching error function according to the relationship between the relative motion of the vehicle chassis and the depth camera, solving the vehicle chassis kinematic parameters and the residual camera extrinsic parameters by minimizing the matching error function, using the vehicle chassis motion sequence and the depth camera motion sequence, the vehicle chassis kinematic parameters including the distance between the left wheel and the right wheel , the left wheel radius , the right wheel radius , and the residual camera extrinsic parameters including the depth camera x, y axis translation and , the yaw angle , thereby realizing the extrinsic self-calibration of the depth camera.

2. The method of claim 1, wherein, Step S3 specifically comprises: S31: according to the installation position of the depth camera, pre-judging the approximate area of the ground pixel block in the color image; S32: sequentially flood filling each pixel point in the area in the color image as a seed, selecting the ground pixel block with a size exceeding a preset threshold, and solving the local spatial coordinates of each pixel point, sequentially executing the above operations on all color images and using the camera pose at each time to transform the local spatial coordinates into global spatial coordinates, and constructing a ground point cloud.

3. The method of claim 2, wherein, Step S4 specifically comprises: S41: using the spatial coordinates of the ground point cloud as data, and using the least squares method to solve the ground plane equation, the expression of the ground plane equation is: using the closed-form solution of the linear least squares problem, wherein x,y,z are the spatial coordinates of a point in the ground point cloud, and , obtaining ground plane equation parameters , is the distance from the origin of the camera coordinate system in the direction of the normal vector of the ground plane to the ground plane; S42: Establishing depth camera z-axis translation a relationship between the ground plane equation parameters, the relationship being: Substitute parameters Obtain depth camera z-axis translation ; S43: according to the coordinate definition, establishing the relationship between the normal vector of the ground plane in the world coordinate system and the camera coordinate system, the relationship is: wherein Ry(θy) is a rotation matrix for rotating about the world coordinate system y-axis by an angle θy, Rz(θz) is a rotation matrix for rotating about the world coordinate system z-axis by an angle θz, Ry(θy) is a rotation matrix for rotating about the world coordinate system y-axis by an angle θy, Rz(θz) is a rotation matrix for rotating about the world coordinate system z-axis by an angle θz, Ry(θy) is a rotation matrix for rotating about the world coordinate system y-axis by an angle θy, Rz(θz) is a rotation matrix for rotating about the world coordinate system z-axis by an angle θz; and simplifying the relationship establishes a relationship between the camera roll angle , the camera pitch angle and the ground plane equation parameters, the relationship being: Substitute the parameters Obtain the camera roll angle , pitch angle .

4. The method of claim 1, wherein, Step S5 specifically comprises: S51: using the camera pose to obtain the relative motion of the camera in each time period, and the relative motion formula is: wherein, is a time period relative motion of the internal depth camera, and are camera poses at time and time respectively. S52: project the relative motion of the camera in each time period to the ground plane to obtain a camera motion sequence; and convert the projection transformation to the ground plane into a pose transformation, which is a transformation from an initial camera pose to a preset camera pose with a roll angle and a pitch angle of zero and such that the directions of the coordinate axes of the camera coordinate system are consistent with the directions of the coordinate axes of the world coordinate system. is: wherein the use of the rotation matrix in is the rotation matrix that rotates around the x-axis of the world coordinate system by the angle is the rotation matrix that rotates around the y-axis of the world coordinate system by the angle ; and is the rotation matrix that rotates around the z-axis of the world coordinate system by the angle Camera motion after projection to ground plane is: extracting a displacement amount in the x, y axis direction of a preset camera coordinate system and a rotation amount around the z axis of the preset camera coordinate system a relative motion of the first camera in the camera motion sequence a relative motion of the second camera in the camera motion sequence a relative motion of the third camera in the camera motion sequence 5. The method of claim 1, wherein, In step S6, the relationship between the vehicle chassis motion and the wheel rotation angle is established according to the differential wheel kinematics model, and the relationship is: wherein, is an amount of displacement in a vehicle advancing direction, is an amount of displacement perpendicular to the vehicle advancing direction, is an amount of rotation of a vehicle chassis, are time periods, respectively are left and right wheel turning angles, , are left and right wheel radii, respectively, is a distance between the left and right wheels; According to the pose transformation of the vehicle chassis to the depth camera in the two-dimensional space, the rotation amount of the vehicle chassis and the depth camera is equal, so the rotation amount in the motion sequence of the depth camera and the rotation angles of the left and right wheels are taken as data, and the least square method is used to obtain the values of the parameters ; an intermediate amount is introduced, the relationship is solved according to the relationship, and the relative motion of the first vehicle chassis in the motion sequence of the vehicle chassis is constituted .

6. The method according to claim 1 or 4, characterized in that, In step S7, the optimization problem of minimizing the matching error is: wherein, , is the distance between the left and right wheels, is the left wheel radius, is the right wheel radius, and is the depth camera x, y axis translation, , , .

7. The method of claim 6, wherein, Step S7 further comprises using the Lagrange multiplier to convert the optimization problem into an unconstrained optimization problem: where , using denotes the function , when the partial derivative is zero minimum, resulting in an equation for . Solving the equations obtains and further obtains the vehicle chassis kinematics parameters and the remaining camera extrinsic parameters.

Images