Image target point pose determination method and system fusing camera-IMU (Inertial Measurement Unit)

Abstract

The invention discloses a camera-IMU (Inertial Measurement Unit) fused image target point pose determination method, which comprises the following steps of: jointly calibrating a camera and an IMU, and determining a camera-IMU tight coupling model according to a calibration result; acquiring a first relative pose between the feature point and the target point of the image; acquiring a second relative pose of the feature point relative to the shifted camera by using a camera-IMU tight coupling model; and determining the pose of the target point relative to the camera according to the first relative pose and the second relative pose. According to the method, the offset of the camera is obtained through the camera-IMU tight coupling model, the IMU has the advantages of high data updating rate and high positioning precision, accumulation of position errors generated in the process of detecting the target pose of the image by the moving camera is made up, the offset of the camera and the offset pose are obtained through the high-precision IMU, and the accuracy of the target pose of the image is improved. And errors caused by camera shaking are reduced, so that the precision of the method is higher.

Description

Technical Field

[0001] The present invention relates to multi-source information fusion technology and image processing technology, and in particular to a method and system for determining the position and posture of an image target point by fusion of a camera and an IMU. Background Art

[0002] QR code is a two-dimensional barcode that encodes information on a two-dimensional plane through small black and white squares. It has the characteristics of high density, large capacity, and easy recognition. In recent years, with the popularization of mobile devices and smart devices, QR code has been widely used in advertising, payment, identity verification and other scenarios as a way to quickly transmit information.

[0003] At present, the method of using a camera to detect QR codes is to shoot with the camera in a stationary state, without taking into account that the camera itself may be offset. For example, a slight vibration of the ground may cause the camera itself to move slightly, which will affect the extraction effect of the central position of the QR code and reduce the recognition accuracy. When using a moving camera to detect QR codes, a large number of position errors will be generated during the movement, and as the movement time increases, the accumulated error will become larger and larger. Summary of the invention

[0004] In order to solve the above problems, the present invention discloses a method and system for determining the position and posture of an image target point by integrating a camera and an IMU.

[0005] The present invention discloses a method for determining the position and posture of an image target point by integrating a camera and an IMU, comprising the following steps:

[0006] Jointly calibrate the camera and IMU, and determine the camera-IMU tight coupling model based on the calibration results;

[0007] Obtaining a first relative position between a feature point and a target point of an image;

[0008] Acquire a second relative pose of the feature point relative to the offset camera using the camera-IMU tightly coupled model;

[0009] The pose of the target point relative to the camera is determined according to the first relative pose and the second relative pose.

[0010] The second relative position of the feature point relative to the offset camera is obtained by using the camera-IMU tight coupling model, specifically:

[0011] The camera-IMU tight coupling model is used to obtain the position and posture of the camera after the offset;

[0012] Obtaining observation data of the camera on the feature point;

[0013] A second relative position and posture of the feature point relative to the camera after the shift is determined according to the shifted position and posture and the observation data.

[0014] The camera-IMU tight coupling model is used to obtain the camera's offset posture, specifically:

[0015] Get the initial position of the camera;

[0016] The camera-IMU tight coupling model is used to obtain the position and posture offset of the camera at a preset time;

[0017] The posture of the camera after the shift is determined according to the initial posture and the posture offset.

[0018] There are multiple feature points, and accordingly, the first relative position between the feature points of the image and the target point is obtained, specifically:

[0019] Get the third relative pose between the feature points of the image,

[0020] According to the third relative posture, a first relative posture between the feature point and the target point of the image is acquired.

[0021] Preferably, the camera and IMU are jointly calibrated, and the camera-IMU tight coupling model is determined according to the calibration results, specifically:

[0022] Jointly calibrate the camera and IMU to obtain the camera parameters, IMU parameters, and the relative parameters between the camera and IMU;

[0023] A camera-IMU tight coupling model is determined according to the camera parameters, IMU parameters and relative parameters.

[0024] Preferably, a camera-IMU tight coupling model is determined according to the camera parameters, IMU parameters and relative parameters, specifically:

[0025] Determine a camera model according to the camera parameters;

[0026] Determine an IMU motion model according to the IMU parameters;

