Magnetic-contact-motion decoupling three-arm robot collaborative operation control method

Abstract

The invention discloses a magnetic-contact-motion decoupling three-arm robot cooperative operation control method, and relates to the technical field of space robots, and the method comprises the steps: building a magnetic force model based on the state parameters of a three-arm robot at the current moment; constructing a target function, a three-arm collaborative equality constraint and a three-arm safety inequality constraint of a discrete multi-constraint trajectory optimization problem based on an expected satellite maintenance control instruction by adopting a sampling model prediction control method; determining a non-contact magnetic constraint based on the magnetic model; based on the magnetic force model and the state parameters, determining a correction value of a tail end expected Cartesian velocity and updating a three-arm collaborative equality constraint; and based on the objective function, the three-arm safety inequality constraint, the non-contact magnetic constraint and the updated three-arm collaborative equality constraint, constructing a discrete multi-constraint trajectory optimization model and solving the model to obtain an optimized motion trajectory, and determining a collaborative operation motion control instruction. According to the method, the motion safety and the force flexibility of in-orbit maintenance of the space robot satellite are improved.

Description

Technical Field

[0001] This application relates to the field of space robot technology, and in particular to a collaborative operation control method for a three-armed robot with magnetic-contact-motion decoupling. Background Technology

[0002] Space robots have become a key technology for on-orbit satellite maintenance, and three-arm robots, through non-contact magnetic anchoring technology, coordinate precise control of tools to perform operations, significantly improving operational flexibility and demonstrating great potential in on-orbit satellite maintenance. However, current research still faces three major bottlenecks: 1) the limited operational capabilities of single-arm robots make it difficult to complete complex collaborative tasks; 2) the magnetic-contact-motion coupling of the end effector in three-arm collaboration (collision risk, and the mixture of magnetic and contact forces) significantly weakens operational safety; and 3) existing models have insufficient prediction accuracy for multi-field coupling effects.

[0003] To address the aforementioned issues, researchers have proposed some solutions, such as single-arm collision detection methods based on trajectory optimization and multi-arm coordinated control strategies. However, none of these solutions resolve the real-time safety constraints and force decoupling problems in three-arm coordinated on-orbit maintenance scenarios. Therefore, a multi-arm coordinated control framework integrating motion safety and force compliance is urgently needed to achieve highly reliable and safe on-orbit satellite maintenance operations. Summary of the Invention

[0004] The purpose of this application is to provide a collaborative operation control method for a three-armed robot with magnetic-contact-motion decoupling, which improves the motion safety and force compliance of the space robot for satellite maintenance in orbit.

[0005] To achieve the above objectives, this application provides the following solution:

[0006] This application provides a magnetic-contact-motion decoupling control method for cooperative operation of a three-armed robot, the magnetic-contact-motion decoupling control method for cooperative operation of a three-armed robot comprising:

[0007] Obtain the state parameters of the three-armed robot at the current moment and the expected satellite maintenance control command; the expected satellite maintenance control command is the expected Cartesian velocity at the end of the three-armed robot during the satellite maintenance operation.

[0008] Based on the state parameters of the three-armed robot at the current moment, a magnetic force model among the multiple magnets at the end of the three-armed robot is established.

[0009] A sampling model predictive control method is adopted, based on the expected satellite maintenance control command, to construct the objective function, three-arm cooperative equality constraint, and three-arm safety inequality constraint for the discrete multi-constraint trajectory optimization problem of a three-arm robot; wherein, the objective function aims to minimize the trajectory cost;

[0010] Based on the magnetic force model between multiple magnets at the end of a three-armed robot, the non-contact magnetic force constraints for the discrete multi-constraint trajectory optimization problem of the three-armed robot are determined.

[0011] Based on the magnetic force model between the multiple magnets at the end of the three-armed robot and the state parameters of the three-armed robot at the current moment, the correction value of the expected Cartesian velocity of the end of the three-armed robot is determined so as to realize the contact compliance constraint of the discrete multi-constraint trajectory optimization problem of the three-armed robot.

[0012] The three-arm cooperative equation constraint is updated by using the correction value of the expected Cartesian velocity of the end effector of the three-arm robot, resulting in the updated three-arm cooperative equation constraint.

[0013] Based on the objective function of the discrete multi-constraint trajectory optimization problem of the three-arm robot, the three-arm safety inequality constraint, the non-contact magnetic constraint, and the updated three-arm cooperative equality constraint, a discrete multi-constraint trajectory optimization model of the three-arm robot is constructed.

[0014] Solve the discrete multi-constraint trajectory optimization model of the three-armed robot to determine the optimized motion trajectory of the three-armed robot;

[0015] Based on the optimized motion trajectory of the three-armed robot, determine the collaborative operation motion control commands for the three-armed robot.

[0016] In one embodiment, the state parameters of the three-armed robot at the current moment include: the joint angle of the three-armed robot at the current moment, the joint velocity of the three-armed robot at the current moment, the end-effector pose of the three-armed robot at the current moment, and the force sensor data of the end-effector of the three-armed robot at the current moment.

[0017] In one embodiment, based on the current state of the three-armed robot, a magnetic force model is established among the multiple magnets at the end of the three-armed robot, specifically including:

[0018] A Bayesian optimization method based on Gaussian process was used to calibrate the magnetic moment amplitude of the magnetic dipoles corresponding to the magnets at the ends of the three robotic arms of the three-arm robot, and the calibrated magnetic moment amplitude of each magnetic dipole was obtained.

[0019] By taking any two magnetic dipoles as one magnetic dipole group, we get three magnetic dipole groups;

[0020] Based on the calibrated magnetic moment amplitudes of the two magnetic dipoles in each magnetic dipole group and the position vector between the two magnetic dipoles, the magnitude of the magnetic force of the interaction between the two magnetic dipoles in the corresponding magnetic dipole group is calculated.

[0021] Based on the magnitude of the magnetic force of the interaction between two magnetic dipoles in each magnetic dipole group, a magnetic force model among the multiple magnets at the end of the three-arm robot is established.

[0022] In one embodiment, the objective function of the discrete multi-constraint trajectory optimization problem for a three-armed robot is expressed as:

[0023]

[0024] Where V is the joint velocity sequence of the robotic arm of the three-armed robot; C(V) is the trajectory cost corresponding to the joint velocity sequence of the three-armed robot. For expectations; The cost of optimizing the termination state of the trajectory of a three-armed robot; Joint states at the termination time for trajectory optimization of a three-armed robot; t is time; x t Joint states at time t for trajectory optimization of a three-armed robot; s(x t ) is x t The state cost function is given by λ, where λ is the inverse temperature coefficient. The transpose of the control input at time t; ∈ t Gaussian noise at time t is required for random optimization.

[0025] In one embodiment, the process of determining the three-arm cooperative equality constraint specifically includes:

[0026] Taking any one of the three-armed robots as the reference robot, the single-arm cooperative equality constraint of the reference robot is determined based on the expected Cartesian velocity at the end of the reference robot, the joint velocity of the reference robot, and the Jacobian matrix at the end of the reference robot.

[0027] Based on the expected Cartesian velocity of the end effector of the reference manipulator and the corresponding rotation matrix, the expected Cartesian velocities of the end effectors of the two manipulators other than the reference manipulator in the three-arm robot are determined.

[0028] Based on the expected Cartesian velocities of the two ends of the three-arm robot (excluding the reference arm), the corresponding Jacobian matrices of the ends of the two arms, and the single-arm cooperative equality constraints of the reference arm, the three-arm cooperative equality constraints are determined.

[0029] In one embodiment, the expression for the three-arm cooperation equation constraint is:

[0030]

[0031] in, For the robotic arm i of a three-armed robot a The expected Cartesian velocity at the end; For the robotic arm j of the three-armed robot a The expected Cartesian velocity at the end; For the robotic arm k of the three-armed robot a The expected Cartesian velocity at the end; For the robotic arm i of a three-armed robot a The Jacobian matrix at the end; For the robotic arm j of the three-armed robot a The Jacobian matrix at the end; For the robotic arm k of the three-armed robot a The Jacobian matrix at the end; v t Let t be the joint velocity of the robotic arm of the three-armed robot at time t.

[0032] In one embodiment, the process of determining the three-arm safety inequality constraint specifically includes:

[0033] Using analytical geometry, the shortest distance between the arm frames of every two robotic arms of the three-arm robot is calculated.

[0034] A preset safety threshold is set, and the three-arm safety inequality constraint is determined based on the shortest distance between the arm skeletons of every two robotic arms of the three-arm robot and the preset safety threshold.

[0035] In one embodiment, the expression for the three-arm safety inequality constraint is:

[0036]

[0037] in, For the robotic arm i of a three-armed robot a and robotic arm j a The shortest distance between the arm skeletons; For the robotic arm i of a three-armed robot a and robotic arm k a The shortest distance between the arm skeletons; For the robotic arm j of the three-armed robot a and robotic arm k a The shortest distance between the arm skeletons; γ safe The preset safety threshold; h safe (t,x t ) is a function relating to the three-arm safety inequality constraint.

[0038] In one embodiment, based on the magnetic force model between the multiple magnets at the end effector of a three-armed robot, the non-contact magnetic constraints for the discrete multi-constraint trajectory optimization problem of the three-armed robot are determined, specifically including:

[0039] Based on the magnetic force model among the multiple magnets at the end of a three-arm robot, the total magnetic force on the magnets at the end of each robotic arm of the three-arm robot is calculated.

[0040] A preset magnetic force threshold is set, and the non-contact magnetic force constraints for the discrete multi-constraint trajectory optimization problem of the three-arm robot are determined based on the total magnetic force on the magnets at the ends of each robotic arm of the three-arm robot and the preset magnetic force threshold.

[0041] In one embodiment, the expression for the correction value of the desired Cartesian velocity at the end effector of the three-armed robot is:

[0042]

[0043] in, For the robotic arm i of a three-armed robot a The correction value for the expected Cartesian velocity at the end; For the robotic arm i of a three-armed robot a Scale factor; For the robotic arm i of a three-armed robot a The inverse of the rotation matrix; For the robotic arm i of a three-armed robot a The actual value of the contact force; Let be the expected value of the contact constant force of the robotic arm of the three-armed robot; For the robotic arm j of the three-armed robot a The correction value for the expected Cartesian velocity at the end; For the robotic arm j of the three-armed robot a Scale factor; For the robotic arm j of the three-armed robot a The inverse of the rotation matrix; For the robotic arm j of the three-armed robot a The actual value of the contact force; for the robotic arm k of the three-armed robot. a The correction value for the expected Cartesian velocity at the end; For the robotic arm k of the three-armed robot a Scale factor; For the robotic arm k of the three-armed robot a The inverse of the rotation matrix; For the robotic arm k of the three-armed robot a The actual value of the contact force.

