A docking method and system based on multi-axis cooperative motion and a storage medium

Abstract

The application discloses a docking method and system based on multi-axis cooperative motion and a storage medium. The docking method comprises the following steps: step one, establishing a multi-axis cooperative motion model; step two, dynamically planning a cooperative motion track of each motion axis; step three, controlling the motion of each motion axis, collecting an actual pose of an end effector in real time, and calculating a first pose deviation between the actual pose and a docking target pose; step four, generating a feedforward compensation amount based on the multi-axis cooperative motion model; processing the first pose deviation to obtain a feedback correction amount; fusing the feedforward compensation amount and the feedback correction amount to generate a track correction instruction, and correcting the cooperative motion track of each motion axis; and step five, repeating steps three to four until a second pose deviation between the end effector and the docking target is smaller than a preset docking threshold, and then completing the docking. The docking method can solve the problem of insufficient end docking positioning precision of the existing multi-axis cooperative motion control method.

Description

Technical Field

[0001] This application relates to the field of multi-axis drive mechanisms, and in particular to a docking method, system and storage medium based on multi-axis cooperative motion. Background Technology

[0002] The multi-axis drive mechanism is a collaborative linkage of multiple independent power units and transmission components, which can complete complex spatial pose adjustment with multiple degrees of freedom.

[0003] In fields such as aerospace, intelligent equipment, and large-scale precision manufacturing, it is necessary for the motion axes of a multi-axis drive mechanism to maintain a high degree of synchronization and coordination in order to complete the high-precision fitting and docking of large-sized components. Currently, the industry generally relies on multi-axis collaborative motion control methods to control the end effector of the mechanism to complete the docking process.

[0004] However, existing multi-axis cooperative motion control methods are subject to interference from multiple factors such as mechanical assembly errors, external working condition disturbances, and data transmission delays when driving multi-axis drive mechanisms to perform docking operations. This results in insufficient docking positioning accuracy of the end effector, reducing the pass rate of workpiece docking and assembly. Summary of the Invention

[0005] The main objective of this application is to provide a docking method, system, and storage medium based on multi-axis cooperative motion, aiming to solve the technical problem that the end effector's docking positioning accuracy is insufficient when driving multi-axis drive mechanisms to perform docking operations, thus reducing the pass rate of workpiece docking and assembly.

[0006] In a first aspect, this application provides a docking method based on multi-axis cooperative motion, applied to a multi-axis drive mechanism, the method comprising: Step 1: Based on the geometric configuration and kinematic constraints of each motion axis of the multi-axis drive mechanism, establish a multi-axis cooperative motion model that characterizes the coupling relationship between each motion axis and realizes forward and inverse kinematic mapping; Step 2: Based on the target pose and the current pose of the end effector of the multi-axis drive mechanism, dynamically plan the cooperative motion trajectory of each motion axis in conjunction with the multi-axis cooperative motion model; Step 3: Control each motion axis to move along the coordinated motion trajectory, and collect the actual pose of the end effector in real time, and calculate the first pose deviation between the actual pose and the pose of the docking target; Step 4: Based on the multi-axis cooperative motion model, generate feedforward compensation amount by combining the predicted interference factors; process the first pose deviation to obtain feedback correction amount; fuse the feedforward compensation amount and feedback correction amount to generate trajectory correction command to correct the cooperative motion trajectory of each motion axis. Step 5: Repeat steps 3 and 4 until the second pose deviation between the end effector and the docking target is less than the preset docking threshold, then the docking is complete.

[0007] Optionally, the method for processing the first pose deviation to obtain the feedback correction amount in step four includes: The first pose deviation is filtered. Based on the multi-axis cooperative motion model, the filtered first pose deviation is converted into the axis deviation amount corresponding to each motion axis; The axis deviation of each motion axis is input into the PID controller or adaptive controller respectively, and the feedback correction amount of each motion axis is calculated.

[0008] Optionally, the trajectory correction command is calculated using fixed-weight fusion or variable-weight segmented fusion feedforward compensation and feedback correction.

