A rigid body rotation angle measurement method based on visual-inertial combination algorithm

Abstract

A method for measuring the rotation angle of a rigid body based on a visual-inertial combined algorithm belongs to the field of rigid body angular motion parameter measurement. The method involves constructing a rectangular target as a reference plane for an industrial camera; the target base is fixedly placed in front of the rotation axis of the rigid body being measured. A cross slide rail drives the rectangular target to achieve small-angle pitch rotation around a horizontal rotation axis. A rigid body connector is installed and fixed at the end of the rotating rigid body, connecting the rigid body to the measurement module. During the rotation of the rigid body, feature images, primarily of the target, are acquired at a preset frequency, and angle calculations are performed. An inertial measurement unit acquires inertial data during the rotation of the rigid body at a predetermined frequency and performs angle calculations. An optimization algorithm is used to fuse inertial and visual sampling data to correct the zero-point drift of the inertial measurement unit, improving the measurement accuracy of the rigid body rotation angle. This invention is applicable to the measurement of dual-axis angular motion.

Description

Technical Field

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

[0002] Rotation angle is a crucial parameter in angular motion, and many large pieces of equipment contain rigid structures that undergo rotational motion. The rotation angle of these structures directly affects the overall operation of the equipment. Especially in scenarios requiring high precision, accurate angle measurement is paramount to improving equipment performance.

[0003] Gyroscopes are commonly used angle measurement devices. Mounted on the structure being measured, they utilize data processing to achieve angle measurement, offering advantages such as fast dynamic response and high sampling rate. However, high-precision gyroscopes are costly and exhibit time-drift and temperature-drift characteristics, causing errors to accumulate over time, making them unsuitable for long-term measurements. Furthermore, they are only suitable for scenarios involving rotating rigid bodies performing single-axis angular motion. Moreover, for rigid structures undergoing dual-axis angular motion, the dynamic measurement accuracy of gyroscopes often fails to meet practical 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 metrology field. However, achieving high-precision dual-axis angular motion dynamic measurement using pure vision algorithms requires deploying multiple cameras around the equipment under test, and the measurement process also necessitates moving the equipment to a fixed measurement site. Therefore, while visual measurement methods offer many advantages, the preparation work before measurement is cumbersome, making it impossible to perform measurements anytime, anywhere. Summary of the Invention

[0004] The purpose of this invention is to provide a method for measuring the rotation angle of a rigid body based on a vision-inertial combined algorithm. This method leverages the advantages of a compact and flexible inertial measurement unit (IMU) while effectively suppressing measurement result drift through combination with a vision algorithm. This enables high-precision dynamic measurement of the rotation angle of a rigid body using a relatively low-precision IMU in conjunction with an industrial camera and vision algorithm. This invention is applicable to the measurement of dual-axis angular motion, reducing the cost of high-precision angle measurement and improving accuracy and convenience.

[0005] The objective of this invention is achieved through the following technical solution:

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

[0007] S1. Construct a rectangular target to serve as a reference plane for industrial camera shooting. An infrared light that emits a crosshair 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 being measured. The rectangular target is fixed on a cross slide rail. The target base and the cross slide rail are connected by a horizontal rotation axis. The cross slide rail 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 rail. By adjusting the pitch rotation, up and down movement, and left and right movement of the cross slide rail, the relative pose of the rectangular target and the industrial camera is adjusted to ensure that the industrial camera's field of view covers the rectangular target and obtains a clear image of the rectangular target. The small angle refers to an angle range of -5° to 10°.

[0009] S3. The rigid body connector is fixed to the end of the rotating rigid body, securely connecting the measured rigid body to the measurement module. The main structure of the rigid body connector contains three small compartments, which respectively house the inertial measurement unit, the industrial camera, and the DC power supply. The fixed positions within the compartments improve 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 has crosshairs printed on it for adjusting the relative position between the target and the measurement module.

[0010] S4, an industrial camera, acquires feature images of the target at a preset frequency during the rigid body rotation process, and calculates the angle based on the feature images of the target.

[0011] S5, Inertial Measurement Unit, which collects inertial data during the rotation of the rigid body under test at a predetermined frequency, and calculates the angle based on the inertial data;

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

[0013] Furthermore, in step S3,

[0014] When placing the rectangular target, the four LEDs should face the industrial camera lens, with the side containing the red LED emitting the crosshair as the top edge, parallel to the ground. The crosshair should illuminate the crosshair markings on the rigid body connector. Adjust the guide rail and rotation axis connected to the rectangular target to ensure that the crosshair emitted by the target coincides with the crosshair markings on the rigid body connector. At this point, the plane of the rectangular target is perpendicular to the center line of the industrial camera lens, serving as the starting angle position for visual measurement, i.e., 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 performing a geometric transformation on the image data:

[0017]

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

[0019]

