Multi-end-effector adaptive collaborative trajectory planning method and system

Abstract

The invention relates to the technical field of machine control, in particular to a multi-end-effector adaptive collaborative trajectory planning method and system. The method comprises the following steps: establishing a decoupling control model of internal force and external force, and defining an independent force tracking target pointing to a master end effector for each slave end effector by performing mathematical transformation on an expected internal force group meeting a force closing condition; a master-slave cooperative control framework is constructed, and a force tracking target is modeled as compression force of a virtual spring between master and slave end effectors; aiming at uncertainty of object rigidity and contact point motion, designing an adaptive estimation law based on the Lyapunov theory to carry out online joint estimation; and generating a reference motion track of the slave end effector based on the estimated parameters and an impedance model to realize robust force tracking. According to the method, a complex multi-point internal force control problem is converted into a structured'master-slave 'motion tracking problem, and stable cooperative operation of multiple end effectors can be realized in an unstructured environment with unknown rigidity and unknown geometrical shapes.

Description

Technical Field

[0001] This invention relates to the field of machine control technology, and in particular to a method and system for adaptive cooperative trajectory planning of multi-end effectors. Background Technology

[0002] As intelligent manufacturing evolves towards greater flexibility and collaboration, multi-robot collaborative operation has become a key technology for overcoming bottlenecks in complex operational tasks. However, existing multi-robot systems face multiple challenges when handling collaborative operations in unstructured environments.

[0003] First, at the force control level, when multiple end effectors (EEs) collaboratively manipulate objects with unknown parameters (such as stiffness, mass, and geometry), there is a dual coupling uncertainty between the force and pose at the contact point. Traditional model-based control methods heavily rely on accurate object dynamic parameters, but online parameter identification often converges slowly, making it difficult to meet the real-time requirements of dynamic operations, and easily leading to system instability during parameter abrupt changes. Existing adaptive control research mostly focuses on single parameter estimation, ignoring the coupling effects between parameters, resulting in decreased control performance in complex collaborative tasks. More critically, internal forces are the core determinant of operational stability, but existing methods often treat them as secondary constraints, lacking a systematic internal force optimization and control framework, making it difficult to achieve parameter robustness while ensuring force closure and anti-slip.

[0004] Secondly, at the system level, task allocation, motion planning, and internal force control are often designed in isolation, lacking a unified optimization framework. This separation leads to suboptimal overall system performance, making it difficult to achieve system-level goals such as load balancing and energy optimization while satisfying kinematic and dynamic constraints.

[0005] Therefore, there is an urgent need for a multi-end actuator collaborative control method that can simultaneously handle multiple parameter uncertainties, achieve robust internal force tracking, and be deeply integrated with upper-level task planning. Summary of the Invention

[0006] In view of this, the purpose of this invention is to propose a multi-end-effector adaptive cooperative trajectory planning system to solve the problem that the overall system performance is suboptimal and it is difficult to achieve system-level goals such as load balancing and energy consumption optimization while satisfying kinematic and dynamic constraints.

[0007] To achieve the above objectives, this invention provides a multi-end-effector adaptive cooperative trajectory planning method, which includes the following steps:

[0008] Step S1: Establish a decoupled control model for the internal and external forces of the multi-end actuator;

[0009] Step S2: Based on the decoupled control model, perform mathematical transformation on the cooperative internal force control target that satisfies the force closure condition, and define an independent force tracking target pointing to the master end actuator for each slave end actuator in the system other than the master end actuator.

[0010] Step S3: Based on the independent force tracking target, construct an absolute-relative motion master-slave cooperative control architecture with the master end effector as the reference;

[0011] Step S4: Model each of the independent force tracking targets from the end effector as the compressive force of a virtual spring between it and the master end effector, wherein the axis of the virtual spring is aligned with the direction of the line connecting the two.

[0012] Step S5: Based on Lyapunov theory, design a joint adaptive estimation law for the stiffness of the object and the uncertainty of the motion at the contact point, so as to estimate the equivalent stiffness and natural length parameters of the virtual spring online.

[0013] Step S6: Based on the impedance control model and the equivalent stiffness and natural length parameters of the virtual spring of the joint adaptive estimation law, generate a reference motion trajectory from the end effector to achieve robust tracking of the independent force tracking target.