[0009] Optionally, the cooperative motion trajectory is the cooperative motion trajectory with the least time or the cooperative motion trajectory with the least energy consumption.

[0010] Optionally, both the first pose deviation and the second pose deviation are six-dimensional pose deviations that include three-dimensional translation components and three-dimensional rotation components.

[0011] Optionally, the predicted disturbance factors include static deviations caused by gravity, nonlinear errors caused by friction, and dynamic deviations caused by load changes.

[0012] Optionally, kinematic constraints include velocity constraints, acceleration constraints, and jerk constraints.

[0013] Secondly, embodiments of this application provide a docking system based on multi-axis cooperative motion, applied to a multi-axis drive mechanism, the system comprising: The pose measurement component is used to acquire the pose information of the end effector and the docking target in real time. Servo drive unit, used to drive the motion of each motion axis of a multi-axis drive mechanism; The controller is electrically connected to the pose measurement component and the servo drive unit, respectively, and is used to execute a docking method based on multi-axis cooperative motion according to the first aspect of the embodiments of this application. Optionally, the pose measurement component includes one or more combinations of a laser tracker, a vision sensor, a displacement sensor, and an inclinometer.

[0014] Thirdly, embodiments of this application provide a storage medium including a stored computer program, wherein, when the computer program is running, it controls the device where the storage medium is located to execute a docking method based on multi-axis cooperative motion as described in the first aspect of embodiments of this application.

[0015] Compared with the prior art, this application has the following beneficial effects: 1. By using a multi-axis motion model and combining predicted interference factors (such as mechanical assembly errors, working condition disturbances, etc.) to generate feedforward compensation, and then generating feedback correction through the first position deviation, the two are then merged to correct the coordinated motion trajectory of each motion axis, thereby suppressing multi-source errors, solving the problem of insufficient end-to-end docking positioning accuracy in existing multi-axis coordinated motion control methods, and improving the qualification rate of workpiece docking assembly.

[0016] 2. By setting step five, the method of the present invention integrates pose acquisition, deviation calculation, trajectory correction, and motion execution into a continuous closed-loop process. During the motion, pose detection and online compensation are performed simultaneously, which improves docking accuracy and makes the movement of the multi-axis drive mechanism smoother, shortens docking time, and improves work efficiency.

[0017] 3. The docking method of the present invention can drive the corresponding multi-axis drive mechanism to perform docking operations according to the docking task, which has the advantage of strong adaptability; by performing real-time dynamic correction of deviation, there is no need to perform high-precision assembly and debugging in the early stage of docking operations, thus improving the fault tolerance and flexibility of the early assembly operations. Attached Figure Description

[0018] Figure 1 A flowchart illustrating a docking method based on multi-axis cooperative motion proposed in an embodiment of this application; Figure 2 This is a schematic diagram of the explosion-proof deflashing system proposed in an embodiment of this application; Figure 3 This is a schematic diagram of the burr removal mechanism proposed in the embodiments of this application.

[0019] The attached diagram is labeled as follows: 1. Explosion-proof robotic arm; 2. XYZ three-axis motion module; 3. Workpiece fixing seat; 4. Flash removal mechanism; 41. Upper flash removal mechanism; 42. Lower flash removal mechanism. Detailed Implementation The technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only a part of the embodiments of this application, and not all of the embodiments. Based on the embodiments of this application, all other embodiments obtained by those of ordinary skill in the art without creative effort are within the scope of protection of this application.

[0020] It should be noted that all directional indicators (such as up, down, left, right, front, back, etc.) in the embodiments of this application are only used to explain the relative positional relationship and movement of each component in a certain specific posture (as shown in the figure). If the specific posture changes, the directional indicator will also change accordingly.