[0044] According to the specific embodiments provided in this application, this application has the following technical effects:

[0045] This application discloses a magnetic-contact-motion decoupling control method for a three-armed robot's cooperative operation. By establishing a magnetic force model among the multiple magnets at the end effector of the three-armed robot, the magnetic force of non-contact magnetic operation can be accurately predicted. By constructing the objective function of the discrete multi-constraint trajectory optimization problem of the three-armed robot, the three-armed cooperative equality constraint, and the three-armed safety inequality constraint, the distance safety between the three arm movements can be ensured during precise satellite on-orbit maintenance tasks. By constructing non-contact magnetic force constraints, the compliant change of magnetic force at the end effector of the three arms can be achieved, avoiding mutual magnetic interference. By calculating the correction value of the expected Cartesian velocity at the end effector of the three-armed robot, contact compliance constraint is achieved. The discrete multi-constraint trajectory optimization model of the three-armed robot enables real-time, reliable, and safe satellite on-orbit maintenance operations. Attached Figure Description

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

[0047] Figure 1 A schematic flowchart of a three-armed robot cooperative operation control method with magnetic-contact-motion decoupling provided in an embodiment of this application;

[0048] Figure 2 This is a schematic diagram of a three-armed robot performing on-orbit maintenance on a satellite.

[0049] Figure 3 for Figure 2 Schematic diagram of the coupled magnetic field of the three magnets in the middle;

[0050] Figure 4 This is a schematic diagram of the spatial skeleton geometry model of a three-armed robot. Detailed Implementation

[0051] 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 some embodiments of this application, and not all embodiments. Based on the embodiments of this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0052] To make the above-mentioned objectives, features and advantages of this application more apparent and understandable, the application will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0053] In one exemplary embodiment, such as Figure 1As shown, a collaborative operation control method for a three-armed robot with magnetic-contact-motion decoupling is provided, including the following steps: Wherein:

[0054] Step S1: Obtain the state parameters of the three-armed robot at the current moment and the desired satellite maintenance control command; the desired satellite maintenance control command is the desired Cartesian velocity of the end effector of the three-armed robot during the satellite maintenance operation. The desired Cartesian velocity includes linear velocity and angular velocity. Figure 2 This is a schematic diagram of a three-armed robot performing on-orbit maintenance on a satellite.

[0055] As an optional implementation, the state parameters of the three-armed robot at the current moment include: the joint angles of the three-armed robot at the current moment, the joint velocities of the three-armed robot at the current moment, the end-effector pose of the three-armed robot at the current moment, and the force sensor data of the end-effector of the three-armed robot at the current moment.

[0056] Step S2: Based on the state parameters of the three-armed robot at the current moment, establish a magnetic force model among the multiple magnets at the end effector of the three-armed robot. The magnetic force model among the multiple magnets at the end effector of the three-armed robot can accurately describe the mutual forces in the coupled magnetic field.

[0057] As an optional implementation method, step S2 specifically includes:

[0058] Step S21: Using a Bayesian optimization method based on Gaussian process, the magnetic moment amplitude of the magnetic dipoles corresponding to the magnets at the ends of the three robotic arms of the three-armed robot is calibrated to obtain the calibrated magnetic moment amplitude of each magnetic dipole.

[0059] Specifically, such as Figure 3 As shown, each of the three-armed robot's robotic arms is equipped with a magnet at its end for non-contact manipulation of repair tools. In the modeling process, each magnet is simplified to a magnetic dipole located at its geometric center. Assume the three magnetic dipoles are represented by i... b j b and k b This indicates that the center positions are pi. b ,pj b and PK b The corresponding magnetic moments are μib, μjb, and μkb. The magnetic field generated by each magnetic dipole in its surrounding space follows a simplified form of the Biot-Savart law, where the magnetic dipole j... b magnetic field B jb In magnetic dipole i b The location can be represented as:

[0060]

[0061] in, For magnetic dipole ib and magnetic dipole j b The position vector between them For magnetic dipole j b The magnetic moment amplitude, μ0 is the free permeability, the superscript ^ is the unit direction vector of the variable, and I is the identity matrix.

