Control method, storage medium and system for six-degree-of-freedom motion system

Abstract

The present application relates to the field of servo control systems and discloses a control method, storage medium, and system for a six-degree-of-freedom motion system. The method includes: planning the motion trajectories of six sliders based on a target posture to obtain six smooth cam curves; fusing the measurement data of a motor encoder and a magnetic scale encoder for each slider to obtain displacement data for each slider; and based on the displacement data of each slider, performing six-axis synchronous coordinated control using a virtual axis as a guide axis and six smooth cam curves as follower axes to enable the six-degree-of-freedom motion system to operate based on the target posture. The beneficial effect is that by setting a precise target posture plan and fusing multi-sensor data, precise control of the six sliders in the six-degree-of-freedom motion system is achieved, which not only improves the control accuracy and response speed, but also enhances its stability and reliability.

Description

Technical Field

[0001] The present application relates to the field of servo control systems, and in particular to a control method, storage medium, and system for a six-degree-of-freedom motion system. Background Art

[0002] A six-degree-of-freedom motion system refers to a system that can realize motion control in six directions (three linear directions and three rotational directions) in three-dimensional space. It can be used to simulate the complex motion of aircraft or robotic arms in space.

[0003] However, the control method for the six-degree-of-freedom motion system in the related art still has some limitations, and a control method that can improve its reliability and accuracy is needed. Summary of the Invention

[0004] This application aims to solve, at least to some extent, one of the technical problems in the related art. To this end, this application proposes a control method, storage medium, and system for a six-degree-of-freedom motion system. The main technical solutions adopted in this application include:

[0005] In the first aspect, an embodiment of the present application provides a control method for a six-degree-of-freedom motion system, the method comprising: planning the motion trajectories of six sliders based on a target posture to obtain six smooth cam curves; for each slider, fusing the measurement data of the motor encoder and the magnetic scale encoder to obtain the displacement data of each slider; based on the displacement data of each slider, using a virtual axis as a guide axis and six smooth cam curves as following axes to perform six-axis synchronous coordinated control so that the six-degree-of-freedom motion system operates based on the target posture.

[0006] In a second aspect, the present application further provides a computer-readable storage medium having a computer program stored thereon, which implements any of the above-mentioned control methods for a six-degree-of-freedom motion system when the computer program is executed by a processor.

[0007] In a third aspect, the present application also provides a six-degree-of-freedom motion system including a memory, a processor, and a computer program stored in the memory, and when the computer program is executed by the processor, any of the above-mentioned control methods for the six-degree-of-freedom motion system is implemented.

[0008] In a fourth aspect, an embodiment of the present application also provides a control device for a six-degree-of-freedom motion system, which includes: a trajectory planning module, which is used to plan the motion trajectories of six sliders based on the target posture to obtain six smooth cam curves; a displacement acquisition module, which is used to fuse the measurement data of the motor encoder and the magnetic scale encoder for each slider to obtain the displacement data of each slider; and a coordination control module, which is used to perform six-axis synchronous coordination control based on the displacement data of each slider, using a virtual axis as the guide axis and six smooth cam curves as the following axes, so that the six-degree-of-freedom motion system can operate based on the target posture.

[0009] In a fifth aspect, the present application also provides a computer device including a memory and a processor, wherein the memory stores a computer program, and the processor implements the steps of any of the above methods when executing the computer program.

[0010] In a sixth aspect, the present invention provides a computer program product, comprising a computer program, which implements the steps of any of the above methods when executed by a processor.

[0011] In the above embodiment, precise control of the six sliders in a six-degree-of-freedom motion system is achieved by setting a precise target posture plan and fusing multi-sensor data. First, the system calculates the motion trajectories of the six sliders based on the target posture, generating six smooth cam curves. Then, the data obtained from the motor encoder and the magnetic scale encoder are fused to obtain the precise displacement data of each slider. Finally, by setting a virtual axis as the leading axis and a synchronous control strategy with the six cam curves as the following axes, precise coordinated motion of the six axes is achieved, ensuring that the six-degree-of-freedom motion system can operate stably and accurately according to the predetermined target posture. At this point, not only the control accuracy and response speed are improved, but also its stability and reliability are enhanced, allowing the six-degree-of-freedom motion system to achieve higher performance standards when performing complex tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0012] In order to more clearly illustrate the specific implementation methods of the present application or the technical solutions in the prior art, the following is a brief introduction to the drawings required for use in the specific implementation methods or the description of the prior art. Obviously, the drawings described below are some implementation methods of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0013] Figure 1a This is a flow chart of a control method for a six-degree-of-freedom motion system according to one embodiment of the present application;

[0014] Figure 1b A block diagram of a PID control method according to an embodiment of the present application;

[0015] Figure 1c This is a principle block diagram of a cross-coupling control method provided according to one embodiment of the present application;

[0016] Figure 1d This is a diagram of an operation interface for synchronous operation control provided according to one embodiment of the present application;

[0017] Figure 2a A flowchart of a motion trajectory planning method provided according to one embodiment of the present application;

[0018] Figure 2b A result diagram of cubic spline interpolation provided according to one embodiment of the present application;

[0019] Figure 3a A flowchart of a method for determining a posture error model according to one embodiment of the present application;

[0020] Figure 3b A schematic diagram of a system with gap compensation control according to one embodiment of the present application;

[0021] Figure 4a A flowchart of a security protection method provided according to one embodiment of the present application;

[0022] Figure 4b A schematic diagram of a three-dimensional security envelope space provided according to one embodiment of the present application;

[0023] Figure 5 1 is a structural block diagram of a control device for a six-degree-of-freedom motion system according to an embodiment of the present application;

[0024] Figure 6 The figure is a diagram of the internal structure of a computer device according to one embodiment of the present application. DETAILED DESCRIPTION

[0025] To make the purpose, technical solutions, and advantages of the embodiments of the present application more clear, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative work are within the scope of protection of this application.

