Monocular vision and inertial navigation combined calibration device and method based on relative quantity measurement

Abstract

The invention belongs to the field of monocular vision and inertial navigation combined calibration, and particularly relates to a monocular vision and inertial navigation combined calibration device and method based on relative quantity measurement. According to the device, inertial navigation is mounted on a checkerboard calibration plate in a strapdown manner to form a joint calibration device, a checkerboard strapdown inertial navigation calculation value is used as relative measurement to deduce an attitude matrix of inertial navigation to be calibrated, and the relative measurement is ingeniously utilized to convert a matrix to be solved. Meanwhile, the relative rotation matrix of the to-be-calibrated camera can be obtained through the relative rotation of the checkerboard calibration plate. Compared with an existing method that the carrier needs to rotate in the direction of two or more axes to obtain the relative rotation amount, the method has the advantages that the carrier does not need to rotate in multiple axes, the difficulty in obtaining the relative rotation amount is greatly reduced for large carriers such as missiles which are inconvenient to rotate, and the difficulty in united calibration of a monocular camera and inertial navigation is also greatly reduced.

Description

Technical Field

[0001] The invention belongs to the field of monocular vision and inertial navigation joint calibration, and specifically relates to a monocular vision and inertial navigation joint calibration device and method based on relative quantity measurement. Background Art

[0002] Monocular vision and inertial navigation sensors are the mainstream sensors of unmanned systems, such as unmanned vehicles and cruise missiles, and the joint calibration of the two is a necessary step for data fusion of the two sensors. There are currently two main algorithms for the joint calibration of inertial navigation and monocular vision. One is the rotation vector method, which first collects two relative rotation matrices of inertial navigation and vision, decomposes them into rotation vectors, and finally uses the direction of the rotation vector to solve the joint calibration matrix of inertial navigation and monocular vision. The other method is the hand-eye calibration method, which also collects two or more relative rotation matrices of inertial navigation and vision, and further uses the Tsai formula to solve the relative rotation matrix.

[0003] The above methods all require two or more relative rotations of the inertial navigation and monocular vision sensors to meet the two-dimensional or more relative rotations required by the solution process. Since the inertial navigation and monocular cameras are fixed on a carrier, and when the carrier is too large to perform relative rotations in multiple axes, this method fails. For example, when calibrating the monocular and inertial navigation sensors on an unmanned vehicle, it is extremely difficult for the unmanned vehicle to perform relative rotations in two axes. For another example, when calibrating the monocular and inertial navigation sensors on a cruise missile, it is extremely inconvenient for the cruise missile to perform relative rotations in two axes.

[0004] In view of this, how to design a joint calibration method of monocular vision and inertial navigation sensors by using external auxiliary calibration device has become an urgent problem to be solved in the engineering field. Summary of the invention

[0005] 1. Technical issues to be resolved

[0006] The technical problem to be solved by the present invention is to design a device that can provide relative quantity measurement when the carrier carrying the camera and the inertial navigation is inconvenient to rotate multiple axes, so as to complete the joint calibration of the inertial navigation and vision. At the same time, a joint calibration method is designed based on the quantity measurement. The method only requires the relative rotation of the carrier in one direction to achieve high-precision joint calibration of the monocular camera and the strapdown inertial navigation.

[0007] (II) Technical solution

[0008] In order to solve the above technical problems, the present invention provides a monocular vision and inertial navigation joint calibration device based on relative quantity measurement, the device comprising: a checkerboard calibration board, a checkerboard strapdown inertial navigation system is installed on the checkerboard calibration board, and the relative rotation relationship between the checkerboard calibration board and the checkerboard strapdown inertial navigation system is known; the size of each grid of the checkerboard is known; a data communication cable is installed on the checkerboard strapdown inertial navigation system; one end of the data communication cable is connected to a joint calibration computer; the joint calibration computer is used to run a joint calibration program;

[0009] The device also includes an inertial navigation system to be calibrated and a camera to be calibrated, both of which are fixedly connected to the carrier and both of which are connected to the joint calibration computer.

[0010] In addition, the present invention also provides a monocular vision and inertial navigation joint calibration method based on relative quantity measurement, the method is implemented based on the device, and the method comprises the following steps:

[0011] Step A. Place the chessboard in the field of view of the camera to be calibrated, rotate and translate the distance of the chessboard relative to the camera to be calibrated, and keep the chessboard in the field of view of the camera, collect 25 pictures, and use Zhang Youzheng calibration method to perform intrinsic parameter calibration on the camera to obtain the camera intrinsic parameter matrix K and distortion vector P;

[0012] Step B. The INS to be calibrated and the chessboard strapdown INS are relatively stationary and rotate in one direction as a whole. Timestamp alignment and transfer alignment based on attitude matching are performed to obtain the initial relative matrix of the INS to be calibrated relative to the chessboard strapdown INS.

