A shock-resistant tracking control method and system for dual-arm robots based on quadratic programming

Abstract

This invention discloses a shock-resistant tracking control method and system for a dual-arm robot based on quadratic programming, applied in the field of robot control technology, aiming to solve the problem of asynchronous impact caused by environmental uncertainty. Specifically, the technical solution of this invention addresses the asynchronous impact problem by introducing a three-stage division, i.e., control mode, consisting of pre-impact, transition, and post-impact phases. For the transition mode, a reference extension concept is introduced. The QP controller employs a superposition mechanism of feedforward and feedback control. First, the impedance and attitude tasks at both the pre-impact and post-impact times are reference-extended to obtain impedance and attitude feedforward information. Then, the impedance and attitude feedforward information is used to adaptively adjust the torque signal in the impedance task and the joint signal in the attitude task at the current moment. Finally, the QP controller uses the adjusted torque and joint signals to employ a feedback control mechanism to control the robotic arm, effectively smoothing error peaks and controlling input jumps.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, and in particular relates to a shock-resistant tracking control method and system for a dual-arm robot based on quadratic programming. Background Technology

[0002] With the rapid development of robotics technology, dual-arm robots are increasingly used in industrial manufacturing, medical surgery, and aerospace, especially when performing high-precision, high-dynamic impact tasks, which places higher demands on their tracking and control performance. However, dual-arm robots face many challenges when performing impact tasks, including asynchronous impacts caused by environmental uncertainties, peak velocity errors, and abrupt jumps in control inputs. These problems not only affect the accuracy and stability of task execution but may also cause potential damage to the robot hardware. Therefore, designing a robust impact-resistant control method for the synchronous control problem of dual-arm robots in complex environments has significant research value and practical significance.

[0003] In existing technologies, control methods for dual-arm robots mainly focus on strategies such as position control, force control, or force / position hybrid control. For example, impedance-based control methods can cope with dynamic changes in the external environment to some extent. However, environmental uncertainties (such as changes in the rigidity of the target object, external disturbances, etc.) can lead to asynchrony during impacts on the dual-arm robot, resulting in peak velocity errors and abrupt changes in control input, which seriously affects system performance. This is especially true for dual-arm robots installed at an angle, where their applicability is further limited, and they also lack effective compensation for coordinate system deviations and reference trajectory offsets caused by the installation angle. Regarding impact-resistant control, existing methods often struggle to simultaneously achieve both trajectory tracking accuracy and robustness to environmental uncertainties. Summary of the Invention

[0004] This invention aims to solve the problem of asynchronous impact caused by environmental uncertainty, and thus provides a shock-resistant tracking control method and system for a dual-arm robot based on quadratic programming. Specifically, the technical solution of this invention addresses the asynchronous impact problem by introducing a three-stage division, namely the control mode, consisting of pre-impact, transition, and post-impact phases. For the transition mode, a reference extension concept is introduced to control the pre-impact time... and the time after the impact The impedance and attitude tasks are referenced and extended, and the QP controller is further extended. Adaptive adjustment of the torque and attitude joint signals of the robotic arm is adopted to effectively smooth error peaks and control input jumps.

[0005] Therefore, the present invention provides the following technical solution:

[0006] On one hand, the present invention provides a shock-resistant tracking control method for a dual-arm robot based on quadratic programming, comprising the following steps:

[0007] Step 1: Use external sensors to monitor impact. If an impact is detected, extract the time of impact. And based on the defined pre-impact time and time after impact This allows for the determination of the collision transition phase and the post-impact phase, while other periods of impact monitoring are considered the pre-impact phase.

[0008] Step 2: Based on the hierarchical control mode, tracking control is performed according to the current stage type. That is, in the pre-impact stage and post-impact stage, the QP controller uses the torque signal in the impedance task and the joint signal in the attitude task to adopt a feedback control mechanism to control the robotic arm; in the collision transition stage, the QP controller adopts a superposition mechanism of feedforward control and feedback control to control the robotic arm.

[0009] The superposition mechanism of feedforward control and feedback control is as follows: first, the time before the impact is calculated... and the time after the impact Both the impedance task and the attitude task are referenced and extended to obtain impedance and attitude feedforward information. Then, the impedance and attitude feedforward information is used to adaptively adjust the torque signal in the impedance task and the joint signal in the attitude task at the current moment. Then, the QP controller uses the adjusted torque signal and joint signal to adopt a feedback control mechanism to control the robotic arm.

[0010] Optionally, during the collision transition phase, when the torque signal in the impedance task and the joint signal in the attitude task are adaptively adjusted using the impedance attitude feedforward information at the current moment, the corresponding mathematical model is as follows:

[0011]

[0012]

[0013] In the formula, Let be the desired torque at the current time t during the collision transition phase and for robotic arm i. Here are mixed parameters, where t is the current time and i is the marker of the robotic arm; , For robotic arm i, the time before impact is respectively and time after impact In the impedance task, refer to the extended torque. For damping gain, ; It is the task space equivalent inertia matrix corresponding to robotic arm i, which can be obtained in real time from the robotic arm. For impedance stiffness, a custom parameter, such as 350; For robotic arm i, post-impact time The corresponding extended reference end effector speed (including linear velocity and angular velocity); Let i be the position of the end effector of the robotic arm at the current time t. Let i be the end effector speed of robotic arm i at the current time t. , For robotic arm i, the time before impact is respectively and time after impact Corresponding to the extended end effector position, This is the transpose of the actual attitude matrix of the end effector, where T is the matrix transpose symbol. Let be the desired attitude matrix of the end effector. The operator for converting a partially symmetric matrix into a vector;