[0026] Six-degree-of-freedom (6DOF) motion systems are capable of controlling motion in six directions (three linear and three rotational) in three-dimensional space. They have a wide range of applications in aviation, aerospace, automotive, and robotics, such as flight simulators, precise robotic arm manipulation, and space station docking simulations. They provide realistic, dynamic environments for training, testing, and research.

[0027] However, despite significant technological progress in six-degree-of-freedom motion systems, the control methods used in related technologies still have the following limitations: Due to errors in machining and assembly, as well as limitations in sensor accuracy, the control methods used in related technologies may encounter limitations in achieving precise control, especially when performing small movements or high-speed dynamic responses; due to factors such as system delays, dynamic characteristic errors of actuators, and elastic deformation of mechanical structures, the six-degree-of-freedom motion system may be unable to accurately follow the predetermined trajectory due to insufficient dynamic response when performing fast or complex movements; and because the control methods used in related technologies are relatively simple, in actual operation, users need to manually perform a large amount of input and adjustment, making the entire operation process cumbersome. In addition, related technologies also lack efficient mechanisms in data processing and trajectory management, resulting in the control system failing to meet high standards in terms of response speed and operating efficiency. This limitation is particularly evident in complex application scenarios that require rapid switching or combining of multiple motion trajectories.

[0028] Based on this, according to an embodiment of the present application, an embodiment of a control method for a six-degree-of-freedom motion system is provided. It should be noted that the steps shown in the flowchart of the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases, the steps shown or described can be executed in an order different from that shown here.

[0029] In this embodiment, a control method for a six-degree-of-freedom motion system is provided. FIG1 is a flow chart of the control method for a six-degree-of-freedom motion system according to an embodiment of the present application. As shown in FIG1 , the flow chart includes the following steps:

[0030] S110 , planning motion trajectories for the six sliders based on the target posture to obtain six smooth cam curves.

[0031] Among them, the target posture may refer to a specific spatial position and direction that needs to be simulated or achieved in a six-degree-of-freedom motion system. For example, it may include data on the translation along the X, Y, and Z axes and the rotation around these three axes, namely, the angle of attack α, the sideslip angle β, and the roll angle γ. It should be understood that the six sliders may refer to mechanical components in a six-degree-of-freedom motion system that can move along linear guides to drive the system to move in three linear directions. Specifically, each slider corresponds to the movement of one degree of freedom, and the control system needs to accurately control the movement of the six sliders to achieve precise positioning of the model in three-dimensional space.

[0032] Furthermore, by planning the target posture, the path that the slider needs to follow can be calculated to ensure that the system can smoothly and accurately move from the initial position to the target position and target posture in a preset direction. Specifically, either a forward or inverse posture solution algorithm can be used. For example, the desired target posture is first determined, including the spatial position of the (X, Y, Z) coordinates and the pitch, roll, and yaw angles. Based on the inverse kinematic solution algorithm, a six-degree-of-freedom control mathematical model is established, including the geometric relationships of the mechanical structure and the motion constraints, to calculate the expected path of each slider. This results in six smooth cam curves describing the path of each slider from its current position to the target position.

[0033] S120 , for each slider, fusing the measurement data of the motor encoder and the magnetic scale encoder to obtain displacement data of each slider.

[0034] Understandably, to ensure that the end-user model of a 6-DOF motion system consistently follows the cam curve generated by the planned trajectory, thereby avoiding collision risk, especially when approaching boundaries, the algorithm must constantly perform a correct solution to ensure that the end-user posture is completely consistent with the planned trajectory without deviation. Therefore, during the correct solution process, accurate displacement data for each slider must be obtained in real time. Therefore, each slider can be equipped with a motor encoder and a magnetic scale encoder at its end, and their measurement data can be obtained in real time.

[0035] A motor encoder can be a sensor that provides information about the rotational position of the motor shaft, while a magnetic scale encoder can be a sensor that measures changes in a magnetic field to provide high-precision position measurement data. It is important to understand that due to the presence of a reducer between the control motor and the slider, the magnetic scale encoder directly measures the slider's displacement, while the motor encoder indirectly measures the slider's displacement. Therefore, to ensure accurate and reliable slider displacement data, the data from these two sensors can be fused. Specifically, after obtaining the measurement data for each slider, a Kalman filter algorithm can be used to process the measurement data from the motor encoder and magnetic scale encoder to obtain the displacement data for each slider.

[0036] S130. Based on the displacement data of each slider, a virtual axis is used as a leading axis and six smooth cam curves are used as following axes to perform six-axis synchronous coordinated control so that the six-degree-of-freedom motion system operates based on the target posture.

[0037] It is understood that the virtual axis can refer to a leading axis used to coordinate the motion of multiple physical axes to achieve complex motion patterns. It can be a physically non-existent axis, but in the control system, it serves as a reference or benchmark, acting as the dominant axis for controlling the motion of other physical axes (following axes). Specifically, by using the virtual axis as the leading axis and the six cam curves as the following axes, based on six-axis synchronous coordinated control, the actuators, that is, the movement of the six sliders of the end effector, are simultaneously controlled so that they can move in a coordinated manner according to a predetermined trajectory and time, ensuring that the six-degree-of-freedom motion system can operate precisely according to the target posture.

[0038] Specifically, after solving the displacement of each slider, in order to ensure that each slider can accurately reach the desired position, each motor needs to move the required angle quickly and accurately. Therefore, a high-precision control strategy needs to be adopted for each motor. For example, PID closed-loop control can be used to improve the dynamic performance of the system while ensuring the stability of the system. Specifically, the PID control strategy is to calculate the output according to the deviation of the system input using the proportion (P), integral (I), and differential (D), and dynamically adjust the controlled object. Its control principle is as follows Figure 1b Please refer to Figure 1b ,in, is the expected value, It refers to the actual output value obtained by calculation. The deviation between the input and actual output value of the PID controller can be calculated by the following formula:

[0039]

[0040] in, It refers to the deviation between the input and actual output value of the PID controller.

