A three-dimensional rotation control method and system for radio frequency immunity test of an electric energy meter

Abstract

The application provides a three-dimensional rotation control method and system for radio frequency immunity test of an electric energy meter, which comprises the relative conversion relationship between a calibration camera and a three-dimensional turntable coordinate system, and establishes a 3D reference feature library of key points on the surface of the electric energy meter; in the test process, feature points are extracted from real-time monitoring images and matched with the reference library, and reliable 2D-3D point pairs are screened out by using a RANSAC algorithm; six degrees of freedom poses of the electric energy meter in the camera coordinate system are solved based on an EPnP algorithm, and then are mapped to the turntable coordinate system through coordinate transformation to obtain actual poses; the current actual poses and target test poses are respectively converted into the start and end angles of the axes of the turntable, and trajectory planning is performed in the joint space; the instantaneous poses are solved through real-time visual feedback, the pose deviation from the theoretical trajectory is calculated, and the angle compensation of each axis is iteratively solved by using a Gauss-Newton method, so that the control command is corrected, closed-loop pose tracking is realized, and the electric energy meter is stably brought to the predetermined test angle.

Description

Technical Field

[0001] This application belongs to the field of control, and in particular relates to a three-dimensional rotation control method and system for testing the radio frequency immunity of an energy meter. Background Technology

[0002] Radio frequency electromagnetic field radiation immunity test is an important experimental item for evaluating anti-interference capability. The test is carried out in a fully anechoic chamber. The energy meter under test is placed on a test turntable with insulating support. The turntable drives the energy meter to rotate in the horizontal and vertical directions to ensure that the four sides of the energy meter and specific sensitive angles are aligned with the radiating antenna, thereby completing a comprehensive immunity scan.

[0003] However, the turntable control for testing the radio frequency immunity of electricity meters can only provide feedback on the rotation angle of the motor shaft, failing to detect backlash, wear gaps, and micro-deformations in the turntable transmission mechanism. This results in discrepancies between the commanded angle issued by the control system and the actual spatial posture of the electricity meter, leading to inaccurate test angles. Vision-based positioning technologies often employ 2D image feature matching, which struggles to calculate the six-degree-of-freedom pose of the electricity meter in three-dimensional space and is easily affected by lighting conditions and background noise in dark rooms, resulting in unstable feature extraction. Furthermore, the turntable control system and the vision measurement system are disconnected; the vision system only checks the position after the turntable stops moving, failing to establish a real-time visual closed-loop feedback mechanism during turntable movement. Existing technologies also lack pose error analysis and trajectory correction capabilities based on Lie algebra space, making it impossible to compensate for mechanical errors in real-time during movement. Therefore, a control method that integrates three-dimensional vision measurement and the turntable kinematic model is urgently needed to achieve real-time tracking and closed-loop correction of the electricity meter's test pose. Summary of the Invention

[0004] To address the problems of existing technologies being easily affected by indoor lighting conditions and background noise in dark rooms, and lacking the ability to analyze pose errors and correct trajectories.

[0005] In the first aspect, the present invention proposes a three-dimensional rotation control method for testing the radio frequency immunity of an energy meter, comprising: The relative position transformation relationship between the camera coordinate system and the 3D turntable coordinate system is calibrated, and a benchmark feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations is established; real-time monitoring images of the electricity meter in the radio frequency anechoic chamber are acquired, the image scale space is constructed and feature points and representations are extracted, the current image representations are matched with the benchmark feature library, and the random sampling consensus algorithm is used to eliminate mismatches to obtain the 2D-3D point pair correspondence relationship; The six-degree-of-freedom pose of the energy meter in the camera coordinate system is calculated based on the EPnP algorithm, and the pose is mapped to the turntable coordinate system using the relative position transformation relationship to obtain the current actual pose of the energy meter. Using the inverse kinematics model of the turntable, the current actual pose and the target test pose are converted into the starting angle and ending angle of each axis of the turntable, respectively. Smooth interpolation planning is performed in the joint space to generate a motion trajectory containing the theoretical angle sequence and the corresponding theoretical pose sequence. The instantaneous pose of the energy meter is calculated in real time. The deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory is calculated in the Lie algebra space. The pose tracking error objective function is constructed. The angle compensation increment of each axis of the turntable is solved by the Gauss-Newton iteration method. The motor control command is corrected until the energy meter stably reaches the predetermined test angle.

[0006] In another aspect, the present invention also proposes a three-dimensional rotation control system for testing the radio frequency immunity of an energy meter, comprising the following modules: The module is used to calibrate the relative position transformation relationship between the camera coordinate system and the 3D turntable coordinate system, and establish a benchmark feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations; real-time monitoring images of the electricity meter in the radio frequency anechoic chamber are acquired, the image scale space is constructed and feature points and representations are extracted, the current image representations are matched with the benchmark feature library, and the random sampling consensus algorithm is used to eliminate mismatches to obtain the 2D-3D point pair correspondence; The mapping module is used to calculate the six-degree-of-freedom pose of the energy meter in the camera coordinate system based on the EPnP algorithm, and to map the pose to the turntable coordinate system using the relative position transformation relationship to obtain the current actual pose of the energy meter. The generation module is used to convert the current actual pose and the target test pose into the starting angle and ending angle of each axis of the turntable using the inverse kinematics model of the turntable. It performs smooth interpolation planning in the joint space to generate a motion trajectory containing the theoretical angle sequence and the corresponding theoretical pose sequence. The correction module is used to calculate the instantaneous pose of the energy meter in real time, calculate the deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory in the Lie algebra space, construct the pose tracking error objective function, use the Gauss-Newton iteration method to solve the angle compensation increment of each axis of the turntable, and correct the motor control command until the energy meter stably reaches the predetermined test angle.

