Rigid body rotation angle measurement method based on visual inertia combination algorithm

Abstract

The invention discloses a rigid body rotation angle measurement method based on a visual inertia combination algorithm, and belongs to the field of rigid body angular motion parameter metering and measurement. The implementation method comprises the following steps: constructing a rectangular target used as a shooting reference plane of an industrial camera; and the target base is fixedly placed in front of a rotating shaft of the measured rotating rigid body. And the cross slide rail drives the rectangular target to realize small-angle pitching rotation around the horizontal rotating shaft. The rigid body connecting piece is installed and fixed at the tail end of the rotating rigid body, and the measured rigid body and the measuring module are fixedly connected together. In the rotation process of the rigid body, collecting a feature image taking a target as a main part at a preset frequency, and carrying out angle calculation; the inertial measurement unit is used for collecting inertial data in the rotation process of the measured rigid body at a preset frequency and carrying out angle calculation; an optimization algorithm is used to fuse inertia and visual sampling data, and the zero drift of the inertia measurement unit is corrected, so that the measurement precision of the rotation angle of the rigid body is improved. The method is suitable for measuring the biaxial angular motion.

Description

Technical Field

[0001] The invention belongs to the technical field of rigid body angular motion parameter metrology and measurement, and relates to a method for measuring a rigid body rotation angle based on a visual-inertial combination algorithm. Background Art

[0002] The rotation angle is one of the important indicators of angular motion parameters. Many large equipment have rigid structures that perform rotational motion. The rotation angle of such structures directly affects the operation of the entire equipment. Especially in scenarios with high precision requirements, accurate angle measurement is the top priority for improving equipment performance indicators.

[0003] Gyroscopes are commonly used angle measurement devices. They are installed on the structure to be measured, and angle measurement is achieved by data processing. They have the characteristics of fast dynamic response and high sampling rate. However, high-precision gyroscopes have high economic costs, and have time drift and temperature drift characteristics. Errors will accumulate over time, which is not conducive to long-term measurement, and are only suitable for scenes where rotating rigid bodies perform single-axis angular motion. In addition, for rigid structures that perform dual-axis angular motion, the dynamic measurement accuracy of gyroscopes often cannot meet actual use requirements. In recent years, with the development of machine vision technology, angle measurement based on visual algorithms has gradually begun to play an important role in the field of metrology. However, when relying on pure visual algorithms to achieve high-precision dynamic measurement of dual-axis angular motion, multiple cameras need to be arranged around the equipment to be measured, and the equipment to be measured also needs to be moved to a fixed measurement site during the measurement process. It can be seen that although the visual measurement method has many advantages, the preparation work before measurement is relatively cumbersome, and it is impossible to measure anytime and anywhere. Summary of the invention

[0004] The purpose of the present invention is to provide a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm, which can not only give play to the small and flexible measurement advantages of the inertial measurement unit, but also effectively suppress the drift of the measurement results by combining with the visual algorithm, and realize the high-precision dynamic measurement of the rotation angle of the rigid body by using a relatively low-precision IMU in conjunction with an industrial camera visual algorithm. The present invention is suitable for the measurement of dual-axis angular motion, can reduce the cost of high-precision angle measurement, and improve the accuracy and convenience of measurement.

[0005] The purpose of the present invention is achieved through the following technical solutions:

[0006] The present invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm, which is characterized by comprising the following steps:

[0007] S1. Construct a rectangular target used as a reference plane for shooting an industrial camera, wherein an infrared light emitting a cross cursor is installed at the midpoint of one long side of the rectangular target, and ordinary red lights are installed at the midpoints of the other three sides;

[0008] S2. The target base is fixedly placed in front of the rotation axis of the rotating rigid body to be measured. The rectangular target is fixed on the cross slide. The target base and the cross slide are connected by a horizontal rotation axis, and the cross slide drives the rectangular target to achieve a small-angle pitch rotation around the horizontal rotation axis. The rectangular target moves up and down and left and right relative to the target base with the cross slide. The relative position of the rectangular target and the industrial camera is adjusted by the pitch rotation, up and down movement, and left and right movement of the cross slide to ensure that the field of view of the industrial camera covers the rectangular target and obtain a clear rectangular target image. The small angle refers to an angle range of -5° to 10°.