[0041] Further, combined with Figure 1b The control law of PID can be obtained from the above formula as shown below:

[0042]

[0043] in, is the proportionality coefficient, is the integration time constant, is the differential time constant, is the integration coefficient, is the differential coefficient.

[0044] Specifically, the proportional link plays a regulatory role based on the size of the deviation. It can accelerate the dynamic response speed of the system and change the controlled variable in the direction of reducing the error. It is an indispensable link in PID control. However, it should be noted that it cannot completely eliminate the steady-state error. The integral link is controlled based on whether the deviation exists. Its function is to eliminate the steady-state error of the system. On the basis of the proportional link, it eliminates the static error of the system, makes the final output value the same as the given value, and improves the system's degree of freedom. The differential link takes action based on the speed of change of the deviation to reflect the changing trend of the deviation signal, that is, the error change rate. It can provide the system with a correction signal in advance based on the changing trend of the system error, thereby accelerating the system's response speed and shortening the adjustment time.

[0045] Furthermore, on the basis of the PID control method, in order to ensure that the desired target posture can be reached in the end, the six sliders need to cooperate with each other to ensure that the target is reached along the optimal path. That is, the motors that control each end effector also need to cooperate with each other to move in a coordinated manner, so as to ensure that the model reaches the specified position smoothly. Among them, coordinated control can refer to coordinated control to ensure the ratio between the position, speed or current between axes. For example, in the coordinated control of multiple motors, there is often no mechanical coaxial situation, so the following control structures can be adopted, such as equal control, master-slave control, cross-coupling control, virtual spindle control or deviation coupling control. Among them, taking the coordinated control using dual-motor cross-coupling control as an example, please refer to Figure 1c The principle of coordinated control using dual motor cross-coupling control can be described as follows: Figure 1c As shown in the figure, it can be seen that the characteristic of cross-coupling control is to compare and differ the speed or position signals, use the difference as the system feedback signal, and track the feedback signal to reduce the motor coordination error and improve the motor collaborative control accuracy.

[0046] It can be understood that, since the main controlled objects in the motion process of the six-degree-of-freedom motion system can be six servo motors. Therefore, in the control process, a virtual axis can be used as the leading axis, and six actual axes can be used as the following axes, and cam synchronization can be used to realize the control of the six servo motors. For example, during control, the target posture to be achieved is first planned according to the inverse solution algorithm of the touch-screen all-in-one machine, and the trajectory points are solved according to the gradient of 0.05° and 0.1mm, and the solved slider trajectory points are sent to the PLC. Then interpolation is performed through the MC_InterpolateCam instruction to form the cam curve of each axis. The cam motion operation between the master axis and the slave axis is then started through the motion control command "MC_CamIn" to realize the motion process. Among them, the synchronous control between the leading axis and the following axis is specified by the synchronous operation function. The basic operating principle of the synchronous axis process object can be referred to. Figure 1d shown.

[0047] Furthermore, the six-degree-of-freedom motion system can also be communicatively connected with remote host computer software, local host computer software, slave computer software and debugging notebook software. Specifically, the local host computer software and slave computer software can be developed first, and the control logic can be verified in the digital simulation software to ensure that the control logic is correct. The control algorithm is then imported into the actual hardware, and further adaptively modified and debugged to ensure that all functions are normal. Finally, the remote host computer software is swapped for development and debugging. During the debugging process, the functions of each software are tested separately, and the results of each time are recorded until the various functional items of the six-degree-of-freedom motion system can be fully realized.

[0048] In the above-described embodiment, precise control of the six sliders in a six-degree-of-freedom motion system is achieved by setting a precise target posture plan and fusing multi-sensor data. First, the system calculates the motion trajectories of the six sliders based on the target posture, generating six smooth cam curves. Then, the data obtained from the motor encoder and the magnetic scale encoder are fused to obtain precise displacement data for each slider. Finally, by setting a virtual axis as the leading axis and a synchronous control strategy with the six cam curves as the following axes, precise coordinated motion of the six axes is achieved, ensuring that the six-degree-of-freedom motion system can operate stably and accurately according to the predetermined target posture. This not only improves the control accuracy and response speed, but also enhances its stability and reliability, enabling the six-degree-of-freedom motion system to achieve higher performance standards when performing complex tasks.

[0049] In some embodiments, please refer to the attached Figure 2a , based on the target posture, the motion trajectory of the six sliders is planned to obtain six smooth cam curves, including:

[0050] S210 , determining current posture information of the six sliders, and performing calculations based on the current posture information of the six sliders and the target posture to obtain a plurality of displacement point information corresponding to each slider.

[0051] The current posture information refers to the position and state data of the six sliders in the six-degree-of-freedom motion system recorded at the initial startup moment. For example, this may include posture information such as the initial position, velocity, and acceleration of each slider. The multiple displacement point information corresponding to each slider may refer to a series of slider position data points calculated based on the current posture information and target posture of the six sliders.

[0052] It is understandable that since the final posture, that is, the multiple displacement point information corresponding to each slider, can be obtained through the sequential transformation of yaw, pitch, roll and translation, the posture inverse solution can be performed based on the current posture information and target posture of the six sliders. For example, based on the initial position and state data of the six sliders, according to the gradient of 0.05° and 0.1mm, combined with the target posture of their respective X, Y, Z linear displacement values ​​and the angle of attack α, sideslip angle β, and roll angle γ, the coordinate transformation process is first performed. The process of homogeneous coordinate transformation from the connected coordinate system to the fixed coordinate system can be shown as follows:

[0053]

[0054] Where, , , the rest are similar. This matrix indicates that in the fixed coordinate system, the moving coordinate system rotates around the X axis first Angle, then rotate around the Y axis Angle, then rotate around the Z axis Angle, and finally the secondary transformation matrix relative to the fixed coordinate system obtained by translating (x, y, z) along the X, Y, and Z axes.

[0055] Furthermore, the dimensions of the six-degree-of-freedom motion system can be obtained by measurement, that is, the coordinate vectors of each hinge point of the moving platform are known, as shown in the following formula:

[0056]

[0057] in, represents the coordinate vector along the X direction; represents the coordinate vector along the Y direction; Represents the coordinate vector along the Z direction.

[0058] Furthermore, given the model posture, the transformation matrix T is also known, so the coordinates of each hinge point of the moving platform in the static coordinate system after the model moves can be shown as follows:

[0059]

[0060] Likewise, represents the coordinate vector along the X direction; represents the coordinate vector along the Y direction; Represents the coordinate vector along the Z direction.

[0061] Furthermore, the coordinate vector of the slider in the static coordinate system can be shown as follows:

[0062]

[0063] in, Represents the coordinate vector of the slider in the static coordinate system; Represents the coordinate vector of the slider along the X direction in the static coordinate system; Represents the coordinate vector of the slider along the Y direction in the static coordinate system; represents the coordinate vector of the slider along the Z direction in the static coordinate system. As can be seen, the slider has only one degree of freedom, that is, there is only one unknown solution in the coordinate vector.

[0064] Furthermore, based on the rod length, which is the physical data of the end effector, the slider position can be calculated as shown in the following formula:

[0065]

[0066] Likewise, Represents the coordinate vector of the slider in the static coordinate system; Represents the coordinate vector of the slider along the X direction in the static coordinate system; Represents the coordinate vector of the slider along the Y direction in the static coordinate system; Represents the coordinate vector of the slider along the Z direction in the static coordinate system; represents the coordinate vector along the X direction; represents the coordinate vector along the Y direction; Represents the coordinate vector along the Z direction.

[0067] Finally, according to the above formula, the position of each slider in the six-degree-of-freedom motion system is solved, that is, the multiple displacement point information corresponding to each slider is obtained.

[0068] S220 , performing interpolation calculation on the multiple displacement point information corresponding to each slider to obtain six smooth cam curves.

[0069] It is understandable that since the multiple displacement point information corresponding to each slider is discrete point information, which only contains the key positions of the slider during the movement process, an interpolation method can be used to integrate the multiple discrete displacement points of each slider into a smooth and continuous movement curve. For example, a cubic spline interpolation method can be used to achieve this. The interpolation curve runs along the interpolation points and curve segments, and the result of the cubic spline curve interpolation can be shown as follows: Figure 2b As shown, a series of cubic polynomials are used to interpolate values ​​within each interval, resulting in a smooth and continuous curve. After interpolation, the range of cam follower values ​​can be greater than the range before interpolation. As can be seen from the figure, according to the cam definition, cubic spline interpolation results in higher dynamic response because the interpolation curve always passes through the specified points. Using cubic spline interpolation to interpolate the slider displacement points obtained after the solution, the corresponding axis cam curve is obtained.

[0070] Furthermore, after interpolation is completed, the gaps between the defined cam interpolation points and segments are closed. The cam operation between the master and slave axes can then be started via the Motion Control command "MC_CamIn".

[0071] In the above-described embodiment, precise target posture planning and advanced motion control algorithms enable accurate motion trajectory planning for the six sliders in a six-degree-of-freedom motion system. First, the system determines the slider's initial posture information and, using a posture inverse algorithm combined with the target posture, calculates a series of displacement points that the slider needs to traverse. Then, using cubic spline interpolation, these discrete displacement points are converted into continuous and smooth cam curves, enabling the slider to precisely move along the predetermined trajectory. This process not only improves the system's dynamic responsiveness but also ensures smoothness and accuracy of motion, enabling the six-degree-of-freedom motion system to efficiently and stably achieve the target posture, thereby enhancing the system's overall performance and reliability.

[0072] In some embodiments, when planning motion trajectories for the six sliders based on the target posture, the method further includes:

[0073] The working mode of the six-degree-of-freedom motion system is switched to the cam mode, and the database is cleaned so that the data corresponding to the current posture information and the target posture are stored in the database.

[0074] The term "database" may refer to a storage medium used to store various data and information related to system operations. For example, it may be a physical storage device, such as a hard disk or solid-state drive, or a logical storage structure, such as a data structure in computer memory. Specifically, the database of a six-degree-of-freedom motion system may include historical configuration parameters and operating setting parameters, historical operating data and system logs used for fault diagnosis and performance analysis, and data for motion trajectory planning between historical initial posture information and historical target posture data.

[0075] It is understandable that the database is designed to significantly improve the operating efficiency of the control system and the convenience of user operation. Specifically, when the system runs for the first time and calculates the six smooth cam curves for each slider, the control system automatically searches the database. If no cam curve data matching the current and target postures is found, the system will perform a database cleaning operation to delete obsolete or useless data, such as historical records, temporary files, and cached data. After that, the system will save the new current posture and target posture data to the database. In future operations, the next time the six-degree-of-freedom motion system responds to a request from the user and switches the working mode to cam mode accordingly, the control system will read this data from the database to provide historical data support for the motion control algorithm.

[0076] After several runs, the system will pre-store the most frequently used trajectories in a database. Users can quickly select a pre-stored trajectory through the touchscreen interface and quickly control the slider to the target position, which not only greatly reduces the complexity of user operations but also improves the response speed of trajectory selection and execution.

[0077] To better adapt to different application scenarios, the system also allows users to flexibly combine multiple commonly used trajectories with similar end points to form new composite trajectories. During the combination process, if there is a very short discontinuity between the ends of two trajectories, a quintic polynomial interpolation can be used to achieve a smooth transition between the new trajectories. This enables the control system's end effector to more efficiently perform a range of complex tasks. Users only need to select the combined trajectory to quickly start the actuator, without having to replan the specific path each time.

[0078] In the above implementation, the use of a database not only improves system flexibility but also reduces time delays associated with trajectory selection, further optimizing the mechanism's startup speed and operational efficiency. Overall, this significantly enhances the system's usability, responsiveness, and adaptability to diverse usage scenarios.

