Camera-IMU relative attitude calibration method, computer storage medium and device

Abstract

The present invention relates to the technical field of inertial navigation systems, and in particular to a camera-IMU relative attitude calibration method, a computer storage medium and a device. The method comprises: in step S1, suspending a non-coplanar first apriltag code and a second apriltag code; combining steps S2-S6 to obtain R I c , using R I c The camera-IMU relative attitude calibration is completed. The computer storage medium stores computer program instructions, and the method is implemented when the computer program instructions are executed by the processor. The device includes at least one processor, at least one memory, and computer program instructions stored in the memory, and the method is implemented when the computer program instructions are executed by the processor. The present invention can greatly simplify the calibration process, shorten the calibration time, achieve the effect of rapid external parameter calibration, and solve the problem of high calibration cost caused by the introduction of auxiliary equipment.

Description

Technical Field

[0001] The present invention relates to the technical field of inertial navigation systems, and in particular to a camera-IMU relative attitude calibration method, a computer storage medium and a device. Background Art

[0002] In recent years, inertial navigation systems have been widely used for motion estimation of objects, such as smart vehicles, unmanned aircraft, and mining trucks. The core of most inertial navigation systems is the inertial measurement unit (abbreviated as IMU), which is used to measure linear acceleration and rotational angular velocity. By integrating the linear acceleration and angular velocity, the position, velocity, and attitude information of the object can be measured. However, the inertial measurement unit has noise and bias, which will cause cumulative errors. At present, other sensors, such as GPS, are often used for combined navigation with the inertial measurement unit to achieve complementary advantages and correct inertial device errors. However, GPS is prone to limitations, such as: 1) GPS signals are not available in environments such as indoors and underground; 2) Due to mirror reflection and multipath effects, the reliability of GPS signals is low near high-rise buildings and high-rise structures; 3) High-precision GPS receivers are very expensive.

[0003] Visual sensors, such as cameras, are small, light, low-cost, and can provide a wealth of environmental information for the carrier. By observing known scenes, such as landmarks, the camera's position and posture can be calculated. By tracking visual features through image sequences, the camera's motion trajectory can be estimated, with no accumulated errors and high long-term stability. However, visual sensors are affected by factors such as feature point matching, and their calculation accuracy is average, the sampling rate is low, and the carrier cannot perform effective motion recovery when making high maneuvers (i.e., fast movements), and the camera cannot provide scale. Inertial measurement units have a large output bandwidth, good high-maneuver tracking, high short-term calculation accuracy, and can provide scale information, but the long-term calculation has large accumulated errors. Combining visual sensors and inertial measurement units to form a visually assisted inertial navigation system has become a hot research topic. The two devices are complementary in measurement characteristics, and combining them for data fusion can greatly improve system performance.

[0004] Existing methods for calibrating external parameters of IMUs and cameras include: 1) directly given based on computer-aided design (CAD) or manual measurement, which requires a lot of time and effort. When the sensor needs to be replaced or repaired, the external parameters cannot be calibrated quickly; 2) moving the device or controlling the device along a certain trajectory by a person, calculating the pose matrix based on hand-eye calibration, which has strict requirements on time alignment and motion trajectory, making the calibration process complicated; 3) calculating by using auxiliary equipment, such as using a high-precision 3D laser scanner to scan the device to be calibrated, and obtaining the point cloud data values ​​of the camera chip and IMU for calibration; however, the auxiliary equipment is expensive and the process is complicated.

[0005] In summary, it is necessary to provide a camera-IMU relative attitude calibration method, a computer storage medium and a device to solve the problems existing in the prior art of being unable to quickly calibrate external parameters, a complicated calibration process and high calibration costs due to the introduction of auxiliary equipment. Summary of the invention

[0006] The purpose of the present invention is to provide a camera-IMU relative attitude calibration method, a computer storage medium and a device. The specific technical solution is as follows:

[0007] In a first aspect, the present invention provides a camera-IMU relative attitude calibration method, comprising:

[0008] Step S1, suspend the first April tag code and the second April tag code, and make them not coplanar; determine the three-dimensional coordinate system of the first April tag code as A1; the coordinate origin of A1 is at the center of the first April tag code, and the coordinate x of A1 is A1 and coordinate y A1 All are in the plane of the first apriltag code, and the coordinate z of A1 is A1 perpendicular to the first apriltag code pointing to the horizontal direction; determine the three-dimensional coordinate system of the second apriltag code as A2; the coordinate origin of A2 is at the center of the second apriltag code, and the coordinate x of A2 A2 and coordinate y A2 Both are in the plane of the second apriltag code, and the coordinate z of A2 A2 A horizontal direction perpendicular to the direction of the second apriltag code;

[0009] Step S2: setting a collection position p within the information collection range for the first april tag code and the second april tag code i , where i = 1, 2; at the acquisition position p i A camera and an inertial measurement unit IMU are set at the position p, and the three-dimensional coordinate system of the camera is determined to be c, and the three-dimensional coordinate system of the inertial measurement unit IMU is determined to be I; in a static state, the camera is used at the acquisition position p i At the positions p, the image of the first april tag code and the image of the second april tag code are respectively collected; in a static state, the inertial measurement unit IMU is used to collect the image of the first april tag code at the collection position p i The gravity acceleration information of the earth is collected at I (p i );

[0010] Step S3: Filter and denoise the image of the first apriltag code and the image of the second apriltag code collected in step S2 to obtain processed images; perform apriltag code detection on the processed images according to the machine vision opencv apriltag algorithm library to obtain the collection position p i Where and in, Indicates the attitude transformation relationship from the camera's three-dimensional coordinate system c to the three-dimensional coordinate system A1 of the first apriltag code; Indicates the attitude transformation relationship from the camera's three-dimensional coordinate system c to the three-dimensional coordinate system A2 of the second apriltag code;

[0011] Step S4: Combined with step S3 And z in step S1 A1 The unit vector of

[001] T get in, Represents the projection coordinates of the horizontal unit vector corresponding to the first apriltag code in the three-dimensional coordinate system c of the camera;

[0012] Combined with the result obtained in step S3 And z in step S1 A2 The unit vector of

[001] T get in, Represents the projection coordinates of the horizontal unit vector corresponding to the second apriltag code in the three-dimensional coordinate system c of the camera;

[0013] Step S5: Combined with step S4 The average value within time t and The average value within time t Find g c (p i );g c (p i ) represents the acquisition position p i The projection coordinates of the gravity vector at the camera in the three-dimensional coordinate system c are normalized; combined with f in step S2 I (p i ) is the average value within time t Find g I (p i );g I (p i ) represents the acquisition position p iThe projection coordinates of the gravity vector at the position in the three-dimensional coordinate system I of the inertial measurement unit IMU are normalized; wherein, the value range of t is 5-10s;

[0014] Step S6: Combine the g obtained in step S5 c (p i ) and g I (p i )get Represents the attitude transformation between the camera and the inertial measurement unit; using Complete the camera-IMU relative attitude calibration.

[0015] Optionally, in step S4, formula (1) is used in combination with the formula obtained in step S3 And z in step S1 A1 The unit vector of

[001] T get The formula (1) is as follows:

[0016]

[0017] Optionally, in step S4, formula (2) is used in combination with the formula obtained in step S3 And z in step S1 A2 The unit vector of

[001] T get The formula (2) is as follows:

[0018]

[0019] Optionally, in step S5, formula (3) is used in combination with step S4. The average value within time t and The average value within time t Find g c (p i ); the formula (3) is as follows:

[0020]

[0021] Optionally, in step S5, formula (4) is used in combination with f in step S2. I (p i ) is the average value within time t Find g I (p i ); the formula (4) is as follows:

[0022]

[0023] Optionally, in step S5, formulas (5) to (9) are used in combination with g obtained in step S5.c (p i ) and g I (p i )get

[0024] Among them, g I (p 1 ) represents the acquisition position p 1 The projection coordinates of the gravity vector at the inertial measurement unit IMU in the three-dimensional coordinate system I are normalized; g I (p 2 ) represents the acquisition position p 2 The projection coordinates of the gravity vector at the inertial measurement unit IMU in the three-dimensional coordinate system I are normalized; g c (p 1 ) represents the acquisition position p 1 The projection coordinates of the gravity vector at the camera in the three-dimensional coordinate system c are normalized; g c (p 2 ) represents the acquisition position p 2 The projection coordinates of the gravity vector at the camera in the three-dimensional coordinate system c are normalized;