[0013] Step C. Collect the raw data of the inertial navigation system to be calibrated, collect the raw data of the chessboard strapdown inertial navigation system for initial alignment, and obtain the attitude matrix of the chessboard strapdown inertial navigation system relative to the navigation coordinate system.

[0014] Step D. Place the chessboard in the field of view of the camera to be calibrated. The camera to be calibrated re-collects a chessboard image. Based on the chessboard size L, K and P in step A, perform relative posture calculation to obtain the rotation matrix of the camera relative to the chessboard coordinate system. At the same time, the chessboard strapdown inertial navigation data is collected to perform navigation settlement and obtain the attitude matrix This is state 1; the navigation solution is a mature method in this field;

[0015] Step E. Rotate the chessboard 45° along the roll axis of the inertial navigation system to be calibrated, and repeat step D. This is state 2, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix

[0016] Step F. The chessboard is rotated 25° along the pitch axis of the inertial navigation system to be calibrated, and step D is repeated; this is state three, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix

[0017] Step G. The chessboard is rotated -25° along the pitch axis of the inertial navigation system to be calibrated, and step D is repeated; this is state 2, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix

[0018] Step H Multiply The transpose of Multiply The transpose of the camera to be calibrated in state 1 and state 2 is obtained respectively. Similarly, the relative rotation matrix of the camera to be calibrated in state three and state four can be obtained

[0019] Step I Left multiply the values ​​in step D by Get the attitude matrix of the inertial navigation system to be calibrated in states 1 to 4

[0020] Step J. Adjustment The corresponding rotation axis and rotation sequence are made the same as those of the camera to be calibrated, and the

[0021] Step K. Step J Multiply The transpose of Multiply The transpose of , respectively, to obtain the relative rotation matrix

[0022] Step L. According to step K and in step H The hand-eye calibration method or the vector method is used to complete the calculation of the joint calibration matrix.

[0023] Among them, in step D, the method for calculating the posture of the camera to be calibrated relative to the chessboard is:

[0024] Step D.1. After the camera takes a picture of the chessboard, detect all the corner points of the chessboard and obtain the corner point coordinates p in the pixel coordinate system. i (x, y), i = 1, 2, ... N, N is the total number of corner points on the chessboard; the corner point detection method used is a mature method in this field;

[0025] Step D.2. The size of each grid on the chessboard is known, and the size of each grid is denoted as L. Based on this, the actual coordinates of the corner points of the chessboard in the chessboard coordinate system are calculated and denoted as q i (x, y), i = 1, 2, ... N, the unit is m; since all corner points are on the chessboard, according to the chessboard coordinate system, z = 0;

[0026] Step D.3. Input the camera intrinsic parameter K and distortion parameter P, and input the corner point coordinates p in the pixel coordinate system i (x, y), i = 1, 2, ... N, use the PnP method to solve the rotation matrix of the camera coordinate system relative to the chessboard; the PnP method used is a mature method in this field (the PnP method solves: given the coordinates of a 3D point, the corresponding 2D point coordinates and the intrinsic parameter matrix, solves the camera position and posture).

[0027] The steps of adjusting the order of the rotating shafts in step J are as follows:

[0028] Step J.1: According to the rotation matrix to Euler angle formula, Converted to Euler angles, they are recorded in order as (θ b1 ,γ b1 ,φ b1 );The rotation matrix to Euler angle formula is a mature formula in this field;

[0029] Step J.2: According to the rotation axis sequence and rotation direction of the camera coordinate system where the camera to be calibrated is located and the inertial navigation coordinate system to be calibrated, and according to the rotation axis and rotation sequence of the camera coordinate system, adjust the positive and negative signs and sequence in step J.1 to obtain (α c ,β c ,x c );

[0030] Step J.3: Use the Euler angles (α) in the camera coordinate system obtained in step J.2 c ,β c ,x c ), according to the Euler angle rotation matrix formula, calculate The Euler angle rotation matrix formula is a mature formula in this field;

[0031] Step J.4: The calculation method is similar.

[0032] Among them, the vector method and hand-eye calibration method in step L are mature methods in this field.

[0033] Among them, the rotation axis selection of steps D, E, F, and G is only a preferred solution, the purpose of which is to perform relative rotation in two-axis directions. Theoretically, any two relative rotations are sufficient, that is, the rotation axis selection of D, E, F, and G can be any two axes in space.

[0034] The setting of the rotation angle in steps D, E, F, and G is only a preferred solution, the purpose of which is to find the relative rotation between the two axes. Theoretically, any value is acceptable, but the larger the relative rotation angle, the more accurate the calculation result.