[0014] Let be the joint acceleration at time t during the collision transition phase and for robotic arm i. For robotic arm i, the time before impact Corresponding to the extended joint acceleration, such as by taking The actual value of joint acceleration at time t is expanded. For robotic arm i, post-impact time The corresponding reference is the extended joint acceleration, such as that based on prior simulation. The joint acceleration at time t is extended to obtain, Define custom parameters for stiffness during attitude control. For the joint angles of robotic arm i The first derivative, for The first derivative; For robotic arm i, the time before impact Corresponding to the extended joint angles; For robotic arm i, post-impact time The corresponding reference is the expanded joint angle.

[0015] Optionally, parameters The calculation is as follows:

[0016]

[0017]

[0018] In the formula, Let be the attitude matrix at the current time t. for The pose matrix at time step, The pose matrix at time step, , for time, angular velocity at time t; For the moment of impact The previous pose matrix is ​​collectively referred to as, For the moment of impact The attitude matrix is ​​collectively referred to as the final pose matrix. It is angular velocity, take The last three, It is the actual velocity at the end.

[0019] Optionally, the QP controller uses torque signals and joint signals to adopt a feedback control mechanism to control the robotic arm. The process is as follows: first, the impedance task error of the impedance task and the attitude error of the attitude task are calculated using torque signals and joint signals, respectively.

[0020] Then, the joint acceleration of the robotic arm is optimized by minimizing the set cost function, and finally the torque of each joint is generated. This is for the robot motor to perform control, and the cost function is an error weighted sum of the impedance task and the attitude task;

[0021] In the collision transition phase, the impedance task error and the attitude error are expressed as follows:

[0022]

[0023] =

[0024] In the formula, For impedance task error, For attitude error, Let i be the acceleration of the end effector corresponding to robotic arm i. Let be the second derivative of the joint angle corresponding to robot arm i.

[0025] Optionally, the reference extension for the end effector position adopts a linear extension, while the reference extension for the end effector speed is constant, specifically:

[0026]

[0027] In the formula, For robotic arm i, the time before impact The corresponding actual position of the end effector; For robotic arm i, post-impact time The corresponding actual position of the end effector; ( ), ( For robotic arm i, the time before impact and time after impact The corresponding actual linear velocity of the end effector;

[0028] , ], ]

[0029] In the formula, , , For robotic arm i, the time before impact is respectively and time after impact The corresponding actual linear velocity and actual angular velocity of the end effector; For robotic arm i, the time before impact The corresponding reference is the extended end effector speed.

[0030] Optionally, define an exclusion interval. This made the time before the impact Time after impact .

[0031] Optionally, the dual-arm robot is a tilted dual-arm robot, wherein the coordinate transformation from the remote control actuator to the robotic arm coordinate system is realized by constructing a mapping relationship between the coordinate system of the remote control handle and the coordinate system of the robotic arm.

[0032] The mapping between the remote control handle coordinate system and the robotic arm coordinate system is as follows:

[0033]

[0034] Furthermore, the rotation compensation of the robot arm's coordinate system relative to the world coordinate system around the y-axis and z-axis during tilted installation is as follows:

[0035]

[0036] In the formula, This is the pose matrix of the end effector in the robot arm coordinate system. This is the initial attitude adjustment matrix. The pose matrix in the coordinate system of the remote control handle. and This indicates rotation about the y-axis and rotation about the z-axis; and This represents the rotation angle around the y-axis and the rotation angle around the z-axis.

[0037] Optionally, before step 1, execute the following:

[0038] Using a remote control handle, the dual-arm robot is controlled to conduct impact tests under the current working conditions or various working conditions, and the time after each impact is recorded. The torque attitude data is used in step 2 to determine the time after impact. The impedance and attitude tasks are referenced and extended.

[0039] In addition, the present invention also provides a robot operating system based on the above method, which includes a remote operation handle, a handle controller, a dual-arm robot, and a robotic arm controller mounted on the dual-arm robot.

[0040] The remote control handle is communicatively connected to the handle controller, and the handle controller is communicatively connected to the robotic arm controller. The robotic arm controller is used to control the robotic arm on the dual-arm robot.

[0041] The handle controller or the robotic arm controller is equipped with a processor and a memory.

[0042] The processor invokes a computer program stored in the memory to implement the steps of the quadratic programming method for shock-resistant tracking control of a dual-arm robot.

[0043] For example, in some implementations, the robot operating system's controller is a host computer used for VR controller coordinate system transformation and position and posture data acquisition and transmission, while the robot arm controller is an industrial computer. The overall communication is based on the LCM framework, with TCP communication at the bottom layer. Specifically, the host computer starts the VR controller data transmission code, and the industrial computer starts the robot arm controller code, waiting for controller data input. After startup, the operator holds two remote control controllers on the left and right sides to control the left and right robot arms respectively.

[0044] The present invention also provides a control system based on the above method, comprising:

[0045] The impact detection module is used to monitor impacts using external sensors. If an impact is detected, the impact time is extracted. And based on the defined pre-impact time and time after impact This allows for the determination of the collision transition phase and the post-impact phase, while other periods of impact monitoring are considered the pre-impact phase.

