A control method and system of a microgravity ground test system and a storage medium

Abstract

The application discloses a kind of control method and system of microgravity ground test system and storage medium, it is related to the control technical field of robot, to solve the real-time compensation demand under high-frequency dynamic motion that existing suspension method is difficult to adapt, leading to the problem of pose tracking error accumulation.It includes the following steps: step S100, obtain image information by binocular camera, correct the radial distortion and tangential distortion of the image obtained by binocular camera, and detect the target motion posture and position information using the AprilTag visual reference library;Step S200, based on the detected motion posture and position information, the robot horizontal motion is closed-loop controlled by current loop, speed loop and position loop three-loop control, a dynamic distortion compensation mechanism is introduced to reduce the influence of robot motion or external disturbance, and the interframe difference method is used to control the motion speed.The present application ensures stable and reliable cooperation to complete various complex and dynamic tasks of robots.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robot control, in particular to a microgravity ground test system control method and system and a storage medium. BACKGROUND

[0002] The microgravity ground test system as the core infrastructure of the ground simulation of space robots and space vehicles has attracted widespread attention and focused investment from domestic and foreign research institutions in recent years. Through high-precision gravity compensation and multi-dimensional dynamic simulation, the system provides crucial technical support for the ground verification of space equipment. As the complexity of space missions continues to increase, the requirements for the ground test system are also increasing. The system needs to have higher dynamic response accuracy, a wider range of working conditions, and stronger multi-physical field coupling simulation capabilities to meet the needs of deep space exploration, on-orbit services, and other new types of missions. The existing methods for simulating microgravity environments on the ground mainly include free-fall motion, water floating, air floating, and suspension.

[0003] The free-fall motion method simulates microgravity through short-term weightlessness generated by sounding rockets, parabolic flight maneuvers, and drop towers. However, it is limited by the short duration of the test and cannot achieve continuous multi-dimensional motion control, making it difficult to meet the long-term verification needs of complex space missions, and it is also expensive. The water floating method uses the buoyancy of water to offset gravity, which can achieve low-friction motion, but the fluid damping effect is significant, and the dynamics of the buoyancy and the real microgravity environment in space differ greatly, which easily introduces additional disturbances. The air floating method uses air film support technology to achieve low-friction motion in the plane, which is suitable for microgravity simulation in two-dimensional planes, but it relies on complex air supply systems and precision machining platforms, which is costly and difficult to maintain, and it is difficult to extend to three-dimensional space multi-degree-of-freedom motion scenarios. In comparison, the suspension method applies counteracting gravity compensation to the measured object through ropes or rigid mechanisms, has the advantages of simple system structure, low cost, and strong scalability, and has become one of the mainstream technical routes.

[0004] However, the traditional suspension method has significant drawbacks: first, gravity compensation control is mostly based on static or quasi-static models, making it difficult to meet the real-time compensation needs of high-frequency dynamic motion, resulting in accumulated pose tracking errors; second, system operation relies on complex multi-level instruction interaction, which has the engineering problems of low human-machine collaboration efficiency and long debugging period. SUMMARY

[0005] The technical problem to be solved by the present application is:

[0006] The existing suspension method cannot meet the real-time compensation needs of high-frequency dynamic motion, resulting in accumulated pose tracking errors.

[0007] The technical solution adopted by the present application to solve the above technical problems is:

[0008] The application provides a control method of a micro-gravity ground test system, comprising the following steps:

[0009] S100, image information is acquired by a binocular camera, radial distortion and tangential distortion of the image acquired by the binocular camera are corrected, and a target motion posture and position information are detected by using an AprilTag visual reference library;

[0010] S200, based on the detected motion posture and position information, a robot horizontal motion is controlled in a closed loop by a current loop, a speed loop and a position loop three-loop control, a dynamic distortion compensation mechanism is introduced to reduce influences of robot self motion or external disturbance, and a frame difference method is used to control the motion speed.

[0011] Further, the radial distortion and the tangential distortion of the image acquired by the binocular camera are corrected in step S100, and specifically,

[0012] the radial distortion is corrected by using a polynomial function of more than two related to a center distance, and the radial distortion of a point is expressed as:

[0013] ;

[0014] In the above formula, is a radial distortion coefficient, is a radial distance;