[0021] In this application, unless otherwise expressly specified and limited, the terms "connection," "fixed," etc., should be interpreted broadly. For example, "fixed" can mean a fixed connection, a detachable connection, or an integral part; it can mean a mechanical connection or an electrical connection; it can mean a direct connection or an indirect connection through an intermediate medium; it can mean the internal communication of two components or the interaction between two components, unless otherwise expressly limited. Those skilled in the art can understand the specific meaning of the above terms in this application according to the specific circumstances.

[0022] Furthermore, if the embodiments of this application involve descriptions such as "first" or "second," these descriptions are for descriptive purposes only and should not be construed as indicating or implying their relative importance or implicitly specifying the number of technical features indicated. Therefore, a feature defined with "first" or "second" may explicitly or implicitly include at least one of those features. Additionally, the meaning of "and / or" throughout the text includes three parallel solutions; for example, "A and / or B" includes solution A, solution B, or a solution where both A and B are satisfied simultaneously. Furthermore, the technical solutions of the various embodiments can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or impossible to implement, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed in this application.

[0023] Reference Figure 1 This application provides a docking method based on multi-axis cooperative motion, applied to a multi-axis drive mechanism, the method including: Step 1: Based on the geometric configuration and kinematic constraints of each motion axis of the multi-axis drive mechanism, establish a multi-axis cooperative motion model that characterizes the coupling relationship between each motion axis and realizes forward and inverse kinematic mapping; Step 2: Based on the target pose and the current pose of the end effector of the multi-axis drive mechanism, dynamically plan the cooperative motion trajectory of each motion axis in conjunction with the multi-axis cooperative motion model; Step 3: Control each motion axis to move along the coordinated motion trajectory, and collect the actual pose of the end effector in real time, and calculate the first pose deviation between the actual pose and the pose of the docking target; Step 4: Based on the multi-axis cooperative motion model, generate feedforward compensation amount by combining the predicted interference factors; process the first pose deviation to obtain feedback correction amount; fuse the feedforward compensation amount and feedback correction amount to generate trajectory correction command to correct the cooperative motion trajectory of each motion axis. Step 5: Repeat steps 3 and 4 until the second pose deviation between the end effector and the docking target is less than the preset docking threshold, then the docking is complete.

[0024] Specifically, in step one, the coordinate system and geometric parameters of each joint of the multi-axis drive mechanism are first calibrated to determine the motion constraints and coupling relationships of the axis system. Then, the forward and inverse kinematic equations are derived using the DH parameter method. Next, multiple solutions are selected by combining the travel and interference constraints of each axis. Finally, the coupling constraints and bidirectional mapping relationships are integrated to construct a multi-axis cooperative motion model, realizing the mutual conversion between the motion quantities of each axis and the spatial pose of the end effector. The multiple motion axes in the model include translation axes, rotation axes, and attitude adjustment axes.

[0025] In step two, the target pose and the current pose of the end effector are used as inputs. The inverse kinematics of each axis are obtained by completing the inverse kinematics calculation through the multi-axis cooperative motion model established in step one. Then, combined with motion constraints, the synchronous cooperative motion trajectory of each axis is generated in segments using an interpolation algorithm to ensure that the end effector smoothly approaches the target under multi-axis linkage. In this embodiment, the target pose is based on the pose of the center point of the workpiece docking end face.

[0026] In step three, the motion axes are controlled to move in tandem according to the synchronous and coordinated motion trajectory of each axis. The actual six-dimensional pose of the end effector is collected in real time through the pose measurement component. The real-time pose of the end effector is compared with the pose of the docking target to solve the first pose deviation.

[0027] In step four, the feedforward compensation amount is calculated by using a multi-axis cooperative motion model and combining predicted disturbance factors such as vibration and load. The feedback correction amount is obtained by filtering and closed-loop calculation of the first attitude deviation. The two types of correction amounts are fused to generate trajectory correction instructions and synchronously update the cooperative motion trajectory of each motion axis. Step four synchronously corrects the motion trajectory by fusing the feedforward compensation amount and the feedback correction amount. This can not only offset the trajectory deviation caused by known disturbances in advance and improve the dynamic response speed, but also continuously eliminate the real-time attitude steady-state error, reduce the docking attitude fluctuation of the end effector, and improve the docking control accuracy.

