Single-target robot arm cooperative operation control method and system

Abstract

This invention provides a single-target robotic arm collaborative operation control method and system, relating to the field of robot control technology. The method includes: after the robotic arm contacts the target object, acquiring multi-source sensor data of the robotic arm; determining the target object, its pose data, and generalized velocity within a preset operation area based on the multi-source sensor data; and determining the rigid body dynamics characteristic parameters of the target object by combining end-effector tactile data and wrist data; obtaining the desired wrench of the target object and constructing a grasping matrix based on the rigid body dynamics characteristic parameters, pose data, and generalized velocity, combined with a preset desired motion trajectory; determining the contact force vector adapted to the target object based on the grasping matrix, and then performing resultant force-internal force decoupling to obtain the task component and internal force component, generating end-effector torque commands for collaborative operation. This invention improves the stability and safety of dual-arm collaborative operation.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and more specifically, to a method and system for controlling the cooperative operation of a single-target robotic arm. Background Technology

[0002] In industrial warehousing, logistics sorting, and collaborative assembly scenarios, robots often need to use both arms to collaboratively grasp rigid or near-rigid targets such as cartons, turnover boxes, and material baskets to solve problems such as insufficient load capacity of a single arm, limited grasping surface, and the need for precise control of the object's posture.

[0003] In related technologies, dual-arm cooperative control is usually designed around a single level, such as the end effector or the object. However, in actual operation, due to factors such as the unknown or offset center of mass of the object, geometric asymmetry of the gripping point, changes in the contact friction coefficient, and external disturbances, problems such as the coupling of resultant force and internal force can easily arise. For example, excessively increasing the clamping force to ensure gripping stability can cause excessive internal squeezing force between the two arms, leading to object deformation; or insufficient clamping force can not resist tangential disturbances, causing the object to drift or overturn. Slippage can also occur. If adjustments are made after these problems occur, the large response lag and unresolved coupling issues can easily lead to an imbalance in the load distribution between the two arms, further disrupting the object's gripping posture and making it difficult to adapt to the complex and ever-changing operational needs of industrial warehousing and logistics sorting scenarios. Summary of the Invention

[0004] The problem addressed by this invention is how to improve the stability and safety of single-target dual-arm cooperative operation.

[0005] To address the aforementioned problems, this invention provides a single-target robotic arm collaborative operation control method and system.

[0006] In a first aspect, the present invention provides a single-target robotic arm cooperative operation control method, comprising:

[0007] When the robot's robotic arm comes into contact with the object being operated, it acquires multi-source sensor data of the robotic arm. The multi-source sensor data includes image data of the robotic arm corresponding to a preset operating area, tactile data of the robotic arm's end effector, wrist data, and joint state data.

[0008] Based on the image data, target recognition and pose estimation are performed in conjunction with the joint state data to determine the operation object and the pose data and generalized velocity of the operation object within the preset operation area.

[0009] Based on the end-effector tactile data and the wrist data, combined with the pose data and generalized velocity of the manipulated object, the rigid body dynamic characteristic parameters of the manipulated object are determined;

[0010] Based on the rigid body dynamics characteristic parameters, the pose data, and the generalized velocity, combined with the preset expected motion trajectory of the robotic arm, the desired wrench for the manipulated object is obtained, and a grasping matrix is ​​constructed based on the end-effector tactile data.

[0011] Based on the grasping matrix, and combined with the desired wrench, a secondary planning is performed to obtain a contact force vector that is adapted to the operation object;

[0012] Based on the contact force vector, the resultant force-internal force decoupling is performed to obtain the task component and internal force component of the robotic arm;

[0013] Based on the task component and the internal force component, the end joint torque command of the robotic arm is generated, and the robotic arm is controlled to perform cooperative operation on the object being manipulated through the joint torque command.

[0014] Optionally, the step of performing target recognition and pose estimation based on the image data and the joint state data to determine the operation object and its pose data and generalized velocity within the preset operation area includes:

[0015] Based on the image data, the target object within the preset operation area is identified and segmented to obtain a three-dimensional point cloud representation of the operation object;

[0016] Based on the pose estimation algorithm, the position and orientation of the manipulated object in the world coordinate system are determined according to the 3D point cloud representation, and the position and orientation are used as the pose data;

[0017] Based on the time series of the pose data, state estimation is performed in conjunction with the joint state data to obtain the linear velocity and angular velocity of the manipulated object, and the linear velocity and angular velocity are used as the generalized velocity.

[0018] Optionally, determining the rigid body dynamics characteristic parameters of the manipulated object based on the end-effector tactile data and the wrist data, combined with the pose data and generalized velocity of the manipulated object, includes:

[0019] Based on the end-effector tactile data, the contact force distribution data between the end effector of the robotic arm and the manipulated object is determined;

[0020] Based on the contact force distribution data and the wrist data, the contact wrench applied by the end effector to the object being operated on is obtained;

[0021] Based on the pose data, the contact wrench is transformed from the local contact coordinate system to the object coordinate system of the object being operated on, thereby obtaining the equivalent external force wrench in the object coordinate system;

[0022] Differentiate the generalized velocity to obtain the generalized acceleration of the object being operated on.

[0023] A dynamic parameter identification model is established based on the equivalent external force wrench, the generalized acceleration, and the generalized velocity.

[0024] The dynamic parameter identification model is recursively solved by the least squares method or filtering algorithm to obtain the mass, center of mass, and inertia tensor of the manipulated object, and the mass, center of mass, and inertia tensor are used as the rigid body dynamic characteristic parameters.

[0025] Optionally, obtaining the desired wrench for the manipulated object based on the rigid body dynamics characteristic parameters, the pose data, and the generalized velocity, combined with the preset desired motion trajectory of the robotic arm, includes:

[0026] The desired generalized acceleration of the manipulated object is determined by calculating the preset desired motion trajectory.

[0027] Substituting the rigid body dynamics characteristic parameters, the pose data, and the desired generalized acceleration into the rigid body dynamics equation of the manipulated object, the ideal total wrench of the manipulated object is obtained.

[0028] The preset desired motion trajectory is compared with the pose data of the manipulated object to obtain the pose tracking error;

[0029] Based on the pose tracking error, a feedback correction wrench is determined using a preset impedance or admittance control law.

[0030] The ideal master wrench and the feedback correction wrench are superimposed to obtain the desired wrench for the object being operated on.

[0031] Optionally, constructing the grasping matrix based on the end-effector tactile data includes:

[0032] Based on the end-effector tactile data, determine the contact position and contact normal of each contact point between the end effector of the robotic arm and the manipulated object;

[0033] Transform the contact position and the contact normal from the local contact coordinate system to the object coordinate system of the manipulated object to obtain the contact position and contact normal in the object coordinate system;

[0034] Based on the contact position and contact normal of each contact point in the object coordinate system, determine the single-point grasping matrix corresponding to each contact point;

[0035] The single-point grasping matrices of all the contact points are stacked and combined to obtain the grasping matrix.

[0036] Optionally, the step of performing secondary planning based on the grasping matrix and the desired wrench to obtain a contact force vector adapted to the operation object includes:

[0037] Based on the centroid of the manipulated object, determine the reference distribution vector of the contact force of the manipulated object between the left and right arms of the robotic arm;

[0038] With the optimization objective of minimizing the deviation between the contact force vector and the reference allocation vector, and minimizing the internal force components in the contact force vector, a quadratic programming objective function is constructed, and constraints on the objective function are set.

[0039] Substitute the objective function and the constraints into a preset quadratic programming solver to obtain the single-arm contact force vectors corresponding to the left arm and the right arm, respectively.

[0040] The contact force vector is obtained based on the single-arm contact force vectors corresponding to the left arm and the right arm, respectively.

[0041] Optionally, the step of decoupling the resultant force and internal force based on the contact force vector to obtain the task component and internal force component of the robotic arm includes:

[0042] Obtain the generalized inverse of the capture matrix;

[0043] Based on the generalized inverse of the grasping matrix, the contact force vector is projected onto the row space of the grasping matrix to obtain the task component of the desired wrench;

[0044] Subtracting the task component from the contact force vector yields the internal force component of the contact force vector located in the null space of the grasping matrix.

[0045] Optionally, generating the end-joint torque command of the robotic arm based on the task component and the internal force component includes:

[0046] Based on the task component and the internal force component, determine the end contact wrenches corresponding to the end effectors of the left and right arms of the robotic arm;