[0009] S3, the rigid body connector is installed and fixed at the end of the rotating rigid body, and the measured rigid body and the measurement module are fixedly connected together. The main structure of the rigid body connector includes three small compartments, which are respectively used to fix the inertial measurement unit, industrial camera and DC power supply. The fixed position inside the compartment improves the consistency of the industrial camera and the inertial measurement unit during the measurement process. The side of the rigid body connector facing the target is printed with a cross scale mark, which is used to adjust the relative position between the target and the measurement module.

[0010] S4, industrial camera, during the rotation of the rigid body, collects characteristic images mainly based on the target at a preset frequency, and realizes angle resolution based on the characteristic images mainly based on the target;

[0011] S5, an inertial measurement unit, which collects inertial data of the measured rigid body during rotation at a predetermined frequency, and implements angle calculation based on the inertial data;

[0012] S6. Use optimization algorithms to fuse inertial and visual sampling data, correct the zero drift of the inertial measurement unit, and improve the measurement accuracy of the rigid body rotation angle.

[0013] Furthermore, in the step S3,

[0014] When placing the rectangular target, the four lights are facing the industrial camera lens, and the side where the red light emitting the cross cursor is located is the upper side, which is parallel to the ground. The cross cursor shines on the cross scale line of the rigid body connector. Adjust the guide rail and the rotation axis connected to the rectangular target to ensure that the cursor emitted by the cross cursor coincides with the cross scale line on the rigid body connector. At this time, the rectangular target plane is perpendicular to the center line of the industrial camera lens, which is used as the starting angle position of the visual measurement, that is, the 0° angle position.

[0015] Furthermore, the specific implementation method of step S4 is as follows:

[0016] When the industrial camera moves with the object being measured, the relative offset between the camera coordinate system and the world coordinate system is obtained by geometrically transforming the image data:

[0017]

[0018] The coordinates of a point p on the rectangular target in the world coordinate system are (x w ,y w ,z w ), the coordinate value of point p in the camera coordinate system is expressed as:

[0019]

[0020] Where R 3×3 and T 3×1 They are the rotation matrix and translation matrix of the camera coordinate system relative to the world coordinate system.

[0021] Based on the pinhole imaging principle, the conversion relationship between the corresponding point coordinates in the camera coordinate system and the point coordinates on the image plane is:

[0022]

[0023] Where x and y are the horizontal and vertical coordinates on the image plane.

[0024] (u, v) is the coordinate value of a pixel point on the image, and the relationship between the pixel coordinates and the image plane coordinates is as follows:

[0025]

[0026] in, dx and dy are inherent parameters of industrial cameras, indicating the length and width of each pixel, in mm; f is the focal length of the industrial camera.

[0027] By integrating the above formulas, we can know that the conversion relationship between the coordinates of a point in the world coordinate system and the pixel coordinates is:

[0028]

[0029] in,

[0030] During the image processing, the key frame image is first extracted for binarization to find the light spots formed by the four red lights on the image. The SIFT algorithm is used to extract the captured light spots as feature points. When constructing the spatial scale, the Gaussian kernel function is used for filtering. The filtering process is represented by the convolution operation of the original image and a 2D Gaussian function G(x, y, σ) with a variable scale. It is expressed as follows:

[0031]

[0032] L(x,y,σ)=G(x,y,σ)*I(x,y)

[0033] The extreme points are found through scale invariance. In order to achieve image rotation invariance, the local features of the image are used to assign the reference direction of each spot feature. The gradient kernel direction distribution characteristics of the pixels in the 3σ domain window are collected. The gradient modulus and direction are as follows:

[0034]

[0035] Each feature point has three pieces of information: position, scale, and direction, expressed as (x, y, σ, θ). The centroid coordinates (x0, y0) of the spot image are calculated using the grayscale centroid method.

[0036]

[0037] The scale information and direction angle matching the centroid point are obtained, that is, the angle is solved based on the feature image mainly based on the target.