[0027] Performing posture conversion on the observation values ​​of the camera model and the IMU motion model according to the relative parameters;

[0028] A camera-IMU tight coupling model is determined based on the camera model, the IMU motion model and the converted observation values.

[0029] Preferably, the camera-IMU tight coupling model is determined according to the camera model, the IMU motion model and the converted observation values, specifically:

[0030] constructing a reprojection error based on the camera model;

[0031] Establishing an IMU error term based on the IMU motion model and the converted observations;

[0032] A camera-IMU tightly coupled model is determined according to the reprojection error and the IMU error term.

[0033] Preferably, the camera-IMU tight coupling model is determined according to the reprojection error and the IMU error term, specifically:

[0034] determining a first information matrix according to the reprojection error;

[0035] Determine a second information matrix according to the IMU error term;

[0036] A camera-IMU tightly coupled model is established according to the reprojection error, the IMU error term, the first information matrix and the second information matrix.

[0037] Preferably, the first relative position between the feature point and the target point of the image is obtained, specifically:

[0038] Use openCV to obtain the first relative pose between the feature points and the target points of the image.

[0039] Preferably, the present invention also includes a camera-IMU fusion image target point position determination system, including:

[0040] Model determination module: used to jointly calibrate the camera and IMU, and determine the camera-IMU tight coupling model based on the calibration results;

[0041] A first relative posture determination module: used to obtain a first relative posture between a feature point and a target point of an image;

[0042] A second relative posture determination module: used for obtaining a second relative posture of the feature point relative to the offset camera by using the camera-IMU tight coupling model;

[0043] Target point posture determination module: used to determine the posture of the target point relative to the camera based on the first relative posture and the second relative posture.

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

[0045] (1) The present invention obtains the camera offset through a camera-IMU tight coupling model. The IMU has the advantages of high data update rate and high positioning accuracy, which makes up for the accumulation of position errors generated by the motion camera in the process of detecting the image target posture. The camera offset and the posture after the offset are obtained through the high-precision IMU, which reduces the error caused by camera shake. Therefore, the method of the present invention has higher accuracy;

[0046] (2) The present invention first obtains the posture relationship between the feature points and target points of the image and the posture relationship between the feature points and the camera respectively, and then determines the posture relationship between the target point and the camera based on the above posture relationship. The calculation process is simple and has high accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 is a flow chart of the method of the present invention;

[0048] Figure 2 The diagram is a schematic diagram of an instrument operation for extracting the coordinates of a target point of a QR code using the method of the present invention in an embodiment;

[0049] Figure 3 This is a schematic diagram of the process of OpenCV identifying the feature points of a QR code;

[0050] Figure 4 It is the feature point of the QR code recognized by OpenCV;

[0051] In the figure, 1 is the left camera; 2 is the right camera; 3 is the inertial measurement unit; and 4 is the QR code. DETAILED DESCRIPTION

[0052] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present invention. However, it should be clear to those skilled in the art that the present invention may be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present invention.

[0053] like Figure 1 As shown, the present invention discloses a method for determining the position and posture of an image target point by fusion camera-IMU (IMU is an inertial measurement unit 3), comprising the following steps:

[0054] S1, jointly calibrate the camera and IMU, and determine the camera-IMU tight coupling model based on the calibration results;

[0055] S1 is specifically:

[0056] S11, jointly calibrate the camera and IMU to obtain camera parameters, IMU parameters, and relative parameters between the camera and IMU;

[0057] S12. Determine a camera-IMU tight coupling model according to camera parameters, IMU parameters and relative parameters.

[0058] S12 is specifically:

[0059] S121, determining a camera model according to camera parameters;

[0060] S122, determining an IMU motion model according to IMU parameters;

[0061] S123, performing posture conversion on the observation values ​​of the camera model and the IMU motion model according to the relative parameters;

[0062] S124, determining a camera-IMU tightly coupled model according to the camera model, the IMU motion model, and the converted observation values.

[0063] S124 is specifically:

[0064] S1241, constructing a reprojection error according to a camera model;

[0065] S1242, establishing an IMU error term according to the IMU motion model and the converted observation value;

