Sphere motion control device and control method based on multi-arm-of-force control platform

Abstract

The invention discloses a sphere motion control device and method based on a multi-arm-of-force control platform, an industrial camera is used for collecting a sphere image, and an image processing unit executes YOLO target detection, sub-pixel circle center extraction and Kalman filtering and outputs the real-time position and speed of a sphere. And the control unit drives the motor to output torque according to the real-time position and speed through three-stage closed-loop operation of a position loop, attitude inverse solution and a current loop, so that the platform generates a target inclination angle and Z-axis acceleration, and various motion states of the sphere on the surface of the platform are controlled. By introducing the Z-axis translation freedom degree and the high-frequency FOC current loop, the function limitation of a traditional cricket ball system is broken through, various control modes such as controllable bounce, trajectory tracking and rotary motion of the ball body are achieved, and the dynamic response capability and the control precision of the system are remarkably improved.

Description

TECHNICAL FIELD

[0001] The application relates to a ball motion control device and a control method based on a multi-force arm control platform, and belongs to the field of computer vision control. BACKGROUND

[0002] The Ball-and-Plate System is a classic experimental platform in the teaching of control theory and mechatronics. The basic principle is to change the inclination of the plate through a two-axis gimbal or a series mechanism, and to drive the ball to move in a two-dimensional plane by using the gravity component. A typical hardware scheme uses two servo motors to control the Pitch and Roll degrees of freedom of the X and Y axes, respectively. The vision part obtains the ball center position through color threshold segmentation. The control algorithm is mostly PID or LQR, and the target is usually limited to center point balance or simple trajectory tracking.

[0003] Technical defects and deficiencies:

[0004] (1) Lack of degrees of freedom: The traditional two-axis series mechanism can only provide two rotational degrees of freedom of Pitch / Roll, lacks the ability to move along the Z axis, and cannot realize the "bounce" action of the ball. Bounce requires the platform to generate a vertical acceleration greater than the gravity acceleration within milliseconds, and the existing motor-reducer combination is difficult to meet due to inertia and bandwidth limitations.

[0005] (2) Low driving bandwidth: Commercial steering machines generally use position loop control, and the current loop update frequency is less than 50Hz, the peak torque output is slow, and the "instantaneous large thrust-fast braking" bounce timing cannot be completed.

[0006] (3) Poor vision robustness: The RGB / HSV threshold scheme is extremely sensitive to changes in light, shadows and background clutter. Once the environmental brightness changes, the positioning fails. At the same time, there is a lack of high-precision estimation of the ball's speed and acceleration, and the system is prone to oscillation during high dynamic control.

[0007] (4) Single control target: Existing devices mostly take "center balance" as the only target. Long-term stable control of the platform edge or any coordinate point often fails due to motor dead zone, mechanical backlash and model failure. In addition, the traditional system cannot drive the ball to revolve around its own axis or around a certain point on the platform, nor does it have a controllable bounce function.

[0008] (5) Algorithm-hardware fragmentation: Although some documents introduce Kalman filtering or PID improvement, they are not deeply coupled with high-frequency current loop and parallel mechanism, and the algorithm performance is limited by low-speed driving, which cannot take advantage of high bandwidth. SUMMARY

[0009] The present invention provides a ball motion control device and control method based on a multi-lever control platform to solve the problems existing in the prior art.

[0010] The technical solutions adopted in this invention are as follows:

[0011] A ball motion control device based on a multi-lever control platform.

[0012] Base;

[0013] platform;

[0014] At least three sets of lever arm assemblies jointly support the platform. Each set of lever arm assemblies includes a motor and a linkage arm with joints. The motor is fixed to the base, and the linkage arm transmits the torque of the motor to the platform, so that the platform has a pitch rotational degree of freedom about the X-axis, a roll rotational degree of freedom about the Y-axis, and a translational degree of freedom about the Z-axis relative to the base, wherein the Z-axis is perpendicular to the surface of the platform.

[0015] An industrial camera, positioned directly above the platform, is used to capture images of the sphere.

[0016] The image processing unit is configured to perform YOLO target detection, subpixel center extraction and Kalman filtering on the image to output the real-time position and velocity of the sphere;