[0035] Among them, steps D, E, F, and G only perform one relative rotation in each of the two axes, which is only the minimum input to ensure the solution of the formula. However, any rotation in any axis direction and any number of times, as long as more than one rotation in each of the two axes, can calculate the result. The solution method is the hand-eye calibration method proposed by Tsai in 1989.

[0036] Among them, the transfer alignment method based on posture matching in step B is a mature method in this field.

[0037] The initial alignment in step C is a mature method in this field.

[0038] (III) Beneficial effects

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

[0040] (1) The present invention provides a device for realizing joint calibration of monocular vision and inertial navigation sensors. The joint calibration can be realized without rotating the carrier. Compared with the traditional method which requires at least relative rotation of the carrier in two different axes, the difficulty of joint calibration of monocular vision and inertial navigation sensors is greatly reduced.

[0041] (2) Based on a device for realizing the joint calibration of monocular vision and inertial navigation sensors, a monocular vision and inertial navigation joint method based on relative measurement is provided, which cleverly replaces carrier measurement with relative measurement, filling the theoretical gap in the field of calibration based on relative measurement.

[0042] (3) A large number of experimental results show that the algorithm has a high joint calibration accuracy, and the joint calibration error is better than 0.03°, which can meet the application requirements in this field in terms of accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] Figure 1 It is the overall flow chart of the present invention;

[0044] Figure 2 The combined calibration device of the present invention

[0045] Figure 3 A diagram showing the rotation process of the joint calibration device in the pitch direction during the transmission alignment process;

[0046] Figure 4 A group of state schematic diagrams of sensors involved in the calibration process of the present invention;

[0047] Figure 5 A group of state schematic diagrams of sensors involved in the calibration process of the present invention;

[0048] Figure 6 A group of state schematic diagrams of sensors involved in the calibration process of the present invention;

[0049] Figure 7 A group of state schematic diagrams of sensors involved in the calibration process of the present invention. DETAILED DESCRIPTION

[0050] In order to make the purpose, content, and advantages of the present invention more clear, the specific implementation methods of the present invention are further described in detail below in conjunction with the accompanying drawings and examples.

[0051] In order to solve the above technical problems, the present invention provides a monocular vision and inertial navigation joint calibration device based on relative quantity measurement, the device comprising: a checkerboard calibration board, a checkerboard strapdown inertial navigation system is installed on the checkerboard calibration board, and the relative rotation relationship between the checkerboard calibration board and the checkerboard strapdown inertial navigation system is known; the size of each grid of the checkerboard is known; a data communication cable is installed on the checkerboard strapdown inertial navigation system; one end of the data communication cable is connected to a joint calibration computer; the joint calibration computer is used to run a joint calibration program;

[0052] The device also includes an inertial navigation system to be calibrated and a camera to be calibrated, both of which are fixedly connected to the carrier and both of which are connected to the joint calibration computer.

[0053] In addition, the present invention also provides a monocular vision and inertial navigation joint calibration method based on relative quantity measurement, the method is implemented based on the device, and the method comprises the following steps:

[0054] Step A. Place the chessboard in the field of view of the camera to be calibrated, rotate and translate the distance of the chessboard relative to the camera to be calibrated, and keep the chessboard in the field of view of the camera, collect 25 pictures, and use Zhang Youzheng calibration method to perform intrinsic parameter calibration on the camera to obtain the camera intrinsic parameter matrix K and distortion vector P;

[0055] Step B. The INS to be calibrated and the chessboard strapdown INS are relatively stationary and rotate in one direction as a whole. Timestamp alignment and transfer alignment based on attitude matching are performed to obtain the initial relative matrix of the INS to be calibrated relative to the chessboard strapdown INS.

[0056] Step C. Collect the raw data of the inertial navigation system to be calibrated, collect the raw data of the chessboard strapdown inertial navigation system for initial alignment, and obtain the attitude matrix of the chessboard strapdown inertial navigation system relative to the navigation coordinate system.

[0057] Step D. Place the chessboard in the field of view of the camera to be calibrated. The camera to be calibrated re-collects a chessboard image. Based on the chessboard size L, K and P in step A, perform relative posture calculation to obtain the rotation matrix of the camera relative to the chessboard coordinate system. At the same time, the chessboard strapdown inertial navigation data is collected to perform navigation settlement and obtain the attitude matrix This is state 1; the navigation solution is a mature method in this field;

[0058] Step E. Rotate the chessboard 45° along the roll axis of the inertial navigation system to be calibrated, and repeat step D. This is state 2, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix

[0059] Step F. The chessboard is rotated 25° along the pitch axis of the inertial navigation system to be calibrated, and step D is repeated; this is state three, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix

[0060] Step G. The chessboard is rotated -25° along the pitch axis of the inertial navigation system to be calibrated, and step D is repeated; this is state 2, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix

[0061] Step H Multiply The transpose of Multiply The transpose of the camera to be calibrated in state 1 and state 2 is obtained respectively. Similarly, the relative rotation matrix of the camera to be calibrated in state three and state four can be obtained

[0062] Step I Left multiply the values ​​in step D by Get the attitude matrix of the inertial navigation system to be calibrated in states 1 to 4

[0063] Step J. Adjustment The corresponding rotation axis and rotation sequence are made the same as those of the camera to be calibrated, and the

[0064] Step K. Step J Multiply The transpose of Multiply The transpose of , respectively, to obtain the relative rotation matrix

[0065] Step L. According to step K and in step H The hand-eye calibration method or the vector method is used to complete the calculation of the joint calibration matrix.

[0066] Among them, in step D, the method for calculating the posture of the camera to be calibrated relative to the chessboard is:

[0067] Step D.1. After the camera takes a picture of the chessboard, detect all the corner points of the chessboard and obtain the corner point coordinates p in the pixel coordinate system. i (x, y), i = 1, 2, ... N, N is the total number of corner points on the chessboard; the corner point detection method used is a mature method in this field;

[0068] Step D.2. The size of each grid on the chessboard is known, and the size of each grid is denoted as L. Based on this, the actual coordinates of the corner points of the chessboard in the chessboard coordinate system are calculated and denoted as q i (x, y), i = 1, 2, ... N, the unit is m; since all corner points are on the chessboard, according to the chessboard coordinate system, z = 0;

[0069] Step D.3. Input the camera intrinsic parameter K and distortion parameter P, and input the corner point coordinates p in the pixel coordinate system i (x, y), i = 1, 2, ... N, use the PnP method to solve the rotation matrix of the camera coordinate system relative to the chessboard; the PnP method used is a mature method in this field (the PnP method solves: given the coordinates of a 3D point, the corresponding 2D point coordinates and the intrinsic parameter matrix, solves the camera position and posture).

[0070] The steps of adjusting the order of the rotating shafts in step J are as follows:

[0071] Step J.1: According to the rotation matrix to Euler angle formula, Converted to Euler angles, they are recorded in order as (θ b1 ,γ b1 ,φ b1 );The rotation matrix to Euler angle formula is a mature formula in this field;

[0072] Step J.2: According to the rotation axis sequence and rotation direction of the camera coordinate system where the camera to be calibrated is located and the inertial navigation coordinate system to be calibrated, and according to the rotation axis and rotation sequence of the camera coordinate system, adjust the positive and negative signs and sequence in step J.1 to obtain (α c ,β c ,x c );

[0073] Step J.3: Use the Euler angles (α) in the camera coordinate system obtained in step J.2 c ,β c ,x c), according to the Euler angle rotation matrix formula, calculate The Euler angle rotation matrix formula is a mature formula in this field;

[0074] Step J.4: The calculation method is similar.

[0075] Among them, the vector method and hand-eye calibration method in step L are mature methods in this field.

[0076] Among them, the rotation axis selection of steps D, E, F, and G is only a preferred solution, the purpose of which is to perform relative rotation in two-axis directions. Theoretically, any two relative rotations are sufficient, that is, the rotation axis selection of D, E, F, and G can be any two axes in space.

[0077] The setting of the rotation angle in steps D, E, F, and G is only a preferred solution, the purpose of which is to find the relative rotation between the two axes. Theoretically, any value is acceptable, but the larger the relative rotation angle, the more accurate the calculation result.

[0078] Among them, steps D, E, F, and G only perform one relative rotation in each of the two axes, which is only the minimum input to ensure the solution of the formula. However, any rotation in any axis direction and any number of times, as long as more than one rotation in each of the two axes, can calculate the result. The solution method is the hand-eye calibration method proposed by Tsai in 1989.

[0079] Among them, the transfer alignment method based on posture matching in step B is a mature method in this field.

[0080] The initial alignment in step C is a mature method in this field.

[0081] Example 1

[0082] In this embodiment, on the one hand, a device for realizing joint calibration of monocular vision and inertial navigation sensor is provided, such as Figure 2 shown.

[0083] The joint calibration device includes: 1-rotating frame, 2-rotating hinge, 3-fixed frame, 4-chessboard calibration plate, 5-chessboard strapdown inertial navigation, 6-joint calibration computer, 7-chessboard strapdown inertial navigation communication data line, 8-communication data line of camera to be calibrated, 9-communication data line of inertial navigation to be calibrated. The joint calibration object includes: 12-camera to be calibrated, 13-inertial navigation to be calibrated, the 12-camera to be calibrated, 13-inertial navigation to be calibrated are fixed on 11-carrier, and the 1-rotating frame is used to fix 11-carrier and carry 11-carrier to rotate with one degree of freedom.