[0015] the tangential distortion is expressed as:

[0016] ;

[0017] In the above formula, is a tangential distortion coefficient, is a tangential distance;

[0018] the corrected coordinates are:

[0019] ;

[0020] the corrected coordinates are projected to a pixel plane, and an intrinsic matrix of the camera is transformed:

[0021] ;

[0022] In the above formula, is a coordinate of the pixel plane after correction, is a focal length of the camera in x , y directions, c x and c ythe coordinates of the camera light spot in the pixel coordinate system;

[0023] The pixel coordinates are obtained as follows:

[0024] .

[0025] Further, the step S100 of detecting the target motion posture and position information by using the AprilTag visual reference library comprises the following steps:

[0026] S110, processing the three-dimensional object image obtained by the binocular camera to obtain two-dimensional image information;

[0027] S120, detecting the edge of the target in the image according to the gradient;

[0028] S130, finding the required quadrilateral pattern in the edge image and screening, eliminating the non-straight edge, and finding the adjacent edge in the straight edge, and finally forming a closed loop if a quadrilateral is formed;

[0029] S140, two-dimensional code encoding and decoding: using multiple black edge block lengths for encoding, for decoding content, generating a point array in the detected quadrilateral for calculating the value of each color block, classifying the color blocks in the quadrilateral to obtain the encoding and ID of the two-dimensional code, and obtaining the rotation of the flying robot by comparing the changes before and after the encoding pose;

[0030] S150, solving the motion posture and position information of the flying robot.

[0031] Further, the step S110 comprises the following steps:

[0032] Suppose the optical center of the left eye camera is , the coordinate system is , the optical center of the right eye camera is , the coordinate system is , and the physical distance between is the baseline of the binocular camera ; the point in space is imaged on the imaging plane of the left eye camera and the right eye camera respectively ; In the pixel coordinate system, the difference in the u-axis value is respectively ; the focal length of the left and right eye cameras is , then:

[0033]

[0034] then:

[0035] In the above formula, is the parallax, is the theoretical maximum depth measured by the binocular camera;

[0036] corresponding three-dimensional point in the left camera coordinate system is:

[0037] .

[0038] Further, the step S150 comprises the following process:

[0039] By using the correspondence of the four sets of points, the single linear transformation matrix of the two-dimensional code is solved by using the direct linear transformation algorithm through the four vertex coordinates detected in the two-dimensional code image and the four standard vertex coordinates of the original template of the two-dimensional code; the rotation transformation vector Rvec and the translation transformation vector Tvec are solved by decomposing the matrix, the correct posture of the two-dimensional code is solved according to the obtained rotation vector, and finally the motion posture and position information of the flying robot are obtained.

[0040] Further, the step S200 comprises the following process:

[0041] For current loop control, first, the three-phase current actually output by the motor is sampled, then the current is converted into the current corresponding to the rotor direct axis through Clark transformation and Park transformation, the converted current is compared with the current expected value to calculate the error, the voltage signal required to correct the error is calculated according to the error, the voltage signal is converted into the switching signal of the control circuit to realize the adjustment of the current; for the speed loop and the position loop, the actual output speed and the position are obtained by increasing the encoder on the basis of the current loop control, the errors in the speed and the position are converted into the current error, and then the current is controlled by using the current loop, so as to realize the control of the speed and the position.

[0042] Further, the step S200 comprises the following process:

[0043]

[0044] In the above formula, is the position error, , is a dynamic adjustment parameter, and t is time.

[0045] Further, the step S200 comprises the following process: ​At this time, the inter-frame difference method is used to update the distortion coefficient in real time:

[0046]

[0047] wherein, 、 is the distortion coefficient before updating, 、 is the distortion coefficient after updating, , 、 is an adaptive parameter.

[0048] The application also provides a control system of the microgravity ground test system, which has program modules corresponding to the steps of the method according to any one of the above technical solutions, and executes the steps in the control method of the microgravity ground test system.

[0049] The application also provides a computer readable storage medium, which stores a computer program configured to realize the steps in the control method of the microgravity ground test system according to any one of the above technical solutions when called by a processor.

[0050] Compared with the prior art, the application has the following beneficial effects:

[0051] The application integrates real-time detection, compensation algorithm and accurate control strategy for high-frequency dynamic motion. For motion detection, the application adopts a fusion mode based on binocular vision and AprilTag code marking to realize real-time and high-precision tracking and analysis of the robot target carried by the test platform in a high-frequency dynamic environment. This algorithm significantly improves the system response speed and robustness, can capture the instantaneous position, speed and motion trend of the target in real time and accurately, effectively avoids the loss of the target when the target moves quickly or the posture changes suddenly, and provides a reliable basis for subsequent real-time compensation control. For motor control, based on the real-time acquired motion state information, the application combines the control strategy of the current loop, the speed loop and the position loop to quickly and accurately move the robot carried by the demonstration platform to the target position in a task containing high-frequency dynamic motion. Through the dynamic distortion compensation mechanism, the influence of the robot's own motion or external disturbance is effectively offset or reduced, and the stable and reliable cooperation of the robot in completing various complex and dynamic tasks preset on the test platform is ensured. BRIEF DESCRIPTION OF DRAWINGS

[0052] Figure 1 is the motion detection flowchart of the microgravity ground test system in the embodiment of the application;

[0053] Figure 2 is the coordinate system transformation flowchart of motion detection in the embodiment of the application;

[0054] Figure 3 AprilTag example diagram for motion detection in embodiments of the present application;

[0055] Figure 4 Control interface schematic diagram for microgravity ground demonstration system in embodiments of the present application;

[0056] Figure 5 Motor control flow chart for microgravity ground test system in embodiments of the present application. DETAILED DESCRIPTION

[0057] In order to make the personnel in the technical field better understand the present application scheme, the exemplary embodiments or examples of the present application will be described in conjunction with the drawings in the following. Obviously, the described embodiments or examples are only a part of the embodiments or examples of the present application, not all. Based on the embodiments or examples in the present application, all other embodiments or examples obtained by those skilled in the art without making creative efforts should belong to the scope of protection of the present application.

[0058] In order to make the above-mentioned purposes, features and advantages of the present application more obvious and easy to understand, the specific embodiments of the present application will be described in detail below in conjunction with the drawings.

[0059] Specific implementation scheme one: the present application provides a control method of a microgravity ground test system, comprising the following steps:

[0060] Step S100, acquiring image information through a binocular camera, correcting the radial distortion and tangential distortion of the image acquired by the binocular camera, and detecting the target motion posture and position information by using an AprilTag visual reference library;

[0061] Step S200, based on the detected motion posture and position information, performing closed-loop control on the horizontal motion of the robot through current loop, speed loop and position loop three-loop control, introducing a dynamic distortion compensation mechanism to reduce the influence of the robot's own motion or external disturbance, and controlling the motion speed by using inter-frame difference method.

[0062] In step S100, in order to ensure real-time and accurate capture of the target motion posture and position, a long-base binocular + AprilTag target vision detection scheme based on OpenCV is adopted for compensation. In the application, a binocular camera is used as a sensor for obtaining target information, left-eye images and right-eye images with a height of 720 pixels and a width of 1280 pixels are collected, a pinhole camera projection model, a camera distortion model and a binocular camera projection model are established to obtain image information, and OpenCV4.5.1 and a checkerboard calibration method are used to calibrate the internal and external parameters of the binocular camera. In the application, AprilTag is adopted, various edges in the image are detected according to the gradient, the required quadrilateral pattern is found in the edge image and screened, then two-dimensional code encoding and two-dimensional code decoding are performed, the ID of the observed code and the rotation of the code are determined, the one-dimensional linear transformation matrix of the two-dimensional code is calculated, and the flight robot motion posture and position information can be obtained by decomposing the matrix.

[0063] Specific implementation scheme two: the main types of camera distortion are radial distortion and tangential distortion. Assuming that the Cartesian coordinates of a point on a plane are represented as , the polar coordinates are represented as , the radial distortion can be regarded as a change of from the distance of the point to the origin, and the tangential distortion can be regarded as a change of from the horizontal angle. In step S100, the radial distortion and the tangential distortion of the binocular camera for obtaining images are corrected, specifically: The radial distortion is corrected by using a polynomial function of the second order or more related to the distance from the center, and the radial distortion of the point

[0064] is represented as:

[0065] ;