[0066] S1243. Determine a camera-IMU tightly coupled model based on the reprojection error and the IMU error term.

[0067] S1243 is as follows:

[0068] S12431, determining a first information matrix according to a reprojection error;

[0069] S12432, determining a second information matrix according to the IMU error term;

[0070] S12433. Establish a tightly coupled model based on the reprojection error, the IMU error term, the first information matrix, and the second information matrix.

[0071] S2, obtaining a first relative position between a feature point of the image and a target point;

[0072] Preferably, openCV is used to obtain the first relative pose between the image feature point and the target point.

[0073] Preferably, there are multiple characteristic points, and accordingly, S2 is specifically:

[0074] S21, obtaining a third relative posture between feature points of the image,

[0075] S22. Acquire a first relative posture between the feature point and the target point of the image according to the third relative posture.

[0076] In this step, the feature points are pre-set points with features that can be recognized by openCV when the image is generated, and the number of feature points can be set as needed.

[0077] Furthermore, openCV can obtain and establish the pose relationship between the feature points and the target point based on the relative pose between the feature points. Taking the QR code as an example to illustrate step S2, after openCV identifies the three feature points (i.e. corner points) on the QR code, it connects the two corner points with the longest straight-line distance, and then draws a perpendicular line through the remaining corner points to obtain the position of the center point of the QR code (i.e. the target point) on the image, thereby obtaining the relative pose between the feature points and the target point.

[0078] S3, using the camera-IMU tight coupling model to obtain the second relative pose of the feature point relative to the offset camera;

[0079] Preferably, S3 is specifically:

[0080] S31, using the camera-IMU tight coupling model to obtain the camera's offset posture;

[0081] Preferably, S31 is specifically:

[0082] S311, obtaining an initial position and posture of the camera; the initial position and posture of the camera may be a known value or an unknown value. When the initial position and posture is an unknown value, the initial position and posture of the camera may be obtained by camera calibration.

[0083] S312, using the camera-IMU tight coupling model to obtain the position and posture offset of the camera at a preset time;

[0084] S313, determining the camera's posture after the shift according to the initial posture and the posture offset.

[0085] S32, obtaining the camera's observation data of the feature points; the observation data is the image captured by the camera, including the pixel coordinates of all points in the image.

[0086] S33, determining a second relative posture of the feature point relative to the camera after the shift according to the shifted posture and the observation data.

[0087] Since the camera and IMU are jointly calibrated, the coordinates of each pixel and the offset posture of the camera can be converted to the same coordinate system according to the calibration results, thereby obtaining the posture of each point relative to the offset camera.

[0088] Furthermore, S2 and S3 are steps performed simultaneously, that is, when openCV recognizes the feature points, the camera can obtain the pose of each point relative to the offset camera. The corresponding data processing system can extract the feature points from the relative poses of all points obtained by the camera according to the recognition results of openCV to obtain the second relative pose of the feature points.

[0089] S4. Determine the position and posture of the target point relative to the camera according to the first relative position and the second relative position.

[0090] Specifically, the relative pose includes relative position (i.e., coordinates in the corresponding coordinate system), relative speed, relative direction, and conversion relationship between coordinates, etc. Since the intrinsic and extrinsic parameters of the camera have been obtained in the process of establishing the camera-IMU tightly coupled model, in step S4, the target point and the feature point can be converted to the same coordinate system according to the intrinsic and extrinsic parameters of the camera and the first relative pose, and then the target point and the camera are converted to the same coordinate system according to the intrinsic and extrinsic parameters of the camera and the second relative pose, so as to obtain the pose of the target point relative to the camera.

[0091] Optionally, the camera of the present invention may be a monocular camera or a binocular camera. This embodiment takes the establishment of a binocular camera-IMU tight coupling model as an example for explanation, and the process is as follows:

[0092] 1. Camera Calibration

[0093] In this embodiment, the calibration board chessboard is used to perform Zhang calibration on the binocular cameras respectively. The calibration method of each camera is the same. The calibration includes obtaining the calibration parameters of the camera and correcting the distortion of the camera. In this application, the calibration parameters include: the intrinsic parameter matrix, extrinsic parameter matrix, distortion parameters of each camera, and the transformation matrix between the two camera coordinate systems.