[0079] In some embodiments, during the six-axis synchronous coordinated control process, please refer to the attached Figure 3a , the method further comprises:

[0080] S310: Determine a posture error model.

[0081] The parameters of the pose error model include the Jacobian matrix, the displacement error of the driving slider, the vector along the direction of the driving rod, and the hinge error.

[0082] It is important to understand that after the inverse algorithm is used to calculate the positions that the six sliders need to reach based on the target posture, thereby obtaining six smooth cam curves, in actual operation, in order to monitor and feedback the actual motion state of the system in real time, it is also necessary to use the forward algorithm to calculate the current posture state based on the actual position of the slider and the sensor data. Specifically, each slider is equipped with a displacement sensor, which can collect the data of the displacement sensor in real time. Based on the collected six slider displacement values, the model posture is solved in real time and compared with the target posture, the mechanism operation envelope, the model non-collision envelope, the required motion range envelope, and the software warning envelope to ensure that the model movement is within a safe and required range.

[0083] For example, a numerical method can be used to solve the position solution of a six-degree-of-freedom motion system. The numerical method generally adopts the optimization search principle. Although it requires a lot of computing time and cannot solve all the position solutions of the parallel mechanism, that is, the end effector, and can only achieve limited accuracy, it can be applied to any form of parallel mechanism. The position solution of the six-degree-of-freedom motion system, that is, the multiple displacement points corresponding to each slider, has been calculated through attitude inverse solution, as shown in the following formula:

[0084]

[0085] in, Represents any of the six sliders; Represents the coordinate vector of the slider in the static coordinate system; Represents the coordinate vector of the slider along the X direction in the static coordinate system; Represents the coordinate vector of the slider along the Y direction in the static coordinate system; Represents the coordinate vector of the slider along the Z direction in the static coordinate system; represents the coordinate vector along the X direction; represents the coordinate vector along the Y direction; represents the coordinate vector along the Z direction; Represents the six-degree-of-freedom displacement point information of slider i.

[0086] Furthermore, the posture of the moving platform can be obtained and slider Location The constraint equations are:

[0087]

[0088] Extending to six degrees of freedom, we get six branch chain equations as follows:

[0089]

[0090] The process of solving the positive solution of the parallel mechanism position is to know the slider Location Get the dynamic platform pose under the condition of Therefore, the positive solution for the position of the parallel slider is converted into the known Solve nonlinear equations about Then the above equations can be simplified as:

[0091]

[0092]

[0093]

[0094] To simplify the calculation, the simplified Newton iteration method can be used to solve the above nonlinear equations. The iterative format is:

[0095]

[0096] Further, it is expanded as follows:

[0097]

[0098] Finally, the posture solution of the moving platform is obtained, that is, The value of .

[0099] It's understandable that due to backlash errors in the mechanical device, to ensure final pose accuracy, after calculating the current pose state using the forward solution algorithm, it's necessary to compensate for the backlash errors at the software level. Specifically, the pose error model can be determined based on parameters such as the Jacobian matrix, the displacement error of the drive slider, the vector along the drive rod, and the hinge error.

[0100] For example, the mathematical expression of the pose error model can be shown as follows:

[0101]

[0102] in, is the Jacobian matrix, is the displacement error of the driving slider, is the vector along the driving rod direction, is the hinge error.

[0103] S320: Perform inverse calculation based on the measured angle error under the posture error model to derive the clearance error of each actuator.

[0104] It is understandable that due to the system structure, that is, the hinge error Direct compensation is not possible. As can be seen from the above formula, the final model pose error caused by the system structural clearance can be compensated by adjusting the slider displacement. However, since the control system's pose error model contains six drive slider displacement parameters, it is relatively easy to perform a kinematic inverse solution, and there is only one correct solution. Therefore, the final model pose error can be inversely solved to obtain the clearance errors of the six actuators by referring to the kinematic inverse solution algorithm. Since the clearance errors of the six actuators are steady-state errors, during the actual debugging and calibration process, the measured angular errors under various model poses can be used to perform inverse calculations and analysis to accurately determine the clearance errors of the six actuators, ultimately achieving clearance compensation control for each actuator.

[0105] S330 , performing displacement compensation on each slider based on the gap error.

[0106] Further, please refer to Figure 3b , Figure 3b This is a schematic diagram of a system with backlash compensation control. Specifically, each slider is compensated for its displacement based on backlash error. First, a displacement sensor measures the actual displacement of each slider in real time. Then, based on the kinematic model of the six-degree-of-freedom motion system, an inverse calculation is performed to determine the theoretical displacement that each slider should achieve without the influence of errors. Furthermore, by comparing the actual and theoretical displacements, a posture error model is used to determine the displacement error of each slider. Based on the posture error model, the required compensation displacement for each slider is calculated to correct for deviations caused by mechanical backlash or other error factors. The calculated compensation amount is then converted into a control signal and sent to the servo controller. After receiving the compensation control signal, the servo controller adjusts the input to the servo motor to achieve precise control of the slider's displacement. Finally, the servo motor moves according to the controller's instructions to compensate for the slider's displacement. Optionally, the compensated displacement is measured again by a displacement sensor, forming a closed-loop control feedback loop to ensure accurate compensation.

[0107] In the above implementation, by establishing a posture error model and performing inverse calculations based on the measured angular errors, the actuator backlash error is derived based on the determined posture of the moving platform. Displacement compensation is then performed to correct for posture deviations caused by structural backlash, ensuring the accuracy of the final posture. This process enables precise motion control and compensates for the effects of mechanical backlash and other error factors, thereby improving overall system performance and reliability.

[0108] In some implementations, the posture error model is solved using a heuristic search algorithm, and the six sliders are compensated for displacement with the minimum comprehensive posture error as the objective function.

[0109] For details, please refer to Figure 3b The pose error model shown in the figure can be solved using heuristic search algorithms such as genetic algorithms and particle swarm optimization. Minimizing the overall pose error is the objective function, compensating for the gap error of each slider. The corrected desired displacement trajectory is then used as the tracking target for trajectory tracking control, which drives the slider to track the pose deviation caused by structural gaps and ensure the accuracy of the model's final pose.