[0020] Where R 3×3 and T 3×1 These are the rotation and translation matrices of the camera coordinate system relative to the world coordinate system, respectively.

[0021] Based on the pinhole imaging principle, the transformation relationship between the coordinates of a point in the camera coordinate system and the coordinates of a point on the image plane is as follows:

[0022]

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

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

[0025]

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

[0027] Combining the above equations, the transformation relationship between the coordinates of a point in the world coordinate system and the pixel coordinates is as follows:

[0028]

[0029] in,

[0030] In image processing, keyframe images are first extracted and binarized to identify the light spots formed by the four red lights in the image. The SIFT algorithm is then used to extract these light spots as feature points. A Gaussian kernel function is used for filtering when constructing the spatial scale. This filtering process is represented as a convolution operation between the original image and a variable-scale 2D Gaussian function G(x,y,σ). This is illustrated below:

[0031]

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

[0033] Extreme points are found using scale invariance. To achieve image rotation invariance, the reference direction of each spot feature is assigned a value using local image features. The gradient kernel direction distribution features of pixels within a 3σ neighborhood window are collected. The gradient magnitude and direction are as follows:

[0034]

[0035] Each feature point possesses three pieces of information: position, scale, and orientation, represented as (x, y, σ, θ). The centroid coordinates (x0, y0) of the spot image are calculated using the gray-scale centroid method.

[0036]

[0037] The scale information and orientation angle matching the centroid are obtained, that is, the angle is calculated based on the feature image with the target as the main feature.

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

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

[0040]

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

[0042]

[0043] Simplifying the above equation, we get:

[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. Within one sampling period, the number of pulses output by the gyroscope is multiplied by a scaling factor to obtain an angular increment. in Angular velocity vector The angular velocity matrix is ​​obtained by calculating the angle based on inertial data.

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

[0047] Visual and inertial algorithms are highly complementary. Monocular vision algorithms cannot solve scale problems, fail to recognize areas with little texture, and cannot accurately solve image blur during rapid motion. Inertial measurement units (IMUs), on the other hand, suffer from zero-point drift, with angular drift changing linearly with respect to time t. This paper estimates the true scale of the motion trajectory by aligning the pose sequences estimated by the IMU with those estimated by an industrial camera. Pre-integration of the IMU data between keyframes of two consecutive images yields constraints on the relative motion. Based on a fixed-hysteresis smoother, rigid body rotation and pitch angles are estimated within a sliding window of a given length. The zero-point drift of the inertial measurement unit is corrected based on these estimations, improving the accuracy of rigid body rotation angle measurement.

[0048] Beneficial effects:

[0049] 1. Monocular vision algorithms cannot solve scale problems, cannot recognize areas with little texture, and cannot accurately calculate image blur during rapid movement. This invention discloses a method for measuring the rotation angle of a rigid body based on a combined visual-inertial algorithm. A monocular camera and an inertial measurement unit are connected together to form a measurement unit, which is installed at the end of the rotating structure. The camera lens faces inward toward the rotation axis and photographs a pre-fixed rectangular target. A crosshair is printed on the side of the measurement unit facing the target. When the crosshair emitted by the infrared light coincides with the crosshair, it indicates that the relative position of the measurement unit and the target meets the measurement requirements. Before measurement, the inertial measurement unit (IMU) and monocular camera are calibrated offline separately, and then the combined unit is calibrated online. The monocular camera rotates with the rigid body, acquiring image information of the target at its operating frequency and calculating the rotation angle. The IMU acquires inertial data at its operating frequency and calculates the rotation angle. During image processing, the centroid of the captured infrared spot is extracted, and the displacement of the centroid on the projection plane is calculated. The visual measurement results and inertial measurement results are fused using an optimization algorithm to correct the zero-point drift of the inertial measurement unit and obtain the optimal solution for the rotation angle. This invention leverages the compact and flexible measurement advantages of the inertial measurement unit and, through its combination with a visual algorithm, effectively suppresses the drift of the measurement results. It achieves high-precision dynamic measurement of the rotation angle of a rigid body using a relatively low-precision IMU in conjunction with an industrial camera and a visual algorithm, and is suitable for measurement scenarios involving dual-axis angular motion.

[0050] 2. This invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm. A rectangular target is constructed as a reference plane for an industrial camera. The target base is fixedly placed in front of the rotation axis of the rigid body being measured. The rectangular target is fixed on a cross slide rail. The target base and the cross slide rail are connected via a horizontal rotation axis. The cross slide rail 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 with the cross slide rail. By adjusting the pitch rotation, up and down movement, and left and right movement of the cross slide rail, the relative pose of the rectangular target and the industrial camera is adjusted, ensuring that the industrial camera's field of view covers the rectangular target, obtaining a clear image of the rectangular target, and improving the accuracy and efficiency of angle calculation based on the target-centric feature image.

[0051] 3. This invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combined algorithm. A rigid body connector is installed and fixed at the end of the rotating rigid body, thus fixing the rigid body under test to the measurement module. The main structure of the rigid body connector includes three small compartments, which respectively house the inertial measurement unit, the industrial camera, and the DC power supply. The fixed positions within the compartments improve 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. The side of the rigid body connector facing the target is marked with a cross scale, which can accurately and efficiently adjust the relative position between the target and the measurement module.

[0053] 5. This invention discloses a method for measuring the rotation angle of a rigid body based on a visual-inertial combination algorithm. During image processing, keyframe images are first extracted and binarized to find the light spots formed by the four red lights in the image. The SIFT algorithm is used to extract the captured light spots as feature points. When constructing the spatial scale, a Gaussian kernel function is used for filtering. The filtering process is represented as a convolution operation between the original image and a variable-scale 2D Gaussian function G(x,y,σ). Extreme points are found through scale invariance. To achieve image rotation invariance, the reference direction of each light spot feature is assigned using local image features. The gradient kernel direction distribution features of pixels within a 3σ neighborhood window are collected. Each feature point has three pieces of information: position, scale, and direction, represented as (x,y,σ,θ). The centroid coordinates (x0,y0) of the light spot image are calculated using the gray-scale centroid method, and the scale information and orientation angle matching the centroid point are obtained, improving the accuracy and efficiency of angle calculation based on visual sampling data. Attached Figure Description

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

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

[0056] Figure 3 This is a schematic diagram of the target structure;

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

[0058] Figure 5 This is a diagram illustrating the operating principle of the fusion algorithm. Detailed Implementation

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

[0060] S1. Construct a rectangular target to serve as a reference plane for industrial camera shooting. An infrared light that emits a crosshair 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 being measured. The rectangular target is fixed on a cross slide rail. The target base and the cross slide rail are connected by a horizontal rotation axis. The cross slide rail 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 rail. By adjusting the pitch rotation, up and down movement, and left and right movement of the cross slide rail, the relative pose of the rectangular target and the industrial camera is adjusted to ensure that the industrial camera's field of view covers the rectangular target and obtains a clear image of the rectangular target. The small angle refers to an angle range of -5° to 10°.

[0062] S3. The rigid body connector is fixed to the end of the rotating rigid body, securely connecting the measured rigid body to the measurement module. The main structure of the rigid body connector contains three small compartments, which respectively house the inertial measurement unit, industrial camera, and DC power supply. The fixed positions within the compartments improve the consistency of the industrial camera and inertial measurement unit during the measurement process. The side of the rigid body connector facing the target has crosshairs for adjusting 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. An industrial camera is used to acquire feature images, primarily of the target, at a preset frequency during the rigid body's rotation. Angle calculation is then performed based on these target-centric feature images. A 2-megapixel industrial camera with a resolution of 1920x1080 and a pixel size of 2.75μm is used. First, the camera is calibrated offline, followed by online calibration, 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 image processing, keyframe images are first extracted and binarized to find the light spots formed by the four red lights in 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. The filtering process is represented as a convolution operation between the original image and a variable-scale 2D Gaussian function G(x,y,σ). This is illustrated below:

[0064]

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

[0066] Extreme points are found using scale invariance. To achieve image rotation invariance, the reference direction of each spot feature is assigned a value using local image features. The gradient kernel direction distribution features of pixels within a 3σ neighborhood window are collected. The gradient magnitude and direction are as follows:

[0067]

[0068] Each feature point possesses three pieces of information: position, scale, and orientation, represented as (x, y, σ, θ). The centroid coordinates (x0, y0) of the spot image are calculated using the gray-scale centroid method.

[0069]

[0070] The scale information and orientation angle matching the centroid are obtained, that is, the angle is calculated based on the feature image with the target as the main feature.

[0071] There will be pixel errors during the shooting process, and considering the efficiency of real-time processing, the final recognizable size is set at an offset of 3 to 4 pixels, i.e., 8.25μm to 11μm;

[0072] The identifiable offset dimensions are converted to angles as follows:

[0073]

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

[0075] S5, Inertial Measurement Unit, which collects inertial data during the rotation of the rigid body under test at a predetermined frequency, and calculates the angle based on the inertial data;

[0076] S6. Using an optimization algorithm to fuse inertial and visual sampling data, IMU data between keyframes of two consecutive images is pre-integrated to obtain constraints on relative motion. Based on a fixed hysteresis smoother, the rigid body rotation and pitch angles are estimated within a sliding window of a given length. This corrects the zero-point drift of the inertial measurement unit, improving the measurement accuracy of the rigid body rotation angle.

[0077] The above detailed description further illustrates the purpose, technical solution, and beneficial effects of the invention. It should be understood that the above description 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 within 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 combined algorithm, characterized in that, Includes the following steps: S1. Construct a rectangular target to serve as a reference plane for industrial camera shooting. An infrared light that emits a crosshair 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 being measured; the rectangular target is fixed on the cross slide rail; the target base and the cross slide rail are connected by a horizontal rotation axis, and the cross slide rail 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 rail; the relative pose 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 rail to ensure that the field of view of the industrial camera covers the rectangular target and obtain a clear image of the rectangular target; the small angle refers to the 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, industrial camera and DC power supply. The fixed position inside the compartment improves the consistency of the industrial camera and inertial measurement unit during the measurement process. The side of the rigid body connector facing the target is marked with a cross scale, which is used to adjust the relative position between the target and the measurement module. S4, an industrial camera, acquires feature images of the target at a preset frequency during the rigid body rotation process, and calculates angles based on the feature images of the target. First, key frame images are extracted and binarized 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. Gaussian kernel function is used for filtering when constructing the spatial scale. S5, Inertial Measurement Unit, which collects inertial data during the rotation of the rigid body under test at a predetermined frequency, and calculates the angle based on the inertial data; S6. Use optimization algorithms to fuse inertial and visual sampling data to correct the zero-point 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 step S3 When placing the rectangular target, the four LEDs face the industrial camera lens, with the side where the red LED emitting the crosshair is located as the top edge, which is parallel to the ground. The crosshair is pointed at the crosshair scale line on the rigid body connector. Adjust the guide rail and rotation axis connected to the rectangular target to ensure that the crosshair emitted by the crosshair coincides with the crosshair 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 for visual measurement, i.e., 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 as follows: 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 performing a geometric transformation on the image data: The coordinates of a point p on a rectangular target in the world coordinate system are (x, y, p). w ,y w ,z w The coordinates of point p in the camera coordinate system are expressed as: Where R 3×3 and T 3×1 These are the rotation and translation matrices of the camera coordinate system relative to the world coordinate system, respectively. Based on the pinhole imaging principle, the transformation relationship between the coordinates of a point in the camera coordinate system and the coordinates of a point on the image plane is as follows: Where x and y are the horizontal and vertical coordinates on the image plane; If (u,v) is the coordinate value of a pixel in the image, then the relationship between the pixel coordinates and the image plane coordinates is as follows: in, dx and dy are inherent parameters of the industrial camera, representing the length and width of each pixel in mm; f is the focal length of the industrial camera. Combining the above equations, the transformation relationship between the coordinates of a point in the world coordinate system and the pixel coordinates is as follows: in, In the image processing, the keyframe images are first extracted and binarized to find the light spots formed by the four red lights in the image. The SIFT algorithm is then used to extract these light spots as feature points. When constructing the spatial scale, a Gaussian kernel function is used for filtering. The filtering process is represented as a convolution operation between the original image and a variable-scale 2D Gaussian function G(x,y,σ), as shown below: L(x,y,σ)=G(x,y,σ)*I(x,y) Extreme points are found using scale invariance. To achieve image rotation invariance, the reference direction of each spot feature is assigned a value using local image features. The gradient kernel direction distribution features of pixels within a 3σ neighborhood window are collected. The gradient magnitude and direction are as follows: Each feature point possesses three pieces of information: position, scale, and orientation, represented as (x, y, σ, θ). The centroid coordinates (x0, y0) of the spot image are calculated using the gray-scale centroid method. The scale information and orientation angle matching the centroid are obtained, that is, the angle is calculated based on the feature image with the target as the main feature.

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 as follows: The inertial measurement unit (IMU) measures the three-axis angular motion components of a rotating rigid body, including the heading angle, by constructing Euler angle differential equations. Pitch angle θ and roll angle γ; the transformation matrix from the vehicle coordinate system to the navigation coordinate system, i.e., the attitude matrix, is: use This represents the angular velocity of the carrier coordinate system relative to the navigation coordinate system. Simplifying the above equation, we get: In the inertial measurement unit used, the gyroscope outputs a digital quantity represented by a series of pulses, each pulse representing an angular increment. Within one sampling period, the number of pulses output by the gyroscope is multiplied by a scaling factor to obtain an angular increment. in Angular velocity vector The angular velocity matrix is ​​obtained by calculating the angle 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 as follows: The true scale of the motion trajectory is estimated by aligning the pose sequence estimated by the IMU with the pose sequence estimated by the industrial camera. Pre-integrate the IMU data between two consecutive keyframes of images to obtain constraints on relative motion; Based on a fixed hysteresis smoother, the rigid body rotation angle and pitch angle are estimated within a sliding window of a given length. The zero-point drift of the inertial measurement unit is corrected based on the estimation results of the rigid body rotation angle and pitch angle, thereby improving the measurement accuracy of the rigid body rotation angle.

Images