[0046] The control module is used to perform tracking control based on the hierarchical control mode and the current stage type. Specifically, in the pre-impact and post-impact stages, the QP controller uses the torque signal in the impedance task and the joint signal in the attitude task to adopt a feedback control mechanism to control the robotic arm. In the collision transition stage, the QP controller adopts a superposition mechanism of feedforward control and feedback control to control the robotic arm.

[0047] The superposition mechanism of feedforward control and feedback control is as follows: first, the time before the impact is calculated... and the time after the impact Both the impedance task and the attitude task are referenced and extended to obtain impedance and attitude feedforward information. Then, the impedance and attitude feedforward information is used to adaptively adjust the torque signal in the impedance task and the joint signal in the attitude task at the current moment. Then, the QP controller uses the adjusted torque signal and joint signal to adopt a feedback control mechanism to control the robotic arm.

[0048] The present invention also provides a computer-readable storage medium storing a computer program, which is invoked by a processor to implement the steps of a quadratic programming-based shock-resistant tracking control method for a dual-arm robot.

[0049] Beneficial effects

[0050] The technical solution of this invention solves the tracking and control challenges of dual-arm robots in performing impact tasks, especially the asynchronous impact problem caused by environmental uncertainties. The method exhibits strong robustness and superior synchronous impact control performance on the dual-arm robot platform, which is significantly better than the traditional baseline method.

[0051] To address asynchronous shocks caused by environmental uncertainties, this method introduces pre-shock, transition, and post-shock control modes. A quadratic programming framework is used to define the control input, and in the transition mode, hybrid parameters are adaptively adjusted to adjust the position feedback and feedforward signals to smoothly avoid error peaks and input step sizes. Furthermore, the method's reference extension procedure can alleviate velocity error peaks and control input jumps to some extent. By extending the signal reference, overlapping references occur before and after the shock, further ensuring velocity and position consistency. More specifically, linear extension is preferred for the end effector position reference, while constant extension is used for the end effector velocity reference.

[0052] Furthermore, the dual-arm robot of the present invention is not only applicable to vertically installed dual-arm robots, but also to tilted dual-arm robots. For tilted dual-arm robots, the present invention proposes to complete the coordinate transformation from the remote operating actuator to the robot coordinate system by constructing a remote operating handle coordinate system and a robot arm coordinate system. Combined with the tilted installation posture compensation model, the reference trajectory can be adjusted in real time to adapt to the installation angle and optimize the initial consistency of position and posture. Attached Figure Description

[0053] Figure 1 This is a control block diagram of a normal QP controller used in the pre-impact and post-impact stages provided by an embodiment of the present invention.

[0054] Figure 2 This is a control block diagram of the collision transition stage provided in this embodiment of the invention, using an extended QP controller;

[0055] Figures 3-5These are simulation diagrams showing the torque versus time of the right arm joint under the condition of applied external impact force at t=1s. The horizontal axis represents time in seconds, and the vertical axis represents torque in Nm. Figure 3 There is no reference extension mechanism; the simulation results are used under the full-state feedback impedance control strategy before the impact. Figure 4 These are simulation results after introducing the reference extension mechanism. Figure 5 yes Figure 4 Simulation results of joints 3 and 4 before and after the impact.

[0056] Figure 6 This is the overall flowchart of the impact-resistant tracking control of a dual-arm robot based on quadratic programming, according to the present invention.

[0057] Figure 7 This is a schematic diagram illustrating a task scenario in which the method of the present invention is applied to the collaborative handling of heavy objects by a dual-arm robot. Detailed Implementation

[0058] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. The technical features involved in the various embodiments of the invention described below can be combined with each other as long as they do not conflict with each other.

[0059] It should be noted that although functional modules are divided in the device schematic diagram and a logical order is shown in the flowchart, in some cases, the steps shown or described may be performed in a different order than the module division in the device or the order in the flowchart. The terms "first," "second," etc., in the specification, claims, and the aforementioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence.

[0060] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs. The terminology used herein is for the purpose of describing embodiments of this application only and is not intended to limit this application.

[0061] To enable those skilled in the art to better understand the technical solution of the present invention, the present invention will be further described in detail below with reference to the accompanying drawings.

[0062] This invention presents a shock-resistant tracking control method for dual-arm robots based on quadratic programming. The core innovation of this method lies in: generating and extending a reference trajectory through a remote operation framework to ensure signal continuity before and after impact; introducing a switchable control mode and combining a quadratic programming framework with a hybrid parameter adaptive strategy to achieve robust compensation for uncertainties. This method significantly improves the performance of synchronous impact control. Experimental verification shows that it outperforms traditional baseline methods (such as standard impedance control or LQR control) on dual-arm robot platforms, especially in reducing torque peaks and vibrations. The quadratic programming (QP) framework is an optimization algorithm used to solve linearly constrained problems under a quadratic objective function. In this invention, the QP framework is innovatively applied to the control input formulation of dual-arm robots. Specifically, QP calculates joint acceleration by minimizing a weighted cost function of multiple tasks (such as impedance and attitude tasks), thereby generating joint torque. The advantage of this framework lies in its ability to handle redundant degrees of freedom and constraints (such as joint limits) in real time, ensuring the smoothness and robustness of control inputs. The innovation of this invention lies in combining QP with reference extension procedures and hybrid adaptive tuning to optimize mode switching for impact scenarios, avoiding the sensitivity to environmental uncertainties inherent in traditional methods.

[0063] The technical concept of the present invention is as follows:

[0064] Step 1: Use external sensors to monitor impact. If an impact is detected, extract the time of impact. And based on the defined pre-impact time and time after impact This allows for the determination of the collision transition phase and the post-impact phase, while other periods of impact monitoring are considered the pre-impact phase.

[0065] Step 2: Based on the hierarchical control mode, tracking control is performed according to the current stage type. That is, in the pre-impact stage and post-impact stage, the QP controller uses the torque signal in the impedance task and the joint signal in the attitude task to adopt a feedback control mechanism to control the robotic arm; in the collision transition stage, the QP controller adopts a superposition mechanism of feedforward control and feedback control to control the robotic arm.

[0066] The superposition mechanism of feedforward control and feedback control is as follows: first, the time before the impact is... and the time after the impact Both the impedance task and the attitude task are referenced and extended to obtain impedance and attitude feedforward information. Then, the impedance and attitude feedforward information is used to adaptively adjust the torque signal in the impedance task and the joint signal in the attitude task at the current moment. Then, the QP controller uses the adjusted torque signal and joint signal to adopt a feedback control mechanism to control the robotic arm.

[0067] The present invention will be described in detail below with reference to specific embodiments. The embodiments described below take a tilted dual-arm robot as an example. It should be understood that in other feasible embodiments, a vertically mounted dual-arm robot is also applicable to the technical solution of the present invention and falls within the protection scope of the present invention.

[0068] Example 1

[0069] This embodiment targets a tilted dual-arm robot. It achieves coordinate transformation from the remote control actuator to the robotic arm coordinate system by establishing a mapping relationship between the remote control handle coordinate system and the robotic arm coordinate system. It should be understood that coordinate transformation is an existing coordinate transformation theory. By applying it to the tilted installation scenario, this invention, combined with a tilted installation posture compensation model, can adjust the reference trajectory in real time to adapt to the installation angle, thereby optimizing the consistency of the initial position and posture. Therefore, the mapping between the remote control handle coordinate system and the robotic arm coordinate system is as follows:

[0070]

[0071] This mapping allows the position and orientation matrix input from the handle to be directly used in the robotic arm coordinate system after transformation.

[0072] It should be understood that the robot arm's coordinate system, during tilted installation, undergoes attitude compensation relative to the world coordinate system around the y-axis and z-axis. The compensation is as follows:

[0073]

[0074] In the formula, This is the pose matrix of the end effector in the robot arm coordinate system. This is the initial attitude adjustment matrix. The pose matrix in the coordinate system of the remote control handle. and This indicates rotation about the y-axis and rotation about the z-axis; and This represents the rotation angle around the y-axis and the rotation angle around the z-axis. In other words, attitude compensation is achieved through rotation around the y-axis and rotation around the z-axis.

[0075] Under this mapping relationship, if a translation transformation occurs, the translation increment compensation of the robot arm coordinate system relative to the world coordinate system is:

[0076]

[0077] in, For Jacobian matrices, For joint increments, This is the increment of the pose matrix.

[0078] In summary, for a tilted dual-arm robot, it is necessary to establish a mapping from the handle to the coordinate system of the tilted robotic arm, so that the position and attitude of the handle correspond to the position and attitude of the robotic arm end effector.

[0079] This embodiment provides a shock-resistant tracking control method for a dual-arm robot based on quadratic programming, comprising the following steps:

[0080] S1, using a remote control via a handle, controls the dual-arm robot to conduct impact tests under the current working conditions or various working conditions, recording the time after each impact. Torque attitude data.

[0081] This invention divides the collision monitoring cycle into three stages: the pre-impact stage, the collision transition stage, and the post-impact stage. The period before collision detection is in the pre-impact mode, and the period after the transition mode is in the post-impact mode. To address issues such as asynchronous impacts, peak velocity errors, and control input jumps, this embodiment optimizes the control process of the collision transition stage by introducing a reference extension concept, i.e., by adjusting the pre-impact time... and the time after the impact The impedance and attitude tasks are used as reference extensions as impedance and attitude feedforward information, which are then used to adjust the torque signal in the impedance task and the joint signal in the attitude task at the current moment. Finally, the QP controller uses the adjusted torque signal and joint signal to adopt a feedback control mechanism to control the robotic arm.

[0082] However, the time after the impact Torque and attitude data cannot be directly acquired during the collision transition phase. Therefore, in this embodiment, it is preferable to first use a QP controller to perform a normal test on this condition. Figure 1 As shown, collect data to implement the extension. For example, Figure 7 As shown, when moving a box, the original QP remote control is used to complete the entire moving process. When the two arms touch the box, there will be an impact. The torque before and after this impact is collected. and posture Used to calculate the extension and .

[0083] In other feasible embodiments, a dataset can be pre-built through a large number of experiments so that data from the same working condition can be directly called in actual applications.

[0084] S2, using external sensors for impact monitoring; if an impact is detected, the impact time is extracted. And based on the defined pre-impact time and time after impact This allows for the determination of the collision transition phase and the post-impact phase, while other periods of impact monitoring are considered the pre-impact phase.

[0085] In some embodiments, impact monitoring can be performed in real time, or triggering conditions for impact monitoring can be set. This invention does not impose specific limitations on this; for example, in this embodiment, it is initially set to a pre-impact mode and is prepared for impact to occur at any time. Regardless of the method, when not monitoring impact, conventional control methods can be used, such as... Figure 2 The control logic is shown.