[0062] While the magnetic dipole model can theoretically describe magnetic interactions, in practical applications, the magnitude of the magnetic moment... and The magnetic moment amplitude can be affected by factors such as material properties and external protective layers, leading to discrepancies between model predictions and actual values. Therefore, a Bayesian optimization method based on Gaussian processes is employed to optimize the magnetic moment amplitude. and Calibration is performed. The calibration process involves initializing the magnetic moment amplitude. and The method constructs a Gaussian process surrogate model with the mean square error between the predicted values ​​of the magnetic force model and the measured values ​​of the triaxial magnetic sensor as the optimization objective, based on the Bayesian optimization framework. A new parameter combination that maximizes the reduction of the optimization objective entropy is generated using a data acquisition function. Simultaneously, the measured values ​​of the sensor are acquired and the current mean square error value is calculated. This parameter-error data pair is incrementally updated to the Gaussian process surrogate model. The next set of optimization parameters is generated iteratively by reconstructing the posterior probability distribution, forming a closed-loop optimization process of "parameter proposal → magnetic field excitation → data acquisition → model update." This process continues until the mean square error change rate is less than 0.1% for five consecutive iterations or the preset maximum number of iterations (200) is reached. Finally, the magnetic moment amplitude calibration result that minimizes the global mean square error (i.e., the calibrated magnetic moment amplitude of each magnetic dipole) is output, enabling the magnetic force model to accurately describe the mutual forces in the coupled magnetic field and improving the actual accuracy of the magnetic force model.

[0063] Step S22: Take any two magnetic dipoles as one magnetic dipole group to obtain three magnetic dipole groups.

[0064] Step S23: Based on the calibrated magnetic moment amplitude of the two magnetic dipoles in each magnetic dipole group and the position vector between the two magnetic dipoles, calculate the magnitude of the magnetic force of the interaction between the two magnetic dipoles in the corresponding magnetic dipole group.

[0065] Specifically, when a magnetic dipole is in a magnetic field generated by other magnetic dipoles, it will be affected by the magnetic field gradient and generate a force. The magnitude of this force is related to the spatial rate of change of the magnetic field gradient and the interaction of the magnetic moments. For example, when the magnetic dipole group includes magnetic dipole i... b and magnetic dipole j b At that time, according to equation (1), through Gradient transformation relationship, calculate magnetic dipole i b In magnetic dipole j bForce in the magnetic field for:

[0066]

[0067] Where T represents transpose.

[0068] Equation (2) shows that the magnitude of the magnetic force is directly proportional to the amplitude of the magnetic moment and inversely proportional to the fourth power of the distance, and the direction is determined by both the orientation of the magnetic dipole and its relative position. Based on equation (2), the magnetic dipole i can be calculated similarly. b In magnetic dipole k b Force in a magnetic field and magnetic dipole j b In magnetic dipole k b Force in a magnetic field According to Newton's third law, the forces between magnetic dipoles are equal in magnitude and opposite in direction.

[0069] Step S24: Based on the magnitude of the magnetic force of the interaction between two magnetic dipoles in each magnetic dipole group, establish a magnetic force model between the end magnets of the three-armed robot.

[0070] Specifically, based on the forces between magnetic dipoles and A magnetic force model f among the three magnets is established in matrix form. mag :

[0071]

[0072] Among them, f mag i b f mag ,j b and f mag ,k b They are magnetic dipoles i b / Magnetic dipole j b and magnetic dipole k b The magnitude of the magnetic force experienced.

[0073] Step S3: Using the sampling model predictive control method, based on the expected satellite maintenance control command, construct the objective function, three-arm cooperative equality constraints, and three-arm safety inequality constraints for the discrete multi-constraint trajectory optimization problem of the three-arm robot; wherein, the objective function aims to minimize the trajectory cost.

[0074] Specifically, the complexity of the three-arm cooperative control of a three-arm robot stems from its high-dimensional state space (the joint angles and velocities of the three 7-DOF robotic arms) and the non-continuous contact task in a dynamic environment. Therefore, based on the sampled model predictive control method, the objective function of the discrete multi-constraint trajectory optimization problem for the three-arm robot is expressed as:

[0075]

[0076] The corresponding constraints are:

[0077] stx t＝0 =x0 (5)

[0078] v t =u t +∈ t (6)

[0079] x t+1 =F(x) t ,v t (7)

[0080] c(t,x t )+B(t,x t )v t =0 (8)

[0081] h(t,x t )≥0 (9)

[0082] Where V is the sampled input v t The sequence (i.e., the joint velocity sequence of the robotic arms of a three-armed robot); v t Let V be the joint velocity of the robotic arm of the three-armed robot at time t (which is also the sampling input of the optimization problem); C(V) is the trajectory cost corresponding to the joint velocity sequence of the three-armed robot. For expectations; x0 represents the cost when the trajectory optimization of the three-armed robot terminates; x0 represents the initial state of the trajectory optimization of the three-armed robot. Joint states at the termination time for trajectory optimization of a three-armed robot; t is time; x t Joint states at time t for trajectory optimization of a three-armed robot; s(x t ) is x t The state cost function is given by λ, where λ is the inverse temperature coefficient. u is the transpose of the control input at time t; t For control input; ∈ t Gaussian noise at time t required for random optimization; F(x) t ,v t c(t,x) is the system dynamics expression; t B(t,x) is a state-related function in the equality constraints; t h(t,x) is a function related to the state and input in the equality constraints; t ) is the relevant function of the inequality constraints. Equality constraints strictly limit the task trajectory, while inequality constraints are transformed into the cost function s(x). tUnconstrained processing is performed in ).

[0083] As an optional implementation, step S3, the process of determining the three-arm cooperative equality constraint, specifically includes:

[0084] Step S311: Take any one of the three-armed robots as the reference robot arm, and determine the single-arm cooperative equality constraint of the reference robot arm based on the expected Cartesian velocity of the end effector of the reference robot arm, the joint velocity of the reference robot arm, and the Jacobian matrix of the end effector of the reference robot arm.