[0110] In the above implementation, a heuristic search algorithm is employed to calculate the optimal displacement compensation for each slider, effectively correcting for posture deviations caused by mechanical backlash or other error factors. Furthermore, through precise displacement compensation, the system ensures the accuracy of the model's final posture, meeting high-precision application requirements. Furthermore, the flexibility of the heuristic search algorithm allows the system to adapt to a variety of motion trajectories and posture changes, enhancing its versatility and adaptability.

[0111] In some embodiments, the pose error model is expressed according to the following mathematical formula:

[0112]

[0113] in, is the Jacobian matrix, is the displacement error of the driving slider, is the vector along the driving rod direction, is the hinge error, is the posture error.

[0114] Specifically, the Jacobian matrix is ​​a matrix that describes the kinematic relationship of a six-degree-of-freedom mechanism. It can convert the displacement of the slider into the change in the position of the moving platform and can be used to determine the relationship between the displacement of the slider and the change in the position of the moving platform. It can be the difference between the actual measured displacement of the slider and the theoretical calculated displacement, which can be caused by factors such as sensor accuracy limitations, machining errors, assembly errors, etc. The vector along the direction of the drive rod It can be caused by the geometric parameters of the mechanism, such as the relative position and direction between the slider and the drive rod. It refers to the error caused by the hinges in the mechanism (such as bearings, connectors, etc.), which may affect the precise position of the slider. It is the error value finally calculated by the model, which can represent the deviation between the actual posture of the system and the target posture. The control system can be adjusted to ensure that the six-degree-of-freedom mechanism can accurately achieve the predetermined target pose.

[0115] In the above embodiment, a pose error model is derived by designing error factors that account for multiple contributing factors. This model then calculates the displacement compensation required for each slider to achieve the target pose. This compensation is then used to adjust the servo controller output to drive the slider along the compensated trajectory, thereby reducing pose errors and ultimately improving system accuracy and performance.

[0116] In some embodiments, the measurement data of the motor encoder and the magnetic scale encoder are fused and processed, including:

[0117] Based on the noise characteristics of the motor encoder and the magnetic scale encoder, the data weighted fusion method is dynamically adjusted to perform weighted fusion on the measurement data of the motor encoder and the magnetic scale encoder.

[0118] Specifically, based on the noise characteristics of the motor encoder and the magnetic scale encoder, process noise and measurement noise models can be established for each encoder to reasonably reflect their accuracy and noise levels. For example, since the motor encoder may be affected by mechanical vibration and thus has relatively high noise, a less accurate noise model can be used for it. On the other hand, the magnetic scale encoder generally has higher accuracy and lower noise, so its noise model is also relatively accurate.

[0119] Furthermore, a Kalman filter is used to fuse the state information, including position and velocity, collected by the two sensors, to obtain a more accurate estimate of the slider's displacement. It is important to note that when collecting the state vector, accurate position information must be obtained at every time step. After obtaining this relatively accurate state information, the Kalman filter algorithm dynamically adjusts the data fusion weights based on the noise characteristics of the two sensors to obtain the displacement data for each slider.

[0120] In the above implementation, the magnetic scale encoder is known for its high-precision static measurement, while the motor encoder excels at tracking dynamic motion. By combining the advantages of both, not only is dynamic motion tracking enhanced, but the overall accuracy of displacement data is also improved. This approach enables six-degree-of-freedom motion control systems to perform complex motion tasks with greater precision and reliability.

[0121] In some embodiments, during the six-axis synchronous coordinated control process, please refer to the attached Figure 4a , the method further comprises:

[0122] S410: Determine a three-dimensional safety envelope space.

[0123] The three-dimensional safety envelope space can refer to a virtual area defined in three-dimensional space that represents the range within which a six-degree-of-freedom motion system can safely operate during operation. Specifically, the space can be established after taking into account all possible physical limitations and obstacles to ensure that the motion system will not collide or exceed physical limitations when performing tasks. Simulation technology can be used to simulate the unreachable area in three-dimensional space to construct the precise three-dimensional safety envelope space. For example, please refer to Figure 4b The three-dimensional safety envelope can include the required motion range envelope, the software warning envelope, the mechanism collision-free envelope, and the mechanism operating range envelope. Specifically, the grating ruler data can be collected in real time to monitor the current slider displacement. The slider displacement is then uploaded to the touch screen all-in-one computer. The touch screen all-in-one computer then uses a forward solution algorithm to calculate the model's posture in real time, and the resulting value is constantly compared with the set safety envelope.

[0124] S420: Determine the current posture based on the displacement data of each slider, and control the six-degree-of-freedom motion system to stop moving if each end of the six-degree-of-freedom motion system is not within a safe range based on the current posture and the three-dimensional safety envelope space.

[0125] Specifically, after determining the relevant data of the three-dimensional safety envelope, the system can use a forward solution algorithm to calculate and obtain the current position in real time based on the displacement data of each slider during runtime, and compare and analyze it with the pre-defined safety envelope. If the current posture approaches or exceeds the user-defined safety envelope threshold, the system will immediately react. Specifically, when the posture exceeds the set safety threshold, the system will automatically trigger the emergency stop mechanism, quickly stopping the movement of the mechanism to avoid collisions or other safety accidents.

[0126] It is understandable that since the three-dimensional safety envelope space can include the required motion range envelope, the software warning envelope, the mechanism non-collision envelope and the mechanism operation range envelope, there are also many ways to determine whether the six-degree-of-freedom motion system is in safe operation.