[0017] The control unit is configured to drive each motor to output torque based on the real-time position and velocity through a three-level closed-loop calculation involving a position loop, an attitude inverse kinematics, and a current loop, thereby enabling the platform to generate the target tilt angle and Z-axis acceleration.

[0018] Furthermore, the motor is an FOC brushless motor, the current loop is an FOC current loop, and the current loop update frequency is ≥10kHz.

[0019] Furthermore, the linkage arm includes a rocker arm, a connecting rod, and a universal joint. The rocker arm is fixedly connected to the motor shaft, one end of the connecting rod is connected to the rocker arm via a rotary joint, and the other end is connected to the bottom surface of the platform via the universal joint, forming a crank-connecting rod structure.

[0020] Furthermore, the control unit includes a host computer and a slave computer, which communicate via a CAN bus. The slave computer is an STM32 series MCU.

[0021] This invention also discloses a control method for a ball motion control device based on a multi-lever control platform, comprising the following steps:

[0022] a) Acquire spherical images of the platform surface using an industrial camera at 60-120 fps;

[0023] b) Perform YOLO target detection, subpixel center extraction, and Kalman filtering sequentially on the image to obtain the real-time position and velocity of the sphere;

[0024] c) Based on the real-time position and speed, the target torque of each motor is generated through three-level closed-loop calculations: position loop, attitude inverse solution, and current loop.

[0025] d) Input the target torque into the motors of each lever arm assembly. The motors drive the platform through the linkage arm, so that the platform simultaneously generates the target tilt angle and a Z-axis acceleration greater than the gravitational acceleration g, in order to control the motion state of the ball on the platform surface.

[0026] Furthermore, after the ball bounces for the first time, its horizontal drift and residual velocity upon landing are measured visually, and the equivalent restitution coefficient e and equivalent friction coefficient μ of the collision model are estimated online using the recursive least squares method. When the next bounce command is generated, the estimated values ​​are substituted into the collision-projectile feedforward formula to obtain the additional tilt angle correction and superimposed on the position loop output.

[0027] Furthermore, during the bouncing process, the instantaneous height observation is calculated by the change in the shadow area of ​​the sphere, and the flight time observation is obtained by using the projectile motion model. The two observations are input into a Kalman filter and fused to obtain a real-time height estimate. A height loop is added to the three-stage closed-loop operation, and the Z-axis acceleration pulse width is adjusted according to the height estimate to achieve closed-loop control of the bouncing height.

[0028] Furthermore, when the sphere's position error norm or velocity prediction value is lower than its respective threshold, the control output from the previous moment is maintained; a complete three-level closed-loop operation is only initiated when either threshold is exceeded.

[0029] Furthermore, in rotation mode, the actual circumference radius of the sphere is calculated in real time. If the deviation from the set radius exceeds a threshold, the amplitude of the platform's conical motion tilt angle is identified and corrected online.

[0030] Furthermore, a 1 kHz interrupt is generated by the lower-level timer to synchronously trigger the industrial camera exposure and current loop sampling, and a hard timestamp is inserted into the CAN frame; the upper-level computer interpolates and aligns the image, current, and attitude data streams according to the timestamp.

[0031] The present invention has the following beneficial effects:

[0032] (1) Without changing the platform diameter, the Z-axis translational degree of freedom is introduced so that the same set of devices can complete both traditional balance and trajectory tasks and controllable bouncing of the sphere, thus expanding the experimental content.

[0033] (2) After coupling the parallel multi-arm, FOC direct drive and visual feedback, the control cycle is shortened, the system response is faster, and the positioning accuracy is improved accordingly.

[0034] (3) The functions of rotation mode and high-level closed-loop bouncing are integrated into the same platform, allowing researchers to compare multiple algorithms without changing the hardware, thus reducing equipment redundancy.

[0035] (4) The event triggering and data synchronization mechanism reduces invalid calculations and communication load, and extends the service life of motors and mechanical parts.

[0036] (5) Online identification of collision parameters and self-calibration of cone motion can reduce the workload of manual calibration and improve experimental efficiency. Attached Figure Description

[0037] Figure 1 This is a structural diagram of the present invention.

[0038] Figure 2 This is a schematic diagram illustrating the principle of how a lever arm assembly drives a ball to bounce. Detailed Implementation

[0039] The invention will now be further described with reference to the accompanying drawings.