[0085] Specifically, the core of collaborative task constraints is to ensure the synchronization of pose changes of the three-arm end effectors. Based on single-arm task constraints, it is assumed that the robotic arm j... a For reference robotic arm, reference robotic arm j a We need to track a desired trajectory whose terminal desired Cartesian velocity is... The single-arm equality constraint is then:

[0086]

[0087] in, For the robotic arm j of the three-armed robot a The expected Cartesian velocity at the end; For the robotic arm j of the three-armed robot a The Jacobian matrix at the end of .

[0088] Step S312: Based on the expected Cartesian velocity of the end effector of the reference manipulator and the corresponding rotation matrix, determine the expected Cartesian velocities of the end effectors of the two manipulators other than the reference manipulator in the three-arm robot.

[0089] Specifically, for collaborative tasks, the robotic arm i a and robotic arm k a The desired Cartesian velocity at the end of the robotic arm needs to be determined based on the robotic arm's j. a The dynamic adjustment of the pose ensures that the end effector movements of the three robotic arms remain synchronized in the global coordinate system, i. a and robotic arm k a The desired speed is obtained by aligning the rotation matrix to obtain the robotic arm i. a and robotic arm k a The expected Cartesian velocities at the terminals are as follows:

[0090]

[0091] in, For the robotic arm i of a three-armed robot a The expected Cartesian velocity at the end; For robotic arm i a The inverse of the rotation matrix; For robotic arm j a The rotation matrix; For robotic arm k a The inverse of the rotation matrix; For the robotic arm k of the three-armed robot a The expected Cartesian velocity at the end of the .

[0092] Step S313: Based on the expected Cartesian velocities of the ends of the two robotic arms other than the reference robotic arm in the three-arm robot, the corresponding Jacobian matrices of the ends of the two robotic arms, and the single-arm cooperative equality constraints of the reference robotic arm, determine the three-arm cooperative equality constraints.

[0093] By constructing equality constraints in the objective function, it is ensured that the sampling trajectory always meets the requirements of the three-arm cooperative task during the optimization process. The cooperative task constraint is constructed as an equality constraint, establishing a three-arm cooperative equality constraint that strictly synchronizes the motion posture of the three-arm end effector with the desired control command.

[0094] As an optional implementation, in step S313, the expression for the three-arm cooperative equality constraint is:

[0095]

[0096] in, For the robotic arm i of a three-armed robot a The Jacobian matrix at the end; For the robotic arm k of the three-armed robot a The Jacobian matrix at the end of .

[0097] As an optional implementation, step S3, the process of determining the three-arm safety inequality constraint, specifically includes:

[0098] Step S321: Using analytical geometry, calculate the shortest distance between the arm frames of every two robotic arms of the three-armed robot.

[0099] Specifically, the safety constraints of the three-arm robot are constructed as inequality constraints. Based on the geometry of the robot's spatial skeleton, collision distances are estimated, and real-time safety constraints for the three-arm movement distances are established. In three-arm collaboration, the risk of collision between robotic arms is a major safety hazard. Traditional collision detection methods (such as the FCL library) are difficult to meet real-time requirements due to their high model complexity. Therefore, this application proposes a simplified collision detection method based on a geometric skeleton. The specific implementation includes the following steps:

[0100] 1) Geometric simplification: A schematic diagram of the geometric model of the three-armed robot's spatial skeleton is shown below. Figure 4 As shown, the end effector of the robotic arm is simplified to a cylinder, and the joints and magnets are simplified to spheres.

[0101] 2) Shortest distance calculation: The shortest distance between the two arm frames is calculated using analytical geometry. When the two links are parallel, the endpoint-segment distance model is used to calculate the shortest distance between the two arm frames; when the two links have an angle, the nearest point is determined by orthogonal projection, and then the shortest distance between the two arm frames is calculated.

[0102] Step S322: Set a preset safety threshold. Based on the shortest distance between the arm skeletons of every two robotic arms of the three-arm robot and the preset safety threshold, determine the three-arm safety inequality constraint.

[0103] Specifically, if the shortest distance is less than the preset safety threshold γ safe Therefore, a penalty term is introduced into the optimization objective to drive the robotic arm to adjust its motion to avoid collisions. Finally, the expression for the three-arm safety inequality constraint is shown in equation (14):

[0104]

[0105] in, For the robotic arm i of a three-armed robot a and robotic arm j a The shortest distance between the arm skeletons; For the robotic arm i of a three-armed robot a and robotic arm k a The shortest distance between the arm skeletons; For the robotic arm j of the three-armed robot a and robotic arm k a The shortest distance between the arm skeletons; γ safe The preset safety threshold; h safe (t,x t ) is a function relating to the three-arm safety inequality constraint.

[0106] Step S4: Based on the magnetic force model between the multiple magnets at the end of the three-armed robot, determine the non-contact magnetic force constraints for the discrete multi-constraint trajectory optimization problem of the three-armed robot.

[0107] As an optional implementation, step S4 specifically includes:

[0108] Step S41: Based on the magnetic force model between the multiple magnets at the end of the three-arm robot, calculate the total magnetic force on the magnets at the end of each robotic arm of the three-arm robot.

[0109] The total magnetic force on the magnets at the ends of each robotic arm is as follows:

[0110]

[0111] in, For the magnet i at the end of the robotic armc The total magnetic force experienced; For magnet i c In magnet j c The force in the magnetic field; For magnet i c In magnet k c The force in the magnetic field; For the magnet j at the end of the robotic arm c The total magnetic force experienced; For magnet j c In magnet k c The force in the magnetic field; For the magnet k at the end of the robotic arm c The total magnetic force experienced.

[0112] Step S42: Set a preset magnetic force threshold, and determine the non-contact magnetic force constraints for the discrete multi-constraint trajectory optimization problem of the three-arm robot based on the total magnetic force on the magnets at the ends of each robotic arm of the three-arm robot and the preset magnetic force threshold.

[0113] Specifically, the goal of non-contact magnetic confinement is to limit the amplitude of the magnetic force on the magnets at the ends of the three arms, preventing collisions or loss of control due to excessive magnetic force. Unlike traditional force feedback control, magnetic confinement does not rely on sensor data but instead uses model prediction to proactively avoid risks, thus enhancing the robustness of the system.

[0114] By comparing the amplitude of the total magnetic force on the magnets at the ends of each robotic arm with a preset magnetic force threshold γ mag By comparison, a non-contact magnetic constraint is constructed to limit the magnetic force amplitude, resulting in the following expression for the non-contact magnetic constraint:

[0115]

[0116] Where, γ mag The preset magnetic force threshold; h mag (t,x t ) is a function of the non-contact magnetic inequality constraint.

[0117] Step S5: Based on the magnetic force model between the multiple magnets at the end of the three-armed robot and the state parameters of the three-armed robot at the current moment, determine the correction value of the expected Cartesian velocity of the end of the three-armed robot to realize the contact compliance constraint of the discrete multi-constraint trajectory optimization problem of the three-armed robot.

[0118] Specifically, the goal of contact compliance constraints is to ensure that, in contact scenarios (such as a tool contacting a satellite surface), the end effector can perform delicate operations while applying a constant contact force. For example, a robotic arm... a For example, the actual value of the contact force is obtained by measuring with a force sensor and subtracting interference from magnetic and gravitational forces. Calculate the actual value of the contact force Expected value of contact constant force The deviation is then corrected by velocity mapping of the robotic arm i. a The desired Cartesian velocity at the end of the robotic arm is adjusted. a The motion posture during the end contact process is adjusted so that the actual contact force is as close as possible to the desired constant contact force, thereby achieving contact compliance control.

[0119] As an optional implementation, the expression for the correction value of the desired Cartesian velocity at the end effector of the three-armed robot is:

[0120]

[0121] in, For the robotic arm i of a three-armed robot a The correction value for the expected Cartesian velocity at the end; For the robotic arm i of a three-armed robot a Scale factor; For the robotic arm i of a three-armed robot a The inverse of the rotation matrix; For the robotic arm i of a three-armed robot a The actual value of the contact force; Let be the expected value of the contact constant force of the robotic arm of the three-armed robot; For the robotic arm j of the three-armed robot a The correction value for the expected Cartesian velocity at the end; For the robotic arm j of the three-armed robot a Scale factor; For the robotic arm j of the three-armed robot a The inverse of the rotation matrix; For the robotic arm j of the three-armed robot a The actual value of the contact force; for the robotic arm k of the three-armed robot. a The correction value for the expected Cartesian velocity at the end; For the robotic arm k of the three-armed robot a Scale factor; For the robotic arm k of the three-armed robot a The inverse of the rotation matrix; For the robotic arm k of the three-armed robot a The actual value of the contact force. The contact force error is converted into a speed correction amount through a scaling factor.

[0122] Step S6: Update the three-arm cooperative equation constraint using the correction value of the expected Cartesian velocity of the end effector of the three-arm robot to obtain the updated three-arm cooperative equation constraint.

[0123] use and Update step three arm coordination equation constraints and Dynamic decoupling of force and motion was achieved during the three-arm coordinated control process.

[0124] Step S7: Based on the objective function of the discrete multi-constraint trajectory optimization problem of the three-armed robot, the three-armed safety inequality constraint, the non-contact magnetic constraint, and the updated three-armed cooperative equality constraint, a discrete multi-constraint trajectory optimization model of the three-armed robot is constructed.

[0125] Step S8: Solve the discrete multi-constraint trajectory optimization model of the three-armed robot to determine the optimized motion trajectory of the three-armed robot.

[0126] The optimized motion trajectory is a sequence of robot joint velocities.

[0127] Step S9: Determine the collaborative motion control commands for the three-armed robot based on the optimized motion trajectory of the three-armed robot.

[0128] Specifically, based on the optimized motion trajectory, firstly, dynamic boundary constraints are applied to the expected joint velocities to be executed in the next control cycle according to the physical limits of the robotic arm joints (maximum rotation angle, velocity threshold), and joint velocity limiting processing is performed on the optimized motion trajectory. Specifically, by comparing the expected value with the limit thresholds of each joint, if the expected value exceeds the allowable range, it is limited to the corresponding boundary value to prevent mechanical damage, drive overload, or loss of control that may occur due to excessive commands.