[0094] Assume 2D image point: m = [u, v] T ; 3D image point: M = [X, Y, Z] T

[0095] Where (u,v) is the pixel coordinate of m; [X,Y,Z] is the world coordinate of M; then we have: homogeneous coordinates:

[0096] The present invention adopts a pinhole camera model, also known as a linear camera model, to describe the mapping from space coordinates to image coordinates. The mapping process involves four coordinate systems, namely, the pixel coordinate system, the image coordinate system, the camera coordinate system, and the world coordinate system. By explaining how each coordinate system is transformed, the intrinsic parameter matrix and the extrinsic parameter matrix of the camera can be obtained:

[0097]

[0098] Where: s is the scale factor from the world coordinate system to the image coordinate system; A is the camera intrinsic parameter matrix; u0, v0) are the image principal point coordinates; α, β are the fusion of focal length and pixel aspect ratio; γ is the radial distortion parameter; [R|t] is the extrinsic parameter matrix, R is the rotation matrix describing the camera direction, and t is the three-dimensional translation vector describing the camera center position.

[0099] Assume that the calibration board is located in the z = 0 plane, and define the i-th column of the rotation matrix R as r i, then:

[0100]

[0101] Then formula (1) can be changed to:

[0102]

[0103] Let H = [h1 h2 h3], then [h1 h2 h3] = λA[r1 r2 t], according to the constraints:

[0104]

[0105] Right now:

[0106]

[0107] From the above formula, we can get:

[0108] V ij =[h i1 h j1 h i1 h j2 +h i2 h j1 h i2 h j2 h i3 h j1 +h i1 h j3 h i3 h j2 +h i2 h j3 h i3 h j3 ] T (6)

[0110] From the constraints we get:

[0111]

[0112] Through three images of at least one chessboard, the above formula is applied to estimate B, and through Cholesky decomposition, the six degrees of freedom of the camera intrinsic parameter matrix A are obtained, namely:

[0113]

[0114] According to [h1 h2 h3] = λA[r1 r2 t], the parameters of the external parameter matrix can be obtained by simplification, that is:

[0115]

[0116] 2. Correct the camera’s distortion

[0117] In order to improve the imaging efficiency of the camera, there is a lens in the lens camera. The shape of the lens and the incomplete parallelism between the lens plane and the imaging plane during the installation process will affect the propagation of light during the imaging process, causing the position of the light to change during projection, including radial and tangential distortion.

[0118] Radial distortion is caused by the change in the shape of the lens. The closer to the edge of the image, the greater the image distortion.

[0119] A polynomial function can be used to correct radial distortion, as shown in the formula:

[0120]

[0121] where [x,y] T Indicates the coordinates of the previous point, [x rectified ,y rectified ] T represents the coordinates of the point after radial distortion correction, r is the distance from the point to the origin before correction, and k1, k2 and k3 are the distortion correction coefficients.

[0122] The distortion caused by the incomplete parallelism between the lens and the imaging plane during camera installation is tangential distortion. The correction of tangential distortion adopts formula (27):

[0123]

[0124] P1 and P2 are the distortion coefficients for a point P[X,Y,Z] in the camera coordinate system. T , its position in the pixel coordinate system can be determined through five distortion coefficients. The steps are as follows:

[0125] Step 1: First normalize the image plane and then project it. Let the normalized coordinates be [x, y] T .

[0126] Step 2: Radial and tangential distortion correction.

[0127]

[0128] Step 3: After correction, it is mapped to the pixel plane through K, as shown in formula (13), and the position of the point on the image can be obtained.

[0129]

[0130] The original image is corrected according to the calibration results, and the left and right views are dedistorted respectively. Then, through the position (rotation and translation) relationship of the binocular camera, the two corrected images are located in the same plane and parallel to each other.

[0131] 2. Calibrate IMU

[0132] Before integrating IMU to detect the center coordinates of the QR code, IMU needs to be preprocessed. The specific process is as follows:

[0133] 1. Establish inertial navigation error model