[0028] Step 5 corrects the pose deviation through iterative correction and uses a preset threshold as the criterion for docking completion. This can continuously reduce the pose difference between the end effector and the docking target, ensuring stable docking accuracy and avoiding residual errors from single trajectory movements that could affect docking accuracy.

[0029] Furthermore, the method for processing the first pose deviation to obtain the feedback correction amount in step four includes: The first pose deviation is filtered. Based on the multi-axis cooperative motion model, the filtered first pose deviation is converted into the axis deviation amount corresponding to each motion axis; The axis deviation of each motion axis is input into the PID controller or adaptive controller respectively, and the feedback correction amount of each motion axis is calculated.

[0030] Specifically, the first acquired pose deviation is first filtered to remove detection noise interference. Then, the multi-axis cooperative motion model is called to convert the filtered six-dimensional spatial pose deviation into the axis displacement and axis rotation deviation of each motion axis. Subsequently, the axis deviation of each motion axis is sent to the PID controller or adaptive controller for calculation, and finally the feedback correction amount corresponding to each motion axis is obtained.

[0031] By filtering the first pose deviation, noise during pose information acquisition is eliminated, avoiding control oscillations caused by noise interference. Then, the axis deviation is calculated and the correction is calculated using a PID controller or adaptive controller, which can improve docking control accuracy and enhance the system's anti-interference capability.

[0032] Furthermore, the trajectory correction command is calculated using fixed-weight fusion or variable-weight segmented fusion feedforward compensation and feedback correction.

[0033] Specifically, when using fixed-weight fusion, constant feedforward weight coefficients and feedback weight coefficients are set, and the feedforward compensation amount and feedback correction amount are weighted and added together to obtain the trajectory correction command. When using variable-weight segmented fusion, the approach segments are divided according to the pose distance between the end effector and the docking target, and different weight ratios are configured for each segment. In the long-distance approach stage, the feedforward weight is increased to quickly track the trajectory, and in the near docking stage, the feedback weight is increased to suppress residual deviations. The two types of compensation amounts after segmented weighting are superimposed to generate the final trajectory correction command.

[0034] By fusing feedforward compensation and feedback correction with fixed weights, the calculation logic can be simplified, making it suitable for docking operations with lower accuracy requirements. By fusing feedforward compensation and feedback correction in segments with variable weights, the proportion of feedforward and feedback can be dynamically adjusted according to the distance between the end point and the docking target. At long distances, feedforward is used to achieve rapid approach, while the feedback weight is increased to accurately eliminate residual errors when docking is close, balancing motion response speed and docking positioning accuracy. Therefore, both fusion methods can be adapted to different operating conditions.

[0035] Furthermore, the cooperative motion trajectory is the cooperative motion trajectory with the least time or the cooperative motion trajectory with the lowest energy consumption.

[0036] Specifically, with optimal time or optimal energy consumption as the objective function, and combined with constraints such as maximum speed, acceleration, torque, and stroke limit of each axis, an optimization solution equation is constructed based on a multi-axis cooperative motion model. The optimization calculation is completed by traversing the motion interpolation nodes of each axis, and the sequence of synchronous motion interpolation points of each axis that meets the docking posture transformation requirements and has the shortest total motion time or the lowest overall drive energy consumption is obtained. These are then spliced ​​to generate the corresponding optimal cooperative motion trajectory. Docking with the optimal time trajectory can fully utilize the motion performance of each axis, shorten the end docking operation time, and improve the docking operation efficiency. Docking with the optimal energy consumption trajectory can reasonably limit the peak output of each axis, reduce mechanical impact and power loss, and reduce equipment wear and operating costs. The two optimized trajectories can be adapted to high-speed operation or low-loss long-term operation scenarios as needed.

[0037] Furthermore, both the first and second pose deviations are six-dimensional pose deviations that include three-dimensional translational and three-dimensional rotational components.