[0084] The 1-rotating frame is hinged to the 3-fixed frame, and the hinge center is the 2-rotating hinge. During use, the 3-fixed frame is always fixed to the 10-ground without any form of movement. The rotating frame can rotate with one degree of freedom in the pitch direction around the 2-rotating hinge, such as Figure 3 shown.

[0085] The 4-chessboard calibration board and the 5-chessboard strapdown inertial navigation system are fixedly installed, and the plane of the 4-chessboard calibration board is parallel to the bottom plane of the 5-chessboard strapdown inertial navigation system. The 6-joint calibration computer is used for data collection and alignment of the 5-chessboard strapdown inertial navigation system, the 12-camera to be calibrated and the 13-insertion navigation system to be calibrated, and the operation of the joint calibration method, and the data transmission is completed by using the 7-chessboard strapdown inertial navigation system communication data line, the 8-camera to be calibrated communication data line, and the 9-insertion navigation system to be calibrated communication data line respectively. A set of state diagrams of the calibration process are shown in FIG. Figure 4 shown.

[0086] The coordinate system of the 13-inertial navigation system to be calibrated is defined as follows: the inertial navigation center is the origin, the carrier forward direction is axis 1, the carrier upward direction is axis 2, and the carrier right direction is axis 3. This coordinate system is a right-handed coordinate system.

[0087] Among them, the coordinate system of the 12-camera to be calibrated is defined as: the camera optical center is the origin, the image row direction is axis 1, the image column direction is axis 2, and the image optical axis direction is axis 3. The coordinate system is a right-handed coordinate system.

[0088] The coordinate system of the 4-chessboard calibration plate is defined as: the upper left corner of the chessboard is the origin, the column direction is axis 1, the row direction is axis 2, and the inside of the chessboard is axis 3. This coordinate system is a right-hand coordinate system.

[0089] The coordinate system of the 5-chessboard strapdown inertial navigation is defined as follows: the center of the 5-chessboard strapdown inertial navigation is the origin, the column direction of the chessboard is axis 1, the row direction is axis 2, and the inward direction of the chessboard is axis 3. This coordinate system is a right-handed coordinate system. The coordinate system of the 4-chessboard calibration plate and the coordinate system of the 5-chessboard strapdown inertial navigation coincide with each other.

[0090] The relative positions and attitude matrices of the 4-chessboard calibration plate and the 5-chessboard strapdown inertial navigation are known.

[0091] On the other hand, a monocular vision and inertial navigation combined method based on relative quantity measurement is designed for the device. The process is as follows: Figure 1 , the steps are as follows:

[0092] Step A. Initially, the 1-rotating frame in the joint calibration device is in the erected state, such as Figure 3As shown. The carrier is fixed with 12-camera to be calibrated and 13-INS to be calibrated, and 11-carrier, 4-chessboard calibration plate plane, and 5-chessboard strapdown INS are fixed on the rotating frame. The system is powered on, the 4-chessboard calibration plate is moved, and the 4-chessboard calibration plate is placed in the field of view of the camera to be calibrated. The distance of the 4-chessboard calibration plate relative to the 12-camera to be calibrated is rotated and translated, and the 4-chessboard calibration plate is always placed in the field of view of the 12-camera to be calibrated. The 25 pictures of the 4-chessboard calibration plate collected by the 12-camera to be calibrated are input into the 6-joint calibration computer through the communication data line of the 8-camera to be calibrated. The 6-joint calibration computer uses the Zhang Youzheng calibration method to perform internal parameter calibration on the 12-camera to be calibrated, and the camera internal parameter matrix K and distortion vector P are obtained. This process lasts about 180s. The Zhang Zhengyou calibration method is a mature method in this field (inputting chessboard data of different postures taken by the camera and outputting the distortion parameters involved in the camera). After completion, the 1-rotating frame in the joint calibration device is rotated to a horizontal state, such as Figure 3 shown.