[0025] By combining equations (5)-(7), we can get the matrix form shown in equation (8):

[0026]

[0027] Transformed from formula (8), we can get formula (9):

[0028]

[0029] Optionally, in the three-dimensional coordinate system c of the camera, the origin of the coordinates of c is at the optical center of the camera, and the coordinates x of c are c and the x coordinate of A1 A1 Parallel, coordinate y of c c and the y coordinate of A1 A1 Parallel, coordinate z of c c is the optical axis of the camera.

[0030] Optionally, a first accelerometer, a second accelerometer and a third accelerometer are arranged in the inertial measurement unit IMU; a first line connecting the first accelerometer and the center position of the inertial measurement unit IMU, a second line connecting the second accelerometer and the center position of the inertial measurement unit IMU, and a third line connecting the third accelerometer and the center position of the inertial measurement unit IMU are all perpendicular to each other; in a three-dimensional coordinate system I of the inertial measurement unit IMU, the coordinate origin of I is at the center position of the inertial measurement unit IMU, and the coordinate x of I is I Set on the first line, the coordinate y of I I Set on the second line, the coordinate z of c I It is arranged on the third connecting line.

[0031] In a second aspect, the present invention provides a computer storage medium having computer program instructions stored thereon, which implement the camera-IMU relative attitude calibration method when the computer program instructions are executed by a processor.

[0032] In a third aspect, the present invention provides a device comprising: at least one processor, at least one memory, and computer program instructions stored in the memory, wherein when the computer program instructions are executed by the processor, the camera-IMU relative attitude calibration method is implemented.

[0033] The application of the technical solution of the present invention has at least the following beneficial effects:

[0034] The camera-IMU relative attitude calibration method, computer storage medium and device provided by the present invention include hanging a non-coplanar first April tag code and a second April tag code in step S1; in a static state in step S2, using a camera at a collection position p i The image of the first April tag code and the image of the second April tag code are collected at the collection position p respectively, and the inertial measurement unit IMU is used to collect the image of the first April tag code and the image of the second April tag code at the collection position p i The gravity acceleration information of the earth is collected at I (p i ); Combine steps S3-S5 to obtain g c (p i ) and g I (p i ), then adopt step S6 to combine g c (p i ) and g I (p i )get use The camera-IMU relative attitude calibration is completed. The present invention adopts the suspension of the non-coplanar first AprilTag code and the second AprilTag code, which, on the one hand, breaks through the limitation of the uneven experimental site; on the other hand, through static acquisition and at different acquisition positions p i The asynchronous acquisition of the position p facilitates improving the accuracy of the camera-IMU relative attitude calibration, thereby achieving a better fusion effect; on the other hand, it can also solve the problem of high calibration cost caused by introducing auxiliary equipment; in addition, the present invention adopts the acquisition position p i There are only two places, i=1 and 2, which greatly simplifies the calibration process, shortens the calibration time, and achieves the effect of rapid external parameter calibration.

[0035] In addition to the above-described purposes, features and advantages, the present invention has other purposes, features and advantages. The present invention will be further described in detail with reference to the accompanying drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] The drawings constituting a part of this application are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0037] Figure 1 It is a structural diagram of the camera-IMU relative attitude calibration method in an embodiment of the present invention. DETAILED DESCRIPTION

[0038] The following will be combined with the accompanying drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field belong to the scope of protection of the present invention.

[0039] Example:

[0040] See also Figure 1 , the camera-IMU relative attitude calibration method includes:

[0041] Step S1, suspend the first April tag code and the second April tag code, and make them not coplanar; determine the three-dimensional coordinate system of the first April tag code as A1; the coordinate origin of A1 is at the center of the first April tag code, and the coordinate x of A1 is A1 and coordinate y A1 All are in the plane of the first apriltag code, and the coordinate z of A1 is A1perpendicular to the first apriltag code pointing to the horizontal direction; determine the three-dimensional coordinate system of the second apriltag code as A2; the coordinate origin of A2 is at the center of the second apriltag code, and the coordinate x of A2 A2 and coordinate y A2 Both are in the plane of the second apriltag code, and the coordinate z of A2 A2 A horizontal direction perpendicular to the direction of the second apriltag code;