[0047] Obtain the Jacobian matrix of the robotic arm;

[0048] The Jacobian matrix is ​​used to map the end contact wrench to the initial torque values ​​of the joints corresponding to the end actuators of the left and right arms.

[0049] Based on the joint state data in the multi-source sensor data, a torque compensation amount is generated;

[0050] The initial torque value of each joint is superimposed with the torque compensation amount to obtain the target torque value of each joint;

[0051] Based on the target torque value of the joint, generate the end joint torque commands corresponding to the left arm and the right arm respectively;

[0052] The robotic arm is controlled to perform coordinated operations on the object according to the joint torque commands of the left and right arms.

[0053] Optionally, it also includes:

[0054] When the robotic arm performs a collaborative operation on the object being manipulated, based on the end-effector tactile data and the wrist data, combined with the contact force vector, the contact pressure distribution and contact slippage trend information between the end effector of the robotic arm and the object being manipulated are extracted.

[0055] Based on the contact pressure distribution and the rigid body dynamics characteristic parameters of the manipulated object, the actual friction coefficient of the contact point is determined.

[0056] The friction margin of the contact point is determined based on the actual friction coefficient and the preset safe friction coefficient threshold.

[0057] Based on the friction margin of all the contact points, determine whether there is a risk of slippage between the robotic arm and the operated object;

[0058] If so, the normal force component of the contact force vector, the stiffness coefficient and damping coefficient of the impedance model of the operating object and the end effector are adjusted according to the preset adjustment rules.

[0059] If not, then the contact force vector, the stiffness coefficient, and the damping coefficient remain unchanged.

[0060] Secondly, the present invention provides a single-target robotic arm cooperative operation control system, comprising:

[0061] The data acquisition unit is used to acquire multi-source sensor data of the robotic arm after the robotic arm contacts the object being operated. The multi-source sensor data includes image data of the robotic arm corresponding to a preset operation area, tactile data of the robotic arm's end effector, wrist data, and joint state data.

[0062] An object recognition unit is used to perform target recognition and pose estimation based on the image data and the joint state data, and to determine the operation object and the pose data and generalized velocity of the operation object within the preset operation area.

[0063] The data analysis unit is used to determine the rigid body dynamic characteristic parameters of the manipulated object based on the end-effector tactile data and the wrist data, combined with the pose data and generalized velocity of the manipulated object;

[0064] The dynamic identification unit is used to obtain the desired wrench of the manipulated object based on the rigid body dynamic characteristic parameters, the pose data and the generalized velocity, combined with the preset desired motion trajectory of the robotic arm;

[0065] A matrix construction unit is used to construct a grasping matrix based on the end-effector tactile data;

[0066] The contact force planning unit is used to perform secondary planning based on the grasping matrix and the desired wrench to obtain a contact force vector that is adapted to the operation object.

[0067] The resultant force decoupling unit is used to decouple the resultant force and internal force according to the contact force vector to obtain the task component and internal force component of the robotic arm.

[0068] The control unit is used to generate end joint torque commands for the robotic arm based on the task component and the internal force component, and to control the robotic arm to perform cooperative operations on the manipulated object through the joint torque commands.

[0069] The single-target robotic arm collaborative operation control method and system of the present invention firstly acquires multi-source sensor data, including image data, end-effector tactile data, wrist data, and joint state data, after the robotic arm contacts the target object. This enables comprehensive perception of the contact and motion states of the target object and the robotic arm from visual, tactile, force, and joint motion perspectives, thereby avoiding state judgment biases caused by the limitations of single-sensor data. Secondly, by combining image data and joint state data, target recognition, pose estimation, and generalized velocity determination of the target object are completed. This allows for accurate acquisition of the target object's pose and velocity information in the world coordinate system, clarifying the real-time state of the target object and ensuring that subsequent dynamic analysis and motion planning match the actual state of the target object. This avoids control inaccuracies caused by pose and velocity estimation errors and prevents problems such as attitude drift of the target object.Furthermore, by integrating end-effector tactile data, wrist data, and the pose and generalized velocity data of the manipulated object to determine rigid body dynamics parameters, the dynamic characteristics such as the mass, center of mass, and inertia tensor of the manipulated object can be accurately analyzed. This solves the problem of mismatch between control planning and the actual object caused by unknown or offset dynamic characteristics of the manipulated object, allowing subsequent expected wrench planning to better fit the actual dynamic needs of the manipulated object and avoiding uneven load distribution and overturning risks caused by deviations in dynamic parameters. Simultaneously, based on rigid body dynamics parameters, the real-time state of the manipulated object, and a preset expected motion trajectory, the expected wrench is obtained. A grasping matrix is ​​constructed based on end-effector tactile data, realizing expected resultant force / torque planning centered on the manipulated object. The grasping matrix establishes a precise mapping relationship between the contact force of the robotic arm and the wrench applied to the manipulated object, allowing the contact force planning to directly target the desired motion needs of the manipulated object. Subsequently, a secondary planning based on the grasping matrix and the expected wrench yields a suitable contact force vector, enabling the contact force distribution of the left and right arms of the robotic arm to match the desired motion needs of the manipulated object. This rationally distributes the load between the two arms, avoiding operational instability caused by overload of a single arm or imbalanced load distribution. Simultaneously, the contact force is always planned around achieving the desired motion of the manipulated object, reducing the generation of ineffective contact forces. Next, the resultant force-internal force decoupling of the contact force vector is performed to obtain the task component and the internal force component. This achieves explicit separation between the effective task component affecting the motion of the manipulated object and the internal force component that only generates interaction between the two arms. This fundamentally solves the coupling problem between resultant force and internal force from the control structure perspective, avoiding deformation of the manipulated object, gripper slippage, or robot arm overload caused by excessive internal force leading to excessive internal squeezing pressure between the two arms. It also avoids contact instability caused by insufficient internal force and eliminates the generation of repulsive forces. To eliminate potential hazards, the system ensures that the two arms form a coordinated force rather than a repulsive force pushing each other. Finally, based on the task component and internal force component, the system generates end-joint torque commands and controls the robotic arms to perform coordinated operations. This accurately translates the planned contact force requirements into execution commands for the robotic arm joints, ensuring that the joint movements of the robotic arms strictly follow the control requirements that conform to the actual state and desired movement of the object being operated on. This ensures that the object always moves along the preset trajectory, achieving precise and stable control of the object by the robotic arms. From the execution level, this guarantees the stability of the coordinated operation of the two arms and improves the safety of single-target coordinated operation of the two arms. Attached Figure Description

[0070] Figure 1 This is a flowchart illustrating the single-target robotic arm collaborative operation control method according to an embodiment of the present invention;

[0071] Figure 2 This is a schematic diagram of the structure of a single-target robotic arm collaborative operation control system according to an embodiment of the present invention. Detailed Implementation

[0072] To make the above-mentioned objects, features, and advantages of the present invention more apparent and understandable, specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. Although some embodiments of the present invention are shown in the drawings, it should be understood that the present invention can be implemented in various forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided to provide a more thorough and complete understanding of the present invention. It should be understood that the accompanying drawings and embodiments of the present invention are for illustrative purposes only and are not intended to limit the scope of protection of the present invention.

[0073] It should be understood that the various steps described in the method embodiments of the present invention may be performed in different orders and / or in parallel. Furthermore, the method embodiments may include additional steps and / or omit the steps shown. The scope of the present invention is not limited in this respect.

[0074] The term "comprising" and its variations as used herein are open-ended, meaning "including but not limited to"; the term "based on" means "at least partially based on"; the term "one embodiment" means "at least one embodiment"; the term "another embodiment" means "at least one additional embodiment"; the term "some embodiments" means "at least some embodiments"; and the term "optionally" means "optional embodiments". Definitions of other terms will be given in the following description. It should be noted that the concepts of "first," "second," etc., mentioned in this invention are used only to distinguish different devices, modules, or units, and are not intended to limit the order of functions performed by these devices, modules, or units or their interdependencies.

[0075] It should be noted that the terms "a" and "a plurality of" used in this invention are illustrative rather than restrictive. Those skilled in the art should understand that, unless otherwise expressly indicated in the context, they should be understood as "one or more".

[0076] Combination Figure 1 As shown in the figure, an embodiment of the present invention provides a single-target robotic arm cooperative operation control method, comprising:

[0077] When the robot's robotic arm comes into contact with the object being operated, it acquires multi-source sensor data of the robotic arm. The multi-source sensor data includes image data of the robotic arm corresponding to a preset operating area, tactile data of the robotic arm's end effector, wrist data, and joint state data.

[0078] Specifically, after the robotic arm contacts the object being manipulated, it needs to acquire multi-source sensor data through a multi-source sensing module. This module includes an RGB-D or binocular vision system for target recognition and coarse pose estimation; a gripper tactile array or tactile image sensor for high-resolution perception of contact distribution, local deformation, and micro-slippage; a six-dimensional force / torque wrist sensor for precise measurement of the interaction between the end effector and the object; and joint encoders and current sensors for feedback on the robotic arm's motion state. The acquired multi-source sensor data is dynamic and coupled. For example, the wrist force sensor measurement includes the combined effects of the object's gravity, inertial force, and contact force, while the tactile data provides details of the micro-slippage and stress distribution at the contact interface.

[0079] Based on the image data and the joint state data, target recognition and pose estimation are performed to determine the operation object and its pose data and generalized velocity within the preset operation area.

[0080] Specifically, firstly, computer vision algorithms are used to identify and segment the objects in the scene, and fit a 3D geometric representation (such as a point cloud or bounding box) of the objects. Based on the geometric representation, pose estimation algorithms are used to determine the specific position and spatial orientation of the objects relative to the world coordinate system, forming the pose data of the objects. However, knowing only the static position of the object is insufficient for dynamic control; its velocity is also required. Therefore, by combining the joint state data of the robotic arm, the motion information of the robotic arm's end effector is determined. Then, based on commonly used state estimation algorithms such as Kalman filtering, noise is smoothed and the delay of visual data is compensated, thereby estimating the linear velocity and angular velocity of the object in space, which constitute the generalized velocity of the objects.

[0081] Based on the end-effector tactile data and the wrist data, combined with the pose data and generalized velocity of the manipulated object, the rigid body dynamic characteristic parameters of the manipulated object are determined.

[0082] Specifically, based on end-effector tactile information, wrist force / torque measurements, and estimated pose data and generalized velocity of the manipulated object, online parameter estimation algorithms, such as filtering or least squares methods, are used to identify the rigid body dynamic characteristic parameters of the manipulated object in real time. The rigid body dynamic characteristic parameters include the mass, center of mass position, and inertia tensor of the manipulated object, thereby providing a matching object model basis for subsequent force and torque planning to cope with the actual situation of unknown object parameters or load bias.

[0083] Based on the rigid body dynamics characteristic parameters, the pose data, and the generalized velocity, combined with the preset desired motion trajectory of the robotic arm, the desired wrench for the manipulated object is obtained, and a grasping matrix is ​​constructed based on the end-effector tactile data.

[0084] Specifically, the identified rigid body dynamics characteristic parameters are used as the core. The rigid body dynamics equations are constructed by combining the pose data and generalized velocity of the manipulated object. The expected motion state of the object tracking the preset expected motion trajectory is calculated. The equations are substituted to obtain the basic force and torque requirements. Then, the expected wrench of the manipulated object is obtained by combining the deviation compensation of trajectory tracking. At the same time, in order to build a mathematical mapping bridge from the contact force to the object wrench, the position, normal and other geometric information of each contact point between the robotic arm and the manipulated object are extracted from the end-effector tactile data. After coordinate system transformation, the basic mapping matrix of each contact point is constructed and integrated to form a grasping matrix that can represent the correspondence between the contact force and the object wrench.

[0085] Based on the grasping matrix, and combined with the desired wrench, a secondary planning is performed to obtain a contact force vector that is adapted to the object being operated on.

[0086] Specifically, in order to ensure that the contact force distribution of the two arms of the robotic arm satisfies the motion requirements of the object to achieve the desired wrench, while also adapting to the robotic arm's own driving capability and avoiding load distribution imbalance, the grasping matrix is ​​used as a mapping constraint between the contact force and the desired wrench. A quadratic programming objective function is constructed with minimizing the deviation between the actual contact force and the reference distribution as its core. Constraints that conform to the robotic arm's execution capability are set. By solving the quadratic programming objective function, a contact force vector that takes into account both motion requirements and execution feasibility and adapts to the object to be manipulated is obtained.

[0087] Based on the contact force vector, the resultant force-internal force decoupling is performed to obtain the task component and internal force component of the robotic arm.

[0088] Specifically, the generalized inverse of the grasping matrix is ​​solved, and the contact force vector is projected onto the row space of the grasping matrix using the generalized inverse. This yields the task component, which can be mapped to the desired wrench and directly drives the object's movement. This task component is then subtracted from the original contact force vector to obtain the internal force component that resides in the null space of the grasping matrix, generates interaction between the arms, and does not affect the object's movement. This separates the effective force driving the object's movement from the interaction force existing only between the arms, fundamentally avoiding problems such as excessive internal pressure and object deformation caused by their coupling.

[0089] Based on the task component and the internal force component, the end joint torque command of the robotic arm is generated, and the robotic arm is controlled to perform cooperative operation on the object being manipulated through the joint torque command.

[0090] Specifically, based on the task components and internal force components, torque commands for the end joints of the robotic arm are generated, thereby converting the previously planned force components into executable torque commands for the robotic arm joints, realizing the transition from force planning to actual collaborative operation. The corresponding actuators of the robotic arm control the coordinated movement of each joint according to the joint torque commands, thereby achieving precise collaborative operation of the robotic arm on the manipulated object.

[0091] The single-target robotic arm cooperative operation control method of the present invention firstly acquires multi-source sensor data, including image data, end-effector tactile data, wrist data, and joint state data, after the robotic arm contacts the target object. This enables comprehensive perception of the contact and motion states of the target object and the robotic arm from visual, tactile, force, and joint motion perspectives, thereby avoiding state judgment biases caused by the limitations of single-sensor data. Secondly, by combining image data and joint state data, target recognition, pose estimation, and generalized velocity determination of the target object are completed. This accurately acquires the pose and velocity information of the target object in the world coordinate system, clearly defining the real-time state of the target object. This ensures that subsequent dynamic analysis and motion planning match the actual state of the target object, avoiding control inaccuracies caused by pose and velocity estimation errors, and preventing problems such as attitude drift of the target object.Furthermore, by integrating end-effector tactile data, wrist data, and the pose and generalized velocity data of the manipulated object to determine rigid body dynamics parameters, the dynamic characteristics such as the mass, center of mass, and inertia tensor of the manipulated object can be accurately analyzed. This solves the problem of mismatch between control planning and the actual object caused by unknown or offset dynamic characteristics of the manipulated object, allowing subsequent expected wrench planning to better fit the actual dynamic needs of the manipulated object and avoiding uneven load distribution and overturning risks caused by deviations in dynamic parameters. Simultaneously, based on rigid body dynamics parameters, the real-time state of the manipulated object, and a preset expected motion trajectory, the expected wrench is obtained. A grasping matrix is ​​constructed based on end-effector tactile data, realizing expected resultant force / torque planning centered on the manipulated object. The grasping matrix establishes a precise mapping relationship between the contact force of the robotic arm and the wrench applied to the manipulated object, allowing the contact force planning to directly target the desired motion needs of the manipulated object. Subsequently, a secondary planning based on the grasping matrix and the expected wrench yields a suitable contact force vector, enabling the contact force distribution of the left and right arms of the robotic arm to match the desired motion needs of the manipulated object. This rationally distributes the load between the two arms, avoiding operational instability caused by overload of a single arm or imbalanced load distribution. Simultaneously, the contact force is always planned around achieving the desired motion of the manipulated object, reducing the generation of ineffective contact forces. Next, the resultant force-internal force decoupling of the contact force vector is performed to obtain the task component and the internal force component. This achieves explicit separation between the effective task component affecting the motion of the manipulated object and the internal force component that only generates interaction between the two arms. This fundamentally solves the coupling problem between resultant force and internal force from the control structure perspective, avoiding deformation of the manipulated object, gripper slippage, or robot arm overload caused by excessive internal force leading to excessive internal squeezing pressure between the two arms. It also avoids contact instability caused by insufficient internal force and eliminates the generation of repulsive forces. To eliminate potential hazards, the system ensures that the two arms form a coordinated force rather than a repulsive force pushing each other. Finally, based on the task component and internal force component, the system generates end-joint torque commands and controls the robotic arms to perform coordinated operations. This accurately translates the planned contact force requirements into execution commands for the robotic arm joints, ensuring that the joint movements of the robotic arms strictly follow the control requirements that conform to the actual state and desired movement of the object being operated on. This ensures that the object always moves along the preset trajectory, achieving precise and stable control of the object by the robotic arms. From the execution level, this guarantees the stability of the coordinated operation of the two arms and improves the safety of single-target coordinated operation of the two arms.