[0086] Once an impact is detected, the moment of impact is extracted. Define the exclusion interval based on the actual impact time. This made the time before the impact Time after impact Among them, the interval The value is an empirical value and can be adjusted based on the experimental results and application scenarios. For example, in this embodiment, it is set to 0.1s.

[0087] S3, based on the hierarchical control mode, performs tracking control according to the current stage type. That is, in the pre-impact stage and post-impact stage, the QP controller uses the torque signal in the impedance task and the joint signal in the attitude task to adopt a feedback control mechanism to control the robotic arm; in the collision transition stage, the QP controller adopts a superposition mechanism of feedforward control and feedback control to control the robotic arm.

[0088] like Figure 2 As shown, in the pre-impact and post-impact stages, the QP controller adopts normal control. In this embodiment, a feedback control mechanism is used to control the robotic arm by utilizing the torque signal in the impedance task and the joint signal in the attitude task.

[0089] For example, when using an HTC VIVE handheld controller to track the operator's hand movements, the desired position of the end effector... Initial attitude adjustment matrix and speed , This indicates the robot arm index, corresponding to the left and right arms; , , The desired torque is obtained by performing an impedance task using the input signal. The QP controller is designed to make the end effector behave as a mass-elastic-drag system connected to a reference frame, with the joint acceleration as the optimization variable. The cost function minimizes the sum of multiple tasks; in this invention, it addresses impedance and attitude tasks. Based on this, the error of the impedance task is defined as:

[0090]

[0091] in, It is the end effector acceleration. It is the mission space equivalent inertia matrix. It is the desired torque, the desired torque Existing methods can be used, and this invention does not limit them, such as existing damping-stiffness models derived from critical damping design:

[0092]

[0093] in, For damping gain (to achieve critical damping). For the user-defined stiffness matrix (adjustable according to the required impedance stiffness), ∨ is the antisymmetric matrix to vector mapping operator. The desired position is derived from the pose data input from the handle. For actual location, Let be the desired pose matrix. This is the actual attitude matrix.

[0094] The posture task is used to address the redundancy problem in 7-DOF robots, defining the desired joint acceleration. :

[0095]

[0096] Among them, the joint angle of a specific joint , To select a matrix, select a specific joint, such as joint 1, with the aim of increasing the maximum workspace; It's the joint angle. yes The derivative, yes The derivative, These are custom parameters. For the desired joint angle, This represents the actual joint angle.

[0097] The error of the attitude task is defined as: = .

[0098] The control model of the QP controller is:

[0099]

[0100] Set joint position, speed, and torque limits:

[0101]

[0102]

[0103]

[0104] In the formula, For QP time steps, Let be the joint angle of robotic arm i. , Represents the minimum and maximum values ​​of the joint angle. , Represents the minimum and maximum values ​​of joint velocity. , This represents the minimum and maximum values ​​of the joint torque. These are the joint accelerations of the left and right robotic arms, respectively, where i represents the robotic arm's marker. , These are the weights for the impedance task and the attitude task, respectively. , This represents the error between the impedance task and the attitude task. Therefore, after the QP controller outputs the joint acceleration, substituting it into the torque formula... , It is the inertia matrix. The Coriolis force is used to obtain the torque of each joint of the robotic arm and send it to the motor for execution.

[0105] The core optimization of this invention lies in the collision transition phase, specifically as shown in Figure 3 below: First, the time before impact... and the time after the impact Both the impedance task and the attitude task are referenced and extended to obtain impedance and attitude feedforward information. Then, the impedance and attitude feedforward information is used to adaptively adjust the torque signal in the impedance task and the joint signal in the attitude task at the current moment. Then, the QP controller uses the adjusted torque signal and joint signal to adopt a feedback control mechanism to control the robotic arm.

[0106] Regarding the time before the impact and the time after the impact The process of obtaining impedance and attitude feedforward information by reference extension for both impedance and attitude tasks is as follows:

[0107] Torque extension is:

[0108]

[0109] In the formula, It is a function of the originally desired torque. , For robotic arm i, the time before impact is respectively and time after impact The impedance task references the extended torque. t represents the current moment, after collision detection. It must be less than t, so Constant as Torque at any given moment; Constant as The torque at any given time is obtained through step S1.

[0110] Joint acceleration is extended to:

[0111]

[0112] In the formula, It is a function of the originally desired joint acceleration. , For robotic arm i, the time before impact is respectively and time after impact The extended joint acceleration is referenced in the posture task.

[0113] In this embodiment, the end effector position expansion adopts linear expansion:

[0114]

[0115] In the formula, , For robotic arm i, the time before impact is respectively Time after impact The corresponding actual position of the end effector; , For robotic arm i, the time before impact is respectively Time after impact Refer to the extended end effector location.

[0116] The velocity is extended to a constant: , ], ]

[0117] In the formula, , For robotic arm i, the time before impact is respectively Time after impact Reference extended end effector speed, , , For robotic arm i, the time before impact is respectively and time after impact The corresponding actual linear velocity and actual angular velocity of the end effector.

[0118] parameter The reference extension is as follows:

[0119]

[0120]

[0121] In this embodiment, the mathematical model for adaptively adjusting the torque signal in the impedance task and the joint signal in the attitude task at the current moment using impedance attitude feedforward information is as follows:

[0122]

[0123]