[0042] Step S2: setting a collection position p within the information collection range for the first april tag code and the second april tag code i , where i = 1, 2; at the acquisition position p i A camera and an inertial measurement unit IMU are set at the position p, and the three-dimensional coordinate system of the camera is determined to be c, and the three-dimensional coordinate system of the inertial measurement unit IMU is determined to be I; in a static state, the camera is used at the acquisition position p i At the positions p, the image of the first april tag code and the image of the second april tag code are respectively collected; in a static state, the inertial measurement unit IMU is used to collect the image of the first april tag code at the collection position p i The gravity acceleration information of the earth is collected at I (p i );

[0043] Step S3: Filter and denoise the image of the first apriltag code and the image of the second apriltag code collected in step S2 to obtain processed images; perform apriltag code detection on the processed images according to the machine vision opencv apriltag algorithm library to obtain the collection position p i Where and in, Indicates the attitude transformation relationship from the camera's three-dimensional coordinate system c to the three-dimensional coordinate system A1 of the first apriltag code; Indicates the attitude transformation relationship from the camera's three-dimensional coordinate system c to the three-dimensional coordinate system A2 of the second apriltag code;

[0044] Step S4: Combined with step S3 And z in step S1 A1 The unit vector of

[001] T get in, Represents the projection coordinates of the horizontal unit vector corresponding to the first apriltag code in the three-dimensional coordinate system c of the camera;

[0045] Combined with the result obtained in step S3 And z in step S1 A2 The unit vector of

[001] T get in, Represents the projection coordinates of the horizontal unit vector corresponding to the second apriltag code in the three-dimensional coordinate system c of the camera;

[0046] Step S5: Combined with step S4 The average value within time t and The average value within time t Find g c (p i );g c (p i ) represents the acquisition position p i The projection coordinates of the gravity vector at the camera in the three-dimensional coordinate system c are normalized; combined with f in step S2 I (p i ) is the average value within time t Find g I (p i );g I (p i ) represents the acquisition position p i The projection coordinates of the gravity vector at the position in the three-dimensional coordinate system I of the inertial measurement unit IMU are normalized; wherein, the value range of t is 5-10s;

[0047] Step S6: Combine g obtained in step S5 c (p i ) and g I (p i )get Represents the attitude transformation between the camera and the inertial measurement unit; using Complete the camera-IMU relative attitude calibration.

[0048] In step S4, formula (1) is used in combination with step S3 to obtain And z in step S1 A1 The unit vector of [0 0 1] T get The formula (1) is as follows:

[0049]

[0050] In step S4, formula (2) is used in combination with step S3 to obtain And z in step S1 A2 The unit vector of [0 0 1] T get The formula (2) is as follows:

[0051]

[0052] In step S5, formula (3) is used in combination with step S4. The average value within time t and The average value within time t Find g c (p i ); the formula (3) is as follows:

[0053]

[0054] In step S5, formula (4) is used in combination with f in step S2. I (p i ) is the average value within time t Find g I (p i ); the formula (4) is as follows:

[0055]

[0056] In step S5, equations (5) to (9) are used in combination with g obtained in step S5. c (p i ) and g I (p i )get

[0057] Among them, g I (p 1 ) represents the acquisition position p 1 The projection coordinates of the gravity vector at the inertial measurement unit IMU in the three-dimensional coordinate system I are normalized; g I (p 2 ) represents the acquisition position p 2 The projection coordinates of the gravity vector at the inertial measurement unit IMU in the three-dimensional coordinate system I are normalized; g c (p 1 ) represents the acquisition position p 1 The projection coordinates of the gravity vector at the camera in the three-dimensional coordinate system c are normalized; g c (p 2 ) represents the acquisition position p 2 The projection coordinates of the gravity vector at the camera in the three-dimensional coordinate system c are normalized;