[0093] Step B. Based on step A, 1-rotating frame in the joint calibration device is kept in a horizontal state, 12-camera to be calibrated and 13-inertial navigation system to be calibrated are fixed on the carrier, and 11-carrier and 4-chessboard calibration plate plane and 5-chessboard strapdown inertial navigation system are fixed on the rotating frame. The system is powered on, and 6-joint calibration computer starts to collect data of 13-inertial navigation system to be calibrated, 12-camera to be calibrated and 5-chessboard strapdown inertial navigation system. After the horizontal state data is maintained for 40 seconds, 1-rotating frame in the joint calibration device starts to make an erection action, that is, 11-carrier and 4-chessboard calibration plate plane and 5-chessboard strapdown inertial navigation system together with the rotating frame make an upward movement in the pitch direction and rotate 60° (only a preferred implementation value of the present invention). The erection process lasts for about 40 seconds, and 6-joint calibration computer continues to collect data. This process performs timestamp alignment and transfer alignment based on attitude matching to obtain the initial relative attitude matrix of the 3-to-be-calibrated INS relative to the 5-chessboard strapdown INS. The transfer alignment method based on attitude matching is a mature method in the art (the transfer alignment method based on attitude matching inputs the data of two inertial navigation systems rotating in the same direction, and outputs the relative attitude matrix of the two. In this case, the data of 5-chessboard strapdown inertial navigation system and 13-to-be-calibrated inertial navigation system in the erection process are input, and the relative attitude matrix of the two is output. ).

[0094] Step C. Based on step B, 1-the rotating frame is kept in the upright state, and the system is powered on. The 5-chessboard strapdown inertial navigation system is placed on the 3-fixed frame and remains stationary. The 6-joint calibration computer collects the raw data of the 5-chessboard strapdown inertial navigation system for 40 seconds, and then the 6-joint calibration computer runs the static base initial alignment software to obtain the attitude matrix of the 5-chessboard strapdown inertial navigation system relative to the navigation coordinate system. The static base initial alignment software is mature software in this field (the inertial navigation system continuously collects data for a period of time without moving, and outputs the attitude matrix of the inertial navigation carrier coordinate system relative to the navigation coordinate system).

[0095] Step D. Based on step B, 1-the rotating frame is kept in the upright state, the system is powered on, and the 4-chessboard calibration plate is placed in the field of view of 12-the camera to be calibrated. 12-the camera to be calibrated re-collects a chessboard image, and based on the chessboard size L, K and P in step A, the relative posture is solved to obtain the rotation matrix of the camera relative to the chessboard coordinate system. At the same time, 5-chessboard strapdown inertial navigation data is collected to perform navigation calculations and obtain the attitude matrix At this time, it is state 1. Figure 3 The navigation solution is a mature method in the art (continuously inputting inertial navigation data and outputting attitude matrix).

[0096] Step E.4 - The checkerboard calibration plate is rotated 45° along 12 - the roll axis of the inertial navigation system to be calibrated (only a preferred value of the present invention), and step D is repeated. At this time, it is state 2, and the relative position relationship of each device is as follows: Figure 4 As shown, we get 12-the rotation matrix of the camera to be calibrated relative to the checkerboard coordinate system At the same time, we get the attitude matrix

[0097] Step F.4 - The checkerboard calibration plate is rotated 25° along 12 - the roll axis of the inertial navigation system to be calibrated (only a preferred value of the present invention), and step D is repeated. At this time, it is state three, and the relative position relationship of each device is as follows: Figure 5 As shown, we get 12-the rotation matrix of the camera to be calibrated relative to the checkerboard coordinate system At the same time, we get the attitude matrix

[0098] Step G.4 - The checkerboard calibration plate rotates along 12 - the roll axis of the inertial navigation system to be calibrated -25° (only a preferred value of the present invention), and repeats step D. At this time, it is state 2, and the relative position relationship of each device is as follows Figure 6 As shown, we get 12-the rotation matrix of the camera to be calibrated relative to the checkerboard coordinate system At the same time, we get the attitude matrix

[0099] Step H Multiply The transpose of the camera to be calibrated in state 1 and state 2 is obtained respectively. Similarly, we can get the relative rotation matrix of state 3 and state 4 12-the camera to be calibrated

[0100] Step I Left multiply the values ​​in step D by Get 13-the attitude matrix of the inertial navigation system to be calibrated in states 1 to 4

[0101] Step J. Adjustment The corresponding rotation axis and rotation sequence are made the same as the rotation axis and rotation sequence of the 12-camera to be calibrated, and the

[0102] Step K. Step J Multiply The transpose of Multiply The transpose of , respectively, to obtain the relative rotation matrix

[0103] Step L. According to step K In step H The hand-eye calibration method or the vector method is used to complete the calculation of the joint calibration matrix.

[0104] Among them, in step D, the method for calculating the posture of the camera to be calibrated relative to the chessboard is:

[0105] Step D.1. After the camera takes a picture of the chessboard, detect all the corner points of the chessboard and obtain the corner point coordinates p in the pixel coordinate system. i (x, y), i = 1, 2, ... N, N is the total number of corner points on the chessboard. The corner point detection method used is a mature method in this field.

[0106] Step D.2. The size of each grid on the chessboard is known, and each grid size is denoted as L. Based on this technology, the actual coordinates of each corner point in the chessboard coordinate system are calculated, and denoted as q i (x, y, z), i = 1, 2, ... N, the unit is m, where since all corner points are on the chessboard, z = 0 according to the chessboard coordinate system.