[0127] For example, when the model posture is within the required motion range envelope, the mechanism can operate normally and reach the specified target posture. When the calculated model posture exceeds the software warning range safety envelope, the corresponding alarm information will be triggered, the control system will slow down and stop, and the corresponding test personnel will check it. After the safety hazards are eliminated, the control will be reset and the work will continue. Similarly, when the calculated model posture exceeds the mechanism without touching the envelope, the control system will immediately brake and stop, and the corresponding test personnel will check it. After the safety hazards are eliminated, the control will be reset. In addition, in terms of mechanical mechanisms, safety limit switches will also be set to ensure that it does not exceed the mechanism's operating range envelope.

[0128] In addition, users can customize safety thresholds according to different application scenarios, that is, customize the three-dimensional safety envelope space, so that the system can provide optimal safety protection in various operating modes.

[0129] In the above-mentioned embodiment, in the six-degree-of-freedom motion control system, the system can ensure safety and accuracy when performing complex tasks through a precisely defined three-dimensional safety envelope space. This virtual area takes into account all physical limitations and obstacles, and simulates inaccessible areas through simulation technology, thereby constructing a precise safe operating range. When the system is running, the displacement data collected in real time is used to determine the current posture, and is compared with the three-dimensional safety envelope space through a forward solution algorithm to ensure that the end effector is always within a safe range. This real-time monitoring and automatic shutdown mechanism reduces the potential threats brought by human errors or environmental changes, while also improving the convenience and response speed of operation, making the six-degree-of-freedom motion system more reliable and efficient.

[0130] It should be understood that, although the various steps in the above flowchart are shown in sequence as indicated by the arrows, these steps are not necessarily performed in the order indicated by the arrows. Unless otherwise specified herein, there is no strict order restriction on the execution of these steps, and these steps can be performed in other orders. Moreover, at least a portion of the steps in the above flowchart may include multiple steps or multiple stages, and these steps or stages are not necessarily performed at the same time, but can be performed at different times. The execution order of these steps or stages is not necessarily to be performed in sequence, but can be performed in turn or alternately with other steps or at least a portion of steps or stages in other steps.

[0131] The embodiment of this specification also provides a control device 500 for a six-degree-of-freedom motion system, such as Figure 5 As shown, it includes: a trajectory planning module 510, a displacement acquisition module 520 and a coordination control module 530, wherein:

[0132] The trajectory planning module 510 is used to plan the motion trajectories of the six sliders based on the target posture to obtain six smooth cam curves.

[0133] The displacement acquisition module 520 is used to fuse the measurement data of the motor encoder and the magnetic scale encoder for each slider to obtain the displacement data of each slider.

[0134] The coordination control module 530 is used to perform six-axis synchronous coordination control based on the displacement data of each slider, using a virtual axis as a leading axis and six smooth cam curves as following axes, so that the six-degree-of-freedom motion system can operate based on the target posture.

[0135] In some embodiments, the trajectory planning module 510 is also used to determine the current posture information of the six sliders, and perform calculations based on the current posture information and target posture of the six sliders to obtain multiple displacement point information corresponding to each slider; interpolation calculations are performed on the multiple displacement point information corresponding to each slider to obtain six smooth cam curves.

[0136] In some embodiments, the coordination control module 530 is further configured to switch the working mode of the six-degree-of-freedom motion system to the cam mode, and clean the database so that data corresponding to the current posture information and the target posture are stored in the database.

[0137] In some embodiments, the coordination control module 530 is also used to determine a posture error model, wherein the parameters of the posture error model include the Jacobian matrix, the displacement error of the driving slider, the vector along the direction of the driving rod, and the hinge error; an inverse calculation is performed based on the measured angle error under the posture error model to derive the gap error of each actuator; and displacement compensation is performed on each slider based on the gap error.

[0138] In some embodiments, the coordination control module 530 is further configured to employ a heuristic search algorithm to solve and obtain a posture error model; and to perform displacement compensation on the six sliders with minimizing the comprehensive posture error as the objective function.

[0139] In some embodiments, the coordination control module 530 is further configured to express the pose error model according to the following mathematical formula:

[0140] in, is the Jacobian matrix, is the displacement error of the driving slider, is the vector along the driving rod direction, is the hinge error, is the posture error.

[0141] In some embodiments, the displacement acquisition module 520 is further configured to dynamically adjust the data weighted fusion method based on the noise characteristics of the motor encoder and the magnetic scale encoder to perform weighted fusion on the measurement data of the motor encoder and the magnetic scale encoder.

[0142] In some embodiments, the coordination control module 530 is further used to determine a three-dimensional safety envelope space; determine a current posture based on the displacement data of each slider, and control the six-degree-of-freedom motion system to stop moving when it is judged based on the current posture and the three-dimensional safety envelope space that each end of the six-degree-of-freedom motion system is not within a safe range.

[0143] The specific definition of a control device for a six-degree-of-freedom motion system can be found in the definition of a control method for a six-degree-of-freedom motion system described above and will not be repeated here. The various modules in the above-mentioned control device for the six-degree-of-freedom motion system can be implemented in whole or in part through software, hardware, or a combination thereof. The above-mentioned modules can be embedded in or independent of the processor in the computer device in hardware form, or can be stored in the memory of the computer device in software form, so that the processor can call and execute the corresponding operations of the above-mentioned modules.

[0144] In this embodiment, a control device for a six-degree-of-freedom motion system is presented in the form of a functional unit, where the unit refers to an ASIC (Application Specific Integrated Circuit) circuit, a processor and memory that executes one or more software or fixed programs, and / or other devices that can provide the above functions.

[0145] The embodiment of the present application further provides a computer device, which may be a terminal, and its internal structure diagram may be as shown in FIG. Figure 6 As shown. The computer device includes a processor, memory, a communication interface, a display screen, and an input device connected via a system bus. The processor of the computer device is used to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and internal memory. The non-volatile storage medium stores an operating system and a computer program. The internal memory provides an environment for the operation of the operating system and computer program in the non-volatile storage medium.

[0146] Those skilled in the art will understand that Figure 6 The structure shown in the figure is only a block diagram of a part of the structure related to the solution of the present application, and does not constitute a limitation on the computer device to which the solution of the present application is applied. The specific computer device may include more or fewer components than shown in the figure, or combine certain components, or have a different component arrangement.