[0038] Furthermore, the specific implementation method of step S5 is as follows:

[0039] The inertial measurement unit measures the three-axis angular motion components of a rotating rigid body by constructing the Euler angle differential equation, including the heading angle Pitch angle θ and roll angle γ. The transformation matrix from the carrier coordinate system to the navigation coordinate system, that is, the attitude matrix is:

[0040]

[0041] use It represents the angular velocity of the carrier coordinate system relative to the navigation coordinate system in the carrier coordinate system.

[0042]

[0043] Rearranging the above formula gives:

[0044]

[0045] In the inertial measurement unit used, the gyroscope outputs a digital quantity represented by a series of pulses, each pulse representing an angular increment. In a sampling period, the number of pulses output by the gyroscope is multiplied by the scale factor to obtain an angular increment. in is the angular velocity vector The angular velocity matrix is ​​used to realize angle calculation based on inertial data.

[0046] Furthermore, the specific implementation method of step S6 is as follows:

[0047] Vision and inertial algorithms are highly complementary. Monocular vision algorithms cannot solve the scale problem, cannot identify areas with little texture, and cannot accurately solve image blur during fast motion. Inertial measurement units have the problem of zero drift, and the angle drift changes linearly with respect to time t. The true scale of the motion trajectory is estimated by aligning the pose sequence estimated by the IMU and the pose sequence estimated by the industrial camera. The IMU data between the key frames of two consecutive images is pre-integrated to obtain the constraints of relative motion. Based on the fixed lag smoother, the rigid body rotation angle and pitch angle are estimated within a sliding window of a given length. The zero drift of the inertial measurement unit is corrected according to the estimated results of the rigid body rotation angle and pitch angle to improve the measurement accuracy of the rigid body rotation angle.

[0048] Beneficial effects:

[0049] 1. Monocular vision algorithms cannot solve the scale problem, cannot identify areas with less texture, and cannot accurately solve image blur during rapid motion. The present invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm. A monocular camera and an inertial measurement unit are connected together to form a measurement unit, which is installed at the end of a rotating structure. The camera lens faces inward toward the rotation axis to shoot a pre-fixed rectangular target. A cross mark line is printed on the side of the measurement unit facing the target. When the cross cursor emitted by the infrared lamp coincides with the mark line, it indicates that the relative position of the measurement unit and the target meets the measurement requirements. Before starting the measurement, the inertial measurement unit IMU and the monocular camera are first calibrated offline, and then the combination of the two is calibrated online; the monocular camera rotates with the rigid body, collects the image information of the target according to the working frequency, and calculates the rotation angle; the IMU collects inertial data according to the working frequency and calculates the rotation angle; the centroid of the infrared light spot captured is extracted during the image processing process, and the displacement of the centroid of the light spot on the projection plane is calculated; the visual measurement results and the inertial measurement results are fused using an optimization algorithm, the zero drift of the inertial measurement unit is corrected, and the optimal solution for the rotation angle is obtained. The present invention can not only give play to the small and flexible measurement advantages of the inertial measurement unit, but also effectively suppress the drift of the measurement results by combining with the visual algorithm, and realize the high-precision dynamic measurement of the rigid body rotation angle using a relatively low-precision IMU in conjunction with the industrial camera visual algorithm, and is suitable for the measurement scenario of dual-axis angular motion.

[0050] 2. The present invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm, which constructs a rectangular target used as a reference plane for shooting by an industrial camera, and the target base is fixedly placed in front of the rotation axis of the rotating rigid body to be measured. The rectangular target is fixed on a cross slide. The target base is connected to the cross slide by a horizontal rotation axis, and the cross slide drives the rectangular target to achieve a small-angle pitch rotation around the horizontal rotation axis. The rectangular target moves up and down and left and right relative to the target base with the cross slide. The relative posture of the rectangular target and the industrial camera is adjusted by the pitch rotation, up and down movement, and left and right movement of the cross slide, ensuring that the field of view of the industrial camera covers the rectangular target, obtaining a clear rectangular target image, and improving the accuracy and efficiency of angle resolution of the feature image based on the target.