[0058] By combining equations (5)-(7), we can get the matrix form shown in equation (8):

[0059]

[0060] From formula (8), we can get R as shown in formula (9): I c :

[0061]

[0062] In the three-dimensional coordinate system c of the camera, the origin of the coordinates of c is at the optical center of the camera, and the coordinates of c are x c and the x coordinate of A1 A1 Parallel, coordinate y of c c and the y coordinate of A1 A1 Parallel, coordinate z of c c is the optical axis of the camera.

[0063] A first accelerometer, a second accelerometer and a third accelerometer are arranged in the inertial measurement unit IMU; a first line connecting the first accelerometer and the center position of the inertial measurement unit IMU, a second line connecting the second accelerometer and the center position of the inertial measurement unit IMU, and a third line connecting the third accelerometer and the center position of the inertial measurement unit IMU are all perpendicular to each other; in a three-dimensional coordinate system I of the inertial measurement unit IMU, the coordinate origin of I is at the center position of the inertial measurement unit IMU, and the coordinate x of I is I Set on the first line, the coordinate y of I I Set on the second line, the coordinate z of c I It is arranged on the third connecting line.

[0064] This embodiment adopts the method of hanging non-coplanar first AprilTag code and second AprilTag code, which, on the one hand, breaks through the limitation of uneven experimental site; on the other hand, through static collection and at different collection positions p i The asynchronous acquisition of the position p facilitates improving the accuracy of the camera-IMU relative attitude calibration, thereby achieving a better fusion effect; on the other hand, it can also solve the problem of high calibration cost caused by introducing auxiliary equipment; in addition, the present invention adopts the acquisition position p i There are only two places, i=1 and 2, which greatly simplifies the calibration process, shortens the calibration time, and achieves the effect of rapid external parameter calibration.

[0065] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. For those skilled in the art, the present invention may have various modifications and variations. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included in the protection scope of the present invention.

Claims

1. Camera-IMU relative attitude calibration method, characterized in that: include: Step S1, hanging a first april tag code and a second april tag code, and making them not coplanar; Determine the three-dimensional coordinate system of the first apriltag code as A1; the coordinate origin of A1 is at the center of the first apriltag code, and the coordinate x of A1 A1 and coordinate y A1 All are in the plane of the first apriltag code, and the coordinate z of A1 is A1 perpendicular to the first apriltag code pointing to the horizontal direction; determine the three-dimensional coordinate system of the second apriltag code as A2; the coordinate origin of A2 is at the center of the second apriltag code, and the coordinate x of A2 A2 and coordinate y A2 Both are in the plane of the second apriltag code, and the coordinate z of A2 A2 A horizontal direction perpendicular to the direction of the second apriltag code; Step S2: setting a collection position p within the information collection range for the first april tag code and the second april tag code i , where i = 1, 2; at the acquisition position p i A camera and an inertial measurement unit IMU are set at the position p, and the three-dimensional coordinate system of the camera is determined to be c, and the three-dimensional coordinate system of the inertial measurement unit IMU is determined to be I; in a static state, the camera is used at the acquisition position p i At the positions p, the image of the first april tag code and the image of the second april tag code are respectively collected; in a static state, the inertial measurement unit IMU is used to collect the image of the first april tag code at the collection position p i The gravity acceleration information of the earth is collected at I (p i ); Step S3: Filter and denoise the image of the first apriltag code and the image of the second apriltag code collected in step S2 to obtain processed images; perform apriltag code detection on the processed images according to the machine vision opencv apriltag algorithm library to obtain the collection position p i Where and in, Indicates the attitude transformation relationship from the camera's three-dimensional coordinate system c to the three-dimensional coordinate system A1 of the first apriltag code; Indicates the attitude transformation relationship from the camera's three-dimensional coordinate system c to the three-dimensional coordinate system A2 of the second apriltag code; Step S4: Combined with step S3 And z in step S1 A1 The unit vector of [001] T get in, Represents the projection coordinates of the horizontal unit vector corresponding to the first apriltag code in the three-dimensional coordinate system c of the camera; Combined with the result obtained in step S3 And z in step S1 A2 The unit vector of [001] T get in, Represents the projection coordinates of the horizontal unit vector corresponding to the second apriltag code in the three-dimensional coordinate system c of the camera; Step S5: Combined with step S4 The average value within time t and The average value within time t Find g c (p i );g c (p i ) represents the acquisition position p i The projection coordinates of the gravity vector at the camera in the three-dimensional coordinate system c are normalized; combined with f in step S2 I (p i ) is the average value within time t Find g I (p i );g I (p i ) represents the acquisition position p i The projection coordinates of the gravity vector at the position in the three-dimensional coordinate system I of the inertial measurement unit IMU are normalized; wherein, the value range of t is 5-10s; Step S6: Combine the g obtained in step S5 c (p i ) and g I (p i )get Represents the attitude transformation between the camera and the inertial measurement unit; using Complete the camera-IMU relative attitude calibration.