[0134] The six-axis IMU is composed of a three-axis gyroscope and a three-axis accelerometer. The gyroscope is used to measure the angular velocity of the carrier's motion, and the accelerometer is used to measure the acceleration of the carrier's own motion. Due to the existence of noise in the measurement value, the model of formula (14) is established between the accelerometer and gyroscope measurement value and the true value:

[0135]

[0136] It indicates The measured value of the accelerometer, expressed as The measured value of the gyroscope, a(t) and w(t) represent the true value of the accelerometer and gyroscope respectively, b a (t) and b g (t) is the random walk error of the accelerometer and gyroscope, u a (t) and u g (t) represent the noise of the two respectively.

[0137] Assume that the noise of the accelerometer and gyroscope are white noise and obey Gaussian distribution, then u a (t) and u g The mathematical expectation of (t) is 0, as shown in the following formula (15):

[0138]

[0139] In the formula, σ a is the noise intensity of the accelerometer, σ g is the noise intensity of the gyroscope.

[0140] In the actual calculation process, the IMU measurement data obtained are all discretely sampled, so the above formula needs to be discretized. There is a conversion relationship between the variance of discrete and continuous Gaussian white noise as shown in the following formula (16):

[0141] u d [k] = σ d ω[k] (16)

[0142] Where ω[k]~N(0,1), σ d is the discrete noise intensity, is the sampling time of the sensor.

[0143] The random walk of the bias of the accelerometer and gyroscope can be modeled as a Wiener process, as shown in formula (17):

[0144] b g (t) = σ bg ω(t) (17)

[0145] where ω(t) is unit Gaussian white noise, σ bg is the intensity of random walk. This model can be regarded as the integral of Gaussian white noise. The noise parameters are caused by the combined influence of the internal structure of the sensor, temperature and other factors. In the discrete case, it is called random walk, as shown in formula (18):

[0146] b d [k] = b d [k-1]+σ bgd ω[k] (18)

[0147] Where ω[k]~N(0,1), σ bgd is the discrete random walk intensity. According to the distribution of random walk, it can be seen that random walk is a Gaussian noise superimposed on the previous noise, and its next step is always random.

[0148] 2. Establish inertial navigation motion model

[0149] From the error model (14), we can get:

[0150]

[0151] in, and is the measured value of angular velocity and acceleration in the IMU coordinate system, w WB (t) and a w (t) are the true values ​​of angular velocity and acceleration, respectively, which are affected by self-bias and noise. w is the gravity in the world coordinate system, It is the rotation matrix from the world coordinate system to the IMU coordinate system.

[0152] The kinematic model of IMU is established as formula (20):

[0153]

[0154] The above formula is a model under continuous state, and the measured data obtained are all in discrete form. Therefore, the above formula is discretized to obtain the discretized kinematic equation as formula (21):

[0155]

[0156] The above equation is the kinematic equation in discrete form. Substituting the noise model in (14) into the above equation, we get the equation of the complete IMU model:

[0157]

[0158] The above formula is the complete IMU kinematic model, which represents the relationship between two adjacent image data, where u ad (t) and u gd (t) is u a (t) and u g (t) is a discrete value.

[0159] 3. Filter the data detected by IMU

[0160] The Hilbert-Huang transform method is used to process the noise data in the IMU. First, the measured data is subjected to empirical mode decomposition, which is a repeated screening process. First, the local maximum and minimum values ​​of the original data s(t) are processed according to the cubic spline function to form the upper and lower envelopes. Then, the difference between the original data and the mean m1(t) of the upper and lower envelopes is made to determine whether the difference h1(t) meets the inherent mode condition. If it does not meet the condition, it is screened again k times until it meets the condition. Finally, the first component c1(t) of the intrinsic mode function (IMF) is obtained, as shown in formula (10):

[0161] c1(t)=h k (t) = h k-1 (t)-m k (t) (23)

[0162] Finally, the first residual r1(t) is obtained by subtracting the original data from the first IMF component. At this time, r1(t) will be used as a new parameter. The above operation is repeated n times until the nth residual cannot be decomposed. Finally, the original measured data can be expressed by the following formula (24) after empirical mode decomposition:

[0163]

[0164] From formula (24), we can get:

[0165]

[0166] 4. Pre-integrate the IMU filtered data