[0051] 3. The present invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm. The rigid body connector is installed and fixed at the end of the rotating rigid body, and the rigid body to be measured and the measurement module are fixedly connected together. The main structure of the rigid body connector includes three small compartments, which are respectively used to fix the inertial measurement unit, the industrial camera and the DC power supply. The fixed position inside the compartment improves the consistency of the industrial camera and the inertial measurement unit during the measurement process.

[0052] 4. The present invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm. A cross scale mark is printed on the side of the rigid body connector facing the target, which can accurately and efficiently adjust the relative position between the target and the measurement module.

[0053] 5. The present invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm. During the image processing process, the key frame image is first extracted for binarization processing to find the light spots formed by four red lights on the image; the SIFT algorithm is used to extract the captured light spots as feature points; a Gaussian kernel function is used for filtering when constructing the spatial scale, and the filtering process is represented by a convolution operation between the original image and a 2D Gaussian function G(x, y, σ) with a variable scale; extreme points are obtained through scale invariance, and in order to achieve image rotation invariance, the reference direction of each light spot feature is assigned using the local features of the image; the gradient kernel direction distribution features of pixels in a 3σ domain window are collected; each feature point has three information of position, scale and direction, represented as (x, y, σ, θ); the grayscale centroid method is used to calculate the centroid coordinates (x0, y0) of the light spot image, and the scale information and direction angle matching the centroid point are obtained to improve the accuracy and efficiency of angle solution based on visual sampling data. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 It is a schematic diagram of the overall structure of a measurement system of a method for measuring a rigid body rotation angle based on a visual-inertial combination algorithm of the present invention;

[0055] Figure 2 It is a schematic diagram of the measurement module structure;

[0056] Figure 3 Schematic diagram of the target structure;

[0057] Figure 4 is a schematic diagram of the target base structure. Figure 4 (a) is the front view of the base structure. Figure 4 (b) is a side view of the base structure;

[0058] Figure 5 This is the schematic diagram of the fusion algorithm operation. DETAILED DESCRIPTION

[0059] like Figure 1 As shown, the present embodiment discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm, and the specific implementation steps are as follows:

[0060] S1. Construct a rectangular target used as a reference plane for shooting an industrial camera, wherein an infrared light emitting a cross cursor is installed at the midpoint of one long side of the rectangular target, and ordinary red lights are installed at the midpoints of the other three sides;

[0061] S2. The target base is fixedly placed in front of the rotation axis of the rotating rigid body to be measured. The rectangular target is fixed on the cross slide. The target base and the cross slide are connected by a horizontal rotation axis, and the cross slide drives the rectangular target to achieve a small-angle pitch rotation around the horizontal rotation axis. The rectangular target moves up and down and left and right relative to the target base with the cross slide. The relative position of the rectangular target and the industrial camera is adjusted by the pitch rotation, up and down movement, and left and right movement of the cross slide to ensure that the field of view of the industrial camera covers the rectangular target and obtain a clear rectangular target image. The small angle refers to an angle range of -5° to 10°.

[0062] S3, the rigid body connector is installed and fixed at the end of the rotating rigid body, and the measured rigid body and the measurement module are fixedly connected together. The main structure of the rigid body connector includes three small compartments, which are respectively used to fix the inertial measurement unit, industrial camera and DC power supply. The fixed position inside the compartment improves the consistency of the industrial camera and the inertial measurement unit during the measurement process. The side of the rigid body connector facing the target is printed with a cross scale mark to adjust the relative position between the target and the measurement module. The optimal test distance between the measurement unit and the target is 4.5m.

[0063] S4, industrial camera, during the rotation of the rigid body, collects feature images mainly based on the target at a preset frequency, and realizes angle resolution based on the feature images mainly based on the target; an industrial camera with 2 million pixels and a resolution of 1920x1080 is used, and the pixel size is 2.75μm. First, the camera is calibrated offline, and then online, to obtain the functional relationship between the angle of rotation of the industrial camera with the measured rigid body and the deformation size of the target light spot. During the image processing process, the key frame image is first extracted for binarization processing to find the light spots formed by the four red lights on the image. The SIFT algorithm is used to extract the captured light spots as feature points. When constructing the spatial scale, the Gaussian kernel function is used for filtering, and the filtering process is represented by the convolution operation of the original image and a 2D Gaussian function G(x,y,σ) with a variable scale. It is expressed as follows:

[0064]

[0065] L(x,y,σ)=G(x,y,σ)*I(x,y)

[0066] The extreme points are found through scale invariance. In order to achieve image rotation invariance, the local features of the image are used to assign the reference direction of each spot feature. The gradient kernel direction distribution characteristics of the pixels in the 3σ domain window are collected. The gradient modulus and direction are as follows:

[0067]

[0068] Each feature point has three pieces of information: position, scale, and direction, expressed as (x, y, σ, θ). The centroid coordinates (x0, y0) of the spot image are calculated using the grayscale centroid method.

[0069]

[0070] The scale information and direction angle matching the centroid point are obtained, that is, the angle is solved based on the feature image mainly based on the target.

[0071] There will be pixel errors during the shooting process, and considering the efficiency of real-time processing, the offset size of 3 to 4 pixels is taken as the final recognizable size, that is, 8.25μm to 11μm;

[0072] The recognizable offset dimensions are converted into angles as follows:

[0073]

[0074] Substituting the pixel size into the above formula, we can get θ≈0.0005°

[0075] S5, an inertial measurement unit, which collects inertial data of the measured rigid body during rotation at a predetermined frequency, and implements angle calculation based on the inertial data;

[0076] S6. Use optimization algorithms to fuse inertial and visual sampling data, pre-integrate the IMU data between two consecutive image key frames, and obtain the constraints of relative motion. Estimate the rigid body rotation angle and pitch angle within a sliding window of a given length based on a fixed lag smoother. Correct the zero drift of the inertial measurement unit and improve the measurement accuracy of the rigid body rotation angle.

[0077] The specific description above further illustrates the purpose, technical solutions and beneficial effects of the invention in detail. It should be understood that the above is only a specific embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention should be included in the scope of protection of the present invention.

Claims

1. A method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm, characterized in that: The steps include: S1. Construct a rectangular target used as a reference plane for shooting an industrial camera, wherein an infrared light emitting a cross cursor is installed at the midpoint of one long side of the rectangular target, and ordinary red lights are installed at the midpoints of the other three sides; S2. The target base is fixedly placed in front of the rotation axis of the rotating rigid body to be measured; the rectangular target is fixed on the cross slide; the target base and the cross slide are connected through a horizontal rotation axis, and the cross slide drives the rectangular target to achieve a small-angle pitch rotation around the horizontal rotation axis; the rectangular target moves up and down and left and right relative to the target base along the cross slide; the relative position of the rectangular target and the industrial camera is adjusted by the pitch rotation, up and down movement and left and right movement of the cross slide to ensure that the field of view of the industrial camera covers the rectangular target and obtain a clear rectangular target image; the small angle refers to an angle range of -5° to 10°; S3, the rigid body connector is installed and fixed at the end of the rotating rigid body, and the measured rigid body and the measurement module are fixedly connected together; the main structure of the rigid body connector includes three small compartments, which respectively fix the inertial measurement unit, the industrial camera and the DC power supply; the consistency of the industrial camera and the inertial measurement unit during the measurement process is improved by the fixed position inside the compartment; the side of the rigid body connector facing the target is printed with a cross scale mark, which is used to adjust the relative position between the target and the measurement module; S4, industrial camera, during the rotation of the rigid body, collects characteristic images mainly based on the target at a preset frequency, and realizes angle resolution based on the characteristic images mainly based on the target; S5, an inertial measurement unit, which collects inertial data of the measured rigid body during rotation at a predetermined frequency, and implements angle calculation based on the inertial data; S6. Use optimization algorithms to fuse inertial and visual sampling data, correct the zero drift of the inertial measurement unit, and improve the measurement accuracy of the rigid body rotation angle.