[0107] Step D.3. Input the camera intrinsic parameter K and distortion parameter P, and input the corner point coordinates p in the pixel coordinate system i (x, y), i = 1, 2, ... N, input, use the PnP method to solve the rotation matrix of the camera coordinate system relative to the chessboard coordinate system. The PnP method used is a mature method in this field (the PnP method solves: given the coordinates of a 3D point, the corresponding 2D point coordinates and the intrinsic parameter matrix, solve the camera position and posture).

[0108] The steps of adjusting the order of the rotating shafts in step J are as follows:

[0109] Step J.1: According to the rotation matrix to Euler angle formula, Converted to Euler angles, they are recorded in order as (θb1 ,γ b1 ,φ b1 ). The rotation matrix to Euler angle formula is a mature formula in the navigation field.

[0110] Step J.2: According to the rotation axis sequence and rotation direction of the camera coordinate system 12- where the camera to be calibrated is located and 13- where the inertial navigation system to be calibrated is located, and according to the rotation axis and rotation sequence of the camera coordinate system, adjust the positive and negative signs and sequence in step J.1 to obtain (α c ,β c ,x c ).

[0111] Step J.3: Use the Euler angles (α) in the camera coordinate system obtained in step J.2 c ,β c ,x c ), according to the Euler angle rotation matrix formula, calculate The Euler angle to rotation matrix formula is a mature formula in this field.

[0112] Step J.4: The calculation method is similar.

[0113] Among them, the vector method and hand-eye calibration method described in step L are mature methods in this field.

[0114] Among them, the rotation axis selection of steps D, E, F, and G is only a preferred solution, the purpose of which is to perform relative rotation in two-axis directions. Theoretically, any two relative rotations are sufficient, that is, the rotation axis selection of D, E, F, and G can be any two axes in space.

[0115] The setting of the rotation angle in steps D, E, F, and G is only a preferred solution, the purpose of which is to find the relative rotation between the two axes. Theoretically, any value is acceptable, but the larger the relative rotation angle, the more accurate the calculation result.

[0116] Among them, steps D, E, F, and G only perform one relative rotation in each of the two axes, which is only the minimum input to ensure the solution of the formula. However, any rotation in any axis direction and any number of times, as long as more than one rotation in each of the two axes, can calculate the result. The solution method is the hand-eye calibration method proposed by Tsai in 1989.

[0117] The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements and modifications can be made without departing from the technical principles of the present invention. These improvements and modifications should also be regarded as the scope of protection of the present invention.

Claims

1. A monocular vision and inertial navigation joint calibration device based on relative quantity measurement, characterized in that: The device comprises: a checkerboard calibration board, a checkerboard strapdown inertial navigation system is installed on the checkerboard calibration board, and the relative rotation relationship between the checkerboard calibration board and the checkerboard strapdown inertial navigation system is known; the size of each grid of the checkerboard is known; a data communication cable is installed on the checkerboard strapdown inertial navigation system; one end of the data communication cable is connected to a joint calibration computer; the joint calibration computer is used to run a joint calibration program; The device also includes an inertial navigation system to be calibrated and a camera to be calibrated, both of which are fixedly connected to the carrier and both of which are connected to the joint calibration computer.

2. A monocular vision and inertial navigation joint calibration method based on relative quantity measurement, characterized in that: The method is implemented based on the device according to claim 1, and the method comprises the following steps: Step A. Place the chessboard in the field of view of the camera to be calibrated, rotate and translate the distance of the chessboard relative to the camera to be calibrated, and keep the chessboard in the field of view of the camera, collect 25 pictures, and use Zhang Youzheng calibration method to perform intrinsic parameter calibration on the camera to obtain the camera intrinsic parameter matrix K and distortion vector P; Step B. The INS to be calibrated and the chessboard strapdown INS are relatively stationary and rotate in one direction as a whole. Timestamp alignment and transfer alignment based on attitude matching are performed to obtain the initial relative matrix of the INS to be calibrated relative to the chessboard strapdown INS. Step C. Collect the raw data of the inertial navigation system to be calibrated, collect the raw data of the chessboard strapdown inertial navigation system for initial alignment, and obtain the attitude matrix of the chessboard strapdown inertial navigation system relative to the navigation coordinate system. Step D. Place the chessboard in the field of view of the camera to be calibrated. The camera to be calibrated re-collects a chessboard image. Based on the chessboard size L, K and P in step A, perform relative posture calculation to obtain the rotation matrix of the camera relative to the chessboard coordinate system. At the same time, the chessboard strapdown inertial navigation data is collected to perform navigation settlement and obtain the attitude matrix This is state 1; Step E. Rotate the chessboard 45° along the roll axis of the inertial navigation system to be calibrated, and repeat step D. This is state 2, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix Step F. The chessboard is rotated 25° along the pitch axis of the inertial navigation system to be calibrated, and step D is repeated; this is state three, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix Step G. The chessboard is rotated -25° along the pitch axis of the inertial navigation system to be calibrated, and step D is repeated; this is state 2, and the rotation matrix of the camera relative to the chessboard coordinate system is obtained. At the same time, we get the attitude matrix Step H Multiply The transpose of Multiply The transpose of the camera to be calibrated in state 1 and state 2 is obtained respectively. Similarly, the relative rotation matrix of the camera to be calibrated in state three and state four can be obtained Step I Left multiply the values ​​in step D by Get the attitude matrix of the inertial navigation system to be calibrated in states 1 to 4 Step J. Adjustment The corresponding rotation axis and rotation sequence are made the same as those of the camera to be calibrated, and the Step K. Step J Multiply The transpose of Multiply The transpose of , respectively, to obtain the relative rotation matrix Step L. According to step K and in step H The hand-eye calibration method or the vector method is used to complete the calculation of the joint calibration matrix.