[0092] Optionally, the step of performing target recognition and pose estimation based on the image data and the joint state data to determine the operation object and its pose data and generalized velocity within the preset operation area includes:

[0093] Based on the image data, the target object within the preset operation area is identified and segmented to obtain a three-dimensional point cloud representation of the operation object;

[0094] Based on the pose estimation algorithm, the position and orientation of the manipulated object in the world coordinate system are determined according to the 3D point cloud representation, and the position and orientation are used as the pose data;

[0095] Based on the time series of the pose data, state estimation is performed in conjunction with the joint state data to obtain the linear velocity and angular velocity of the manipulated object, and the linear velocity and angular velocity are used as the generalized velocity.

[0096] Specifically, firstly, computer vision algorithms are used to perform target recognition and segmentation on the collected image data of the preset operation area. Feature information of the operation object is extracted from the image background and fitted into a three-dimensional point cloud representation to restore the spatial geometric shape of the operation object. Then, based on the pose estimation algorithm, the spatial position coordinates and spatial orientation angle of the operation object in the world coordinate system are calculated based on the three-dimensional point cloud of the operation object. The two are integrated as the pose data x0∈SE(3) of the operation object to determine the static spatial state of the object. Finally, a time series is formed based on the continuously collected pose data. Combined with the joint state data of the robotic arm collected by the joint encoder, current sensor, etc., the Kalman filter state estimation algorithm is used to perform noise smoothing and visual delay compensation on the data to calculate the linear velocity and angular velocity of the operation object in space. The linear velocity and angular velocity are integrated into the generalized velocity (v) of the operation object. o ∈R 6 This invention provides a comprehensive estimation of the static pose and dynamic velocity of the manipulated object, offering accurate object state data for subsequent dynamic modeling and motion planning. In a preferred embodiment, taking the collaborative handling of cardboard boxes by a robotic arm in an industrial warehousing scenario as an example, the robotic arm's RGB-D vision system acquires image data of a preset operating area. First, a visual segmentation algorithm identifies the cardboard box target from the image and removes background information such as shelves and the ground. The point cloud data of the cardboard box is then fitted and optimized to obtain an accurate 3D point cloud representation. Next, based on the ICP pose estimation algorithm, the 3D point cloud of the cardboard box is registered with a preset cardboard box model point cloud, calculating the cardboard box's 3D position coordinates (x, y, z) in the world coordinate system and its spatial orientation (α, β, γ) expressed in Euler angles. This is used as the cardboard box's pose. Data was collected; subsequently, 50 consecutive frames of carton pose data were acquired to form a time series. This data, combined with joint state data such as joint angles and angular velocities collected by the robotic arm's joint encoder, was processed using an extended Kalman filter (EKF) algorithm to smooth Gaussian noise from visual acquisition and compensate for approximately 20ms transmission delay in visual data. Finally, the linear velocities of the carton along the x, y, and z axes in space were calculated to be 0.1 m / s, 0.05 m / s, and 0.2 m / s, respectively, and the angular velocities around the x, y, and z axes were calculated to be 0.02 rad / s, 0.01 rad / s, and 0.03 rad / s, respectively. These parameters together constitute the generalized velocity v of the carton.o =[0.1,0.05,0.2,0.02,0.01,0.03] T ∈R 6 .

[0097] In this embodiment of the invention, the spatial geometric features of the manipulated object are accurately restored by fitting the three-dimensional point cloud of the visual image data, providing a reliable geometric basis for pose estimation and avoiding pose estimation deviations caused by insufficient two-dimensional image information. At the same time, the generalized velocity is obtained by combining the pose data time series and joint state data to estimate the state, which makes up for the deficiency that it is difficult to accurately obtain the dynamic motion parameters of the object by relying solely on visual data. The noise smoothing and delay compensation are achieved through the filtering algorithm, which greatly improves the accuracy and real-time performance of the linear velocity and angular velocity estimation of the manipulated object, and finally achieves high-precision estimation of the static pose and dynamic velocity of the manipulated object.

[0098] Optionally, determining the rigid body dynamics characteristic parameters of the manipulated object based on the end-effector tactile data and the wrist data, combined with the pose data and generalized velocity of the manipulated object, includes:

[0099] Based on the end-effector tactile data, the contact force distribution data between the end effector of the robotic arm and the manipulated object is determined;

[0100] Based on the contact force distribution data and the wrist data, the contact wrench applied by the end effector to the object being operated on is obtained;

[0101] Based on the pose data, the contact wrench is transformed from the local contact coordinate system to the object coordinate system of the object being operated on, thereby obtaining the equivalent external force wrench in the object coordinate system;

[0102] Differentiate the generalized velocity to obtain the generalized acceleration of the object being operated on.

[0103] A dynamic parameter identification model is established based on the equivalent external force wrench, the generalized acceleration, and the generalized velocity.

[0104] The dynamic parameter identification model is recursively solved by the least squares method or filtering algorithm to obtain the mass, center of mass, and inertia tensor of the manipulated object, and the mass, center of mass, and inertia tensor are used as the rigid body dynamic characteristic parameters.

[0105] Specifically, this embodiment first extracts stress and pressure distribution information of the contact interface from the end-effector tactile data collected by the end-effector tactile array / tactile image sensor to determine the contact force distribution data between the robotic arm end effector and the manipulated object. Then, the contact force distribution data is fused and calculated with the wrist data collected by the wrist six-dimensional force / torque sensor to obtain the contact wrench applied by the end effector to the manipulated object (a comprehensive physical quantity of the spatial resultant force and resultant torque applied to the manipulated object when the robotic arm end effector interacts with the manipulated object). Among them, the contact force distribution data is collected by the tactile array or tactile image sensor at the end of the robotic arm, and the core includes the contact interface between the robotic arm end effector and the manipulated object. The pressure / stress distribution, the location and area of ​​the contact point, the force distribution in the contact normal direction, and the force distribution trend in the contact tangential direction can also include detailed data such as the force changes corresponding to local deformation of the contact area and the dynamic changes in the contact force distribution caused by micro-slippage. It can accurately reflect the spatial distribution characteristics of the force on the contact surface. The wrist data is precise quantitative data collected by the six-dimensional force / torque sensor of the robotic arm wrist. Specifically, it includes the contact force values ​​along the three coordinate axes of space (x, y, z) and the contact torque values ​​around the three coordinate axes of space (x, y, z). It is a comprehensive measurement of the overall force and torque between the end effector and the manipulated object, directly reflecting the magnitude of the resultant force and resultant torque of their interaction. Subsequently, based on the pose data of the manipulated object, the contact wrench is transformed from the local contact coordinate system of the end effector to the object coordinate system of the manipulated object through a coordinate transformation matrix, thus obtaining an equivalent external force wrench adapted for object dynamics analysis. Simultaneously, a differential operation is performed on the generalized velocity of the manipulated object to calculate its generalized acceleration. Specifically, in a preferred embodiment of the invention, the generalized acceleration is calculated by performing a differential operation on the generalized velocity of the manipulated object. The specific differential operation used is a first-order numerical differential operation, since the generalized velocity is a discrete time series data (dimension R) estimated from the pose data time series combined with joint state data. 6 Since discrete data (including linear velocity and angular velocity components) cannot be directly analyzed analytically, first-order numerical differentiation methods suitable for discrete data are used in engineering. Commonly used methods include forward difference, backward difference, or central difference. Among them, the central difference method is preferred due to its higher accuracy and better noise resistance. Specifically, for continuously acquired multi-frame generalized discrete velocity data, the following formula is used for calculation:

[0106] ;