[0066] In the above formula, is the radial distortion coefficient, is the radial distance;

[0067] The tangential distortion is represented as follows:

[0068] ;

[0069] In the above formula, is the tangential distortion coefficient, is the tangential distance;

[0070] The corrected coordinates are:

[0071] ;

[0072] ​Project the corrected coordinates to the pixel plane, transform the intrinsic matrix of the camera:

[0073] ;

[0074] In the above formula, is the coordinate of the corrected pixel plane, is the focal length of the camera in x , y direction, c x and c y is the coordinate of the camera light point in the pixel coordinate system;

[0075] The pixel coordinate is obtained as:

[0076] .

[0077] The other embodiments of the present embodiment are the same as the specific embodiment one.

[0078] Specific embodiment three: as shown in Figure 3 , an AprilTag for motion detection is added to the actual flying robot, and the target is quickly detected and the relative position is calculated through a specific mark. The AprilTag visual reference library is used to detect the target motion posture and position information in step S100, including the following steps:

[0079] S110, the three-dimensional object image obtained by the binocular camera is processed by a model to obtain two-dimensional image information, and the three-dimensional object in the field of view of the camera is projected onto a two-dimensional imaging plane;

[0080] S120, the edges of the target in the image are detected according to the gradient;

[0081] S130, find the required quadrilateral pattern in the edge image and screen out, eliminate non-straight edges, and find adjacent edges in straight edges. Finally, if a closed loop is formed, it is detected as a quadrilateral;

[0082] S140, two-dimensional code encoding and decoding:

[0083] Three black edge block lengths of 8, 7 and 6 are used for encoding. For decoding content, a point array is generated in the detected quadrilateral for calculating the value of each color block, and a simple classifier is constructed according to the local binary pattern (Local Binary Patterns) to classify the color blocks in the quadrilateral. The positive example color block is encoded as 1, and the negative example color block is encoded as 0, to obtain the encoding and the encoding ID and the encoding rotation of the two-dimensional code. After obtaining the encoding, match it with the known library to determine whether the decoded two-dimensional code is correct.

[0084] S150, solving the motion posture and position information of the flying robot.

[0085] The other aspects of the embodiment are the same as those of embodiment two.

[0086] In the embodiment, the Canny operator method is used, although the detected edges are reduced, but the noise in the image is also reduced.

[0087] For the obtained binary edge image, polygon analysis is needed, first, Topological structural analysis of digitized binary images is used to find the polygon. The algorithm uses the idea of coding, first, different edges are given different coding values. From the starting point, the pixels of the edge are edited, and the edge points of the same type as the starting point are found. When scanning to the starting point, the polygon closed loop is formed, and the next starting point is switched to repeat the operation, until all binary points are traversed, and the edges less than 4 are excluded, and the ones that meet the conditions are further processed.

[0088] Using the Finding the convex hull of a simple polygon algorithm, the convex hull of each polygon itself is calculated, and the area of the convex hull and the polygon is calculated. When the polygon area is larger than the convex hull, the polygon is excluded, and the remaining polygons that meet the conditions are retained.

[0089] For the final polygon that meets the conditions, the Douglas-Peuckeralgorithm is used for quadrilateral approximation. Find the two endpoints A and B of the curve, and draw the chord between the curve, which is the line segment AB. Find the point C that is farthest from the line segment AB on the curve, and calculate the distance d between them. Compare the distance d with the threshold value set in advance. If the distance d is less than the threshold value, the straight line can be directly approximated as a curve, and the curve segment is processed. If the distance d is greater than the given threshold value, the curve is divided into AC and BC by point C, and the line is taken for 1 to 3 processing. When all the curves are processed, connect the broken lines formed by the segmentation points in turn, so that the curve before processing can be approximated as a straight line, and the polygons with a vertex number not equal to 4 are excluded.

[0090] Since the quadrilateral detected according to the above method does not necessarily meet the use requirements, it is necessary to code, match and check the quadrilateral obtained in the above step. For the obtained quadrilateral, firstly, the point array inside the quadrilateral is determined. The quadrilateral in the above step is stored in the form of four vertices, and the specific coordinates of the point array inside the quadrilateral can be determined according to the four points. The number of point arrays is different in different coding modes, and the coding mode is determined by the user when calibrating the object. Different coding modes generate different inner point coordinates.