2. The camera-IMU relative attitude calibration method according to claim 1, characterized in that: In step S4, formula (1) is used in combination with step S3 to obtain And z in step S1 A1 The unit vector of [001] T get The formula (1) is as follows:

3. The camera-IMU relative attitude calibration method according to claim 1, characterized in that: In step S4, formula (2) is used in combination with step S3 to obtain And z in step S1 A2 The unit vector of [001] T get The formula (2) is as follows:

4. The camera-IMU relative attitude calibration method according to claim 1, characterized in that: In step S5, formula (3) is used in combination with step S4. The average value within time t and The average value within time t Find g c (p i ); the formula (3) is as follows:

5. The camera-IMU relative attitude calibration method according to claim 1, characterized in that: In step S5, formula (4) is used in combination with f in step S2. I (p i ) is the average value within time t Find g I (p i ); the formula (4) is as follows:

6. The camera-IMU relative attitude calibration method according to any one of claims 1 to 5, characterized in that: In step S5, equations (5) to (9) are used in combination with g obtained in step S5. c (p i ) and g I (p i )get Among them, g I (p1) represents the normalized projection coordinates of the gravity vector at the acquisition position p1 in the three-dimensional coordinate system I of the inertial measurement unit IMU; g I (p2) represents the normalized projection coordinates of the gravity vector at the acquisition position p2 in the three-dimensional coordinate system I of the inertial measurement unit IMU; g c (p1) represents the normalized projection coordinates of the gravity vector at the acquisition position p1 in the camera's three-dimensional coordinate system c; g c (p2) represents the normalized projection coordinates of the gravity vector at the acquisition position p2 in the three-dimensional coordinate system c of the camera; By combining equations (5)-(7), we can get the matrix form shown in equation (8): Transformed from formula (8), we can get formula (9):

7. The camera-IMU relative attitude calibration method according to claim 6, characterized in that: In the three-dimensional coordinate system c of the camera, the origin of the coordinates of c is at the optical center of the camera, and the coordinates of c are x c and the x coordinate of A1 A1 Parallel, coordinate y of c c and the y coordinate of A1 A1 Parallel, coordinate z of c c is the optical axis of the camera.

8. The camera-IMU relative attitude calibration method according to claim 6, characterized in that: A first accelerometer, a second accelerometer and a third accelerometer are arranged in the inertial measurement unit IMU; a first line connecting the first accelerometer and the center position of the inertial measurement unit IMU, a second line connecting the second accelerometer and the center position of the inertial measurement unit IMU, and a third line connecting the third accelerometer and the center position of the inertial measurement unit IMU are all perpendicular to each other; in a three-dimensional coordinate system I of the inertial measurement unit IMU, the coordinate origin of I is at the center position of the inertial measurement unit IMU, and the coordinate x of I is I Set on the first line, the coordinate y of I I Set on the second line, the coordinate z of c I It is arranged on the third connecting line.

9. A computer storage medium, characterized in that: Computer program instructions are stored thereon, and when the computer program instructions are executed by a processor, the camera-IMU relative attitude calibration method as claimed in claim 6 is implemented.

10. A device, characterized in that: include: At least one processor, at least one memory, and computer program instructions stored in the memory, when the computer program instructions are executed by the processor, implement the camera-IMU relative attitude calibration method as claimed in claim 6.

Images