[0167] It can be seen from the kinematic model of the IMU that when there are multiple frames of IMU data between the i-th and j-th frames, for the sake of simplicity in calculation, it is now assumed that the first frame of the IMU data is aligned with the i-th frame, that is, the i-th key frame, and the last frame of the IMU data is aligned with the j-th frame, that is, the j-th key frame. After alignment, the IMU data between two consecutive key frames are pre-integrated.

[0168] Assuming the time interval Δt, according to the kinematic formula, the update equation is calculated as shown in formula (26):

[0169]

[0170] The relative motion between two frames of images is calculated using the pre-integration method and is used as the IMU measurement value. The relative motion increment between time i and time j is defined as formula (27):

[0171]

[0172] Δv ij and Δt ij It is not a real speed and position increment. It can be observed that the definition of this relative motion increment makes the right side of the equation independent of the state and gravity at time i. Therefore, the IMU update formula between two consecutive key frame images can be written as formula (28):

[0173]

[0174] Among them, the Jacobian matrix Then the IMU pose is solved.

[0175] After calibrating the camera and IMU separately, the camera and IMU are jointly calibrated to obtain the conversion relationship between the camera and IMU.

[0176] 3. Establishing the reprojection error and the first information matrix

[0177] Extract position points from the image of the stereo camera and match them to calculate the camera's pose.

[0178] like Figure 2 As shown in the figure, assume that a position point P1 is detected in the image of the left camera 1, and the corresponding matching point P2 (same-named image point) is detected in the image of the right camera 2. Since there is a known fixed baseline and focal length between the two cameras, these two pieces of information and the pixel coordinates of P1 and P2 can be used to calculate the position point P in the three-dimensional space through the principle of triangulation.

[0179] Then, the position point P in the three-dimensional space is projected back to the image plane to obtain the theoretical projection point. Finally, the reprojection error between the theoretical projection point and the actual observation point is calculated, and the reprojection error is minimized through an optimization algorithm. The optimal camera posture is solved through an optimization algorithm (such as EPnP, ICP, etc.).

[0180] Taking the position points as landmarks, the camera reprojection error model is established as shown in formula (29):

[0181]

[0182] in is the camera reprojection error, h i (·) represents the i-th camera model, j is the landmark index, is the coordinate of the jth landmark. is the transformation matrix from the camera coordinate system to the world coordinate system of the kth frame, z i,j,k is the coordinate of the position point in the image coordinate system.

[0183] Then the information matrix of the reprojection error is for:

[0184]

[0185] In formula (30), is the covariance matrix.

[0186] 4. Establish IMU error term

[0187] The state of the IMU in the k-th frame image It can be expressed as formula (31), where The state is represented by quaternions. The state includes position, velocity, attitude (the orientation of the camera in three-dimensional space), and the bias of the accelerometer and gyroscope.

[0188]

[0189] In the tangent space on the manifold, the error of position and state is located as the difference between the estimated value and the true value. According to the exponential mapping and logarithmic mapping, the error state of the IMU on the k-th frame image is solved as follows:

[0190]

[0191] The IMU error term is expressed as a function of the camera state at time k and k+1, and the IMU measurement is recorded as Assume that the conditional probability density function follows a normal distribution: The state prediction item is The covariance is

[0192] Then the error term of IMU is as shown in formula (32):

[0193]

[0194] In formula (20), It represents the estimation of the IMU coordinate system relative to the world coordinate system at the k+1th frame image, and * represents the multiplication of quaternions. As with the camera reprojection error solution, its information matrix According to the error propagation process, it is formula (21):

[0195]

[0196] The Jacobian matrix in equation (21) can be obtained directly, but there is a non-zero solution because the rotation error is generally non-zero. Therefore, according to the pre-integration, the pose error between the k-th frame and the k+1-th frame image is obtained as equation (22):

[0197]

[0198] The Jacobian matrix in equation (34) above remains unchanged and can be calculated in the pre-integration stage. It mainly describes the change in the measured value due to the change in the bias estimate.

[0199] 5. Establish a camera-IMU tight coupling model

[0200] Substitute formula (34) into formula (32), and finally combine the reprojection error and the IMU error to establish the function model of system fusion optimization (35):