[0007] This invention achieves non-contact measurement of the spatial pose of a power meter in an anechoic chamber environment by constructing a benchmark feature library containing the 3D coordinates and feature representations of key points and combining the calibration relationship between the camera and the turntable coordinate system. Image-scale spatial feature extraction and random sampling consensus algorithms are used to avoid visual noise and feature mismatches in the anechoic chamber environment. Combining the EPnP algorithm and the turntable inverse kinematics model, the spatial pose is mapped to joint angles, and a continuous and stable motion trajectory is generated through smooth interpolation planning in joint space, avoiding shocks during equipment operation. A visual closed-loop feedback mechanism based on Lie algebra spatial deviation calculation is used to monitor the difference between the actual pose of the power meter and the theoretically planned trajectory in real time during movement. The Gauss-Newton iterative method is used to solve for the angle compensation increment and correct the motor control commands. This reduces the impact of turntable mechanical transmission backlash, fixture installation errors, and cumulative errors during movement on positioning, ensuring the reliability of electromagnetic compatibility test results. Attached Figure Description

[0008] Figure 1 A flowchart of the first embodiment; Figure 2 This is a schematic diagram illustrating the mapping between a 3D feature library and a CAD model. Figure 3 This is a schematic diagram of visual closed-loop feedback motion trajectory tracking. Detailed Implementation

[0009] The features and exemplary embodiments of various aspects of this application will be described in detail below. To make the objectives, technical solutions, and advantages of this application clearer, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are only intended to explain this application and not to limit it. For those skilled in the art, this application can be implemented without some of these specific details. The following description of the embodiments is merely to provide a better understanding of this application by illustrating examples.

[0010] It should be noted that, in this document, relational terms such as "first" and "second" are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising..." does not exclude the presence of additional identical elements in the process, method, article, or apparatus that includes said element.

[0011] In the first embodiment, the present invention proposes a three-dimensional rotation control method for testing the radio frequency immunity of an energy meter, such as... Figure 1 As shown, it includes: S1. Calibrate the relative position transformation relationship between the camera coordinate system and the 3D turntable coordinate system, and establish a benchmark feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations; acquire real-time monitoring images of the electricity meter in the radio frequency anechoic chamber, construct the image scale space and extract feature points and representations, match the current image representations with the benchmark feature library, use the random sampling consensus algorithm to eliminate mismatches, and obtain the 2D-3D point pair correspondence relationship; A checkerboard calibration board with known geometric dimensions is placed on the turntable. The turntable is rotated at multiple angles, and a camera is used to capture images of the calibration board at each angle. The pixel coordinates of the corner points of the calibration board are extracted. The camera intrinsic parameter matrix is ​​calculated using the Zhang Zhengyou calibration method. The rotation matrix and translation vector from the turntable base coordinate system to the camera coordinate system are solved using the hand-eye calibration algorithm. The electricity meter is placed at the initial zero position of the turntable. A dense point cloud on the surface of the electricity meter is obtained using a 3D scanner. Points with gradient changes are selected as key points. The 3D spatial coordinates of the key points relative to the origin of the electricity meter coordinate system are calculated. A scale-invariant feature transformation algorithm is used to generate a 128-dimensional feature vector for each key point. The 3D spatial coordinates and the corresponding feature vectors are stored as a benchmark feature library.

[0012] The video stream data output from the RF darkroom camera is read by an image acquisition card. The acquired RGB images are converted into grayscale images. Gaussian convolution and downsampling are performed on the grayscale images to construct a Gaussian difference pyramid. Extreme points in the scale space are detected as candidate feature points. Low-contrast points are eliminated by Taylor expansion, and edge response points are eliminated by the principal curvature ratio of the Hessian matrix. The gradient magnitude and direction of the pixel neighborhood where the feature point is located are calculated to generate feature representations. The Euclidean distance between the current image feature representation and the representations in the benchmark feature library is calculated. The nearest and second nearest neighbors are found. When the ratio of the nearest neighbor distance to the second nearest neighbor distance is less than a set threshold, the matching point pair is retained. Four sets of matching point pairs are randomly selected and the camera pose matrix is ​​solved by the PnP algorithm. The remaining matching points are verified based on the reprojection error. Through multiple iterations, the geometric model with the most interior points is selected. The corresponding set of the selected image 2D pixel coordinates and benchmark library 3D spatial coordinates is output.

[0013] In an optional embodiment, the relative position transformation relationship between the calibrated camera coordinate system and the 3D turntable coordinate system is established to create a reference feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations, including: A high-resolution frontal image of the energy meter in its zero-position orientation is acquired, and the key points and feature representations in the image are extracted using the SIFT algorithm. Read the standard CAD 3D model of the electricity meter and establish the mapping relationship between the pixel coordinates (u,v) of key points and the 3D coordinates (X,Y,Z) of the corresponding physical points in the CAD model; The feature identifier, pixel coordinates, and 3D spatial coordinates are combined to form an associated dataset, which is then stored in a lookup table to generate a benchmark feature library.