[0107] in, Let the generalized acceleration be the acceleration in the k-th frame. and Let be the generalized velocities of the adjacent frames of the k-th frame, respectively. The time interval for sensor data acquisition is defined as follows: The equivalent external force wrench, generalized acceleration, and generalized velocity are combined, and a dynamic parameter identification model is constructed using mass, center of mass, and inertia tensor as parameters to be determined. This model is then solved in real-time using algorithms such as recursive least squares or extended Kalman filtering to separate the mass, center of mass, and inertia tensor of the manipulated object from the coupled force and motion data. Multiple sets of synchronously acquired equivalent external force wrench, generalized acceleration, and generalized velocity data are substituted into the dynamic equations. ;in, For the inertia tensor of the object being manipulated, For gravity, Let be the centroid position vector. For the generalized acceleration of the object being manipulated, Let w be the generalized velocity of the manipulated object, and w be the equivalent external force acting on the manipulated object. The recursive least squares method is used to update the velocity frame by frame. , and The estimated values ​​of the parameters are then used to deduce the mass, center of mass, and inertia tensor of the carton, thus completing the online identification of the dynamic parameters.

[0108] In this embodiment of the invention, a contact wrench is acquired by fusing end-effector tactile data and wrist data. This fully leverages the advantages of contact distribution perception and the measurement advantages of the wrist force / torque sensor, ensuring both detailed and accurate perception of contact force. This provides a reliable force data foundation for identifying dynamic parameters. Furthermore, a numerical solution algorithm is used for recursive solving, separating core rigid body dynamic characteristic parameters such as mass, center of mass, and inertia tensor from the coupled force and motion data. This solves the modeling problems caused by unknown mass, center of mass offset, and load variations of the manipulated object in actual operations. It provides a dynamic model basis that closely matches the actual manipulated object, effectively avoiding problems such as object posture drift, load distribution imbalance, overturning, or slippage caused by mismatched dynamic parameters. This improves the accuracy and adaptability of force control and motion control in collaborative robotic arm operations.

[0109] Optionally, obtaining the desired wrench for the manipulated object based on the rigid body dynamics characteristic parameters, the pose data, and the generalized velocity, combined with the preset desired motion trajectory of the robotic arm, includes:

[0110] The desired generalized acceleration of the manipulated object is determined by calculating the preset desired motion trajectory.

[0111] Substituting the rigid body dynamics characteristic parameters, the pose data, and the desired generalized acceleration into the rigid body dynamics equation of the manipulated object, the ideal total wrench of the manipulated object is obtained.

[0112] The preset desired motion trajectory is compared with the pose data of the manipulated object to obtain the pose tracking error;

[0113] Based on the pose tracking error, a feedback correction wrench is determined using a preset impedance or admittance control law.

[0114] The ideal master wrench and the feedback correction wrench are superimposed to obtain the desired wrench for the object being operated on.

[0115] Specifically, the desired generalized acceleration of the manipulated object is first obtained by solving the preset desired motion trajectory. Then, the model feedforward force is calculated based on the dynamic equations, and combined with error-based feedback control force. Specifically, the identified rigid body dynamic characteristic parameters such as mass, center of mass, and inertia tensor, along with the current pose data, generalized velocity, and the desired generalized acceleration derived from the preset desired motion trajectory, are substituted into the rigid body dynamic equations of the manipulated object to calculate the theoretical force / torque required to achieve the ideal motion, i.e., the ideal total wrench, thus completing feedforward control. Simultaneously, the pose tracking error is calculated by comparing the preset desired motion trajectory with the real-time pose data of the manipulated object. Then, through a preset impedance or admittance control law, this position / attitude error is mapped into a compliant feedback correction wrench for correcting trajectory deviations. Finally, the ideal total wrench and the feedback correction wrench are vector-superimposed to obtain the final desired wrench acting on the manipulated object, achieving a combination of model feedforward and error feedback, ensuring that the control combines accuracy and compliance.

[0116] In this embodiment of the invention, by combining feedforward control based on an accurate model with compliant feedback control based on errors, the motion control performance of the manipulated object in dual-arm cooperative operation is improved. First, the ideal master wrench can pre-compensate for known dynamic effects such as the object's inertia, gravity, and Coriolis / centrifugal force, enabling rapid and accurate response to desired motion commands. This significantly reduces tracking delay and decreases dependence on high-gain feedback, thereby improving the system's dynamic response speed and overall tracking accuracy. Second, the feedback correction wrench, through impedance or admittance control laws, transforms unavoidable model errors, sensor noise, and unmodeled external disturbances (such as minor collisions or wind resistance) into compliant correction forces. This improves environmental interaction compliance and robustness, absorbs disturbances, and prevents system oscillations, contact instability, or object slippage caused by excessive rigid correction forces.

[0117] Optionally, constructing the grasping matrix based on the end-effector tactile data includes:

[0118] Based on the end-effector tactile data, determine the contact position and contact normal of each contact point between the end effector of the robotic arm and the manipulated object;

[0119] Transform the contact position and the contact normal from the local contact coordinate system to the object coordinate system of the manipulated object to obtain the contact position and contact normal in the object coordinate system;

[0120] Based on the contact position and contact normal of each contact point in the object coordinate system, determine the single-point grasping matrix corresponding to each contact point;

[0121] The single-point grasping matrices of all the contact points are stacked and combined to obtain the grasping matrix.

[0122] Specifically, for a rigid body object, its dynamics are expressed as follows:

[0123] ;

[0124] in, The pose data of the object being manipulated. For the operation object in Inertia matrix under pose The Coriolis force and centrifugal force matrix of the manipulated object. For the generalized acceleration of the object being manipulated, For the generalized velocity of the object being operated on, For the operation object in The gravity term vector in pose (represents the equivalent force and torque of gravity acting on the manipulated object in the object coordinate system). To capture the matrix, The contact force vector, The external disturbance wrench. Combining the dynamic expression, the equivalent external force wrench acting on the workpiece consists of the total force wrench acting on the workpiece and the external disturbance wrench, i.e.: w Therefore, to construct the grasping matrix, the first step is to utilize the microscopic contact information provided by end-effectors, such as tactile arrays, to analyze the contact position and normal of each contact point between the robotic arm's end effector and the manipulated object. Next, the contact position and normal of each contact point are unified from the sensor's local contact coordinate system to the object's coordinate system centered on the manipulated object through coordinate transformation. Finally, the single-point grasping matrix G is calculated for each contact point using the single-point grasping matrix formula. i The physical meaning of the single-point grasping matrix is ​​that if a unit normal force is applied to a single point, then the equivalent force and torque generated by that point about the center of the carton are determined by the single-point grasping matrix G. i The corresponding column is given. Its expression is:

[0125] G i =[I3,0 3×3 ;[r i ]×I3]Ti ;

[0126] Where I3 is a 3-order identity matrix, 0 3×3 It is a 3rd order zero matrix, [r i ] is the position vector r of the contact point. i The cross product matrix formed, T i This is the rotational transformation matrix that maps local contact forces to the object's coordinate system; its construction depends on the contact normal. G i This defines how the force / torque applied at the contact point contributes to the equivalent force and torque acting on the centroid of the manipulated object (or the origin of the object's coordinate system). Finally, the calculated G for all contact points (including those of the left and right arms) is... i Vertical stacking is performed, which combines them into a complete grasping matrix G. For example, G is calculated for all five contact points. i G L1 G L2 G L3 G L4 G L5 , five G i Vertical stacking forms a 6×15 grasping matrix (assuming each contact force is 3-dimensional, then 5 points have a total of 15 dimensions). The grasping matrix establishes a linear mapping from all end contact force vectors to the total wrench w applied to the manipulated object, i.e., w=Gf, which serves as the basis for subsequent advanced control algorithms such as force distribution and resultant force-internal force decoupling.

[0127] In this embodiment of the invention, by dynamically constructing a grasping matrix using high-resolution tactile data, the problem of force control caused by uncertain, asymmetrical, or time-varying contact geometry in dual-arm collaborative operation is solved. The true position and normal of each contact point are directly obtained through tactile data and can be updated online (such as when the object slightly slips), so that the grasping matrix always reflects the true force transmission path, avoiding force distribution errors and internal stress accumulation caused by model errors.

[0128] Optionally, the step of performing secondary planning based on the grasping matrix and the desired wrench to obtain a contact force vector adapted to the operation object includes:

[0129] Based on the centroid of the manipulated object, determine the reference distribution vector of the contact force of the manipulated object between the left and right arms of the robotic arm;