[0091] In the determined quadrilateral, the average value Value1 of the pixels of the outermost periphery of the point array in the gray-scale image is extracted, and the average value Value2 of the pixels of the next outer periphery of the point array is extracted. According to the design of the two-dimensional code library of AprilTag itself, there is a clear threshold line between Value1 and Value2 under the same lighting environment. The threshold value is determined as the average of Value1 and Value2. The part of the pixel value of all point coordinates of the entire point array that is higher than the threshold value is coded as 0, and the part that is lower than the threshold value is coded as 1. Thus, coding is performed from the first row, and the entire point array is coded. Arranging the codes will obtain a string of binary codes, and the length of the string is determined by the specific coding mode. Each quadrilateral can obtain a string of binary codes representing the coding of the two-dimensional code in this state. An incorrect quadrilateral often generates an incorrect code, and an incorrect code cannot be matched to the corresponding ID within the threshold value range, so the quadrilateral with an incorrect code can be easily excluded.

[0092] Matching with the known coding library further determines whether the coding is reliable. Since the observed coding exists in a rotating state, the obtained coding should be rotated by 90° three times to obtain the coding in four directions, and then compared with the coding library one by one to solve the Hamming distance between the codings. When the Hamming distance between the observed coding and a coding in the known library is less than a given threshold value, it is determined that the ID of the observed coding is the ID of the coding in the matching coding library, and the Hamming distance is recorded. If the coding library does not have a matching coding, it is determined that the observed coding is incorrect, and the quadrilateral corresponding to the coding is discarded.

[0093] Specific embodiment four: step S110 includes the following steps:

[0094] As shown in Figure 1 , the binocular camera projection model estimates the depth of each pixel by using the disparity of the left eye image and the right eye image synchronously captured by the binocular camera, so as to determine the distance of the object. The left eye camera and the right eye camera of the binocular camera used in the present application can be regarded as pinhole cameras.

[0095] Suppose the optical center of the left eye camera is , the coordinate system is , and the optical center of the right eye camera is , coordinate system is , and the physical distance of binocular camera baseline . The point in space in the imaging plane of the left eye camera, the right eye camera imaging is . In the pixel coordinate system, only the value of the u-axis is different, respectively . The focal length of the left and right eye cameras is , according to the similarity of the triangle, then:

[0096] (5)

[0097] then: (6)

[0098] In the above formula, is the parallax, is the theoretical maximum depth measured by the binocular camera;

[0099] The corresponding three-dimensional point The coordinates in the left camera coordinate system are:

[0100] (7)

[0101] The other embodiments of the present embodiment are the same as embodiment three.

[0102] The present embodiment directly projects the three-dimensional object within the camera field of view onto the two-dimensional imaging plane, and calibrates the camera intrinsic parameters using Zhang's calibration method, layered calibration method, etc.

[0103] As shown in Figure 2 , the pixel coordinate system is for a single image, with the upper left corner of the image as the origin, the horizontal left direction of the image as the horizontal axis, and the vertical downward direction of the image as the vertical axis. The pixel coordinate system is in units of pixels; if the pixels are converted to distances, with the image center as the origin, the coordinate axis direction remains unchanged. This coordinate system is called the image coordinate system. The camera coordinate system is composed of the focusing center of the camera as the origin, the horizontal left direction parallel to the image coordinate system (parallel to the horizontal axis of the image coordinate system) as the x-axis, and the vertical downward direction as the y-axis. The camera optical axis is the z-axis. The world coordinate system can describe the position of the camera in the real space. In the world coordinate system, the pose of any object can be converted to the camera coordinate system by the rotation matrix and the translation vector .

[0104] Embodiment five: step S150 includes the following process:

[0105] The single linear transformation matrix (Homography Matrix) of the two-dimensional code is calculated, and the rotation matrix and the translation matrix are further decomposed. The single linear transformation matrix relative to the original coordinates is solved according to the coordinates of the four vertices of the quadrilateral, and the related posture information of the two-dimensional code is obtained in combination with the information of the multi-camera. The rotation transformation vector Rvec and the translation transformation vector Tvec are solved by decomposing the matrix, and the correct posture of the two-dimensional code is solved according to the obtained rotation vector, and finally the motion posture and position information of the flying robot are obtained. The other aspects of this embodiment are the same as those of embodiment four.