[0014] Since RF immunity testing requires omnidirectional rotation, acquiring only a frontal image will result in feature loss when the turntable rotates to the side or back. Therefore, when establishing the benchmark feature library, the turntable can optionally be controlled to hover at key angles of 0° front, 90° side, 180° back, and 270° side.

[0015] High-resolution industrial cameras with electromagnetic shielding are used to acquire multi-view images under standard lighting conditions. CAD models in STEP or IGES format are imported, and SIFT feature points extracted from different perspectives are registered onto a unified CAD model coordinate system using multi-view geometry principles or ICP algorithms. Figure 2 As shown, the constructed feature library is stored using a KD-Tree structure, ensuring that the vision system can retrieve the 3D feature points corresponding to the visible surfaces in the current field of view from the library regardless of the orientation of the electricity meter.

[0016] In an optional embodiment, the process of acquiring real-time monitoring images of the energy meter in the radio frequency anechoic chamber, constructing the image scale space, and extracting feature points and representations includes: The acquired real-time monitoring images are converted into grayscale images, and the images are subjected to continuous Gaussian blurring to construct a Gaussian difference pyramid containing 4 octaves and 5 layers in each octave. Extreme value detection is performed between adjacent layers of the pyramid, and the maximum or minimum points within a 26-neighborhood of the image pixel are detected as candidate feature points. The gradient magnitude and direction in the neighborhood of the candidate feature point are calculated to generate a SIFT feature representation with a 128-dimensional vector, and the vector is then normalized.

[0017] In the image preprocessing stage, the acquired RGB images are converted to grayscale images using a weighted average method. An initial Gaussian blur coefficient is then set. According to the scaling factor Downsampling is performed to construct a Gaussian difference pyramid containing four octaves, each containing five layers of images, covering different scale features of the electricity meter from the overall outline to the local features.

[0018] In the extreme value detection stage, a point in the DoG pyramid is compared with its 8 neighboring points in the same layer and 9 points in each of the adjacent layers above and below, totaling 26 neighboring pixels. Only points that are extreme values ​​are retained. Subpixel interpolation is performed using Taylor series expansion, achieving a positioning accuracy of 0.1 pixels. Simultaneously, the principal curvature ratio of the Hessian matrix is ​​calculated, and edge response points with ratios greater than 10 are removed to enhance stability. In the representation generation stage, a 16×16 pixel neighborhood around the feature point is selected, and the cumulative gradient magnitudes in 8 directions are calculated to form a 128-dimensional vector. To eliminate the effects of uneven illumination caused by fluctuations in light intensity or changes in turntable angle within the RF anechoic chamber, the vector is normalized using the L2 norm; if a component is greater than 0.2, it is truncated and re-normalized.

[0019] In an optional embodiment, the step of matching the current image representation with a benchmark feature library, eliminating false matches using a random sampling consensus algorithm, and obtaining a 2D-3D point pair correspondence includes: Calculate the Euclidean distance between the current image feature representation and the baseline representation stored in the baseline feature library; Feature point pairs with a ratio of nearest neighbor distance to second nearest neighbor distance of less than 0.6 are selected as the initial 2D-3D matching point set; Four pairs of points are randomly selected from the initial set of matching points, and the camera pose matrix is ​​calculated using the PnP algorithm. Calculate the error of reprojecting all remaining matching points through the pose matrix, and retain points with an error of less than 3 pixels as inliers; The selection and calculation process is iterated 1000 times to select the point set corresponding to the model with the most interior points. Then, through the index relationship of the benchmark feature library, the 2D coordinates of the current image corresponding to the interior points are associated with the 3D coordinates in the benchmark library to generate a 2D-3D point pair correspondence.

[0020] In the feature matching stage, a fast approximate nearest neighbor search algorithm is used to calculate the Euclidean distance between 128-dimensional vectors. A Roche ratio test is performed, with a threshold of 0.6. Only when the ratio of the nearest neighbor to the second nearest neighbor distance is less than this value is the match considered a candidate.

[0021] Geometric validation was performed using the PnP-RANSAC mechanism, with a confidence level of 0.995 and a maximum of 1000 iterations. In each iteration, a small number of 2D-3D point pairs were randomly selected to calculate the pose hypothesis model, and the corresponding 3D points from the benchmark library were projected back into the current image. The reprojection error between the projected points and the actually extracted 2D points was calculated. ,in, These are the 2D pixel coordinates of the feature points actually detected by the feature extraction algorithm in the current image. R represents the coordinates of known 3D points stored in the baseline feature library, R is the rotation matrix, and t is the translation vector. This is a projection function used to project 3D points in the camera coordinate system into 2D pixels on the image. Points with an error of less than 3 pixels are considered inliers. The output is a clean set of 2D-3D point pairs, excluding outliers, to prevent control divergence caused by feature mismatches.

[0022] S2, Based on the EPnP algorithm, calculate the six-degree-of-freedom pose of the energy meter in the camera coordinate system, and use the relative position transformation relationship to map the pose to the turntable coordinate system to obtain the current actual pose of the energy meter; Four virtual control points are selected in the energy meter coordinate system. The three-dimensional coordinates of all feature points are represented as a weighted sum of the four control points. A system of linear equations is constructed based on the camera projection relationship and the correspondence between 2D and 3D point pairs. Singular value decomposition is performed on the coefficient matrix. The right singular vector corresponding to the minimum singular value is used to solve for the coordinates of the control points in the camera coordinate system. This process restores the coordinates of all 3D feature points in the camera coordinate system. The rotation matrix and translation vector of the energy meter relative to the camera coordinate system are solved using the singular value decomposition method. The Levenburg-Marquardt algorithm is used to minimize the reprojection error and optimize the solution. The transformation matrix from the turntable base coordinate system to the camera coordinate system obtained in the calibration step is inverted and multiplied by the pose matrix of the energy meter in the camera coordinate system to calculate the current actual pose matrix of the energy meter relative to the turntable base coordinate system.