[0130] With the optimization objective of minimizing the deviation between the contact force vector and the reference allocation vector, and minimizing the internal force components in the contact force vector, a quadratic programming objective function is constructed, and constraints on the objective function are set.

[0131] Substitute the objective function and the constraints into a preset quadratic programming solver to obtain the single-arm contact force vectors corresponding to the left arm and the right arm, respectively.

[0132] The contact force vector is obtained based on the single-arm contact force vectors corresponding to the left arm and the right arm, respectively.

[0133] Specifically, the appropriate contact force vector is obtained through quadratic programming. Specifically, it is first determined based on the reference distribution vector determined by the centroid of the manipulated object, i.e., using the centroid position vector C. o Contact geometry (contact point position r) L and r R Calculate the load distribution factor α, and construct the ideal force distribution f under static or quasi-static conditions accordingly. ref For example, to distribute the vertical resultant force proportionally. Specifically, when solving for the contact force vector using quadratic programming, the load distribution coefficient is first calculated based on the centroid estimate and the contact point positions. Then, the projection of the centroid onto the line connecting the left and right contact points is obtained using the centroid projection formula. That is:

[0134] ;

[0135] Where, r L and r R These are the contact points of the left arm and the right arm, respectively; C o Let u be the centroid position vector, and u be the unit direction vector. Then, via... Determine the ideal static load distribution, and then construct a contact force reference distribution vector so that the resultant forces in the vertical directions of the left and right arms satisfy the following: and .in, This represents the reference contact force component that the left arm end effector needs to bear in the vertical direction. This represents the reference contact force component that the right arm end effector needs to bear in the vertical direction. Here, mg is the load distribution factor, and mg is the weight of the object being manipulated. The inertial force required to achieve the desired vertical acceleration, Let be the desired acceleration component of the manipulated object in the vertical direction. Then, with the optimization objective of minimizing the deviation between the contact force vector and the reference distribution vector, while simultaneously minimizing the internal force components in the contact force vector, a quadratic programming objective function is constructed, the expression of which is:

[0136] ;

[0137] in, To minimize the contact force vector f, where f is the contact force vector, Assign a reference vector to the contact force. The weight matrix is ​​the weighted contact force deviation. The weight matrix is ​​the weighted matrix of the internal force components. These are the internal force components in the contact force vector. The regularization coefficient is . It is the 2-norm of the vector.

[0138] Next, the constraints of the objective function are set, wherein the constraints include:

[0139] Equality constraint, i.e., Gf=w des This ensures that the force distribution can accurately achieve the desired wrench; where G is the gripping matrix, f is the contact force vector, and w des For the desired wrench.

[0140] Inequality constraint, i.e., A f f≤b f It consists of a linearized friction cone, upper and lower bounds of the normal force, and a contact torque limiter, ensuring that the force is physically feasible; among them, A f and b f These are matrices and vectors used to linearize a set of physical constraints.

[0141] Internal force upper limit constraint, i.e. ,in, The maximum value of the internal force component in the contact force vector directly suppresses excessive internal squeezing / repulsion forces. Finally, by solving this constrained quadratic programming problem, the single-arm contact force vectors corresponding to the left and right arms are obtained. and And combine them to obtain the overall contact force vector f. =[ ] T .

[0142] In this embodiment of the invention, by introducing a centroid-based reference allocation vector, the system can automatically set the ideal output ratio of the left and right arms according to the actual load distribution of the object (such as cargo offset). Through optimization, the actual allocation is made as close as possible to this ideal state, thus avoiding single-arm overload and other arm drag caused by blind average or fixed-ratio allocation when the centroid shifts. This effectively prevents object tipping, joint torque saturation, or low energy efficiency. Secondly, internal force suppression is incorporated into the optimization framework. Internal force components are directly penalized in the objective function, and an upper bound for internal forces is set in the constraints. The optimization solution automatically finds the force solution that minimizes mutual compression or repulsion between the two arms while meeting motion requirements, thereby proactively reducing internal stress, lowering the risk of gripper slippage, and protecting the object from compression deformation. Finally, the introduction of multiple hard constraints ensures the physical feasibility and safety of the solution, improving the success rate, stability, and safety of single-object handling in complex environments.

[0143] Optionally, the step of decoupling the resultant force and internal force based on the contact force vector to obtain the task component and internal force component of the robotic arm includes:

[0144] Obtain the generalized inverse of the capture matrix;

[0145] Based on the generalized inverse of the grasping matrix, the contact force vector is projected onto the row space of the grasping matrix to obtain the task component of the desired wrench;

[0146] Subtracting the task component from the contact force vector yields the internal force component of the contact force vector located in the null space of the grasping matrix.

[0147] Specifically, any contact force vector can be decomposed into a task component and an internal force component, i.e.: f = f task +f int Where f is the contact force vector, f task For task components, f int These are the internal force components. First, the generalized inverse (usually a pseudo-inverse) of the grasping matrix is ​​calculated to obtain the mathematical tool for decomposition. Then, using the generalized inverse, the contact force vector f is projected onto the row space of the grasping matrix; the resulting projection is the task component, i.e.:

[0148] f task =G + w des ;

[0149] Among them, G + It is the generalized inverse of the capture matrix G, and the task components satisfy Gf task =w des This means that it can produce the desired wrench. des The partial contact force is the effective component that enables the object's motion. Finally, subtracting the task component from the original contact force vector f yields the internal force component, i.e.:

[0150] f int =f f task ;

[0151] Furthermore, according to the definition of null space, the internal force components reside in the null space of the grasping matrix, satisfying Gf int =0, therefore, it has no effect on the overall motion and rotation of the object. This process mathematically completes the orthogonal decomposition of any contact force vector, conceptually and numerically separating the force used to drive motion from the force that only produces internal stress.

[0152] In this embodiment of the invention, by using zero-space projection, internal force components that do not produce macroscopic motion effects but consume energy, increase load, and may cause deformation or slippage of the object are isolated, transforming them from implicit couplings into explicit variables. This provides accurate data for subsequent real-time monitoring, alarms, and dynamic suppression. Secondly, by ensuring that the task components meet the desired wrench and limiting the internal force components within a safe threshold, the control structure ensures that the forces of the two robotic arms mainly contribute to the target motion of the object being transported collaboratively, rather than opposing each other. This effectively prevents wasted effort, energy waste, excessive internal stress on the object, and clamping instability caused by excessive internal forces.

[0153] Optionally, generating the end-joint torque command of the robotic arm based on the task component and the internal force component includes:

[0154] Based on the task component and the internal force component, determine the end contact wrenches corresponding to the end effectors of the left and right arms of the robotic arm;

[0155] Obtain the Jacobian matrix of the robotic arm;

[0156] The Jacobian matrix is ​​used to map the end contact wrench to the initial torque values ​​of the joints corresponding to the end actuators of the left and right arms.

[0157] Based on the joint state data in the multi-source sensor data, a torque compensation amount is generated;

[0158] The initial torque value of each joint is superimposed with the torque compensation amount to obtain the target torque value of each joint;

[0159] Based on the target torque value of the joint, generate the end joint torque commands corresponding to the left arm and the right arm respectively;

[0160] The robotic arm is controlled to perform coordinated operations on the object according to the joint torque commands of the left and right arms.

[0161] Specifically, this embodiment first clarifies that the input consists of task components and internal force components, but the more direct operational basis is the end contact wrench determined by these task components and internal force components, and applied to the end effectors of the left and right arms respectively. Next, force-to-torque mapping is performed using the Jacobian matrix of the robotic arm. The expression for the standard torque mapping is:

[0162] ;

[0163] in, This refers to the joint torque command for the i-th arm (left arm / right arm) of the robotic arm. Let be the transpose of the Jacobian matrix of the i-th arm, used to map the end effector wrench to the joint torque space. For the end effector of the i-th arm, there is an end contact wrench. Let be the projection matrix of the Jacobian matrix null space of the i-th arm, used for the mapping of constraint internal force components. This is the zero-space joint torque term corresponding to the internal force component of the i-th arm, which only acts on the internal force adjustment of both arms. This is the gravity compensation torque term (torque compensation amount) for the i-th arm, used to counteract the effect of the robot arm's own gravity on the joint torque. Additionally, the transpose in the standard torque mapping expression is used to implement the torque from the end effector in Cartesian space. Joint space torque The mathematical basis of the mapping is then established. Subsequently, a torque compensation value is generated and superimposed on the initial torque value to counteract the interference caused by the robot arm's own gravity on the end effector force. Finally, independent left and right arm joint torque commands are generated and executed, thus completing the entire process from high-level force planning to low-level execution. When the manipulated object reaches the target position, the desired wrench is lowered at a preset rate to maintain relative constraints, slowly lowering and releasing preload.