[0014] Preferably, in step S1, the establishment of the decoupling control model includes:

[0015] Introduce a virtual link connecting the end effector contact point and the object reference point;

[0016] Construct a linear mapping matrix G that describes the relationship between the force / torque at each contact point and the net external force / torque on the object;

[0017] Calculate the null basis matrix V of the linear mapping matrix G, and establish the generalized force vector applied by the end effector. The mathematical model decomposes the forces into external and internal components, and this model is expressed as:

[0018] ;

[0019] in For the generalized inverse of G, The net external force vector acting on the object. Let be any vector corresponding to the internal force subspace.

[0020] Preferably, in step S2, the mathematical transformation of the cooperative internal force control target that satisfies the force closure condition includes:

[0021] Construct the first transformation matrix to map the generalized force vector applied by the end effector into the desired internal force vector that meets the force closure condition;

[0022] Construct a second transformation matrix to map the generalized force vector to the independent force tracking target vector;

[0023] By calculating the generalized inverse of the first transformation matrix, an equivalent mapping relationship is established between the desired internal force vector and the independent force tracking target vector.

[0024] Preferably, in step S5, the joint adaptive estimation law is:

[0025]

[0026]

[0027] in, To estimate the equivalent stiffness of the virtual spring, The natural length parameter of the estimated virtual spring.

[0028] These are the estimated internal forces calculated based on the current estimation parameters. These are the actual measured internal force values. The current position of the end effector along the direction of the connecting line is given by t, where t is time. and All are positive definite gains. This indicates the end effector, and i represents the index of the polynomial coefficients.

[0029] Preferably, in step S6, the reference motion trajectory is calculated using the following formula:

[0030]

[0031] in, The reference motion trajectory position is generated, and M, B, and K are impedance control parameters. For based on estimated parameters The reconstructed virtual spring natural length endpoint estimated position, The independent force tracking target value is obtained through the mathematical transformation.

[0032] Preferably, the method further includes smoothing the trajectory using a first-order low-pass filter after generating the reference motion trajectory, with the smoothing formula being: ,in is the smoothing coefficient, and k is the time step.

[0033] Preferably, the actual measured internal force value is obtained in real time by a force sensor installed on the end effector.

[0034] The present invention also provides a multi-end-effector adaptive cooperative trajectory planning system, the system comprising:

[0035] At least one controller is configured to execute the multi-end actuator adaptive cooperative trajectory planning method as described above;

[0036] Multiple robot units, each robot unit including at least one robotic arm and an end effector mounted at the end of the robotic arm;

[0037] A force sensing module, installed on the end effector, is used to measure the internal force in contact with the object.

[0038] The beneficial effects of this invention are:

[0039] 1. This invention achieves robust internal force tracking for multi-parameter uncertainties. By introducing an equivalent transformation of the internal force space, the multi-point internal force network, which is difficult to control directly, is transformed into a structurally clear internal force tracking problem from the end effector to the master end effector. Combined with an adaptive algorithm based on Lyapunov theory, it can jointly estimate the unknown equivalent stiffness of the object and the time-varying contact point motion online, achieving high-precision internal force stability control in dynamic uncertain environments without prior acquisition of the object's geometric, mass, and stiffness parameters. Experiments show that for low-stiffness objects, the internal force tracking error is less than 1N (6%); for high-stiffness objects, the tracking error is less than 2N (8%).

[0040] 2. This method innovatively transforms the complex force control problem into a controllable motion planning problem, modeling the internal force tracking target as the compression force of a virtual spring. This transforms the traditional control challenge of internal force tracking into tracking the relative motion between the two ends of the virtual spring. This transformation allows the use of mature motion planning and control theories to solve the force control problem, simplifies controller design, and improves the system's feasibility and stability.

[0041] 3. The method proposed in this invention forms a complete technical chain encompassing decoupled modeling, internal force transformation, master-slave architecture, virtual spring modeling, adaptive estimation, and trajectory generation. This framework is clearly modular, capable of operating independently as a force controller, and easily integrated with upper-level task planning, path planning, and multi-robot collaborative algorithms. It provides a core foundation for constructing complex multi-robot collaborative systems and offers a modular and scalable system integration framework.