[0023] In an optional embodiment, the calculation of the six-degree-of-freedom pose of the energy meter in the camera coordinate system based on the EPnP algorithm includes: Four non-coplanar control points are selected in three-dimensional space, and all three-dimensional points in the reference feature library are represented as a weighted sum of the four control points. Based on the camera intrinsic parameter matrix and the extracted 2D feature point coordinates, a system of linear equations containing the coordinates of control points in the camera coordinate system is constructed. Singular value decomposition is performed on the coefficient matrix of the linear equation system to obtain the coordinates of the four control points in the camera coordinate system; Based on the correspondence between control points in the world coordinate system and the camera coordinate system, the rotation matrix and translation vector are solved using the Pruk analysis method.

[0024] The EPnP algorithm defines four virtual control points and selects the centroid of all 3D reference points participating in the matching as... And select along the main axis of the data. , , Ensure non-coplanarity. Use n 3D points from the benchmark library. Represented as a linear combination of control points: Weight It is a geometric property.

[0025] Using the camera intrinsic parameter matrix K, including the focal length , and main points , Establish camera coordinates for control points based on the currently observed 2D points. The system of linear equations is given. A 2n×12 matrix M is constructed and SVD decomposition is performed. The right singular vector corresponding to the minimum singular value is used to recover the coordinates of the control points. The Procrustes analysis method is used to minimize the... Solve for the rotation matrix R, 3×3, and translation vector t, 3×1, from the world coordinate system to the camera coordinate system, which is the current actual six-degree-of-freedom pose of the energy meter.

[0026] S3. Using the inverse kinematics model of the turntable, the current actual pose and the target test pose are converted into the starting angle and ending angle of each axis of the turntable, respectively. Smooth interpolation planning is performed in the joint space to generate a motion trajectory containing the theoretical angle sequence and the corresponding theoretical pose sequence. Establish a DH parameter table for a dual-axis or tri-axis turntable, derive the forward and inverse kinematic equations of the turntable, and input the obtained current actual pose matrix and the preset target test pose matrix into the inverse kinematics solver to calculate the current joint angles and target joint angles of the azimuth and pitch axes, respectively. Set the total motion time, and use a fifth-order polynomial interpolation algorithm to calculate the angular position, angular velocity, and angular acceleration for each interpolation cycle from the start time to the end time, ensuring that the velocity and acceleration at the start and end points are zero. Input the interpolated angle sequence into the forward kinematic equations to calculate the theoretical pose matrix of the end of the energy meter at each time step, and save the timestamped theoretical angle sequence and theoretical pose sequence as a reference trajectory. In one embodiment, the interpolated angle sequence is input into the forward kinematic equations to calculate the pose matrix of the turntable end coordinate system relative to the base at each time step. Furthermore, a fixed eccentricity offset matrix is ​​used to transfer the coordinate system from the pre-calibrated energy meter coordinate system to the turntable end coordinate system. The theoretical reference pose of the energy meter at each moment is calculated. The theoretical angle sequence and theoretical pose sequence with timestamps are saved as reference trajectories for visual closure.

[0027] In one embodiment, the inverse kinematics model of the turntable is essentially an inverse mapping that transforms the target's pose in Cartesian space into a combination of angles on the turntable in joint space. Specifically, the current actual pose calculated by the vision system is substituted into the inverse kinematics equations to deduce the current angles of each axis of the turntable, which are then used as the starting angles for motion planning. Similarly, the preset target test pose is substituted into the same equations to calculate the target angles on each axis necessary to achieve that pose, which are then used as the ending angles. During the solution process, the mechanical limit constraints of the turntable and the shortest stroke principle are usually combined to eliminate invalid solutions from multiple sets of solutions and select the optimal solution, thereby providing accurate start and end boundary conditions for smooth trajectory interpolation planning in joint space.

[0028] In an optional embodiment, the step of performing smooth interpolation planning in joint space to generate a motion trajectory containing a theoretical angle sequence and a corresponding theoretical pose sequence includes: Obtain the starting angle of each axis of the turntable. and termination angle ; Set the total interpolation time T and the sampling period. Fifth-order polynomial interpolation was used to calculate each discrete time step. Theoretical joint angle This ensures smooth speed and acceleration. Theoretical joint angle Substituting the forward kinematics model of the turntable, we can solve for the theoretical pose matrix corresponding to the end of the turntable at each moment. It serves as the tracking benchmark for visual servoing.

[0029] The trajectory planning determines the start and end angles of the turntable based on the testing standards. The total motion duration T and control cycle are set. Using a fifth-degree polynomial To represent angle changes, coefficients are solved through boundary conditions to generate an impact-free angle sequence, avoiding motion blur in visual images caused by turntable jitter.