[0201]

[0202] In formula (23), I is the number of cameras and K is the total number of image frames. R ) is the optimization function, and the solution is to make D(x R ) value is the smallest position x R .

[0203] Solve for x using the Gauss-Newton method R The incremental equation for the variable Δx to be solved is established as formula (36):

[0204] HΔx=g (36)

[0205] Where H = J(x) T J(x), g = -J(x) T D(x), J(x) is the Jacobian matrix of D(x), and J is used in the Gauss-Newton method. TJ is an approximation of the second-order Hessian matrix, which converges quickly and obtains the optimized pose of the IMU relative to the world coordinate system, that is, the optimized pose of the camera after the IMU is coupled with the camera. The real-time image target point information is obtained through recognition.

[0206] like Figure 2 As shown, taking QR code 4 as an example, the instrument operation process of extracting the center point of QR code 4 in this embodiment is as follows:

[0207] The left camera 1 and the right camera 2 are calibrated separately to obtain the internal and external parameters and distortion coefficients of each camera. After the calibration, the cameras are placed on the device platform of the IMU sensor.

[0208] When the IMU is turned off, the calibrated camera is used to acquire images of the QR code 4 with different baseline lengths.

[0209] When the IMU is turned on, the calibrated camera is used to acquire images of the QR code 4 with different baseline lengths.

[0210] Acquire and process the IMU attitude data under two different baseline states.

[0211] like Figure 3 As shown in the figure, it is a schematic diagram of the process of obtaining the first relative pose in OpenCV 4.2.0, where the corner points are the feature points mentioned above, and the center point is the target point to be located. The text description of the process is as follows:

[0212] 1. Convert color image to grayscale image + Gaussian filtering;

[0213] 2. Binarization (Otsu automatic threshold);

[0214] 3. Morphological filtering (opening operation + closing operation);

[0215] 4. Canny edge detection;

[0216] 5. Contour extraction;

[0217] 6. Filter out three layers of independent contours containing characteristics;

[0218] 7. If the number of three-layer contours screened out is greater than 3, further screening is performed and only three are retained (e.g. Figure 4 shown);

[0219] 8. Sort the center points of the three corner elements of QR code 4;

[0220] 9. Accurately locate QR code 4.

[0221] Step 9 is as follows: (1) Obtain three corner elements, a total of 12 corner points (through the previous fitting quadrilateral results)

[0222] (2) Calculate the four corner points of the fourth corner element

[0223] (3) Calculate the four center points and further calculate the 4 center points of the QR code.

[0224] (4) Combine the IMU data to perform error evaluation on the center point coordinates.

[0225] The present invention also discloses a camera-IMU fusion image target point position and posture determination system, comprising:

[0226] Model determination module: used to jointly calibrate the camera and IMU, and determine the camera-IMU tight coupling model based on the calibration results;

[0227] A first relative posture determination module: used to obtain a first relative posture between a feature point and a target point of an image;

[0228] A second relative pose determination module: used to obtain a second relative pose of the feature point relative to the offset camera using a camera-IMU tight coupling model;

[0229] Target point posture determination module: used to determine the posture of the target point relative to the camera based on the first relative posture and the second relative posture.

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

[0231] (1) The present invention obtains the camera offset through a camera-IMU tight coupling model. The IMU has the advantages of high data update rate and high positioning accuracy, which makes up for the accumulation of position errors generated by the motion camera in the process of detecting the image target posture. The camera offset and the position information after the offset are obtained through the high-precision IMU, which reduces the error of the camera posture. Therefore, the method of the present invention has higher accuracy.

[0232] (2) The present invention first obtains the pose relationship between the image feature point and the target point and the pose relationship between the feature point and the camera respectively, and then determines the pose relationship between the target point and the camera based on the above pose relationship. The calculation process is simple and has high accuracy.

[0233] The above are only a few embodiments of the present application and do not constitute any form of limitation to the present application. Although the present application is disclosed as above with preferred embodiments, it is not intended to limit the present application. Any technician familiar with the profession, without departing from the scope of the technical solution of the present application, using the technical content disclosed above to make slight changes or modifications are equivalent to equivalent implementation cases and fall within the scope of the technical solution.