[0040] like Figure 1 and Figure 2 This invention discloses a ball motion control device based on a multi-lever control platform, comprising a base 1, three sets of lever arm assemblies 2, a platform 3, an industrial camera 4, an image processing unit, and a control unit.

[0041] The base 1 is constructed as a single unit using high-rigidity materials. The platform 3 is horizontally positioned above the base 1 and is supported by at least three sets of lever arm assemblies 2 symmetrically distributed to ensure that the platform 3 is subjected to uniform force and moves smoothly. Each set of lever arm assemblies 2 includes a motor 21 and a linkage arm 22. The motor 21 is an FOC brushless motor, whose fixed end is securely mounted on the base 1, and its output shaft is connected to the linkage arm 22.

[0042] The linkage arm 22 includes a rocker arm 221, a connecting rod 222, and a universal joint. One end of the rocker arm 221 is keyed to the output shaft of the motor 21, and the other end is movably hinged to one end of the connecting rod 222 through a rotary joint. The other end of the connecting rod 222 is connected to the bottom surface of the platform 3 through a universal joint, forming a complete crank-connecting rod transmission structure. This structure can efficiently convert the torque output by the motor 21 into multi-degree-of-freedom motion of the platform 3, enabling the platform 3 to not only achieve pitch rotation around the X-axis and roll rotation around the Y-axis relative to the base 1, but also have translational freedom along the Z-axis perpendicular to the surface of the platform 3, providing core structural support for the diverse motions of the ball, such as balance, rolling, rotation, and bouncing.

[0043] An industrial camera 4 is fixedly mounted on top of platform 3 via a camera bracket. The industrial camera 4 adopts a global shutter design and continuously acquires image data of the sphere on the surface of platform 3 at a frame rate of 60-120 fps. The image resolution is set to 640×480, which ensures the integrity of image information while meeting the real-time requirements of subsequent image processing.

[0044] The acquired image data stream is transmitted to the image processing unit in real time. The image processing unit first calls the lightweight YOLO deep learning model to perform target detection on the image, extracts the feature information of the sphere to lock its location, and outputs the corresponding region of interest.

[0045] Subsequently, within this area, sub-pixel-level center extraction is performed using Canny edge detection and Hough circle transform to obtain the precise pixel coordinates of the sphere's center. These coordinates are then converted to the position (X, Y) in the platform 3 physical coordinate system via perspective transformation. Simultaneously, the sphere's velocity (v) is calculated by differencing the coordinates of adjacent frames. x ,v y Finally, the position and velocity data are smoothed by Kalman filtering to eliminate errors caused by image noise and frame rate limitations, and the real-time status data of the sphere is output to provide reliable feedback for the control unit's calculations.

[0046] The control unit consists of a host computer and a slave computer, which establish bidirectional communication via a CAN bus. The slave computer uses an STM32 series MCU to ensure real-time computing performance. After receiving the real-time status data of the sphere, the control unit initiates a three-level closed-loop calculation process:

[0047] First, position loop calculations are performed based on the sphere's current position (X, Y) and velocity (v). x ,v y The deviations from the target position and target velocity are used to calculate the target attitude parameters required for platform 3;

[0048] Next, through attitude inverse kinematics calculation, combined with geometric parameters such as the size of platform 3 and the link length of lever arm assembly 2, the target attitude parameters are converted into the target torque of each motor 21. Finally, through the FOC current loop (update frequency not less than 10 kHz), the target torque is converted into a specific current control signal. This current loop decouples the stator current through Clark transformation and Park transformation to achieve precise and quiet control of the output torque of motor 21.

[0049] After receiving the current control signal, each motor 21 drives the platform 3 via the linkage arm 22 to synchronously generate the target tilt angle and Z-axis acceleration. When the Z-axis acceleration is greater than the gravitational acceleration g, the platform 3 applies an instantaneous upward thrust to the ball, causing the ball to gain an initial vertical velocity and detach from the surface of the platform 3, thus initiating the bouncing motion. At the instant the ball collides with the platform 3, the control unit calculates the velocity change based on a preset collision model formula.

[0050] The first is the formula corresponding to the equivalent restoration coefficient e (normal direction):

[0051] ,

[0052] in, This represents the normal velocity of the sphere after the collision. This represents the normal velocity of the sphere before the collision;