[0030] A forward kinematic model of the turntable is established using the DH parameter method. The theoretical joint angles at each moment are input and multiplied by the link transformation matrix. Solve for the theoretical pose matrix corresponding to the end of the turntable. The matrix contains theoretical rotations. Peaceful relocation As a reference value for the visual closed-loop system, it ensures that the energy meter moves in the radio frequency field according to the planned path.

[0031] S4 calculates the instantaneous pose of the energy meter in real time, calculates the deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory in the Lie algebra space, constructs the pose tracking error objective function, uses the Gauss-Newton iteration method to solve the angle compensation increment of each axis of the turntable, corrects the motor control command, until the energy meter stably reaches the predetermined test angle.

[0032] In each control cycle of the turntable movement, the camera is synchronously triggered to acquire images. The feature extraction and EPnP solution steps are repeated to obtain the instantaneous measurement pose matrix of the energy meter. The product of the inverse matrix of the measurement pose matrix and the theoretical pose matrix at the current moment is calculated to obtain the deviation matrix. The rotation deviation matrix is ​​mapped to the Lie algebra space using logarithmic mapping to obtain the three-dimensional rotation error vector, and combined with the translation error vector. A least squares objective function with the magnitude of the error vector as the independent variable is constructed. The geometric Jacobian matrix of the turntable is calculated. The normal equation composed of the Jacobian matrix and the error vector is solved using the Gauss-Newton method to obtain the angle adjustment amount of each joint axis of the turntable. The calculated angle adjustment amount is superimposed on the theoretical angle command of the next control cycle, and the corrected pulse control signal is sent to the motor driver. This process is repeated until the magnitude of the pose error vector is less than the preset accuracy threshold and the turntable reaches the target position.

[0033] To achieve tracking, the actual pose calculated by vision is obtained. and theoretical pose In an optional embodiment, the real-time calculation of the instantaneous pose of the energy meter, calculating the deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory in the Lie algebra space, includes: Let the theoretical pose matrix at the current moment in the planned trajectory be denoted as... The instantaneous pose matrix calculated in real time is denoted as Both are 4×4 transformation matrices; Calculate the error matrix ; For the error matrix Perform logarithmic mapping operations to calculate , where the superscript ∨ represents the vee operator, resulting in a 6-dimensional column vector, which is the rotation and translation deviation in the SE(3) Lie algebra space.

[0034] Calculate the relative error of Lie group space This represents the difference in transformation from the current actual pose to the theoretical target pose, such as... Figure 3 As shown.

[0035] Since rotation matrices do not possess the property of linear superposition and cannot be used for control law calculations, a logarithmic mapping is used to project the error onto the tangent space se(3). This is achieved through matrix logarithmic operations. The antisymmetric matrix is ​​obtained, and then six components are extracted using the VEE operator to generate the error vector. The first three dimensions For translation error, the last 3 dimensions The rotational error vector represents the instantaneous deviation of the six degrees of freedom.

[0036] In an optional embodiment, the step of constructing the pose tracking error objective function, using the Gauss-Newton iteration method to solve for the angle compensation increment of each axis of the turntable, and correcting the motor control commands until the energy meter stably reaches the predetermined test angle includes: Construct the robot Jacobian matrix J that reflects the relationship between the angular velocity of each axis joint of the turntable and the Lie algebraic rate of change of the end-effector pose; According to the formula Establish the normal equation, where This represents the current Lie algebra space bias; Solving the equation yields the angle compensation increment. ; Will The corrected motor control command is generated by superimposing it onto the theoretical joint angle command at the current moment.

[0037] Based on the current joint angles of the turntable, the geometric Jacobian matrix is ​​constructed using the analytical method of differential kinematics. N represents the number of axes, and the matrix establishes a linear mapping from joint space velocity to Cartesian space Lie algebra velocity. For the least squares problem of visual servoing, the Gauss-Newton method is used for optimization, with Lie algebra bias... Establish the normal equation for the target residual. .

[0038] The compensation increment is obtained by solving the above equation using Cholesky decomposition. To ensure stability and prevent overshoot caused by visual noise or delay, it is necessary to... Perform amplitude limiting and gain adjustment. , ,in These are the theoretical joint angles of each axis of the turntable at the current moment. The final joint control command angles after correction of each axis of the turntable. This is the gain adjustment coefficient for angle compensation increment. The generated instructions are sent to the servo driver via fieldbus to eliminate mechanical transmission errors and energy meter installation eccentricity errors in real time, realizing three-dimensional rotational disturbance immunity testing.

[0039] In the second embodiment, the present invention also proposes a three-dimensional rotation control system for testing the radio frequency immunity of an energy meter, comprising the following modules: The module is used to calibrate the relative position transformation relationship between the camera coordinate system and the 3D turntable coordinate system, and establish a benchmark feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations; real-time monitoring images of the electricity meter in the radio frequency anechoic chamber are acquired, the image scale space is constructed and feature points and representations are extracted, the current image representations are matched with the benchmark feature library, and the random sampling consensus algorithm is used to eliminate mismatches to obtain the 2D-3D point pair correspondence; The mapping module is used to calculate the six-degree-of-freedom pose of the energy meter in the camera coordinate system based on the EPnP algorithm, and to map the pose to the turntable coordinate system using the relative position transformation relationship to obtain the current actual pose of the energy meter. The generation module is used to convert the current actual pose and the target test pose into the starting angle and ending angle of each axis of the turntable using the inverse kinematics model of the turntable. It performs smooth interpolation planning in the joint space to generate a motion trajectory containing the theoretical angle sequence and the corresponding theoretical pose sequence. The correction module is used to calculate the instantaneous pose of the energy meter in real time, calculate the deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory in the Lie algebra space, construct the pose tracking error objective function, use the Gauss-Newton iteration method to solve the angle compensation increment of each axis of the turntable, and correct the motor control command until the energy meter stably reaches the predetermined test angle.