[0106] Embodiment six: the three-loop control of the current loop, the speed loop and the position loop for the closed-loop control of the horizontal motion of the robot in S200 includes the following processes:

[0107] The motor control is used to realize the accurate control of the robot carried by the microgravity demonstration platform in the XY axis arbitrary translation, and the freedom of the robot in the XY direction is realized through the detected motion information. In the Z direction, the gravity influence is balanced through the suspension device, and the robot posture is adjusted in real time through the internal rotation structure. The motor control of the microgravity ground test system adopts FOC control, including three-loop control of the current loop, the speed loop and the position loop.

[0108] For current loop control, first, the three-phase current actually output by the motor is sampled, then the current is converted into the current corresponding to the rotor direct axis through Clark transformation and Park transformation, and then the converted current is compared with the expected value to calculate the error, and the error is sent to the PI controller to calculate the voltage signal required for the correction error, and then the voltage signal is converted into the switching signal of the actual control circuit to realize the adjustment of the current. For the control of the speed loop and the position loop, the actual output speed and position are obtained by adding an encoder on the basis of the current loop control, and a PID controller is added to convert the errors of the speed and position into the errors of the current, and then the current is controlled by using the current loop, so as to realize the control of the speed and the position.

[0109] As shown in Figure 5 , the FOC (Field Oriented Control) of the embodiment is to control the rotation of the motor by controlling the current of the stator winding. Through Clark transformation and Park transformation, the controlled current is linearized to obtain and . The electromagnetic torque of the motor is calculated as follows:

[0110] (8)

[0111] In formula (8), is the number of pole pairs, is the permanent magnet flux, For inductance.

[0112] electromagnetic torque The size and 、 Related, increase , As it increases, the angular acceleration of the motor increases, and if When it is a negative value, it can generate positive electromagnetic torque; the larger its absolute value is, the The larger it is, the greater the angular acceleration of the motor.

[0113] The rest of this implementation plan is the same as the specific implementation plan five.

[0114] Specific implementation plan seven: Establish a vision-motor closed-loop control, feed back the posture information detected in step S100 to the current loop control module in real time, and dynamically adjust the expected value of the q-axis current .

[0115]

[0116] In the above formula, is the position error, 、 is a dynamic adjustment parameter, and t is time.

[0117] The rest of this implementation plan is the same as the specific implementation plan six.

[0118] Specific implementation scheme eight: In step S200, the dynamic distortion compensation mechanism is introduced and the motion speed is controlled by using the inter-frame difference method. Specifically, when the robot motion speed is greater than the speed threshold, When , the distortion coefficient is updated in real time using the inter-frame difference method;

[0119]

[0120] in, 、 is the distortion coefficient before updating, 、 is the updated distortion coefficient, , 、 is an adaptive parameter.

[0121] The rest of this implementation plan is the same as the specific implementation plan seven.

[0122] like Figure 4As shown, the microgravity ground demonstration system control interface is built by using Qt user interface application development software. Through the control interface, the camera device can be selected and connected, the detection of AprilTag code is started and the dynamic display of recognition results is performed, and the microgravity demonstration platform is manually controlled.

[0123] Figure 4 The CAM0 and CAM1 correspond to two cameras in the microgravity platform, and the drop-down box behind them can select the camera device. The ReFresh button can refresh the camera device connected to the current computer. The “OPEN CAM0” button and the “OPEN CAM1” button are used to open the CAM0 and CAM1 camera devices, respectively. The “START CAM0 DETECT” button and the “START CAM1DETECT” button are used to start the detection of AprilTag code and start the dynamic display of recognition results in the left window and the right window, respectively. The three-dimensional coordinates shown in the image are the real coordinates of the robot in the camera coordinate system (unit: meter).

[0124] The “INIT BaseVAL” button is used to initialize the balance zero position, i.e., the robot is in a suspended free and stationary state. The initialized value is indicated by the “horizontal (x-axis) reference position” and the “vertical (y-axis) reference position” below the interface.