3. The monocular vision and inertial navigation joint calibration method based on relative quantity measurement as claimed in claim 2, characterized in that: In step D, the method for calculating the posture of the camera to be calibrated relative to the chessboard is: Step D.

1. After the camera takes a picture of the chessboard, detect all the corner points of the chessboard and obtain the corner point coordinates p in the pixel coordinate system. i (x,y),i=1,2,...N, N is the total number of corner points on the chessboard; Step D.

2. The size of each grid on the chessboard is known, and the size of each grid is denoted as L. Based on this, the actual coordinates of the corner points of the chessboard in the chessboard coordinate system are calculated and denoted as q i (x, y), i = 1, 2, ... N, the unit is m; since all corner points are on the chessboard, according to the chessboard coordinate system, z = 0; Step D.

3. Input the camera intrinsic parameter K and distortion parameter P, and input the corner point coordinates p in the pixel coordinate system i (x, y), i = 1, 2, ... N, use the PnP method to solve the rotation matrix of the camera coordinate system relative to the chessboard.

4. The monocular vision and inertial navigation joint calibration method based on relative quantity measurement according to claim 3 is characterized in that: The steps of adjusting the order of the rotating shafts in step J are: Step J.1: According to the rotation matrix to Euler angle formula, Converted to Euler angles, they are recorded in order as (θ b1 ,γ b1 ,φ b1 ); Step J.2: According to the rotation axis sequence and rotation direction of the camera coordinate system where the camera to be calibrated is located and the inertial navigation coordinate system to be calibrated, and according to the rotation axis and rotation sequence of the camera coordinate system, adjust the positive and negative signs and sequence in step J.1 to obtain (α c ,β c ,x c ); Step J.3: Use the Euler angles (α) in the camera coordinate system obtained in step J.2 c ,β c ,x c ), according to the Euler angle rotation matrix formula, calculate Step J.4: The calculation method is similar.

5. The monocular vision and inertial navigation joint calibration method based on relative quantity measurement according to claim 4 is characterized in that: The vector method and hand-eye calibration method in step L are mature methods in the art.

6. The monocular vision and inertial navigation joint calibration method based on relative quantity measurement according to claim 4, characterized in that: The rotation axis selection of steps D, E, F, and G is only a preferred solution, the purpose of which is to perform relative rotation in two-axis directions. Theoretically, two arbitrary relative rotations are sufficient, that is, the rotation axis selection of D, E, F, and G can be any two axes in space.

7. The monocular vision and inertial navigation joint calibration method based on relative quantity measurement according to claim 4, characterized in that: The setting of the rotation angle in steps D, E, F, and G is only a preferred solution, the purpose of which is to find the relative rotation between the two axes. Theoretically, any value is acceptable, but the larger the relative rotation angle, the more accurate the calculation result.

8. The monocular vision and inertial navigation joint calibration method based on relative quantity measurement according to claim 4, characterized in that: The steps D, E, F, and G only perform one relative rotation in each of the two axes, which is only the minimum input to ensure the solution of the formula. However, any rotation in any axis direction and any number of rotations, as long as more than one rotation in each of the two axes, can calculate the result. The solution method is the hand-eye calibration method proposed by Tsai in 1989.

9. The monocular vision and inertial navigation joint calibration method based on relative quantity measurement according to claim 4, characterized in that: The transfer alignment method based on posture matching in step B is a mature method in the art.

10. The monocular vision and inertial navigation joint calibration method based on relative quantity measurement according to claim 4, characterized in that: The initial alignment in step C is a mature method in the art.

Images