[0040] In an optional embodiment, the relative position transformation relationship between the calibrated camera coordinate system and the 3D turntable coordinate system is established to create a reference feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations, including: A high-resolution frontal image of the energy meter in its zero-position orientation is acquired, and the key points and feature representations in the image are extracted using the SIFT algorithm. Read the standard CAD 3D model of the electricity meter and establish the mapping relationship between the pixel coordinates (u,v) of key points and the 3D coordinates (X,Y,Z) of the corresponding physical points in the CAD model; The feature identifier, pixel coordinates, and 3D spatial coordinates are combined to form an associated dataset, which is then stored in a lookup table to generate a benchmark feature library.

[0041] In an optional embodiment, the process of acquiring real-time monitoring images of the energy meter in the radio frequency anechoic chamber, constructing the image scale space, and extracting feature points and representations includes: The acquired real-time monitoring images are converted into grayscale images, and the images are subjected to continuous Gaussian blurring to construct a Gaussian difference pyramid containing 4 octaves and 5 layers in each octave. Extreme value detection is performed between adjacent layers of the pyramid, and the maximum or minimum points within a 26-neighborhood of the image pixel are detected as candidate feature points. The gradient magnitude and direction in the neighborhood of the candidate feature point are calculated to generate a SIFT feature representation with a 128-dimensional vector, and the vector is then normalized.

[0042] In an optional embodiment, the step of matching the current image representation with a benchmark feature library, eliminating false matches using a random sampling consensus algorithm, and obtaining a 2D-3D point pair correspondence includes: Calculate the Euclidean distance between the current image feature representation and the baseline representation stored in the baseline feature library; Feature point pairs with a ratio of nearest neighbor distance to second nearest neighbor distance of less than 0.6 are selected as the initial 2D-3D matching point set; Four pairs of points are randomly selected from the initial set of matching points, and the camera pose matrix is ​​calculated using the PnP algorithm. Calculate the error of reprojecting all remaining matching points through the pose matrix, and retain points with an error of less than 3 pixels as inliers; The selection and calculation process is iterated 1000 times to select the point set corresponding to the model with the most interior points. Then, through the index relationship of the benchmark feature library, the 2D coordinates of the current image corresponding to the interior points are associated with the 3D coordinates in the benchmark library to generate a 2D-3D point pair correspondence.

[0043] In an optional embodiment, the calculation of the six-degree-of-freedom pose of the energy meter in the camera coordinate system based on the EPnP algorithm includes: Four non-coplanar control points are selected in three-dimensional space, and all three-dimensional points in the reference feature library are represented as a weighted sum of the four control points. Based on the camera intrinsic parameter matrix and the extracted 2D feature point coordinates, a system of linear equations containing the coordinates of control points in the camera coordinate system is constructed. Singular value decomposition is performed on the coefficient matrix of the linear equation system to obtain the coordinates of the four control points in the camera coordinate system; Based on the correspondence between control points in the world coordinate system and the camera coordinate system, the rotation matrix and translation vector are solved using the Pruk analysis method.

[0044] In an optional embodiment, the step of performing smooth interpolation planning in joint space to generate a motion trajectory containing a theoretical angle sequence and a corresponding theoretical pose sequence includes: Obtain the starting angle of each axis of the turntable. and termination angle ; Set the total interpolation time T and the sampling period. Fifth-order polynomial interpolation was used to calculate each discrete time step. Theoretical joint angle This ensures smooth speed and acceleration. Theoretical joint angle Substituting the forward kinematics model of the turntable, we can solve for the theoretical pose matrix corresponding to the end of the turntable at each moment. It serves as the tracking benchmark for visual servoing.

[0045] In an optional embodiment, the real-time calculation of the instantaneous pose of the energy meter, calculating the deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory in the Lie algebra space, includes: Let the theoretical pose matrix at the current moment in the planned trajectory be denoted as... The instantaneous pose matrix calculated in real time is denoted as Both are 4×4 transformation matrices; Calculate the error matrix ; For the error matrix Perform logarithmic mapping operations to calculate , where the superscript ∨ represents the vee operator, resulting in a 6-dimensional column vector, which is the rotation and translation deviation in the SE(3) Lie algebra space.

[0046] In an optional embodiment, the step of constructing the pose tracking error objective function, using the Gauss-Newton iteration method to solve for the angle compensation increment of each axis of the turntable, and correcting the motor control commands until the energy meter stably reaches the predetermined test angle includes: Construct the robot Jacobian matrix J that reflects the relationship between the angular velocity of each axis joint of the turntable and the Lie algebraic rate of change of the end-effector pose; According to the formula Establish the normal equation, where This represents the current Lie algebra space bias; Solving the equation yields the angle compensation increment. ; Will The corrected motor control command is generated by superimposing it onto the theoretical joint angle command at the current moment.

[0047] It should be clarified that this application is not limited to the specific configurations and processes described above and shown in the figures. For the sake of brevity, detailed descriptions of known methods are omitted here. In the above embodiments, several specific steps are described and shown as examples. However, the method process of this application is not limited to the specific steps described and shown. Those skilled in the art can make various changes, modifications, and additions, or change the order of steps, after understanding the spirit of this application.