[0038] Specifically, the three-dimensional translation components are , and The three-dimensional rotational components are , and .

[0039] Furthermore, the predicted interference factors include static deviations caused by gravity, nonlinear errors caused by friction, and dynamic deviations caused by load changes.

[0040] Specifically, the gravity offset, nonlinear curves of friction force of each motion axis throughout its full stroke, and dynamic disturbance data corresponding to different loads of the multi-axis drive mechanism are collected in advance through calibration experiments. The above three types of disturbance laws are pre-entered into the multi-axis cooperative motion model. During the motion, the current attitude of the mechanism, the axis motion speed, and the real-time load parameters are read in real time. The pre-stored disturbance data is retrieved to complete the real-time calculation and generate the corresponding feedforward compensation amount.

[0041] Specifically, kinematic constraints include velocity constraints, acceleration constraints, and jerk constraints.

[0042] This application also provides a docking system based on multi-axis cooperative motion, applied to a multi-axis drive mechanism. The system includes: a pose measurement component for real-time acquisition of the pose information of the end effector and the docking target; a servo drive unit for driving the motion axes of the multi-axis drive mechanism; and a controller electrically connected to the pose measurement component and the servo drive unit, respectively. The controller is used to execute the docking method based on multi-axis cooperative motion described in the method embodiment of this application.

[0043] Specifically, the servo drive unit includes multiple servo drivers (such as servo motors) for driving the corresponding motion axes. The servo drivers drive the corresponding motion axes to move according to the issued instructions.

[0044] Furthermore, the pose measurement component includes one or more combinations of a laser tracker, a vision sensor, a displacement sensor, and an inclinometer.

[0045] Specifically, the pose measurement component can collect the pose deviation between the end effector and the docking target interface in real time, and select various combinations of measurement methods. On the one hand, it can realize cross-verification of multiple sets of measurement data, and on the other hand, it can perform average statistical calculation on multiple data to offset the deviation caused by random disturbances.

[0046] Reference Figure 2 and Figure 3 In actual production, taking the explosion-proof deflashing system as an example, the explosion-proof deflashing system includes an explosion-proof robotic arm 1, an XYZ three-axis motion module 2, a workpiece fixing seat 3, and a deflash removal mechanism 4. The deflash removal mechanism 4 includes an upper deflash removal mechanism 41 and a lower deflash removal mechanism 42. Precision machining operations are achieved by using a docking method based on multi-axis coordinated motion provided by the present invention.

[0047] During operation, the explosion-proof robotic arm 1 transports the workpiece to be processed to the workpiece fixing seat 3, and the workpiece is clamped and limited by the clamping structure on the workpiece fixing seat 3. Then, based on the geometric parameters of each axis of the explosion-proof robotic arm 1 and the speed and acceleration motion constraints of the XYZ three-axis motion module 2, a multi-axis collaborative motion model is built. Then, the docking target pose (processing pose) is preset according to the shape of the workpiece, and the time-optimal or energy-optimal multi-axis collaborative motion trajectory is planned. Then, the XYZ three-axis motion module 2 is controlled to drive the flash removal mechanism 4 to feed in the horizontal, vertical and horizontal directions.

[0048] During the processing, the actual position and orientation of the end effector are collected in real time, and the first position deviation is calculated. The feedforward compensation amount is generated by combining gravity, friction and load disturbance factors. The first position deviation is filtered and sent to the PID controller or adaptive controller to obtain the feedback correction amount. The motion trajectory is corrected in real time by fusion of the two types of compensation amounts through variable weighting to offset the positioning error caused by processing disturbance.

[0049] The system continuously cycles through trajectory correction and pose detection until the end-processing pose deviation is less than a set threshold, at which point the removal of upper and lower burrs from the workpiece is completed. This entire method integrates trajectory planning and online error compensation, improving the accuracy and stability of multi-axis coordinated motion, and is suitable for high-precision automated deburring operations in explosion-proof environments.