[0042] 4. This invention ensures the stability and safety (anti-slip) of cooperative operation through robust internal force tracking. Simultaneously, this method lays a solid foundation for subsequent system-level load balancing and energy consumption optimization. Experimental verification shows that this method can control the relative position oscillation amplitude within 1mm, significantly improving the accuracy and stability of operation, and enhancing the overall operational performance of the system in unstructured environments. Attached Figure Description

[0043] To more clearly illustrate the technical solutions in this invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only for this invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0044] Figure 1 This is a schematic diagram of the adaptive cooperative trajectory planning method according to an embodiment of the present invention;

[0045] Figure 2 This is a schematic diagram of the expected internal force and C-EF relationship in an embodiment of the present invention;

[0046] Figure 3 This is a schematic diagram of the overall control framework for master-slave EE cooperative motion according to an embodiment of the present invention;

[0047] Figure 4 This is an absolute motion error diagram of Embodiment 2 of the present invention;

[0048] Figure 5 These are the xz and yz plane projections of the relative motion positions of each component from the EE in Embodiment 2 of the present invention;

[0049] Figure 6 This is a relative motion error diagram for Embodiment 2 of the present invention. Detailed Implementation

[0050] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments.

[0051] It should be noted that, unless otherwise defined, the technical or scientific terms used in this invention should have the ordinary meaning understood by one of ordinary skill in the art to which this invention pertains. The terms "first," "second," and similar terms used in this invention do not indicate any order, quantity, or importance, but are merely used to distinguish different components. Terms such as "comprising" or "including" mean that the element or object preceding the word encompasses the elements or objects listed following the word and their equivalents, without excluding other elements or objects. Terms such as "connected" or "linked" are not limited to physical or mechanical connections, but can include electrical connections, whether direct or indirect. Terms such as "upper," "lower," "left," and "right" are used only to indicate relative positional relationships; when the absolute position of the described object changes, the relative positional relationship may also change accordingly.

[0052] Example 1:

[0053] This specification provides an adaptive cooperative trajectory planning method for multiple end effectors, using the example of four end effectors (EEs) collaboratively operating a rigid object with unknown parameters. The method is as follows: Figure 1 As shown, the specific steps include:

[0054] S1. First, to describe the force interaction between multiple end effectors (EEs) and the object, the concept of a "virtual link" is introduced. The core principle of introducing the virtual link is to establish a unified and simplified mechanical model to describe the relationship between multiple dispersed contact points and the overall motion of the object. This model abstracts the complex contact geometry and force transmission path into a hypothetical rigid connection from each contact point to a common reference point (such as an approximate center of mass) inside the object. The key benefit of this abstraction is that it allows us to uniformly map the contact forces applied by all end effectors, which are in different directions, to the resultant external force and resultant torque on this common reference point, thereby clearly separating the "external force" component that changes the overall motion of the object. All force components that do not contribute to the resultant external force are naturally classified into the category of "internal forces." They only affect the stress distribution inside the object and the load distribution among the actuators, and are key to maintaining stable gripping and preventing slippage. Therefore, this decoupling model lays the theoretical foundation for independently designing and analyzing object motion control (through external forces) and gripping stability control (through internal forces). Assuming each EE has point contact with the object, from the point of contact... To a set reference point on the object An imaginary rigid link exists. This virtual link is only used for mechanical analysis and is not a physical entity.

[0055] Define the world coordinate system Let the i-th EE exert force and torque on the object through the virtual link. The following is represented as a 6-dimensional vector. Then, the net external force and net torque acting on the object... Can be represented as all Linear superposition: ;

[0056] in G is the stacked vector of forces at the top of all virtual links, where G = [ ] is a 6×6n mapping matrix. It is a 6th-order identity matrix.

[0057] Since the rank of G is 6, its null space dimension is 6n-6. Let V be a set of basis matrices of the null space of G, then the equation... The general solution is: ,in It is any 6n-6 dimensional vector. It is the generalized inverse of G.