[0129] Subsequently, the verified target joint velocities are used as control commands, and a smooth and continuous trajectory curve is generated using a cubic spline interpolation algorithm. The joint velocities at the next moment in the motion trajectory are used as the cooperative motion commands for the three-armed robot, and spline interpolation is performed and the commands are sent to the robot actuator at a frequency of 1000 Hz. Specifically, by constructing cubic polynomial curve segments between each adjacent discrete target position point, a smooth and continuously differentiable joint velocity trajectory curve that runs through all target position points is generated, thereby eliminating step abrupt changes and ensuring smooth and coherent robot motion.

[0130] Finally, the interpolated joint space trajectory curve is transmitted to the robotic arm's underlying driver in real time at a high frequency of 1000Hz to ensure millisecond-level response and sub-millimeter-level accuracy of motion commands, enabling highly dynamic collaborative operation in the scenario of on-orbit maintenance of robotic satellites.

[0131] It should be noted that the user information (including but not limited to user device information, user personal information, etc.) and data (including but not limited to data used for analysis, data stored, data displayed, etc.) involved in this application are all information and data authorized by the user or fully authorized by all parties, and the collection, use and processing of the relevant data must comply with relevant regulations.

[0132] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium, and when executed, it can include the processes of the embodiments of the above methods. Any references to memory, databases, or other media used in the embodiments provided in this application can include at least one of non-volatile and volatile memory. Non-volatile memory can include read-only memory (ROM), magnetic tape, floppy disk, flash memory, optical memory, high-density embedded non-volatile memory, resistive random access memory (ReRAM), magnetic random access memory (MRAM), ferroelectric random access memory (FRAM), phase change memory (PCM), graphene memory, etc. Volatile memory can include random access memory (RAM) or external cache memory, etc. By way of illustration and not limitation, RAM can take many forms, such as Static Random Access Memory (SRAM) or Dynamic Random Access Memory (DRAM).

[0133] The databases involved in the embodiments provided in this application may include at least one type of relational database and non-relational database. Non-relational databases may include, but are not limited to, blockchain-based distributed databases. The processors involved in the embodiments provided in this application may be general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic devices, quantum computing-based data processing logic devices, etc., and are not limited to these.

[0134] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0135] This document uses specific examples to illustrate the principles and implementation methods of this application. The descriptions of the above embodiments are only for the purpose of helping to understand the methods and core ideas of this application. Furthermore, those skilled in the art will recognize that, based on the ideas of this application, there will be changes in the specific implementation methods and application scope. Therefore, the content of this specification should not be construed as a limitation of this application.

Claims

1. A method for cooperative operation control of a three-armed robot with magnetic-contact-motion decoupling, characterized in that, The magnetic-contact-motion decoupling method for cooperative operation control of a three-armed robot includes: Obtain the state parameters of the three-armed robot at the current moment and the expected satellite maintenance control command; the expected satellite maintenance control command is the expected Cartesian velocity at the end of the three-armed robot during the satellite maintenance operation. Based on the state parameters of the three-armed robot at the current moment, a magnetic force model among the multiple magnets at the end of the three-armed robot is established. A sampling model predictive control method is adopted, based on the expected satellite maintenance control command, to construct the objective function, three-arm cooperative equality constraint, and three-arm safety inequality constraint for the discrete multi-constraint trajectory optimization problem of a three-arm robot; wherein, the objective function aims to minimize the trajectory cost; Based on the magnetic force model between multiple magnets at the end of a three-armed robot, the non-contact magnetic force constraints for the discrete multi-constraint trajectory optimization problem of the three-armed robot are determined. Based on the magnetic force model between the multiple magnets at the end of the three-armed robot and the state parameters of the three-armed robot at the current moment, the correction value of the expected Cartesian velocity of the end of the three-armed robot is determined so as to realize the contact compliance constraint of the discrete multi-constraint trajectory optimization problem of the three-armed robot. The three-arm cooperative equation constraint is updated by using the correction value of the expected Cartesian velocity of the end effector of the three-arm robot, resulting in the updated three-arm cooperative equation constraint. Based on the objective function of the discrete multi-constraint trajectory optimization problem of the three-arm robot, the three-arm safety inequality constraint, the non-contact magnetic constraint, and the updated three-arm cooperative equality constraint, a discrete multi-constraint trajectory optimization model of the three-arm robot is constructed. Solve the discrete multi-constraint trajectory optimization model of the three-armed robot to determine the optimized motion trajectory of the three-armed robot; Based on the optimized motion trajectory of the three-armed robot, determine the collaborative operation motion control commands for the three-armed robot.

2. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 1, characterized in that, The state parameters of the three-armed robot at the current moment include: the joint angles of the three-armed robot at the current moment, the joint velocities of the three-armed robot at the current moment, the end-effector pose of the three-armed robot at the current moment, and the force sensor data of the end-effector of the three-armed robot at the current moment.

3. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 2, characterized in that, Based on the current state of the three-armed robot, a magnetic force model is established among the multiple magnets at the end effector of the robot, specifically including: A Bayesian optimization method based on Gaussian process was used to calibrate the magnetic moment amplitude of the magnetic dipoles corresponding to the magnets at the ends of the three robotic arms of the three-arm robot, and the calibrated magnetic moment amplitude of each magnetic dipole was obtained. By taking any two magnetic dipoles as one magnetic dipole group, we get three magnetic dipole groups; Based on the calibrated magnetic moment amplitudes of the two magnetic dipoles in each magnetic dipole group and the position vector between the two magnetic dipoles, the magnitude of the magnetic force of the interaction between the two magnetic dipoles in the corresponding magnetic dipole group is calculated. Based on the magnitude of the magnetic force of the interaction between two magnetic dipoles in each magnetic dipole group, a magnetic force model among the multiple magnets at the end of the three-arm robot is established.

4. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 3, characterized in that, The objective function of the discrete multi-constraint trajectory optimization problem for a three-armed robot is expressed as follows: Where V is the joint velocity sequence of the robotic arm of the three-armed robot; C(V) is the trajectory cost corresponding to the joint velocity sequence of the three-armed robot. For expectations; The cost of optimizing the termination state of the trajectory of a three-armed robot; Joint states at the termination time for trajectory optimization of a three-armed robot; t is time; x t Joint states at time t for trajectory optimization of a three-armed robot; s(x t ) is x t The state cost function is given by λ, where λ is the inverse temperature coefficient. The transpose of the control input at time t; ∈ t Gaussian noise at time t is required for random optimization.

5. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 4, characterized in that, The process of determining the three-arm cooperative equality constraints specifically includes: Taking any one of the three-armed robots as the reference robot, the single-arm cooperative equality constraint of the reference robot is determined based on the expected Cartesian velocity at the end of the reference robot, the joint velocity of the reference robot, and the Jacobian matrix at the end of the reference robot. Based on the expected Cartesian velocity of the end effector of the reference manipulator and the corresponding rotation matrix, the expected Cartesian velocities of the end effectors of the two manipulators other than the reference manipulator in the three-arm robot are determined. Based on the expected Cartesian velocities of the two ends of the three-arm robot (excluding the reference arm), the corresponding Jacobian matrices of the ends of the two arms, and the single-arm cooperative equality constraints of the reference arm, the three-arm cooperative equality constraints are determined.

6. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 5, characterized in that, The expression for the three-arm coordination equality constraint is: in, For the robotic arm i of a three-armed robot a The expected Cartesian velocity at the end; For the robotic arm j of the three-armed robot a The expected Cartesian velocity at the end; For the robotic arm k of the three-armed robot a The expected Cartesian velocity at the end; For the robotic arm i of a three-armed robot a The Jacobian matrix at the end; For the robotic arm j of the three-armed robot a The Jacobian matrix at the end; For the robotic arm k of the three-armed robot a The Jacobian matrix at the end; v t Let t be the joint velocity of the robotic arm of the three-armed robot at time t.

7. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 6, characterized in that, The process of determining the three-arm safety inequality constraint specifically includes: Using analytical geometry, the shortest distance between the arm frames of every two robotic arms of the three-arm robot is calculated. A preset safety threshold is set, and the three-arm safety inequality constraint is determined based on the shortest distance between the arm skeletons of every two robotic arms of the three-arm robot and the preset safety threshold.

8. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 7, characterized in that, The expression for the three-arm safety inequality constraint is: in, For the robotic arm i of a three-armed robot a and robotic arm j a The shortest distance between the arm skeletons; For the robotic arm i of a three-armed robot a and robotic arm k a The shortest distance between the arm skeletons; For the robotic arm j of the three-armed robot a and robotic arm k a The shortest distance between the arm skeletons; γ safe The preset safety threshold; h safe (t,x t ) is a function relating to the three-arm safety inequality constraint.

9. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 8, characterized in that, Based on the magnetic force model among the multiple magnets at the end effector of a three-armed robot, the non-contact magnetic constraints for the discrete multi-constraint trajectory optimization problem of the three-armed robot are determined, specifically including: Based on the magnetic force model among the multiple magnets at the end of a three-arm robot, the total magnetic force on the magnets at the end of each robotic arm of the three-arm robot is calculated. A preset magnetic force threshold is set, and the non-contact magnetic force constraints for the discrete multi-constraint trajectory optimization problem of the three-arm robot are determined based on the total magnetic force on the magnets at the ends of each robotic arm of the three-arm robot and the preset magnetic force threshold.

10. The three-arm robot cooperative operation control method with magnetic-contact-motion decoupling according to claim 9, characterized in that, The expression for the correction value of the desired Cartesian velocity at the end effector of the three-armed robot is: in, For the robotic arm i of a three-armed robot a The correction value for the expected Cartesian velocity at the end; For the robotic arm i of a three-armed robot a Scale factor; For the robotic arm i of a three-armed robot a The inverse of the rotation matrix; For the robotic arm i of a three-armed robot a The actual value of the contact force; Let be the expected value of the contact constant force of the robotic arm of the three-armed robot; For the robotic arm j of the three-armed robot a The correction value for the expected Cartesian velocity at the end; For the robotic arm j of the three-armed robot a Scale factor; For the robotic arm j of the three-armed robot a The inverse of the rotation matrix; For the robotic arm j of the three-armed robot a The actual value of the contact force; For the robotic arm k of the three-armed robot a The correction value for the expected Cartesian velocity at the end; For the robotic arm k of the three-armed robot a Scale factor; For the robotic arm k of the three-armed robot a The inverse of the rotation matrix; For the robotic arm k of the three-armed robot a The actual value of the contact force.

Images