[0050] This application also provides a storage medium, which includes a stored computer program, wherein the computer program controls the device where the storage medium is located to execute the docking method based on multi-axis cooperative motion described in the method embodiment of this application when it is running.

[0051] The above are merely preferred embodiments of this application and do not limit the patent scope of this application. Any equivalent structural or procedural transformations made using the content of this application's specification and drawings, or direct or indirect applications in other related technical fields, are similarly included within the patent protection scope of this application.

Claims

1. A docking method based on multi-axis cooperative motion, characterized in that, Applied to multi-axis drive mechanisms, the method includes: Step 1: Based on the geometric configuration and kinematic constraints of each motion axis of the multi-axis drive mechanism, establish a multi-axis cooperative motion model that characterizes the coupling relationship between each motion axis and realizes forward and inverse kinematic mapping; Step 2: Based on the target pose and the current pose of the end effector of the multi-axis drive mechanism, dynamically plan the cooperative motion trajectory of each motion axis in conjunction with the multi-axis cooperative motion model; Step 3: Control each motion axis to move along the coordinated motion trajectory, and collect the actual pose of the end effector in real time, and calculate the first pose deviation between the actual pose and the pose of the docking target; Step 4: Based on the multi-axis cooperative motion model, generate feedforward compensation amount by combining the predicted interference factors; process the first pose deviation to obtain feedback correction amount; fuse the feedforward compensation amount and feedback correction amount to generate trajectory correction command to correct the cooperative motion trajectory of each motion axis. Step 5: Repeat steps 3 and 4 until the second pose deviation between the end effector and the docking target is less than the preset docking threshold, then the docking is complete.

2. The docking method based on multi-axis cooperative motion as described in claim 1, characterized in that, The method for processing the first pose deviation to obtain the feedback correction amount in step four includes: The first pose deviation is filtered and denoised. Based on the multi-axis cooperative motion model, the first pose deviation after filtering and noise reduction is converted into the axis deviation amount corresponding to each motion axis; The axis deviation of each motion axis is input into the PID controller or adaptive controller respectively, and the feedback correction amount of each motion axis is calculated.

3. The docking method based on multi-axis cooperative motion as described in claim 1, characterized in that, The trajectory correction command is calculated using fixed-weight fusion or variable-weight segmented fusion feedforward compensation and feedback correction.

4. The docking method based on multi-axis cooperative motion as described in claim 1, characterized in that, The coordinated motion trajectory is the coordinated motion trajectory with the least time or the least energy consumption.

5. The docking method based on multi-axis cooperative motion as described in claim 1, characterized in that, Both the first pose deviation and the second pose deviation are six-dimensional pose deviations that include three-dimensional translation components and three-dimensional rotation components.

6. The docking method based on multi-axis cooperative motion as described in claim 1, characterized in that, The predicted interference factors include static deviations caused by gravity, nonlinear errors caused by friction, and dynamic deviations caused by load changes.

7. The docking method based on multi-axis cooperative motion as described in claim 1, characterized in that, The kinematic constraints include velocity constraints, acceleration constraints, and jerk constraints.

8. A docking system based on multi-axis cooperative motion, characterized in that, The system, applied to a multi-axis drive mechanism, includes: The pose measurement component is used to acquire the pose information of the end effector and the docking target in real time. Servo drive unit, used to drive the motion of each motion axis of a multi-axis drive mechanism; The controller is electrically connected to the pose measurement component and the servo drive unit, respectively, and the controller is used to execute the docking method based on multi-axis cooperative motion as described in any one of claims 1 to 7.

9. The docking system based on multi-axis cooperative motion as described in claim 8, characterized in that, The pose measurement component includes one or more combinations of a laser tracker, a vision sensor, a displacement sensor, and an inclinometer.

10. A storage medium, characterized in that, The storage medium includes a stored computer program, wherein, when the computer program is executed, it controls the device containing the storage medium to perform the docking method based on multi-axis cooperative motion as described in any one of claims 1 to 7.

Images