[0058] S2, directly using the zero-space basis V and internal force vectors The control mechanism is not intuitive. This method proposes an equivalent transformation to map the "expected internal force," which aligns with human operational intuition, into a commanded internal force (C-EF) that is easy for the controller to track. The design idea of ​​the equivalent transformation of the internal force space stems from a deep consideration of control operability. Although the intuitive expected internal force conforms to physical cognition and is easy to set to satisfy force closure, its components usually involve complex coupling relationships between multiple actuators. This means that adjusting any internal force will affect other internal forces, making it difficult to directly design an independent, decoupled tracking controller. The transformation principle proposed in this invention is to reparameterize this coupled "expected internal force" space into a completely new "commanded internal force" space through a linear, full-rank mathematical mapping. In this new space, each internal force component is explicitly defined as the interaction force between a subordinate actuator and a single active actuator. This "one-to-many" star structure has a significant advantage in principle: it transforms the controlled object from a complex networked coupled system into a set of multiple independent channels with the active actuator as a common reference. This allows for the design of an independent force-tracking controller for each slave actuator, with a clear objective (tracking the force on the active actuator) and well-defined disturbances (mainly from the motion of the active actuator). This greatly simplifies the complexity of the control problem at the principle level and improves the feasibility and robustness of the system.

[0059] First, the operator or advanced planner, based on task requirements, provides a set of intuitive desired internal forces that satisfy force closure conditions and anti-slip requirements. For example, for four EEs, denoted as... ,in, The master EE and the others are slave EEs, and the corresponding virtual link top force vectors are respectively The intuitive internal force relationship can be defined as follows, whereby this set of internal forces is represented by three virtual relative force vectors:

[0060]

[0061] in, By constructing the transformation matrix and , making , Establish the equivalent transformation relationship between the two:

[0062]

[0063] This means that once the intuitive desired internal force is given, a unique set of easily trackable command internal forces is identified, therefore subsequent control using this method will directly track them. Each component in ,like Figure 2 This is a schematic diagram of the expected internal force and the C-EF relationship.

[0064] S3, the motion trajectory of the main EE and Provided by an external system (such as a remote interface or global task planner), where Indicates location, This represents the attitude. Based on kinematic relationships, the desired motion of the slave EE is determined by the motion of the master EE and the desired relative pose, forming a closed-loop motion chain. The master EE is responsible for tracking the absolute motion trajectory, while the slave EE, in addition to following the motion of the master EE, will receive additional adjustment commands for internal force tracking from its controller.

[0065] S4. For each slave EE, construct a one-dimensional virtual spring model along the direction of its connection to the master EE (called the "holding line"). This spring connects to the current position of the slave EE. And a time-varying "natural length endpoint". The compressive force of the spring This represents the magnitude of the internal force of the command, satisfying Hooke's Law:

[0066]

[0067] in, It is the equivalent stiffness of the virtual spring, equivalent to the combined stiffness of the manipulated object at the contact point, and the desired commanded internal force. This corresponds to a desired spring compression. Therefore, the tracking command internal force... The problem was transformed into control over the location of the EE. To track by and parameters and The issue of determining the desired location.