[0053] Secondly, the formula corresponding to the equivalent friction coefficient μ (tangential):

[0054] ,

[0055] ,

[0056] in, This represents the normal velocity of the sphere before the collision. , These represent the changes in velocity of the sphere in the X and Y directions after the collision, respectively.

[0057] The control unit acquires the velocity data of the sphere before and after the collision through the image processing unit, substitutes these data into the above collision model formula, constructs a linear observation equation about e and μ, and then uses the recursive least squares method to perform online iterative calculations to continuously update the estimated values ​​of e and μ, so that they gradually approach the real parameter values ​​in the actual collision process.

[0058] After the sphere leaves the platform, the control unit calculates its flight time based on the projectile motion model formula:

[0059] ,

[0060] in, It is the vertical velocity of the sphere at the instant it separates from the platform (estimated by the position difference between visual frames). It is gravitational acceleration. This is the desired bounce height. Simultaneously, an industrial camera continuously captures images of the shadow cast by the sphere on the platform surface. Through a pre-defined "shadow area-height" mapping, the real-time height of the sphere is obtained. The control unit then inputs this observation value, along with the height value calculated by the projectile motion model, into a Kalman filter for fusion processing, yielding the sphere's precise real-time height. Finally, through a newly added height loop operation, the Z-axis acceleration pulse width of the platform is dynamically adjusted based on the deviation between the real-time height and the desired height, achieving closed-loop control of the sphere's bounce height.

[0061] The control unit incorporates threshold judgment logic, pre-setting position error norm thresholds and velocity prediction thresholds. It compares in real time the magnitude of the deviation between the current position and the target position of the sphere, the magnitude of the deviation between the current velocity and the target velocity, and the corresponding threshold values. When both are below the threshold, it indicates that the sphere is in a stable motion state, and the control unit maintains the control output from the previous moment to avoid mechanical wear and invalid calculations caused by frequent motor starts and stops. Only when either threshold is exceeded will the complete three-level closed-loop calculation be restarted, reducing system energy consumption while ensuring control accuracy.

[0062] In rotation mode, the control unit drives the motors to work together through attitude inverse kinematics calculations, causing the platform to perform a small-amplitude conical motion. The centripetal force generated by this motion drives the sphere to revolve around a set radius. An industrial camera captures the sphere's trajectory in real time, and the image processing unit calculates the sphere's actual circumference radius by analyzing the coordinates of consecutive frames. The control unit compares the actual circumference radius with the set radius. If the deviation exceeds a preset threshold, an online identification process is initiated to analyze the cause of the deviation and correct the platform's conical motion tilt angle amplitude, adjusting the magnitude of the centripetal force to quickly return the sphere's actual circumference radius to the set value, ensuring the sphere rotates stably along the preset trajectory.

[0063] To eliminate time delays during data acquisition and transmission and ensure the accuracy of control calculations, the lower-level computer generates a 1 kHz interrupt signal via a timer. This interrupt signal synchronously triggers the exposure action of the industrial camera and the sampling action of the current loop. Simultaneously, a hard timestamp is inserted into the CAN frame sent from the lower-level computer to the upper-level computer to record the precise moment of data acquisition. After receiving image data from the industrial camera, current data from the current loop, and attitude data from the platform, the upper-level computer uses linear interpolation to perform time alignment processing on the data streams from different sources based on the hard timestamps in each data frame. This ensures that the image, current, and attitude data within the same control cycle correspond to the system state at the same moment, providing a time-synchronized data source for the control unit's calculations.

[0064] Through the coordinated operation of the above-mentioned device structure and control logic, the device can achieve various motion modes such as balancing the sphere at any position on the platform surface, tracking a preset trajectory, bouncing at a controllable height, rotating stably, and rolling to avoid obstacles, thus meeting the diverse needs of different algorithm verification in control theory teaching and complex scenario simulation in scientific research experiments.

[0065] The above description is only a preferred embodiment of the present invention. It should be noted that those skilled in the art can make several improvements without departing from the principle of the present invention, and these improvements should also be considered within the scope of protection of the present invention.

Claims