[0164] In this embodiment of the invention, by accurately mapping and compensating based on robot kinematics and dynamics, the high-level task and internal force planning results are transformed into low-level control commands that can directly drive joint motors, thereby forming a closed loop from decision-making to physical execution, ensuring the integrity, accuracy and stability of the dual-arm collaborative operation.

[0165] Optionally, it also includes:

[0166] When the robotic arm performs a collaborative operation on the object being manipulated, based on the end-effector tactile data and the wrist data, combined with the contact force vector, the contact pressure distribution and contact slippage trend information between the end effector of the robotic arm and the object being manipulated are extracted.

[0167] Based on the contact pressure distribution and the rigid body dynamics characteristic parameters of the manipulated object, the actual friction coefficient of the contact point is determined.

[0168] The friction margin of the contact point is determined based on the actual friction coefficient and the preset safe friction coefficient threshold.

[0169] Based on the friction margin of all the contact points, determine whether there is a risk of slippage between the robotic arm and the operated object;

[0170] If so, the normal force component of the contact force vector, the stiffness coefficient and damping coefficient of the impedance model of the operating object and the end effector are adjusted according to the preset adjustment rules.

[0171] If not, then the contact force vector, the stiffness coefficient, and the damping coefficient remain unchanged.

[0172] Specifically, during operation, contact pressure distribution and contact slippage trend information are continuously extracted based on end-effector tactile data and wrist data, combined with contact force vectors. In a preferred embodiment of the invention, the slippage severity s can be continuously estimated using the deformation field, shear displacement, and contact area change rate characteristics of the tactile image. slip ∈[0,1]. Next, based on the contact pressure distribution and rigid body dynamic characteristic parameters, especially mass, the actual friction coefficient is determined by online estimation using the measured ratio of tangential force to normal force, combined with the motion state. Then, the friction margin is calculated based on the actual friction coefficient. The calculation formula is:

[0173] ρ=μF n ;

[0174] Where μ is the actual friction coefficient, ρ is the friction margin, and F n For normal force, The force is tangential. In this embodiment of the invention, ρ>0 is used as a safety criterion, and then the existence of slippage risk is determined based on the friction margin of all contact points. In another preferred embodiment of the invention,

[0175] When a risk is detected, a preset adjustment rule is triggered, which increases the normal force component of the contact force vector. Specifically, it actively increases the normal preload when the risk of slippage rises to increase the maximum static friction force, thereby actively suppressing slippage. Simultaneously, the stiffness coefficient and damping coefficient are adjusted in real time to actively suppress slippage of the operated object. The expression for the preset adjustment rule is:

[0176] ;

[0177] ;

[0178] ;

[0179] in, This is the compensation increment for the normal contact force. and These are the gain parameters corresponding to the slip coefficient and the contact pressure coefficient, respectively. s is a saturation function slip ρ represents the slip severity, and ρ represents the friction margin. This is the stiffness coefficient. and These are the adaptive adjustment coefficients for stiffness and damping, respectively. is the damping coefficient.

[0180] In another preferred embodiment of the invention, a dynamic friction margin constraint is introduced, namely:

[0181] μF n ≥ +δ(s slip );

[0182] Wherein, δ(s) slip ) represents the slip index s slip The increased safety margin function is used to automatically increase the safety margin when the system approaches the slip state, thus automatically leaving a larger safety margin when approaching slip.

[0183] In this embodiment of the invention, by fusing end-effector tactile data, wrist data, and contact force vectors, information on contact pressure distribution and contact slippage trend is extracted. Then, the actual friction coefficient of the contact point is determined by combining the rigid body dynamic characteristic parameters of the manipulated object. By comparing it with a preset safe friction coefficient threshold, the friction margin is obtained to determine whether there is a risk of slippage. When there is a risk of slippage, the normal force component of the contact force vector and the stiffness and damping coefficient of the impedance model are actively adjusted. This achieves active perception and dynamic suppression of the slippage risk of the manipulated object. It avoids safety problems such as slippage and falling of the manipulated object due to insufficient contact force or too small friction margin. Furthermore, by adaptively adjusting the impedance model parameters, the system maintains operational compliance while suppressing slippage, avoiding local deformation of the manipulated object or overload of the robotic arm joints due to excessive normal force. This effectively improves the stability and operational safety of gripping during the collaborative operation of the robotic arm, and also enhances the system's adaptability to manipulated objects of different materials and loads.

[0184] Combination Figure 2 As shown, another embodiment of the present invention provides a single-target robotic arm cooperative operation control system, comprising:

[0185] The data acquisition unit is used to acquire multi-source sensor data of the robotic arm after the robotic arm contacts the object being operated. The multi-source sensor data includes image data of the robotic arm corresponding to a preset operation area, tactile data of the robotic arm's end effector, wrist data, and joint state data.

[0186] An object recognition unit is used to perform target recognition and pose estimation based on the image data and the joint state data, and to determine the operation object and the pose data and generalized velocity of the operation object within the preset operation area.

[0187] The data analysis unit is used to determine the rigid body dynamic characteristic parameters of the manipulated object based on the end-effector tactile data and the wrist data, combined with the pose data and generalized velocity of the manipulated object;

[0188] The dynamic identification unit is used to obtain the desired wrench of the manipulated object based on the rigid body dynamic characteristic parameters, the pose data and the generalized velocity, combined with the preset desired motion trajectory of the robotic arm;

[0189] A matrix construction unit is used to construct a grasping matrix based on the end-effector tactile data;

[0190] The contact force planning unit is used to perform secondary planning based on the grasping matrix and the desired wrench to obtain a contact force vector that is adapted to the operation object.

[0191] The resultant force decoupling unit is used to decouple the resultant force and internal force according to the contact force vector to obtain the task component and internal force component of the robotic arm.

[0192] The control unit is used to generate end joint torque commands for the robotic arm based on the task component and the internal force component, and to control the robotic arm to perform cooperative operations on the manipulated object through the joint torque commands.

[0193] The single-target robotic arm collaborative operation control system of the present invention has the same advantages over the prior art as the single-target robotic arm collaborative operation control method described above, and will not be repeated here.

[0194] While the present invention has been disclosed above, its scope of protection is not limited thereto. Those skilled in the art can make various changes and modifications without departing from the spirit and scope of the present invention, and all such changes and modifications will fall within the scope of protection of the present invention.

Claims

1. A single-target robot arm cooperative operation control method characterized by comprising: include: When the robot's robotic arm comes into contact with the object being operated, it acquires multi-source sensor data of the robotic arm. The multi-source sensor data includes image data of the robotic arm corresponding to a preset operating area, tactile data of the robotic arm's end effector, wrist data, and joint state data. Based on the image data, target recognition and pose estimation are performed in conjunction with the joint state data to determine the operation object and the pose data and generalized velocity of the operation object within the preset operation area. Based on the end-effector tactile data and the wrist data, combined with the pose data and generalized velocity of the manipulated object, the rigid body dynamic characteristic parameters of the manipulated object are determined. Specifically, this includes: determining the contact force distribution data between the end effector of the robotic arm and the manipulated object based on the end-effector tactile data; obtaining the contact wrench applied to the manipulated object by the end effector based on the contact force distribution data and the wrist data; transforming the contact wrench from the local contact coordinate system to the object coordinate system of the manipulated object based on the pose data to obtain the equivalent external force wrench in the object coordinate system; performing differential calculation on the generalized velocity to obtain the generalized acceleration of the manipulated object; establishing a dynamic parameter identification model based on the equivalent external force wrench, the generalized acceleration, and the generalized velocity; and recursively solving the dynamic parameter identification model using the least squares method or a filtering algorithm to obtain the mass, center of mass, and inertia tensor of the manipulated object, and using the mass, center of mass, and inertia tensor as the rigid body dynamic characteristic parameters. Based on the rigid body dynamics characteristic parameters, the pose data, and the generalized velocity, combined with the robot arm's preset desired motion trajectory, the desired wrench of the manipulated object is obtained, and a grasping matrix is ​​constructed based on the end-effector tactile data. Specifically, obtaining the desired wrench of the manipulated object based on the rigid body dynamics characteristic parameters, the pose data, and the generalized velocity, combined with the robot arm's preset desired motion trajectory, includes: calculating the desired generalized acceleration of the manipulated object based on the preset desired motion trajectory; substituting the rigid body dynamics characteristic parameters, the pose data, and the desired generalized acceleration into the rigid body dynamics equations of the manipulated object to obtain the ideal total wrench of the manipulated object; comparing the preset desired motion trajectory with the pose data of the manipulated object to obtain the pose tracking error; determining a feedback correction wrench based on the pose tracking error using a preset impedance or admittance control law; and superimposing the ideal total wrench and the feedback correction wrench to obtain the desired wrench of the manipulated object. Based on the grasping matrix, and combined with the desired wrench, a secondary planning is performed to obtain a contact force vector that is adapted to the operation object; Based on the contact force vector, the resultant force-internal force decoupling is performed to obtain the task component and internal force component of the robotic arm; Based on the task component and the internal force component, the end joint torque command of the robotic arm is generated, and the robotic arm is controlled to perform cooperative operation on the object being manipulated through the joint torque command.