[0125] The check boxes “Enable beam servo motor” and “Enable load servo motor” can control the enablement of the two motor drivers of the microgravity demonstration platform, i.e., to make the motor excitation. The “Emergency Stop” button is used to quickly disable the two motor drivers in an emergency, i.e., to make the motor brake. In the normal state, clicking the check box changes the state of the check box to enable or disable the motor driver.

[0126] The serial connection function is in the upper right corner of the software interface. After clicking the “Refresh” button, select the serial number of the underlying drive board in the drop-down box. The “Open serial” function is used to open the serial port selected in the drop-down box. After opening the serial port, the red indicator will change to green. The front, rear, left and right buttons below the interface are four buttons for manually controlling the microgravity demonstration platform when debugging the software performance.

[0127] The microgravity ground test system control interface of the embodiment; the interface integrates real-time motion state display and instruction input function, users not only can control the demonstration platform conveniently, can also observe the recognition result, motion trajectory and compensation state of the robot directly and in real time. This graphical and interactive control mode avoids the complex code instruction input process of the traditional method, significantly reduces the operation difficulty of the test system, and improves the human-machine cooperation efficiency in high-frequency dynamic tasks.

[0128] The control method (algorithm) of the microgravity ground test system proposed by the present application is the underlying technical core of the present application, and various products can be derived based on the algorithm.

[0129] Based on the method proposed by the present application, a microgravity ground test system control system is developed by using a program language, the system has program modules corresponding to the steps of the above technical solutions, and when running, the steps in the above microgravity ground test system control method are executed.

[0130] The computer program of the developed system (software) is stored on a computer readable storage medium, and the computer program is configured to realize the steps of the above microgravity ground test system control method when called by a processor. That is, the present application is materialized on a carrier to become a computer program product.

[0131] The various embodiments of the systems and techniques described herein can be implemented in digital electronic circuitry, integrated circuitry, specially designed ASICs (application specific integrated circuits), computer hardware, firmware, software, and / or combinations of them. These various embodiments can include implementation in one or more computer programs that are executable and / or interpretable on a programmable system including at least one programmable processor, which can be special or general purpose, coupled to receive data and instructions from, and to transmit data and instructions to, a storage system, at least one input device, and at least one output device.

[0132] The computing procedures (also referred to as programs, software, software applications, or code) in the present application include machine instructions for programmable processors, and can be implemented using high-level procedural and / or object-oriented programming languages, and / or assembly / machine languages. As used herein, the terms "machine-readable medium" and "computer-readable medium" refer to any computer program product, apparatus and / or device (e.g., magnetic discs, optical disks, memory, Programmable Logic Devices (PLDs)) used to provide machine instructions and / or data to a programmable processor, including a machine-readable medium that receives machine instructions as a machine-readable signal. The term "machine-readable signal" refers to any signal used to provide machine instructions and / or data to a programmable processor.

[0133] Although the present application has been disclosed with reference to the above embodiments, the scope of the present application is not limited to the above embodiments. It will be understood by those of ordinary skill in the art that various changes and modifications can be made to the present application without departing from the spirit and scope of the present application.

Claims

1. A control method for a microgravity ground test system, characterized in that: The steps include: S100, acquiring image information through a binocular camera, correcting radial distortion and tangential distortion of the image acquired by the binocular camera, and detecting target motion posture and position information using the AprilTag visual reference library; S200, based on the detected motion posture and position information, perform closed-loop control on the robot's horizontal motion through the current loop, speed loop, and position loop control, introduce a dynamic distortion compensation mechanism to reduce the influence of the robot's own motion or external disturbances, and use the inter-frame difference method to control the motion speed; The closed-loop control of the robot's horizontal motion by the three-loop control of current loop, speed loop, and position loop described in step S200 includes the following process: For current loop control, the three-phase current output by the actual motor is sampled first. Then, the current is converted to the current corresponding to the rotor's orthogonal axis through Clark transform and Park transform. The converted current is then compared with the expected current value to calculate the error. The voltage signal required to correct the error is calculated based on the error. The voltage signal is converted into a switching signal for the control circuit to adjust the current. For the speed loop and position loop, an encoder is added to the current loop control to obtain the actual output speed and position. The speed and position errors are converted into current errors. The current loop is then used to control the current, thereby achieving speed and position control. In step S200, the inter-frame difference method is used to control the motion speed, specifically: when the robot motion speed is greater than the speed threshold v max When , the inter-frame difference method is used to update the distortion coefficient in real time: Among them, k1 and p1 are the distortion coefficients before updating, k′1 and p′1 are the distortion coefficients after updating, Δv=vv max , α and β are adaptive parameters.