[0068] This step models the internal force tracking target as the compression force of a virtual spring, which embodies profound physical and control principles. From a physical perspective, it unifies the comprehensive mechanical response of the manipulated object at the contact point (including all unknown and potentially nonlinear characteristics such as the object's own elastic deformation and the local deformation of the contact surface) into a linear spring. From a control principle perspective, this modeling successfully transforms a "force tracking" problem (directly controlling the force but constrained by unknown environmental dynamics) into a "motion tracking" problem (controlling the position to make the spring generate the desired force). The latter is a more mature and direct problem in the field of robot control. More importantly, it clearly points out the two fundamental uncertainties that must be addressed to achieve accurate force tracking: the equivalent stiffness of the environment and the natural length endpoint of the spring when it is not compressed.

[0069] S5, the position of the endpoint of the natural length of the virtual spring. Since factors such as contact point slippage and object deformation are time-varying and unknown, they are modeled as a polynomial function of time t:

[0070]

[0071] in, Let these be the coefficients to be estimated. Define the estimated parameter vector. Its estimated value is .

[0072] Based on Lyapunov stability theory, the following adaptive update law is designed to estimate these parameters online:

[0073]

[0074]

[0075] in, These are the estimated internal forces calculated based on the current estimation parameters. These are the actual measured internal force values. The current position of the end effector along the direction of the connecting line is given by t, where t is time. and All are positive definite gains. This indicates the end effector, and i represents the index of the polynomial coefficients.

[0076] The designed adaptive update law is based on Lyapunov stability theory. The core idea is to construct a Lyapunov function (usually a positive definite quadratic form) that includes parameter estimation errors, and to ensure that the rate of change of this function over time is negative definite or semi-negative definite by designing the parameter update rule. In this method, the driving signal of the update law is the error between the estimated internal force and the measured values ​​from the sensor. This error signal is multiplied by different gains and regression quantities related to specific parameters, thereby directionally adjusting the estimates of the stiffness and trajectory polynomial coefficients. In principle, this design guarantees that under continuous system motion (satisfying continuous excitation conditions), the internal force estimation error will asymptotically converge to zero, while the parameter estimation error is bounded. This means that even with no initial knowledge of the object's parameters, the controller can "learn online" and approximate the true system dynamics during operation, thus achieving internal force tracking without relying on an accurate prior model.

[0077] S6. In order to accurately track the command internal force and eliminate steady-state error, a reference motion trajectory needs to be generated from EE. Based on the principle of impedance control, the following trajectory generator is designed:

[0078]

[0079] in, The reference motion trajectory position is generated, and M, B, and K are the set impedance mass, damping, and stiffness parameters, respectively. For based on estimated parameters The reconstructed virtual spring natural length endpoint estimated position, The independent force tracking target value is obtained through the mathematical transformation.

[0080] The trajectory This will be used as the setpoint for the underlying position controller to drive the movement of the EE, thereby achieving robust, zero-steady-error tracking of the internal force of the desired command, such as... Figure 3 The overall control framework for master-slave EE coordinated motion.

[0081] Example 2:

[0082] To verify the effectiveness of the method in Example 1, tests were conducted on an experimental platform consisting of two robotic arms. Each robotic arm had two parallel custom-designed three-dimensional force sensors mounted at its end effector, forming a total of four end effectors (EEs). The system software was built based on the Robot Operating System (ROS).

[0083] Those skilled in the art will recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed in this application can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementation should not be considered beyond the scope of this application.

[0084] Experimental setup:

[0085] Test objects: Two objects with very different characteristics were selected for testing: (a) a soft, irregularly shaped flexible package (low-stiffness object); (b) a hard, smooth, sealed container filled with miscellaneous items (high-stiffness object, approximately 3 kg in mass). The precise geometric dimensions, mass distribution, coefficient of friction, and stiffness of the objects were not provided to the controller beforehand.

[0086] Control parameters: Adaptive gain The impedance parameters are M = diag(0.01, 0.01, 0.01) kg, B = diag(0.1, 0.1, 0.1) Ns / m, and K = diag(5, 5, 5) N / m. All parameters from the EE are the same.

[0087] Task: The operator remotely assigns a complex motion trajectory involving translation and rotation to the main EE, while simultaneously providing a set of intuitively desired internal forces that satisfy force closure conditions. The system then initiates the method of this invention, controlling four EEs to collaboratively move an object along the given trajectory.

[0088] Experimental results:

[0089] Internal force tracking accuracy:

[0090] For low-stiffness flexible wrapping, the absolute position and orientation errors are as follows: Figure 4 As shown, due to the presence of unknown uncertainties, such as coordinate calibration errors, numerical errors, and kinematic uncertainties, the absolute error exhibits a small and acceptable oscillation near zero. The projections of the relative position trajectories of each EE onto the xz and zy planes of the world coordinate system are shown below. Figure 5 As shown in the diagram, the EE trajectory reveals that after the C-EF adaptive tracking algorithm is triggered in the second stage, each slave EE adaptively estimates the unknown parameters to generate a reference trajectory, approaches the master EE along the hold line, and tracks the C-EF. It can be observed that the relative trajectory is well maintained under the influence of the reference trajectory. Under reasonable expected C-EF conditions, each slave EE, under the control constraints of maintaining its relative position and the action of the feedback force controller, estimates the relative trajectory along the hold line. and By tracking the expected C-EF, stability is achieved throughout the entire operation. Relative position and orientation trajectory errors, such as... Figure 6 As shown. During the holding phase of the operation, there is a certain range of oscillations near zero. The position oscillation amplitude is generally less than 1 mm, and the direction oscillation amplitude is generally less than 1°. However, the two slave EEs that are far from the master EE will experience larger position and direction errors due to the inertia of the objects when the absolute direction of motion changes rapidly. The maximum deviation can reach 5 mm and 5°, respectively.

[0091] Table 1 below compares the force tracking performance in uncertain contact environments, focusing on the effectiveness and applicability of force tracking under uncertain and unknown parameter environments. It can be seen that the method established in this invention converges to the desired values ​​(-7N, -13N, -18N) from the three commanded internal forces of the EE within approximately 7 seconds. After convergence, the steady-state error of internal force tracking is less than 1 Newton throughout the entire dynamic motion process, with a maximum overshoot of approximately 15%. In multi-EE collaborative application scenarios, it is more competitive in terms of error, parameter setting requirements, and computational complexity.

[0092] Table 1 Comparison of force tracking performance in uncertain contact environments

[0093]

[0094] For high-stiffness containers, there is no significant error in relative position. However, due to the high stiffness and smooth surface, the relative position error is increased compared to operating flexible enclosures, and internal force convergence takes longer.

[0095] The results show that the method of this invention significantly outperforms traditional methods in terms of steady-state error, overshoot, and convergence speed in internal force tracking. It exhibits unique advantages, particularly in that it eliminates the need for precise parameter modeling, simplifies parameter setting, and is naturally applicable to multi-EE cooperative scenarios. Pure position control cannot stabilize internal forces; fixed impedance control suffers from huge errors when parameters are unknown; variable impedance control heavily relies on model accuracy and its performance degrades in completely unknown environments. This invention, through internal force space transformation and adaptive estimation, fundamentally solves the problem of robust cooperative control under multiple parameter uncertainties.

[0096] In summary, the method provided by this invention effectively solves the problem of internal force tracking and control when multi-end effectors operate unknown objects through innovative internal force space transformation and adaptive estimation framework, and realizes high-precision, high-robustness and strong adaptability cooperative operation, providing a reliable technical solution for complex robot cooperative tasks in unstructured environments.

[0097] Example 3:

[0098] This embodiment provides a multi-end-effector adaptive cooperative trajectory planning system, which includes:

[0099] At least one controller is configured to execute the multi-end effector adaptive cooperative trajectory planning method of Embodiment 1;

[0100] Multiple robot units, each robot unit including at least one robotic arm and an end effector mounted at the end of the robotic arm;

[0101] A force sensing module, installed on the end effector, is used to measure the internal force in contact with the object.

[0102] In the embodiments provided in this application, it should be understood that the disclosed devices / terminal equipment and methods can be implemented in other ways. For example, the device / terminal equipment embodiments described above are merely illustrative. For instance, the division of modules or units is only a logical functional division, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the displayed or discussed mutual coupling or direct coupling or communication connection may be through some interfaces; the indirect coupling or communication connection between devices or units may be electrical, mechanical, or other forms.

[0103] Furthermore, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The integrated unit can be implemented in hardware or as a software functional unit.

[0104] If the integrated module / unit is implemented as a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, all or part of the processes in the methods of the above embodiments can also be implemented by a computer program instructing related hardware. The computer program can be stored in a computer-readable storage medium, and when executed by a processor, it can implement the steps of the various method embodiments described above. The computer program includes computer program code, which can be in the form of source code, object code, executable files, or certain intermediate forms. The computer-readable medium can include: any entity or device capable of carrying the computer program code, recording media, USB flash drives, portable hard drives, magnetic disks, optical disks, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signals, telecommunication signals, and software distribution media, etc. It should be noted that the content included in the computer-readable medium can be appropriately added or removed according to the requirements of legislation and patent practice in the jurisdiction. For example, in some jurisdictions, according to legislation and patent practice, computer-readable media do not include electrical carrier signals and telecommunication signals.

[0105] The implementation of all or part of the processes in the methods of the above embodiments can also be accomplished by a computer program product. When the computer program product is run on a terminal device, the terminal device can implement the steps in the various method embodiments described above.

[0106] The embodiments described above are only used to illustrate the technical solutions of this application, and are not intended to limit it. Although this application has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of this application, and should all be included within the protection scope of this application.

Claims

1. A multi-end effector adaptive cooperative trajectory planning method, characterized in that, The method includes the following steps: Step S1: Establish a decoupled control model for the internal and external forces of the multi-end actuator; Step S2: Based on the decoupled control model, perform mathematical transformation on the cooperative internal force control target that satisfies the force closure condition, and define an independent force tracking target pointing to the master end actuator for each slave end actuator in the system other than the master end actuator. Step S3: Based on the independent force tracking target, construct an absolute-relative motion master-slave cooperative control architecture with the master end effector as the reference; Step S4: Model each of the independent force tracking targets from the end effector as the compressive force of a virtual spring between it and the master end effector, wherein the axis of the virtual spring is aligned with the direction of the line connecting the two. Step S5: Based on Lyapunov theory, design a joint adaptive estimation law for the stiffness of the object and the uncertainty of the motion at the contact point, so as to estimate the equivalent stiffness and natural length parameters of the virtual spring online. Step S6: Based on the impedance control model and the equivalent stiffness and natural length parameters of the virtual spring of the joint adaptive estimation law, generate a reference motion trajectory from the end effector to achieve robust tracking of the independent force tracking target.

2. The multi-end effector adaptive cooperative trajectory planning method according to claim 1, characterized in that, In step S1, the establishment of the decoupling control model includes: Introduce a virtual link connecting the end effector contact point and the object reference point; Construct a linear mapping matrix G that describes the relationship between the force / torque at each contact point and the net external force / torque on the object; Calculate the null basis matrix V of the linear mapping matrix G, and establish the generalized force vector applied by the end effector. The mathematical model decomposes the forces into external and internal components, and this model is expressed as: ； in For the generalized inverse of G, The net external force vector acting on the object. Let be any vector corresponding to the internal force subspace.

3. The multi-end effector adaptive cooperative trajectory planning method according to claim 1, characterized in that, In step S2, the mathematical transformation of the cooperative internal force control target that satisfies the force closure condition includes: Construct the first transformation matrix to map the generalized force vector applied by the end effector into the desired internal force vector that meets the force closure condition; Construct a second transformation matrix to map the generalized force vector to the independent force tracking target vector; By calculating the generalized inverse of the first transformation matrix, an equivalent mapping relationship is established between the desired internal force vector and the independent force tracking target vector.

4. The multi-end effector adaptive cooperative trajectory planning method according to claim 1, characterized in that, In step S5, the joint adaptive estimation law is: in, To estimate the equivalent stiffness of the virtual spring, The natural length parameter of the estimated virtual spring. These are the estimated internal forces calculated based on the current estimation parameters. These are the actual measured internal force values. The current position of the end effector along the direction of the connecting line is given by t, where t is time. and All are positive definite gains. This indicates the end effector, and i represents the index of the polynomial coefficients.

5. The multi-end effector adaptive cooperative trajectory planning method according to claim 4, characterized in that, In step S6, the reference motion trajectory is calculated using the following formula: in, The reference motion trajectory position is generated, and M, B, and K are impedance control parameters. For based on estimated parameters The reconstructed virtual spring natural length endpoint estimated position, The independent force tracking target value is obtained through the mathematical transformation.

6. The multi-end effector adaptive cooperative trajectory planning method according to claim 5, characterized in that, The method further includes smoothing the trajectory using a first-order low-pass filter after generating the reference motion trajectory, with the smoothing formula being: ,in is the smoothing coefficient, and k is the time step.

7. The multi-end effector adaptive cooperative trajectory planning method according to claim 4, characterized in that, The actual measured internal force value is obtained in real time by a force sensor installed on the end effector.

8. A multi-end-effector adaptive cooperative trajectory planning system, characterized in that, The system includes: At least one controller is configured to perform the multi-end actuator adaptive cooperative trajectory planning method as described in any one of claims 1 to 7; Multiple robot units, each robot unit including at least one robotic arm and an end effector mounted at the end of the robotic arm; A force sensing module, installed on the end effector, is used to measure the internal force in contact with the object.

Images