2. The single-target robot cooperative operation control method according to claim 1, characterized by, The step of performing target recognition and pose estimation based on the image data and the joint state data to determine the operation object and its pose data and generalized velocity within the preset operation area includes: Based on the image data, the target object within the preset operation area is identified and segmented to obtain a three-dimensional point cloud representation of the operation object; Based on the pose estimation algorithm, the position and orientation of the manipulated object in the world coordinate system are determined according to the 3D point cloud representation, and the position and orientation are used as the pose data; Based on the time series of the pose data, state estimation is performed in conjunction with the joint state data to obtain the linear velocity and angular velocity of the manipulated object, and the linear velocity and angular velocity are used as the generalized velocity.

3. The single-target robotic arm cooperative operation control method according to claim 1, characterized in that, The step of constructing a grasping matrix based on the end-effector tactile data includes: Based on the end-effector tactile data, determine the contact position and contact normal of each contact point between the end effector of the robotic arm and the manipulated object; Transform the contact position and the contact normal from the local contact coordinate system to the object coordinate system of the manipulated object to obtain the contact position and contact normal in the object coordinate system; Based on the contact position and contact normal of each contact point in the object coordinate system, determine the single-point grasping matrix corresponding to each contact point; The single-point grasping matrices of all the contact points are stacked and combined to obtain the grasping matrix.

4. The single-target robotic arm cooperative operation control method according to claim 1, characterized in that, The process of performing secondary planning based on the grasping matrix and the desired wrench to obtain a contact force vector adapted to the operation object includes: Based on the centroid of the manipulated object, determine the reference distribution vector of the contact force of the manipulated object between the left and right arms of the robotic arm; With the optimization objective of minimizing the deviation between the contact force vector and the reference allocation vector, and minimizing the internal force components in the contact force vector, a quadratic programming objective function is constructed, and constraints on the objective function are set. Substitute the objective function and the constraints into a preset quadratic programming solver to obtain the single-arm contact force vectors corresponding to the left arm and the right arm, respectively. The contact force vector is obtained based on the single-arm contact force vectors corresponding to the left arm and the right arm, respectively.

5. The single-target robotic arm cooperative operation control method according to claim 1, characterized in that, The step of decoupling the resultant force and internal force based on the contact force vector to obtain the task component and internal force component of the robotic arm includes: Obtain the generalized inverse of the capture matrix; Based on the generalized inverse of the grasping matrix, the contact force vector is projected onto the row space of the grasping matrix to obtain the task component of the desired wrench; Subtracting the task component from the contact force vector yields the internal force component of the contact force vector located in the null space of the grasping matrix.

6. The single-target robotic arm cooperative operation control method according to claim 1, characterized in that, The step of generating the end-joint torque command of the robotic arm based on the task component and the internal force component includes: Based on the task component and the internal force component, determine the end contact wrenches corresponding to the end effectors of the left and right arms of the robotic arm; Obtain the Jacobian matrix of the robotic arm; The Jacobian matrix is ​​used to map the end contact wrench to the initial torque values ​​of the joints corresponding to the end actuators of the left and right arms. Based on the joint state data in the multi-source sensor data, a torque compensation amount is generated; The initial torque value of each joint is superimposed with the torque compensation amount to obtain the target torque value of each joint; Based on the target torque value of the joint, generate the end joint torque commands corresponding to the left arm and the right arm respectively; The robotic arm is controlled to perform coordinated operations on the object according to the joint torque commands of the left and right arms.

7. The single-target robotic arm cooperative operation control method according to claim 3, characterized in that, Also includes: When the robotic arm performs a collaborative operation on the object being manipulated, based on the end-effector tactile data and the wrist data, combined with the contact force vector, the contact pressure distribution and contact slippage trend information between the end effector of the robotic arm and the object being manipulated are extracted. Based on the contact pressure distribution and the rigid body dynamics characteristic parameters of the manipulated object, the actual friction coefficient of the contact point is determined. The friction margin of the contact point is determined based on the actual friction coefficient and the preset safe friction coefficient threshold. Based on the friction margin of all the contact points, determine whether there is a risk of slippage between the robotic arm and the object being operated; If so, the normal force component of the contact force vector, the stiffness coefficient and damping coefficient of the impedance model of the operating object and the end effector are adjusted according to the preset adjustment rules. If not, then the contact force vector, the stiffness coefficient, and the damping coefficient remain unchanged.

8. A single-target robotic arm cooperative operation control system, characterized in that, include: The data acquisition unit is used to acquire multi-source sensor data of the robotic arm after the robotic arm contacts the object being operated. The multi-source sensor data includes image data of the robotic arm corresponding to a preset operation area, tactile data of the robotic arm's end effector, wrist data, and joint state data. An object recognition unit is used to perform target recognition and pose estimation based on the image data and the joint state data, and to determine the operation object and the pose data and generalized velocity of the operation object within the preset operation area. The data analysis unit is used to determine the rigid body dynamic characteristic parameters of the manipulated object based on the end-effector tactile data and the wrist data, combined with the pose data and generalized velocity of the manipulated object. Specifically, this includes: determining the contact force distribution data between the end effector of the robotic arm and the manipulated object based on the end-effector tactile data; obtaining the contact wrench applied to the manipulated object by the end effector based on the contact force distribution data and the wrist data; transforming the contact wrench from the local contact coordinate system to the object coordinate system of the manipulated object based on the pose data to obtain the equivalent external force wrench in the object coordinate system; performing differential calculation on the generalized velocity to obtain the generalized acceleration of the manipulated object; establishing a dynamic parameter identification model based on the equivalent external force wrench, the generalized acceleration, and the generalized velocity; and recursively solving the dynamic parameter identification model using the least squares method or a filtering algorithm to obtain the mass, center of mass, and inertia tensor of the manipulated object, and using the mass, center of mass, and inertia tensor as the rigid body dynamic characteristic parameters. A dynamic identification unit is used to obtain the desired wrench of the manipulated object based on the rigid body dynamic characteristic parameters, the pose data, and the generalized velocity, combined with the preset desired motion trajectory of the robotic arm. Specifically, this includes: calculating the desired generalized acceleration of the manipulated object based on the preset desired motion trajectory; substituting the rigid body dynamic characteristic parameters, the pose data, and the desired generalized acceleration into the rigid body dynamic equation of the manipulated object to obtain the ideal total wrench of the manipulated object; comparing the preset desired motion trajectory with the pose data of the manipulated object to obtain the pose tracking error; determining a feedback correction wrench based on the pose tracking error using a preset impedance or admittance control law; and superimposing the ideal total wrench and the feedback correction wrench to obtain the desired wrench of the manipulated object. A matrix construction unit is used to construct a grasping matrix based on the end-effector tactile data; The contact force planning unit is used to perform secondary planning based on the grasping matrix and the desired wrench to obtain a contact force vector that is adapted to the operation object. The resultant force decoupling unit is used to decouple the resultant force and internal force according to the contact force vector to obtain the task component and internal force component of the robotic arm. The control unit is used to generate end joint torque commands for the robotic arm based on the task component and the internal force component, and to control the robotic arm to perform cooperative operations on the manipulated object through the joint torque commands.

Images