[0048] The functional modules shown in the above-described block diagram can be implemented as hardware, software, firmware, or a combination thereof. When implemented in hardware, they can be, for example, electronic circuits, application-specific integrated circuits (ASICs), appropriate firmware, plug-ins, function cards, etc. When implemented in software, the elements of this application are programs or code segments used to perform the required tasks. Programs or code segments can be stored on a machine-readable medium or transmitted over a transmission medium or communication link via data signals carried on a carrier wave. "Machine-readable medium" can include any medium capable of storing or transmitting information. Examples of machine-readable media include electronic circuits, semiconductor memory devices, ROM, flash memory, erasable ROM (EROM), floppy disks, CD-ROMs, optical disks, hard disks, fiber optic media, radio frequency (RF) links, etc. Code segments can be downloaded via computer networks such as the Internet, intranets, etc.

[0049] It should also be noted that the exemplary embodiments mentioned in this application describe methods or systems based on a series of steps or apparatus. However, this application is not limited to the order of the above steps; that is, the steps can be performed in the order mentioned in the embodiments, or in a different order, or several steps can be performed simultaneously.

[0050] The aspects of this application have been described above with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of this application. It should be understood that each block in the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, or other programmable data processing apparatus to produce a machine such that these instructions, executable via the processor of the computer or other programmable data processing apparatus, enable the implementation of the functions / actions specified in one or more blocks of the flowchart illustrations and / or block diagrams. Such a processor can be, but is not limited to, a general-purpose processor, a special-purpose processor, a special application processor, or a field-programmable logic circuit. It is also understood that each block in the block diagrams and / or flowcharts, and combinations of blocks in the block diagrams and / or flowcharts, can also be implemented by dedicated hardware performing the specified functions or actions, or can be implemented by a combination of dedicated hardware and computer instructions.

[0051] The above description is merely a specific implementation of this application. Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems, modules, and units described above can be referred to the corresponding processes in the foregoing method embodiments, and will not be repeated here. It should be understood that the protection scope of this application is not limited thereto. Any person skilled in the art can easily conceive of various equivalent modifications or substitutions within the technical scope disclosed in this application, and these modifications or substitutions should all be covered within the protection scope of this application.

Claims

1. A three-dimensional rotation control method for testing the radio frequency immunity of an energy meter, characterized in that, include: The relative position transformation relationship between the camera coordinate system and the 3D turntable coordinate system is calibrated, and a benchmark feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations is established; real-time monitoring images of the electricity meter in the radio frequency anechoic chamber are acquired, the image scale space is constructed and feature points and representations are extracted, the current image representations are matched with the benchmark feature library, and the random sampling consensus algorithm is used to eliminate mismatches to obtain the 2D-3D point pair correspondence relationship; The six-degree-of-freedom pose of the energy meter in the camera coordinate system is calculated based on the EPnP algorithm, and the pose is mapped to the turntable coordinate system using the relative position transformation relationship to obtain the current actual pose of the energy meter. Using the inverse kinematics model of the turntable, the current actual pose and the target test pose are converted into the starting angle and ending angle of each axis of the turntable, respectively. Smooth interpolation planning is performed in the joint space to generate a motion trajectory containing the theoretical angle sequence and the corresponding theoretical pose sequence. The instantaneous pose of the energy meter is calculated in real time. The deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory is calculated in the Lie algebra space. The pose tracking error objective function is constructed. The angle compensation increment of each axis of the turntable is solved by the Gauss-Newton iteration method. The motor control command is corrected until the energy meter stably reaches the predetermined test angle.

2. The method according to claim 1, characterized in that, The relative position transformation relationship between the calibrated camera coordinate system and the 3D turntable coordinate system is established to create a benchmark feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations, including: A high-resolution frontal image of the energy meter in its zero-position orientation is acquired, and the key points and feature representations in the image are extracted using the SIFT algorithm. Read the standard CAD 3D model of the electricity meter and establish the mapping relationship between the pixel coordinates (u,v) of key points and the 3D coordinates (X,Y,Z) of the corresponding physical points in the CAD model; The feature identifier, pixel coordinates, and 3D spatial coordinates are combined to form an associated dataset, which is then stored in a lookup table to generate a benchmark feature library.

3. The method according to claim 2, characterized in that, The process of acquiring real-time monitoring images of the energy meter in the radio frequency anechoic chamber, constructing the image scale space, and extracting feature points and symbols includes: The acquired real-time monitoring images are converted into grayscale images, and the images are subjected to continuous Gaussian blurring to construct a Gaussian difference pyramid containing 4 octaves and 5 layers in each octave. Extreme value detection is performed between adjacent layers of the pyramid, and the maximum or minimum points within a 26-neighborhood of the image pixel are detected as candidate feature points. The gradient magnitude and direction in the neighborhood of the candidate feature point are calculated to generate a SIFT feature representation with a 128-dimensional vector, and the vector is then normalized.