[0124] In the formula, Let be the desired torque at the current time t during the collision transition phase and for robotic arm i. Here are mixed parameters, where t is the current time and i is the marker of the robotic arm; , For robotic arm i, the time before impact is respectively and time after impact In the impedance task, refer to the extended torque. For damping gain, ; It is the task space equivalent inertia matrix corresponding to robotic arm i. For impedance stiffness; For robotic arm i, post-impact time Corresponding to the extended end effector speed; Let i be the position of the end effector of the robotic arm at the current time t. Let i be the end effector speed of robotic arm i at the current time t. For impedance stiffness, , For robotic arm i, the time before impact is respectively and time after impact Corresponding to the extended end effector position, This is the transpose of the actual attitude matrix of the end effector, where T is the matrix transpose symbol. Let be the desired attitude matrix of the end effector. This is an operator for converting a partially symmetric matrix into a vector.

[0125] Let be the joint acceleration at time t during the collision transition phase and for robotic arm i. For robotic arm i, the time before impact Corresponding to the extended joint acceleration, For robotic arm i, post-impact time Corresponding to the extended joint acceleration, For the stiffness of the attitude task. Let be the first derivative with respect to the joint angle of robotic arm i. for The first derivative, For robotic arm i, the time before impact Corresponding to the extended joint angles, For robotic arm i, post-impact time The corresponding reference is the expanded joint angle.

[0126] In some embodiments, the mixing parameter is a preset value; in this embodiment, the mixing parameter is... , For a custom time interval, such as 0.1s.

[0127] The process of controlling the robotic arm using a feedback control mechanism based on the adjusted torque signal and joint signal by the QP controller is as follows:

[0128] First, using torque signals and joint signals, calculate the impedance task error for the impedance task and the attitude error for the attitude task, respectively.

[0129] Then, the joint acceleration of the robotic arm is optimized by minimizing the set cost function, and finally the torque of each joint is generated. This is used to control the robot's motors, and the cost function is a weighted average of the errors from the impedance task and the attitude task. (This is related to the time before the impact mentioned earlier.) and the time after the impact The difference lies in the collision transition phase, where the impedance task error and the attitude error are expressed as follows:

[0130]

[0131] =

[0132] In the formula, For impedance task error, For attitude error, Let i be the acceleration of the end effector corresponding to robotic arm i. Let be the second derivative of the joint angle corresponding to robot arm i. Let be the equivalent inertia matrix in the task space corresponding to robotic arm i.

[0133] In summary, the technical solution of this invention addresses the problem of asynchronous impact by introducing a three-stage division—pre-impact, transition, and post-impact—which constitutes the control mode. For the transition mode, a reference extension concept is introduced to control the pre-impact time. and the time after the impact The impedance and attitude tasks are referenced and extended, and the QP controller is further extended. Adaptive adjustment of the torque and attitude joint signals of the robotic arm is adopted to effectively smooth error peaks and control input jumps.

[0134] like Figure 3 When subjected to a sudden external impact at t=1s, due to the lack of a mode switching and reference extension mechanism for the impact dynamics, the controller still uses the pre-impact full-state feedback impedance control strategy. At this time, the end effector speed undergoes a drastic jump at the moment of impact, causing a sharp increase in impedance task error. The joint acceleration solved by the quadratic programming generates high-frequency oscillations, ultimately manifesting as significant abrupt peaks in the torque of each joint (e.g., the torque of joints 3 and 4 instantaneously exceeds the rated value by 150%). This phenomenon not only compromises trajectory tracking accuracy but may also trigger mechanical structure resonance and actuator overload.

[0135] like Figure 4 Under the same impact conditions (t=1s), the proposed switchable control mode (pre-impact → transition → post-impact) and reference extension program were enabled. Upon detecting the first impact, the system immediately switched to transition mode, disabling velocity feedback, introducing a time-adaptive position hybrid feedforward mechanism, and maintaining continuity using overlapping reference signals before and after the impact. Results showed that all joint torques achieved a smooth transition near the impact moment, without significant abrupt changes or oscillations. The peak torque decreased by approximately 68% compared to the unenabled mode, and the system quickly converged to a stable tracking state, fully demonstrating the method's robust suppression capability against asynchronous impacts.

[0136] like Figure 5 The torque curves of joints 3 and 4 were focused on within a 0.3s window before and after the impact moment (t=1s). In the transition mode, the torque smoothly increased from the pre-impact level to the post-impact steady state without any peaks or steps, indicating a smooth transition and minimal impact from the torque impact.

[0137] In summary, as Figure 6 As shown, the technical solution of this invention utilizes external sensors for impact monitoring to determine the collision transition stage and the post-impact stage, while other periods of impact monitoring are considered as the pre-impact stage. During the pre-impact and post-impact stages, a normal quadratic programming (QP) controller is used for impedance and attitude tracking control. During the collision transition stage, the system switches to an extended QP controller, which uses reference extended and hybrid parameters α(t) to achieve feedforward and feedback superposition, thereby smoothing the peak torque and control input jumps, and ultimately achieving robust tracking control of the dual-arm robot in high-impact tasks such as grasping, handling, striking, and assembly.

[0138] Example 2

[0139] This invention provides a robot operating system that applies the above-described control method, comprising a remote operating handle, a handle controller, a dual-arm robot, and a robotic arm controller mounted on the dual-arm robot. The remote operating handle is communicatively connected to the handle controller, and the handle controller is communicatively connected to the robotic arm controller. The robotic arm controller is used to control the robotic arms on the dual-arm robot.