1. A ball motion control device based on a multi-lever control platform, characterized in that: Base; platform; At least three sets of lever arm assemblies jointly support the platform. Each set of lever arm assemblies includes a motor and a linkage arm with joints. The motor is fixed to the base, and the linkage arm transmits the torque of the motor to the platform, so that the platform has a pitch rotational degree of freedom about the X-axis, a roll rotational degree of freedom about the Y-axis, and a translational degree of freedom about the Z-axis relative to the base, wherein the Z-axis is perpendicular to the surface of the platform. An industrial camera, positioned directly above the platform, is used to capture images of the sphere. The image processing unit is configured to perform YOLO target detection, subpixel center extraction and Kalman filtering on the image to output the real-time position and velocity of the sphere; The control unit is configured to drive each motor to output torque based on the real-time position and velocity through a three-level closed-loop calculation involving a position loop, an attitude inverse kinematics, and a current loop, thereby enabling the platform to generate the target tilt angle and Z-axis acceleration.

2. The ball motion control device based on a multi-lever control platform as described in claim 1, characterized in that: The motor is an FOC brushless motor, the current loop is an FOC current loop, and the current loop update frequency is ≥10 kHz.

3. The ball motion control device based on a multi-lever control platform as described in claim 1, characterized in that: The linkage arm includes a rocker arm, a connecting rod, and a universal joint. The rocker arm is fixedly connected to the motor shaft. One end of the connecting rod is connected to the rocker arm via a rotary joint, and the other end is connected to the bottom surface of the platform via the universal joint, forming a crank-connecting rod structure.

4. The ball motion control device based on a multi-lever control platform as described in claim 1, characterized in that: The control unit includes a host computer and a slave computer, which communicate via a CAN bus. The slave computer is an STM32 series MCU.

5. A control method for a ball motion control device based on a multi-lever control platform as described in any one of claims 1-4, characterized in that: Includes the following steps: a) Acquire spherical images of the platform surface using an industrial camera at 60-120 fps; b) Perform YOLO target detection, subpixel center extraction, and Kalman filtering sequentially on the image to obtain the real-time position and velocity of the sphere; c) Based on the real-time position and speed, the target torque of each motor is generated through three-level closed-loop calculations: position loop, attitude inverse solution, and current loop. d) Input the target torque into the motors of each lever arm assembly. The motors drive the platform through the linkage arm, so that the platform simultaneously generates the target tilt angle and a Z-axis acceleration greater than the gravitational acceleration g, in order to control the motion state of the ball on the platform surface.

6. The control method for the ball motion control device based on a multi-lever control platform as described in claim 5, characterized in that: After the ball bounces for the first time, its horizontal drift and residual velocity upon landing are measured visually, and the equivalent restitution coefficient e and equivalent friction coefficient μ of the collision model are estimated online using the recursive least squares method. When the next bounce command is generated, the estimated values ​​are substituted into the collision-projectile feedforward formula to obtain the additional tilt angle correction and superimposed on the position loop output.

7. The control method for the ball motion control device based on a multi-lever control platform as described in claim 5, characterized in that: During the bounce, the instantaneous height observation is calculated by the change in the shadow area of ​​the sphere, and the flight time observation is obtained by the projectile motion model. The two observations are input into the Kalman filter and fused to obtain the real-time height estimate. A height loop is added to the three-stage closed-loop operation, and the Z-axis acceleration pulse width is adjusted according to the height estimate to realize the closed-loop control of the bounce height.

8. The control method for the ball motion control device based on a multi-lever control platform as described in claim 5, characterized in that: When the sphere's position error norm or velocity prediction value is lower than its respective threshold, the control output from the previous moment is maintained; a complete three-level closed-loop operation is only initiated when either threshold is exceeded.

9. The control method for the ball motion control device based on a multi-lever control platform as described in claim 5, characterized in that: In rotation mode, the actual circumference radius of the sphere is calculated in real time. If the deviation from the set radius exceeds the threshold, the platform's conical motion tilt angle amplitude is identified and corrected online.

10. The control method for the ball motion control device based on a multi-lever control platform as described in claim 5, characterized in that: A 1 kHz interrupt is generated by the lower-level timer to synchronously trigger the exposure of the industrial camera and the sampling of the current loop, and a hard timestamp is inserted into the CAN frame; the upper-level computer interpolates and aligns the image, current, and attitude data streams according to the timestamp.

Images