[0147] The present application also provides a computer-readable storage medium. The above-mentioned method according to the embodiment of the present application can be implemented in hardware, firmware, or as computer code that can be recorded on a storage medium, or downloaded via a network and originally stored in a remote storage medium or a non-transitory machine-readable storage medium and then stored in a local storage medium, so that the method described herein can be stored in such software processing on a storage medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware. The storage medium can be a magnetic disk, an optical disk, a read-only memory, a random access memory, a flash memory, a hard disk, or a solid-state drive, etc.; further, the storage medium can also include a combination of the above-mentioned types of memory.

[0148] An embodiment of the present application provides a computer program product, which includes computer instructions stored in a computer-readable storage medium. A processor of a computer device reads the computer instructions from the computer-readable storage medium and executes the computer instructions, causing the computer device to perform a method according to any embodiment of the present application.

[0149] The control method, storage medium, and system for a six-degree-of-freedom motion system described in the above embodiments can be implemented by a computer chip or entity, or by a product with a certain function. A typical implementation device is a computer. For ease of description, the above device is described separately by function in various units. Of course, when implementing this application, the functions of each unit can be implemented in the same or multiple software and / or hardware.

[0150] This application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the application. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, 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, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate a device for implementing the functions specified in one or more processes in the flowchart and / or one or more blocks in the block diagram.

[0151] Throughout this specification, reference to terms such as "one embodiment," "some embodiments," "examples," "specific examples," or "some examples" means that a specific feature, structure, material, or characteristic described in conjunction with that embodiment or example is included in at least one embodiment or example of the present application. In this specification, schematic representations of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0152] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include at least one of such features. Throughout the description of this application, "plurality" means at least two, for example, two, three, etc., unless otherwise specifically defined.

[0153] It should also be noted that the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, commodity, or apparatus that includes a series of elements includes not only those elements but also other elements not explicitly listed, or includes elements inherent to such process, method, commodity, or apparatus. In the absence of further limitations, an element defined by the phrase "comprises a ..." does not exclude the presence of other identical elements in the process, method, commodity, or apparatus that includes the element.

[0154] Each embodiment in this specification is described in a progressive manner. Similar parts between the embodiments can be referred to in detail. Each embodiment focuses on the differences from other embodiments. Since it is basically similar to the method embodiment, the description is relatively simple. For relevant parts, refer to the partial description of the method embodiment.

[0155] The foregoing is merely an embodiment of the present application and is not intended to limit the present application. For those skilled in the art, the present application may have various changes and variations. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present application should all be included within the scope of the claims of the present application.

[0156] Although the embodiments of the present application have been described with reference to the accompanying drawings, those skilled in the art may make various modifications and variations without departing from the spirit and scope of the present application, and such modifications and variations shall fall within the scope defined by the appended claims.

Claims

1. A control method for a six-degree-of-freedom motion system, characterized in that: include: The motion trajectories of the six sliders are planned based on the target posture to obtain six smooth cam curves; For each slider, the measurement data of the motor encoder and the magnetic scale encoder are fused and processed to obtain the displacement data of each slider; Based on the displacement data of each slider, a virtual axis is used as a leading axis, and the six smooth cam curves are used as following axes to perform six-axis synchronous coordinated control so that the six-degree-of-freedom motion system operates based on the target posture; The six-axis synchronous coordinated control process includes: determining a posture error model, wherein the parameters of the posture error model include a Jacobian matrix, a displacement error of a driving slider, a vector along the driving rod direction, and a hinge error; performing an inverse calculation based on the measured angle error under the posture error model to derive the clearance error of each actuator; and performing displacement compensation on each slider based on the clearance error. The pose error model is expressed according to the following mathematical formula: in, is the Jacobian matrix, is the displacement error of the driving slider, is the vector along the driving rod direction, is the hinge error, is the posture error.

2. The control method of the six-degree-of-freedom motion system according to claim 1, characterized in that: Based on the target posture, the motion trajectory of the six sliders is planned to obtain six smooth cam curves, including: Determine current posture information of the six sliders, and perform calculation based on the current posture information of the six sliders and the target posture to obtain multiple displacement point information corresponding to each slider; Interpolation calculation is performed on the multiple displacement point information corresponding to each slider to obtain the six smooth cam curves.

3. The control method of the six-degree-of-freedom motion system according to claim 2, characterized in that: When planning motion trajectories for the six sliders based on the target posture, the method further includes: The working mode of the six-degree-of-freedom motion system is switched to a cam mode, and the database is cleaned so that the current posture information and the data corresponding to the target posture are stored in the database.

4. The control method of the six-degree-of-freedom motion system according to claim 1, characterized in that: The posture error model is solved by a heuristic search algorithm, and the displacement compensation of the six sliders is performed with the minimum comprehensive posture error as the objective function.

5. The control method of the six-degree-of-freedom motion system according to claim 1, characterized in that: The measurement data of the motor encoder and the magnetic scale encoder are integrated and processed, including: Based on the noise characteristics of the motor encoder and the magnetic scale encoder, the data weighted fusion method is dynamically adjusted to perform weighted fusion on the measurement data of the motor encoder and the magnetic scale encoder.

6. The control method of the six-degree-of-freedom motion system according to claim 1, characterized in that: During the six-axis synchronous coordinated control process, the method further includes: Determine the three-dimensional safety envelope space; The current posture is determined based on the displacement data of each slider, and when it is determined based on the current posture and the three-dimensional safety envelope space that each end of the six-degree-of-freedom motion system is not within a safety range, the six-degree-of-freedom motion system is controlled to stop moving.

7. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the control method of the six-degree-of-freedom motion system according to any one of claims 1 to 6 is implemented.

8. A six-degree-of-freedom motion system comprising a memory, a processor, and a computer program stored in the memory, wherein: When the computer program is executed by the processor, the control method of the six-degree-of-freedom motion system according to any one of claims 1 to 6 is implemented.

Images