[0140] The handle controller or robotic arm controller is equipped with a processor and a memory. The processor calls the computer program stored in the memory to implement the steps of a dual-arm robot impact-resistant tracking control method based on quadratic programming.

[0141] For example, execute steps 1-2 or steps S1-S3.

[0142] For example, in some embodiments, it is generally preferred that steps 1-2 or steps S1-S3 are executed by the robotic arm controller, while the hand controller is used to perform VR hand controller coordinate system transformation and position and attitude data acquisition and transmission. However, in other implementations, it is also feasible for the hand controller at the remote end to perform the calculation process of steps 1-2 or steps S1-S3.

[0143] It should also be noted that two QP controllers can be set up in the controller by means of hardware or software, one for normal control and the other for extended control; however, it should be understood that a single QP controller can also be used to implement both normal control and extended control by means of hardware or software, and this invention does not impose specific limitations on this.

[0144] The specific steps can be referred to in the aforementioned method embodiments.

[0145] The processor can be a Central Processing Unit (CPU), but it can also be other general-purpose processors, digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor can be a microprocessor or any conventional processor. The processor is used to execute relevant programs to implement the technical solutions provided in the embodiments of the present invention.

[0146] The memory can be implemented in the form of read-only memory (ROM), static storage device, dynamic storage device, or random access memory (RAM). The memory can store the operating system and other applications. When the technical solutions provided in the embodiments of this specification are implemented through software or firmware, the relevant program code is stored in the memory, and the processor calls the algorithm program of the control method of the embodiments of this invention to execute it.

[0147] Example 3

[0148] This invention provides a computer-readable storage medium storing a computer program that is invoked by a processor to implement the steps of a quadratic programming-based shock-resistant tracking control method for a dual-arm robot.

[0149] For example, execute steps 1-2 or steps S1-S3.

[0150] For details on the implementation of each step, please refer to the description of the control method embodiment above.

[0151] The readable storage medium is a computer-readable storage medium, which can be an internal storage unit of the hardware and software device described in any of the foregoing embodiments, such as the hard drive or memory of the controller. The readable storage medium can also be an external storage device of the controller, such as a plug-in hard drive, Smart MediaCard (SMC), Secure Digital (SD) card, or Flash Card equipped on the controller. Further, the readable storage medium can include both internal storage units and external storage devices of the controller. The readable storage medium is used to store the computer program and other programs and data required by the controller. The readable storage medium can also be used to temporarily store data that has been output or will be output.

[0152] Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of the present invention. The aforementioned readable storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, read-only memory (ROM), random access memory (RAM), magnetic disks, or optical disks.

[0153] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product embodied on one or more computer-readable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. This application refers to flowchart illustrations and / or instructions executed by a processor of a method, apparatus (system), and computer program product according to embodiments of this application to create means for implementing the functions specified in one or more flowchart illustrations and / or one or more block diagrams. These computer program instructions may also be stored in a computer-readable storage medium capable of directing a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means that implement the functions specified in one or more flowchart illustrations and / or one or more block diagrams. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, such that the instructions, which execute on the computer or other programmable apparatus, provide steps for implementing the functions specified in one or more flowcharts and / or one or more blocks of a block diagram.

[0154] It should be emphasized that the examples described in this invention are illustrative rather than limiting. Therefore, this invention is not limited to the examples described in the specific embodiments. Any other embodiments derived by those skilled in the art based on the technical solutions of this invention, without departing from the spirit and scope of this invention, whether modifications or substitutions, are also within the protection scope of this invention.

Claims

1. A method for impact-resistant tracking control of dual-arm robots based on quadratic programming, characterized by: The method comprises the following steps: Step 1: Impact monitoring with external sensors, if impact motion is detected, the impact time is extracted , and based on the defined pre-impact time and post-impact time , the collision transition phase and post-impact phase are determined, and other periods of impact monitoring are considered as pre-impact phase; Step 2: Based on the hierarchical control mode, the tracking control is performed according to the type of the stage in which the current time point is located, that is, in the pre-impact stage and the post-impact stage, the QP controller uses the torque signal in the impedance task and the joint signal in the posture task to adopt a feedback control mechanism to control the robot arm; in the collision transition stage, the QP controller adopts a superposition mechanism of feedforward control and feedback control to control the robot arm; The superposition mechanism of the feedforward control and the feedback control is that impedance tasks and posture tasks at a time before impact and a time after impact are both referenced and expanded to obtain impedance-posture feedforward information, and then the impedance-posture feedforward information is used to adaptively adjust torque signals in the impedance tasks and joint signals in the posture tasks at the current time, and then a QP controller uses the adjusted torque signals and joint signals to control the robot arm by using a feedback control mechanism. In the collision transition stage, when the impedance posture feedforward information is used to adaptively adjust the torque signal in the impedance task and the joint signal in the posture task, the corresponding mathematical model is as follows: ； ； wherein, is the desired torque for the robot i at the current time t during the impact transition phase; is the mixing parameter, t is the current time, i is the index of the robot; , is the impedance task reference extended torque for the robot i at the pre-impact time and the post-impact time , is the damping gain; is the impedance stiffness, is the impedance task reference extended end-effector velocity for the robot i at the post-impact time , is the end-effector position of the robot i at the current time t, is the end-effector velocity of the robot i at the current time t, , is the impedance task reference extended end-effector position for the robot i at the pre-impact time and the post-impact time , is the actual pose matrix of the end-effector, T is the matrix transpose symbol, is the desired pose matrix of the end-effector, is the operator symbol that transforms a skew-symmetric matrix into a vector; joint acceleration of the robot i at the current time t for the collision transition phase; pre-impact time for the robot i corresponding reference extended joint acceleration, post-impact time for the robot i corresponding reference extended joint acceleration, stiffness of the pose task, first derivative of the joint angle of the robot i first derivative of the joint angle of the robot i first derivative of the joint angle of the robot i first derivative of the joint angle of the robot i pre-impact time for the robot i corresponding reference extended joint angle; post-impact time for the robot i corresponding reference extended joint angle; The reference expansion of the end effector position adopts linear expansion, and the reference expansion of the end effector velocity is a constant, which is specifically: ； wherein is the time before impact for manipulator i is the corresponding end effector actual position; is the time after impact for manipulator i is the corresponding end effector actual position; is the time before impact for manipulator i and the time after impact is the corresponding end effector actual linear velocity;​​​ , ]， ] ； wherein, , , are the pre-impact time and post-impact time for the i-th robot arm, respectively, corresponding to the actual linear and angular velocities of the end-effector; are the pre-impact time for the i-th robot arm, respectively, corresponding to the reference extended end-effector velocity.