Claims

1. A method for determining the position and posture of an image target point by fusion of a camera and an IMU, characterized in that: The following steps are involved: Jointly calibrate the camera and IMU, and determine the camera-IMU tight coupling model based on the calibration results; Obtaining a first relative position between a feature point and a target point of an image; Acquire a second relative pose of the feature point relative to the offset camera using the camera-IMU tightly coupled model; The pose of the target point relative to the camera is determined according to the first relative pose and the second relative pose.

2. The method for determining the position and posture of an image target point using a fusion camera-IMU according to claim 1, characterized in that: The second relative position of the feature point relative to the offset camera is obtained by using the camera-IMU tight coupling model, specifically: The camera-IMU tight coupling model is used to obtain the position and posture of the camera after the offset; Obtaining observation data of the camera on the feature point; A second relative position and posture of the feature point relative to the camera after the shift is determined according to the shifted position and posture and the observation data.

3. The method for determining the position and posture of an image target point using a fusion camera-IMU according to claim 2, characterized in that: The camera-IMU tight coupling model is used to obtain the camera's offset posture, specifically: Get the initial position of the camera; The camera-IMU tight coupling model is used to obtain the position and posture offset of the camera at a preset time; The posture of the camera after the shift is determined according to the initial posture and the posture offset.

4. The method for determining the position and posture of an image target point using a fusion camera-IMU according to claim 1, characterized in that: There are multiple feature points, and accordingly, the first relative position between the feature points of the image and the target point is obtained, specifically: Get the third relative pose between the feature points of the image, According to the third relative posture, a first relative posture between the feature point and the target point of the image is acquired.

5. The method for determining the position and posture of an image target point using a fusion camera-IMU according to claim 1, characterized in that: Jointly calibrate the camera and IMU, and determine the camera-IMU tight coupling model based on the calibration results, specifically: Jointly calibrate the camera and IMU to obtain the camera parameters, IMU parameters, and the relative parameters between the camera and IMU; A camera-IMU tight coupling model is determined according to the camera parameters, IMU parameters and relative parameters.

6. The method for determining the position and posture of an image target point using a fusion camera-IMU according to claim 5, characterized in that: According to the camera parameters, IMU parameters and relative parameters, a camera-IMU tight coupling model is determined, specifically: Determine a camera model according to the camera parameters; Determine an IMU motion model according to the IMU parameters; Performing posture conversion on the observation values ​​of the camera model and the IMU motion model according to the relative parameters; A camera-IMU tight coupling model is determined based on the camera model, the IMU motion model and the converted observation values.

7. The method for determining the position and posture of an image target point using a fusion camera-IMU according to claim 6, characterized in that: The camera-IMU tight coupling model is determined according to the camera model, the IMU motion model and the converted observation values, specifically: constructing a reprojection error based on the camera model; Establishing an IMU error term based on the IMU motion model and the converted observations; A camera-IMU tightly coupled model is determined according to the reprojection error and the IMU error term.

8. The method for determining the position and posture of an image target point using a fusion camera-IMU according to claim 7, characterized in that: The camera-IMU tight coupling model is determined according to the reprojection error and the IMU error term, specifically: determining a first information matrix according to the reprojection error; Determine a second information matrix according to the IMU error term; A camera-IMU tightly coupled model is established according to the reprojection error, the IMU error term, the first information matrix and the second information matrix.

9. The method for determining the position and posture of an image target point using a fusion camera-IMU according to claim 1, characterized in that: Get the first relative pose between the feature point and the target point of the image, specifically: Use openCV to obtain the first relative pose between the feature points and the target points of the image.

10. A camera-IMU fusion image target point position determination system, characterized in that: include: Model determination module: used to jointly calibrate the camera and IMU, and determine the camera-IMU tight coupling model based on the calibration results; A first relative posture determination module: used to obtain a first relative posture between a feature point and a target point of an image; A second relative posture determination module: used for obtaining a second relative posture of the feature point relative to the offset camera by using the camera-IMU tight coupling model; Target point posture determination module: used to determine the posture of the target point relative to the camera based on the first relative posture and the second relative posture.

Images