4. The method according to claim 1, characterized in that, The process of matching the current image representation with the benchmark feature library, eliminating false matches using a random sampling consensus algorithm, and obtaining the 2D-3D point pair correspondence includes: Calculate the Euclidean distance between the current image feature representation and the baseline representation stored in the baseline feature library; Feature point pairs with a ratio of nearest neighbor distance to second nearest neighbor distance of less than 0.6 are selected as the initial 2D-3D matching point set; Four pairs of points are randomly selected from the initial set of matching points, and the camera pose matrix is ​​calculated using the PnP algorithm. Calculate the error of reprojecting all remaining matching points through the pose matrix, and retain points with an error of less than 3 pixels as inliers; The selection and calculation process is iterated 1000 times to select the point set corresponding to the model with the most interior points. Then, through the index relationship of the benchmark feature library, the 2D coordinates of the current image corresponding to the interior points are associated with the 3D coordinates in the benchmark library to generate a 2D-3D point pair correspondence.

5. The method according to claim 1, characterized in that, The calculation of the six-degree-of-freedom pose of the energy meter in the camera coordinate system based on the EPnP algorithm includes: Four non-coplanar control points are selected in three-dimensional space, and all three-dimensional points in the reference feature library are represented as a weighted sum of the four control points. Based on the camera intrinsic parameter matrix and the extracted 2D feature point coordinates, a system of linear equations containing the coordinates of control points in the camera coordinate system is constructed. Singular value decomposition is performed on the coefficient matrix of the linear equation system to obtain the coordinates of the four control points in the camera coordinate system; Based on the correspondence between control points in the world coordinate system and the camera coordinate system, the rotation matrix and translation vector are solved using the Pruk analysis method.

6. The method according to claim 1, characterized in that, The step of performing smooth interpolation planning within the joint space to generate a motion trajectory containing a theoretical angle sequence and a corresponding theoretical pose sequence includes: Obtain the starting angle of each axis of the turntable. and termination angle ; Set the total interpolation time T and the sampling period. Fifth-order polynomial interpolation was used to calculate each discrete time step. Theoretical joint angle This ensures smooth speed and acceleration. Theoretical joint angle Substituting the forward kinematics model of the turntable, we can solve for the theoretical pose matrix corresponding to the end of the turntable at each moment. It serves as the tracking benchmark for visual servoing.

7. The method according to claim 5, characterized in that, The real-time calculation of the instantaneous pose of the energy meter includes calculating the deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory in the Lie algebra space, including: Let the theoretical pose matrix at the current moment in the planned trajectory be denoted as... The instantaneous pose matrix calculated in real time is denoted as Both are 4×4 transformation matrices; Calculate the error matrix ; For the error matrix Perform logarithmic mapping operations to calculate , where the superscript ∨ represents the vee operator, resulting in a 6-dimensional column vector, which is the rotation and translation deviation in the SE(3) Lie algebra space.

8. The method according to claim 1, characterized in that, The construction of the pose tracking error objective function, the solution of the angle compensation increment of each axis of the turntable using the Gauss-Newton iteration method, and the correction of the motor control commands until the energy meter stably reaches the predetermined test angle, includes: Construct the robot Jacobian matrix J that reflects the relationship between the angular velocity of each axis joint of the turntable and the Lie algebraic rate of change of the end-effector pose; According to the formula Establish the normal equation, where This represents the current Lie algebra space bias; Solving the equation yields the angle compensation increment. ; Will The corrected motor control command is generated by superimposing it onto the theoretical joint angle command at the current moment.

9. A three-dimensional rotation control system for testing the radio frequency immunity of an electricity meter, characterized in that, Includes the following modules: The module is used to calibrate the relative position transformation relationship between the camera coordinate system and the 3D turntable coordinate system, and establish a benchmark feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations; real-time monitoring images of the electricity meter in the radio frequency anechoic chamber are acquired, the image scale space is constructed and feature points and representations are extracted, the current image representations are matched with the benchmark feature library, and the random sampling consensus algorithm is used to eliminate mismatches to obtain the 2D-3D point pair correspondence; The mapping module is used to calculate the six-degree-of-freedom pose of the energy meter in the camera coordinate system based on the EPnP algorithm, and to map the pose to the turntable coordinate system using the relative position transformation relationship to obtain the current actual pose of the energy meter. The generation module is used to convert the current actual pose and the target test pose into the starting angle and ending angle of each axis of the turntable using the inverse kinematics model of the turntable. It performs smooth interpolation planning in the joint space to generate a motion trajectory containing the theoretical angle sequence and the corresponding theoretical pose sequence. The correction module is used to calculate the instantaneous pose of the energy meter in real time, calculate the deviation between the instantaneous pose and the theoretical pose at the current moment in the planned trajectory in the Lie algebra space, construct the pose tracking error objective function, use the Gauss-Newton iteration method to solve the angle compensation increment of each axis of the turntable, and correct the motor control command until the energy meter stably reaches the predetermined test angle.

10. The system according to claim 9, characterized in that, The relative position transformation relationship between the calibrated camera coordinate system and the 3D turntable coordinate system is established to create a benchmark feature library containing the 3D spatial coordinate information of key points on the surface of the electricity meter and corresponding feature representations, including: A high-resolution frontal image of the energy meter in its zero-position orientation is acquired, and the key points and feature representations in the image are extracted using the SIFT algorithm. Read the standard CAD 3D model of the electricity meter and establish the mapping relationship between the pixel coordinates (u,v) of key points and the 3D coordinates (X,Y,Z) of the corresponding physical points in the CAD model; The feature identifier, pixel coordinates, and 3D spatial coordinates are combined to form an associated dataset, which is then stored in a lookup table to generate a benchmark feature library.

Images