2. The method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm according to claim 1, characterized in that: In the S3 step, When placing the rectangular target, the four lights are facing the industrial camera lens, the side where the red light emitting the cross cursor is located is the upper side, and the upper side is parallel to the ground; the cross cursor is shone on the cross scale line of the rigid body connector; the guide rail and the rotation axis connected to the rectangular target are adjusted to ensure that the cursor emitted by the cross cursor coincides with the cross scale line on the rigid body connector; at this time, the plane of the rectangular target is perpendicular to the center line of the industrial camera lens, which serves as the starting angle position of the visual measurement, that is, the 0° angle position.

3. The method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm according to claim 1, characterized in that: The specific implementation method of step S4 is: When the industrial camera moves with the object being measured, the relative offset between the camera coordinate system and the world coordinate system is obtained by geometrically transforming the image data: The coordinates of a point p on the rectangular target in the world coordinate system are (x w ,y w ,z w ), the coordinate value of point p in the camera coordinate system is expressed as: Where R 3×3 and T 3×1 They are the rotation matrix and translation matrix of the camera coordinate system relative to the world coordinate system; Based on the pinhole imaging principle, the conversion relationship between the corresponding point coordinates in the camera coordinate system and the point coordinates on the image plane is: Where x and y are the horizontal and vertical coordinates on the image plane; (u, v) is the coordinate value of a pixel point on the image, and the relationship between the pixel coordinates and the image plane coordinates is as follows: in, dx, dy are the inherent parameters of the industrial camera, indicating the length and width of each pixel, in mm; f is the focal length of the industrial camera; By integrating the above formulas, the conversion relationship between the coordinates of a point in the world coordinate system and the pixel coordinates is: in, In the image processing process, the key frame image is first extracted for binarization processing to find the light spots formed by the four red lights on the image; the SIFT algorithm is used to extract the captured light spots as feature points; the Gaussian kernel function is used for filtering when constructing the spatial scale. The filtering process is represented by the convolution operation of the original image and a variable-scale 2D Gaussian function G(x, y, σ); it is expressed as follows: L(x,y,σ)=G(x,y,σ)*I(x,y) The extreme points are found through scale invariance. In order to achieve the invariance of image rotation, the local features of the image are used to assign the reference direction of each spot feature; the gradient kernel direction distribution characteristics of the pixels in the 3σ domain window are collected; the gradient modulus and direction are as follows: Each feature point has three pieces of information: position, scale, and direction, expressed as (x, y, σ, θ); the centroid coordinates (x0, y0) of the spot image are calculated using the grayscale centroid method. The scale information and direction angle matching the centroid point are obtained, that is, the angle is solved based on the feature image mainly based on the target.

4. The method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm according to claim 1, characterized in that: The specific implementation method of step S5 is: The inertial measurement unit measures the three-axis angular motion components of a rotating rigid body by constructing the Euler angle differential equation, including the heading angle Pitch angle θ and roll angle γ; the transformation matrix from the carrier coordinate system to the navigation coordinate system, that is, the attitude matrix is: use It represents the angular velocity of the carrier coordinate system relative to the navigation coordinate system in the carrier coordinate system. Rearranging the above formula gives: In the inertial measurement unit used, the gyroscope outputs a digital quantity represented by a series of pulses, each pulse representing an angular increment. In a sampling period, the number of pulses output by the gyroscope is multiplied by the scale factor to obtain an angular increment. in is the angular velocity vector The angular velocity matrix is ​​used to realize angle calculation based on inertial data.

5. The method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm according to claim 1, characterized in that: The specific implementation method of step S6 is: The true scale of the motion trajectory is estimated by aligning the pose sequence estimated by the IMU and the pose sequence estimated by the industrial camera; Pre-integrate the IMU data between two consecutive image key frames to obtain the relative motion constraints; The rigid body rotation angle and pitch angle are estimated based on a fixed lag smoother within a sliding window of a given length. The zero drift of the inertial measurement unit is corrected according to the estimated results of the rigid body rotation angle and pitch angle to improve the measurement accuracy of the rigid body rotation angle.

Images