2. The method of claim 1, wherein: Parameters The calculation of the parameters is: ； ； In the formula, is the attitude matrix at the current time t, is the attitude matrix at the time t0, is the attitude matrix at the time t0, is the attitude matrix at the time t0, is the attitude matrix at the time t0, , is the angular velocity at the time t0, is the time t0, is the angular velocity at the time t0; is the attitude matrix before the impact time t0 is the attitude matrix after the impact time t0 is the attitude matrix before the impact time t0 is the attitude matrix after the impact time t0 3. The method of claim 1, wherein: The process in which the QP controller uses the torque signal and the joint signal to adopt a feedback control mechanism to control the robot arm is as follows: first, the torque signal and the joint signal are used to calculate the impedance task error of the impedance task and the posture error of the posture task, respectively; The joint acceleration of the robot arm is optimized by minimizing a set cost function, and finally the torque of each joint is generated for the robot motor to execute control, the cost function is the error of impedance task and posture task weighted In the collision transition stage, the impedance task error and the posture error are expressed as: ； = ； wherein is the impedance task error, is the pose error, is the end-effector acceleration of the robot arm i, is the second derivative of the joint angle of the robot arm i, is the task space equivalent inertia matrix of the robot arm i.

4. The method of claim 1, wherein: The dual-arm robot is a tilt-mounted dual-arm robot, wherein the coordinate transformation from the remote operation handle to the robot arm coordinate system is realized by constructing the mapping relationship between the remote operation handle coordinate system and the robot arm coordinate system. The mapping between the remote operation handle coordinate system and the robot arm coordinate system is as follows: ； And the rotation compensation of the robot arm coordinate system in the tilt-mounted state around the y-axis and the z-axis relative to the world coordinate system is as follows: ； In the formula, is a pose matrix of the end effector in the coordinate system of the robot arm, is an initial pose adjustment matrix, is a pose matrix in the coordinate system of the remote control handle, and denote a rotation around the y-axis and a rotation around the z-axis; and denote the rotation angle around the y-axis and the rotation angle around the z-axis.

5. The method of claim 1, wherein: Before step 1 is performed, the following is performed first: Using a remote control handle, the dual-arm robot is controlled to conduct impact tests under the current working conditions or various working conditions, and the time after each impact is recorded. The torque attitude data is used in step 2 to determine the time after impact. The impedance and attitude tasks are referenced and extended.

6. A robotic system, comprising: There is a remote operation handle, a handle controller, a dual-arm robot, and a robot arm controller arranged on the dual-arm robot; The remote operation handle is in communication connection with the handle controller, and the handle controller is in communication connection with the robot arm controller, and the robot arm controller is used to control the robot arm on the dual-arm robot; The handle controller or the robot arm control is provided with a processor and a memory; The processor calls the computer program stored in the memory to realize the following: The steps of the method of any one of claims 1-5.

7. A control system based on the method according to any one of claims 1 to 5, characterized in that: Comprise: An impact detection module is used to monitor the impact with the external sensor, and if the impact action is monitored, the impact time is extracted , and based on the defined pre-impact time and post-impact time , the collision transition phase and the post-impact phase are determined, and other periods of impact monitoring are considered as pre-impact phase; The control module is used to perform tracking control according to the type of the stage in which the current time point is located based on the hierarchical control mode, that is, in the pre-impact stage and the post-impact stage, the QP controller uses the torque signal in the impedance task and the joint signal in the posture task to adopt a feedback control mechanism to control the robot arm; in the collision transition stage, the QP controller adopts a superposition mechanism of feedforward control and feedback control to control the robot arm; The superposition mechanism of the feedforward control and the feedback control is that impedance tasks and posture tasks at a time before impact and a time after impact are both referenced and expanded to obtain impedance-posture feedforward information, and then the impedance-posture feedforward information is used to adaptively adjust torque signals in the impedance tasks and joint signals in the posture tasks at the current time, and then a QP controller uses the adjusted torque signals and joint signals to control the robot arm by using a feedback control mechanism.

8. A computer-readable storage medium, characterized in that: The computer program is stored, and the computer program is called by the processor to realize the following: The steps of the method of any one of claims 1-5.

Images