2. The control method of the microgravity ground test system according to claim 1, characterized in that: The radial distortion and tangential distortion of the image acquired by the binocular camera are corrected in step S100, specifically: The radial distortion is corrected by a polynomial function of quadratic or higher related to the center distance. T The correction process of radial distortion is: x d =x(1+k1r 2 +k2r 4 +k3r 6 ) and d =y(1+k1r 2 +k2r 4 +k3r 6 ) In the above formula, is the coordinate on the corrected plane, k1, k2, k3 are radial distortion coefficients, and r is the radial distance; The following formula is used to correct the tangential distortion: x d =x+2p1xy+p2(r 2 +2x 2 ) y d =y+2p2xy+p1(r 2 +2y 2 ) In the above formula, p1 and p2 are tangential distortion coefficients; The coordinates after calibration are: Projecting the corrected coordinates onto the pixel plane yields: In the above formula, u and v are the coordinates of the pixel plane after correction, f x 、f y is the focal length of the camera in the x and y directions, c x and c y is the coordinate of the camera light point in the pixel coordinate system.

3. The control method of the microgravity ground test system according to claim 2, characterized in that: The detection of target motion posture and position information using the AprilTag visual reference library in step S100 includes the following steps: S110, processing the three-dimensional object image acquired by the binocular camera to obtain two-dimensional image information; S120, detecting the edge of the target in the image according to the gradient; S130, finding the required quadrilateral pattern in the edge image and screening it, eliminating non-straight edges, searching for adjacent edges on straight edges, and finally detecting a quadrilateral if a closed loop is formed; S140, performing QR code encoding and decoding: using multiple black-bordered color block lengths for encoding, generating a point array within the detected quadrilateral for calculating the value of each color block for decoding, classifying the color blocks within the quadrilateral, obtaining the QR code and the encoded ID, and determining the rotation of the flying robot by comparing the changes before and after encoding. S150: Calculate the motion posture and position information of the flying robot.

4. The control method of the microgravity ground test system according to claim 3, characterized in that: Step S110 includes the following steps: Assume that the optical center of the left camera is O L , the coordinate system is O L -x L -y L -z L , the optical center of the right camera is O R , the coordinate system is O R -x R -y R -z R , O L With O R The physical distance is the baseline b of the binocular camera; the images of the midpoint P in the imaging plane of the left camera and the right camera are P L 、P R ;P L 、P R In the pixel coordinate system, the difference in the u-axis value is recorded as u L 、u R ; The focal length of the left and right cameras is both f, then: but: In the above formula, d is the parallax, and fb is the theoretical maximum depth measured by the binocular camera; P L 、P R The corresponding three-dimensional point P is in the left camera coordinate system O L -x L -y L -z L The coordinates below are:

5. The control method of the microgravity ground test system according to claim 4, characterized in that: Step S150 includes the following process: By comparing the four vertex coordinates detected in the QR code image with the four standard vertex coordinates of its original template, and utilizing the correspondence between the four sets of point pairs, a direct linear transformation algorithm is used to solve the single linear transformation matrix of the QR code; decomposing the matrix solves the rotation transformation vector Rvec and the translation transformation vector Tvec, and based on the obtained rotation vector, the correct posture of the QR code is solved, and finally the motion posture and position information of the flying robot are obtained.

6. The control method of the microgravity ground test system according to claim 5, characterized in that: In step S200, the current loop control also includes dynamically adjusting the current expected value I q_ref : In the above formula, e pos is the position error, K p , K d is a dynamic adjustment parameter, and t is time.

7. A control system for a microgravity ground test system, characterized in that: The system has a program module corresponding to the steps of the method described in any one of claims 1 to 6, and executes the steps of the control method of the microgravity ground test system when running.

8. A computer-readable storage medium, characterized in that The computer-readable storage medium stores a computer program, and the computer program is configured to implement the steps of the control method of the microgravity ground test system according to any one of claims 1 to 6 when called by a processor.

Images