A force-position hybrid control method for modular dual-arm robot handling

Abstract

The application provides a force-position hybrid control method for modular dual-arm robot carrying, comprising the following steps: based on the orthogonal decomposition principle of the operation space, the operation space is divided into a force control subspace and a position control subspace, and a control framework of the modular dual-arm robot is established; a dual-arm force tracking strategy is designed in the force control subspace, and adaptive admittance control is adopted to realize high-precision tracking of the expected force; based on the closed chain constraint and Jacobian mapping, a dual-arm synchronization strategy is designed to realize coordinated movement of the dual-arm robot; a dual-arm coordinated trajectory tracking strategy is designed in the position control subspace, the left arm adopts sliding mode control to robustly track the reference trajectory, and the right arm minimizes the trajectory tracking error and keeps the internal force balance through model predictive control; a dynamic weight adjustment strategy of force control and position control is designed to simultaneously track the expected force and trajectory in the dual-arm robot carrying process. According to the technical scheme provided by the embodiment of the application, stable and accurate carrying of the modular dual-arm robot in the carrying process can be realized.

Description

[Technical Field]

[0001] This invention belongs to the field of robot control and relates to a force-position hybrid control method for handling in modular dual-arm robots. [Background Technology]

[0002] With the rapid development of industrial automation and intelligent manufacturing, modular dual-arm robots are emerging as a new type of robot system. Modular dual-arm robots flexibly combine functional modules such as robotic arms and end effectors through standardized interfaces, exhibiting high reconfigurability and adaptability. In industrial production, material handling is a common and crucial task. Dual-arm robots, with their human-like operation characteristics and collaborative work capabilities, can efficiently complete various complex material handling tasks.

[0003] Currently, dual-arm robot handling mainly employs the following methods: a cooperative control method based on a master-slave architecture, where a master arm guides the movement and the slave arm follows the master arm to complete the handling; a synchronous control method based on a symmetrical architecture, treating both arms as equivalent to jointly complete the handling task; and a compliant operation method based on impedance control, which achieves compliant interaction by adjusting the robot's mechanical properties. However, these methods often suffer from insufficient control precision and poor robustness in practical applications.

[0004] While collaborative control methods based on a master-slave architecture offer a simple control structure, the following accuracy of the slave arm is difficult to guarantee, hindering stable transport. Synchronous control methods based on a symmetrical architecture, while avoiding the inherent drawbacks of master-slave architectures, suffer from poor transport stability in complex environments, especially under external interference, due to the lack of an active guidance mechanism. Compliant manipulation methods based on impedance control enable compliant interaction between the robot and its environment, but there is a trade-off between high-precision position tracking and force tracking, making it difficult to simultaneously guarantee position and force control accuracy. These methods often fail to meet the stability and precision requirements of industrial production in practical applications. Therefore, this paper proposes a force-position hybrid control method based on a symmetrical architecture for stable dual-arm transport, ensuring that the dual-arm robot can accurately and stably complete transport tasks in dynamic environments. [Summary of the Invention]

[0005] In view of this, the present invention provides a force-position hybrid control method for handling in modular dual-arm robots. The proposed method can solve the problem of the following accuracy of coordinated operation in dual-arm robots that cannot be solved by traditional synchronous control methods, and significantly improves the handling performance of dual-arm robots in complex environments.

[0006] This invention provides a force-position hybrid control method for handling in a modular dual-arm robot, comprising:

[0007] Step S1 is based on the principle of orthogonal decomposition of the operation space, dividing the operation space into force control subspace and position control subspace, and establishing a modular dual-arm robot control framework;

[0008] Step S2 involves designing a dual-arm force tracking strategy in the force control subspace and employing adaptive admittance control to achieve high-precision tracking of the desired force.

[0009] Step S3 is based on closed-chain constraints and Jacobian mapping to design a dual-arm synchronization strategy to achieve coordinated movement of the dual-arm robot;

[0010] Step S4: Design a dual-arm coordinated trajectory tracking strategy in the position control subspace. The left arm uses sliding mode control to robustly track the reference trajectory, while the right arm uses model predictive control to minimize the trajectory tracking error and maintain internal force balance.

[0011] Step S5 involves designing a dynamic weight adjustment strategy for force control and position control, while simultaneously tracking the desired force and trajectory during the handling process of the dual-arm robot.

[0012] In the above method, step S1 includes:

[0013] Step S1.1 Define the position and orientation of the end effector of the modular dual-arm robot:

[0014]

[0015] Where, x L x R Let p represent the pose vectors of the left and right arm end effectors, respectively. L p R θ represents the position vectors of the end effectors of the left and right arms, respectively. L θ R This represents the attitude vector of the left arm end effector;

[0016] Step S1.2 Analyze the constraints that the dual-arm robot must follow during the handling process and establish the constraint equations:

[0017] (1) During the handling process, there is a certain constraint relationship between the arms, among which

[0018] Dynamic constraints: z k+i+1 =f(z) k+i ,τ R,K+i (i = 0, 1, ..., N-1);

[0019] End effector constraint: τ min ≤τ≤τ max ;

[0020] Closed-loop kinematic constraints:

[0021] Position constraint: p R =pL -d const

[0022] Attitude constraints: R L =R R ;

[0023] Where: z i Let τ represent the state of the system at time i. R,K+i τ represents the control torque applied to the right arm at time K+i; min τ max Represent the lower and upper limits of the system joint torque τ, respectively; R L R R These represent the rotation matrices of the end effectors of the left and right arms, respectively; d const It is a fixed relative position vector, determined by the geometric properties of the object being transported;

[0024] (2) End effector torque constraint;

[0025] To ensure that the right arm does not exceed the predetermined speed and force limits during operation, the following control constraints are defined:

[0026] τ R =set(τ position +τ force ,τ min ,τ max )

[0027] The `set()` function means: `set(v,a,b) = max(a,min(v,b))`, used to limit the control input, τ position τ represents the torque output of the position-controlled subspace. force τ represents the torque output of the force-controlled subspace. R This indicates the torque input of the right arm;

[0028] Step S1.3 divides the control space of the dual-arm robot into force control space and position control space. Specifically, the projection matrix of the subspace is obtained by calculating the Jacobian matrix of the constraint equation and performing singular value decomposition.

[0029] Step S1.3.1 Calculate the Jacobian matrix of the constraint equations:

[0030]

[0031] Where Φ(x) represents the constraint equation of the dual-arm robot during the handling process, J c The Jacobian matrix represents the constraint equations, which describes the relationship between the constraints and the pose of the robot's end effector.

[0032] Step S1.3.2 Perform singular value decomposition on the Jacobian matrix:

[0033]

[0034] Where U represents the left singular matrix, Σ represents the singular value matrix, and V represents the right singular matrix;

[0035] Step S1.3.3 yields the force-controlled subspace projection matrix P. f and the positional control subspace projection matrix P m :

[0036]

[0037] Among them, P f P represents the projection matrix that maps the control space to the force control subspace. m This indicates that the control space is mapped to the projection of the bit subspace.

[0038] In the above method, step S2 includes:

[0039] Step S2.1 Design adaptive admittance control in the force control subspace, including:

[0040] Step S2.1.1 Set up the admittance control model. Based on the control accuracy requirements, design the feedforward gain to ensure that the robot can accurately track the desired force and position.

[0041]

[0042] Among them, M d D represents the virtual inertia matrix. d K represents the virtual damping matrix. d K represents the virtual stiffness matrix. e Indicates the feedforward gain, x, Indicates the position, velocity, and acceleration of the end effector, x d F represents the desired position. ff Indicates feedforward control force;

[0043] Step S2.1.2 Design the adaptive law in the adaptive admittance controller as follows:

[0044]

[0045] Where, Γ M ,Γ D ,Γ K Let F represent the positive definite adaptive gain matrix, e = F e -F d Represented as tracking force error, an adaptive mechanism is introduced to dynamically adjust the admittance parameter and generate a corrected position trajectory command, thereby enabling the dual-arm end effector to accurately track the internal force.

[0046] In the above method, step S3 includes:

[0047] Step S3.1 When both arms grasp the same object, establish synchronization error information:

[0048]

[0049] Among them, K v It is the speed error gain coefficient;

[0050] Step S3.2 Design a dual-arm synchronization strategy, including:

[0051] Step S3.2.1 During the synchronized movement of both arms, the actual motion state of the left arm is mapped to the joint space of the right arm:

[0052]

[0053] in, It is the pseudo-inverse of the right-hand Jacobian matrix, K p It is the proportional gain, D p It is the damping gain; when the synchronization error e sync When the threshold is exceeded, increase the proportional gain K. p To enhance the system's ability to correct errors, the damping gain D is increased. p To enhance system stability;

[0054] Step S3.2.2 Based on the contact force feedback of the right arm, calculate the compensating torque of the left arm;

[0055] During synchronous control, in order to synchronize the movement of the right arm with that of the left arm, it is necessary to calculate the compensating torque of the right arm:

[0056]

[0057] in, This indicates the actual contact force of the right arm. Indicates the expected force of the right arm. This indicates the compensating torque of the left arm;

[0058] Step S3.2.3 Dynamically adjust the admittance parameters of the right arm;

[0059] During actual contact, changes in environmental stiffness will cause changes in the contact force of the right arm, leading to an imbalance of internal forces. Therefore, the admittance parameters of the left arm are dynamically adjusted to adapt to environmental changes.

[0060]

[0061] in, Let ΔF be the admittance parameter after the left arm is updated. RIt is the change in the contact force of the right arm, and α is the adjustment coefficient. It is the initial admittance matrix of the left arm;

[0062] Step S3.3 Updates the Jacobian matrix in real time, including:

[0063] Step S3.3.1 Adaptive update of the Jacobian matrix;

[0064] To address the failure of traditional Jacobian matrices in singular configurations, an online Jacobian estimation based on sensor data is employed. An adaptive update formula is used to adjust the value of the Jacobian matrix in real time.

[0065]

[0066] Where γ = 0.2 represents the learning rate, λ = 0.00001 represents the regularization coefficient, and q represents the learning rate. i This represents the joint angle vector of the robot at time i;

[0067] Step S3.3.2 Design a singular value damping strategy

[0068] When the system approaches a singular configuration, the determinant of the Jacobian matrix may approach zero, thus affecting the control effect. To avoid this problem, a singularity avoidance strategy is designed:

[0069] When det(JJ) is detected T When ) < ε, adjust the terminal velocity To reduce speed and thus avoid singularities; if the joint angle q i Its joint limit angle q limit The difference is less than the preset threshold δ (i.e., |q i -q limit If |<δ, where δ=5°), then adjustments are made in the null space to prevent the system from entering the singular region.

[0070] In the above method, step S4 includes:

[0071] Step S4.1 During the handling process, the left arm needs to track the reference trajectory with high precision and resist external disturbances during handling. Sliding mode control is used to achieve trajectory tracking, including:

[0072] Step S4.1.1 Define the sliding surface for sliding mode control:

[0073]

[0074] Where e represents the error between the actual trajectory and the reference trajectory, and λ is the convergence rate parameter. By adjusting λ, the system response speed can be controlled and the stability of trajectory tracking can be ensured.

[0075] Step S4.1.2 employs a reaching rate to suppress the jitter generated by the sliding surface driven by the sign function sgn(s), and introduces dynamic compensation to counteract the nonlinear dynamic characteristics of the system:

[0076]

[0077] Where M(q) represents the Cartesian mass matrix, Let G(q) represent the velocity vector in the virtual Cartesian space, G(q) represent the gravity term vector in the Cartesian space, K represent the switching gain of the sliding mode control, and φ represent the boundary layer thickness, which is set to 0.05.

[0078] Step S4.2 In the dual-arm coordinated operation, the right arm needs to track the reference trajectory with high precision while achieving coordinated control of both arms, including:

[0079] Step S4.2.1 Define the end-effector pose of the right arm and its derivative:

[0080]

[0081] Where, x R , Let represent the end-effector pose of the right arm and its derivative, and

[0082] Step S4.2.2 employs the Euclidean optimization method to control the right arm. During the optimization process, the control input is adjusted in real time, and the control update formula is as follows:

[0083] z k+1 =z k +T s ·f(z k ,τ R,k )+Δ dist

[0084] Where, f(z) k ,τ R,k ) represents the trajectory control function, Δ dist Represented as a disturbance term;

[0085] Step S4.2.3: The dual-arm handling process forms a closed chain; the state of the left arm (x) needs to be changed. L As the external input for the predictive control of the right arm model, in order to improve the coordination accuracy between the two arms, the formula for the external input of the model is as follows:

[0086]

[0087] in, Represents the trajectory tracking weight matrix. This represents the internal force tracking weight matrix. Let J represent the control input penalty matrix, N represent the prediction time domain, M represent the control time domain, and J be the optimization objective function.

[0088] Step S4.2.4 Construct the right arm control input torque τ R Minimize function:

[0089]

[0090] Where H represents the Hessian matrix, which is derived from Q. x Q f R is composed of , and g represents the gradient vector containing the deviation between the reference trajectory and the current state.

[0091] In the above method, step S5 includes:

[0092] Step S5.1 Adaptively allocates the weights between force control and position control based on the real-time status during the handling process, including:

[0093] Step S5.1.1 To allocate the priorities of force control and position control in real time, a global cost function J is defined. total Taking into account both position and force errors, the specific global cost function is as follows:

[0094] J total =γ(t)·J position +β(t)·J force ,γ(t)+β(t)=1

[0095] Among them, J position =||xx d || 2 J represents the trajectory tracking error. force =||F int -F d || 2 Let γ(t) represent the internal force tracking error, and β(t) ∈ [0,1] and β(t) ∈ [0,1] represent the dynamic weighting coefficients.

[0096] Step S5.1.2 Set an adaptive weight adjustment function to adjust the weight coefficients in real time according to the changes in position error and force error. The weight adjustment function is defined as follows:

[0097]

[0098] Among them, e x =xx d e represents the trajectory tracking error. F =F int -F d This represents the internal force tracking error. ε > 0. When γ(t) ≥ 0.5, position control takes priority, and when β(t) ≥ 0.5, force control takes priority. [Attached Image Description]

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

[0100] Figure 1 This is a flowchart illustrating a force-position hybrid control method for handling in a modular dual-arm robot, as provided in an embodiment of the present invention.

[0101] Figure 2 This is a schematic diagram of the six-degree-of-freedom modular dual-arm robot model used in the simulation experiment of this invention embodiment;

[0102] Figure 3 This is a block diagram of the force-position hybrid control of the dual-arm robot in an embodiment of the present invention;

[0103] Figure 4 This is a schematic diagram showing the actual output force of the end effector of the dual-arm robot under no interference force in an embodiment of the present invention;

[0104] Figure 5 This is a schematic diagram showing the actual output force of the end effector of the dual-arm robot under interference in an embodiment of the present invention;

[0105] Figure 6 This is a schematic diagram of the end-effector trajectory results of the dual-arm robot in an embodiment of the present invention. [Specific Implementation Examples]

[0106] To better understand the technical solution of the present invention, the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0107] It should be understood that the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without inventive effort are within the scope of protection of the present invention.

[0108] This invention provides a force-position hybrid control method for handling in a modular dual-arm robot. Please refer to [link / reference]. Figure 1 This is a flowchart illustrating a novel force-position hybrid control method for stable dual-arm handling provided by an example of the present invention, as shown below. Figure 1 As shown, the method includes the following steps:

[0109] Step 101: Based on the principle of orthogonal decomposition of the operation space, the operation space is divided into force control subspace and position control subspace to establish a modular dual-arm robot control framework.

[0110] Specifically, define the position and orientation of the end effector of the modular dual-arm robot:

[0111]

[0112] Where, x L x R Let p represent the pose vectors of the left and right arm end effectors, respectively. L p R θ represents the position vectors of the end effectors of the left and right arms, respectively. L θ R This represents the attitude vector of the left arm end effector;

[0113] Secondly, the constraints that the modular dual-arm robot must adhere to during the handling process are analyzed:

[0114] (1) During the handling process, there is a certain constraint relationship between the arms, among which

[0115] Dynamic constraints: z k+i+1 =f(z) k+i ,τ R,K+i (i = 0, 1, ..., N-1);

[0116] End effector constraint: τ min ≤τ≤τ max ;

[0117] Closed-loop kinematic constraints:

[0118] Position constraint: p R =p L -d const

[0119] Attitude constraints: R L =R R ;

[0120] Where: z i Let τ represent the state of the system at time i. R,K+i τ represents the control torque applied to the right arm at time K+i; min τ max Represent the lower and upper limits of the system joint torque τ, respectively; R L R R These represent the rotation matrices of the end effectors of the left and right arms, respectively; d const It is a fixed relative position vector, determined by the geometric properties of the object being transported;

[0121] (2) End effector torque constraint;

[0122] To ensure that the right arm does not exceed the predetermined speed and force limits during operation, the following control constraints are defined:

[0123] τ total =set(τ position +τ force ,τ min ,τ max )

[0124] The `set()` function means: `set(v,a,b) = max(a,min(v,b))`, used to limit the control input, τ position τ represents the torque output of the position-controlled subspace. force τ represents the torque output of the force-controlled subspace. R This indicates the torque input of the right arm;

[0125] Then, the control space of the dual-arm robot is divided into force control space and position control space. Specifically, the projection matrix of the subspace is obtained by calculating the Jacobian matrix of the constraint equations and performing singular value decomposition.

[0126] Step 1: Calculate the Jacobian matrix of the constraint equations:

[0127]

[0128] Where Φ(x) represents the constraint equation of the dual-arm robot during the handling process, J c The Jacobian matrix represents the constraint equations, which describes the relationship between the constraints and the pose of the robot's end effector.

[0129] Step 2: Perform singular value decomposition on the Jacobian matrix:

[0130]

[0131] Where U represents the left singular matrix, Σ represents the singular value matrix, and V represents the right singular matrix;

[0132] Step 3, obtain the force control and position subspace projection matrix:

[0133]

[0134] Among them, P f Represented as the force-controlled subspace projection matrix, P f P represents the projection matrix that maps the control space to the force control subspace. m Represented as the positional subspace projection matrix, P m This indicates that the control space is mapped to the projection of the bit subspace.

[0135] Step 102: Design a dual-arm force tracking strategy in the force control subspace and use adaptive admittance control to achieve high-precision tracking of the desired force;

[0136] Specifically, adaptive admittance control is designed in the force control subspace:

[0137] Step 1: Set up the admittance control model and design an appropriate feedforward gain based on the control accuracy requirements to ensure that the robot can accurately track the desired force and position.

[0138]

[0139] Among them, M d D represents the virtual inertia matrix. d K represents the virtual damping matrix. d K represents the virtual stiffness matrix. e Indicates the feedforward gain, x, Indicates the position, velocity, and acceleration of the end effector, x d F represents the desired position. ff This represents the feedforward control force (calculated from the error between the desired position and the current actual position);

[0140] Step 2 involves using an adaptive mechanism to dynamically adjust the gain matrix, ensuring the control system's adaptability and robustness to environmental changes.

[0141] The adaptive law in an adaptive admittance controller can be designed as follows:

[0142]

[0143] Where, Γ M ,Γ D ,Γ K Let e ​​= F be the positive definite adaptive gain matrix. e -F d Represented as tracking force error, an adaptive mechanism is introduced to dynamically adjust the admittance parameter and generate a corrected position trajectory command, thereby enabling the dual-arm end effector to accurately track the internal force.

[0144] Step 103: Design a dual-arm synchronization strategy based on closed-chain constraints and Jacobian mapping to achieve coordinated movement of the dual-arm robot;

[0145] Specifically, when both arms grasp the same object, a rigid connection constraint is established:

[0146]

[0147] Among them, K vIt is the speed error gain coefficient, which adds a speed error term while dealing with position synchronization error, enabling the system to adjust speed synchronization more precisely, thereby improving dynamic response performance;

[0148] Secondly, the dual-arm synchronization strategy includes:

[0149] Step 1: During the synchronized movement of both arms, map the actual motion state of the left arm onto the joint space of the right arm:

[0150]

[0151] in, It is the pseudo-inverse of the right-hand Jacobian matrix, K p It is the proportional gain, D p It is the damping gain; when the synchronization error e sync When the threshold is exceeded, increase the proportional gain K. p To enhance the system's ability to correct errors, the damping gain D is increased. p To enhance system stability;

[0152] Step 2: Based on the contact force feedback of the right arm, calculate the compensating torque of the left arm;

[0153] During synchronous control, there may be a gap between the actual contact force and the desired contact force of the right arm. In order to synchronize the movement of the right arm with that of the left arm, it is necessary to calculate the compensation torque of the right arm to adjust the force of the right arm and make it as consistent as possible with that of the left arm.

[0154]

[0155] in, This indicates the actual contact force of the right arm. Indicates the expected force of the right arm. This indicates the left arm compensation torque, designed to adjust the gap between the actual force and the desired force of the right arm, ensuring mechanical consistency between the two arms;

[0156] Step 3: During actual contact, the contact force of the right arm may change due to variations in environmental stiffness, leading to an imbalance of internal forces. Therefore, the admittance parameters of the left arm are dynamically adjusted to adapt to these environmental changes.

[0157]

[0158] in, Let ΔF be the admittance parameter of the left arm. R It is the change in the contact force of the right arm, and α is the adjustment coefficient. This is the default mass matrix for the left arm;

[0159] Finally, the real-time Jacobian update:

[0160] Step 1: To address the failure of traditional Jacobian matrices in singular configurations, online Jacobian estimation based on sensor data is employed. An adaptive update formula is used to adjust the value of the Jacobian matrix in real time.

[0161]

[0162] Where γ = 0.2 represents the learning rate, λ = 0.00001 represents the regularization coefficient, and q represents the learning rate. i This represents the joint angle vector of the robot at time i;

[0163] Step 2: When the system approaches a singular configuration, the determinant of the Jacobian matrix may approach zero, thus affecting the control effect. To avoid this problem, a singularity avoidance strategy is designed:

[0164] When det(JJ) is detected T When ) < ε, adjust the terminal velocity To reduce speed and thus avoid singularities; if the joint angle q i Its joint limit angle q limit The difference is less than the preset threshold δ (i.e., |q i -q limit If |<δ, where δ=5°), then adjustments are made in the null space to prevent the system from entering the singular region.

[0165] Step 104: Design a dual-arm coordinated trajectory tracking strategy in the position control subspace. The left arm uses sliding mode control to robustly track the reference trajectory, while the right arm uses model predictive control to minimize the trajectory tracking error and maintain internal force balance.

[0166] Specifically, during the handling process, the left arm needs to track the reference trajectory with high precision and resist external disturbances during handling. Sliding mode control is used to achieve trajectory tracking, including:

[0167] Step 1, define the sliding surface for sliding mode control:

[0168]

[0169] Where e represents the error between the actual trajectory and the reference trajectory, and λ is the convergence rate parameter. By adjusting λ, the system response speed can be controlled and the stability of trajectory tracking can be ensured.

[0170] Step 2 involves using the approach rate to suppress the jitter generated by the sliding surface driven by the sign function sgn(s), and introducing dynamic compensation to counteract the nonlinear dynamic characteristics of the system:

[0171]

[0172] Where M(q) represents the Cartesian mass matrix, Let G(q) represent the velocity vector in the virtual Cartesian space, G(q) represent the gravity term vector in the Cartesian space, K represent the switching gain of the sliding mode control, and φ represent the boundary layer thickness, which is set to 0.05.

[0173] Secondly, in dual-arm collaborative operation, the right arm needs to track the reference trajectory with high precision while achieving coordinated control of both arms:

[0174] Step 1, define the end-effector pose of the right arm and its derivative:

[0175]

[0176] Where, x R , Let represent the end-effector pose of the right arm and its derivative, and

[0177] Step 2: The right arm is controlled using the Euclidean optimization method. During the optimization process, the control input is adjusted in real time. The control update formula is as follows:

[0178] z k+1 =z k +T s ·f(z k ,τ R,k )+Δ dist

[0179] Where, f(z) k ,τ R,k ) represents the trajectory control function, Δ dist Represented as a disturbance term;

[0180] Step 3: The bi-arm carrying process forms a closed chain; the left arm's position needs to be adjusted. L As the external input for the predictive control of the right arm model, in order to improve the coordination accuracy between the two arms, the formula for the external input of the model is as follows:

[0181]

[0182] in, Represents the trajectory tracking weight matrix. This represents the internal force tracking weight matrix.

[0183] Let J represent the control input penalty matrix, N represent the prediction time domain, and M represent the control time domain. By optimizing the objective function J, the model predictive control can adjust the control input of the right arm in real time to ensure accurate tracking and internal force balance when the two arms work together.

[0184] Step 4: Construct the right arm control input torque τ R Minimize function:

[0185]

[0186] Where H represents the Hessian matrix, which is derived from Q. x Q f R is composed of , and g represents the gradient vector containing the deviation between the reference trajectory and the current state.

[0187] Step 105: Design a dynamic weight adjustment strategy for force control and position control, while tracking the desired force and trajectory during the handling process of the dual-arm robot;

[0188] Specifically, based on the real-time status during the handling process, the weights between force control and position control are adaptively allocated, including:

[0189] Step 1: To allocate the priorities of force control and position control in real time, we define a global cost function J. total Taking into account both position and force errors, the specific global cost function is as follows:

[0190] J total =γ(t)·J position +β(t)·J force ,γ(t)+β(t)=1

[0191] Among them, J position =||xx d || 2 J represents the trajectory tracking error. force =||F int -F d || 2 Let γ(t) represent the internal force tracking error, and β(t) ∈ [0,1] and β(t) ∈ [0,1] represent the dynamic weighting coefficients.

[0192] Step 2: Set an adaptive weight adjustment function to adjust the weight coefficients in real time based on changes in position error and force error. The weight adjustment function is defined as follows:

[0193]

[0194] Among them, e x =xx d e represents the trajectory tracking error. F =F int -F d This represents the internal force tracking error. ε > 0. When γ(t) ≥ 0.5, position control takes priority, and when β(t) ≥ 0.5, force control takes priority.

[0195] Based on the method provided in the embodiments of the present invention, a simulation experiment was conducted on a six-degree-of-freedom modular dual-arm robot. To accurately describe the robot's kinematic characteristics, it is first necessary to establish the robot's DH parameter model, using the link length 'a'... i α, connecting rod torsion angle i Joint offset d i Joint angle θ i This describes the spatial relationship between adjacent joints. Where d i θ represents the offset of joint i, that is, the displacement along the axis of rotation of joint i; i This represents the rotation angle of joint i, that is, the rotation angle about the axis of joint i; a i α represents the length of link i, i.e., the distance along the common perpendicular line; i This represents the torsion angle of link i, which is the included angle between two adjacent pivots.

[0196] Table 1 Left Arm DH Parameters

[0197]

[0198] Table 2 Right Arm DH Parameters

[0199]

[0200] A simulation environment was built using Matlab software for experimental verification. The experimental environment was a personal computer with an Intel(R) Core(TM) i7-6700 CPU @ 3.40GHz processor. This embodiment uses a two-dimensional plane to demonstrate the effectiveness of the force-potential hybrid control method; please refer to the relevant documentation. Figure 4 , Figure 5 , Figure 6 . Figure 4 To conduct a modular dual-arm robot collaborative force-position hybrid control experiment without external disturbances, the control results show that the end contact forces of the two arms in three directions tend to stabilize over time. Figure 5 To conduct a modular dual-arm robot collaborative force-position hybrid control experiment under external disturbances, the control results show that the actual contact force at the end of the dual-arm robot oscillates, but the actual contact force in the three directions tends to stabilize over time. Figure 6 It can be shown that the actual trajectory of the end effector of the dual-arm robot basically coincides with the expected transport trajectory.

[0201] The technical solutions of the embodiments of the present invention have the following beneficial effects:

[0202] This invention can dynamically adjust the weights of force control and position control based on real-time trajectory and force errors, ensuring that the modular dual-arm robot can accurately track the desired trajectory while maintaining internal force balance, thus enhancing the robustness and stability of the system. By combining sliding mode control and model predictive control, the left and right arms can coordinate and cooperate to adapt to environmental changes, improving the adaptability and efficiency of the dual-arm robot.

[0203] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0204] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A force-position hybrid control method for handling in a modular dual-arm robot, characterized in that, The method includes: Step S1 is based on the principle of orthogonal decomposition of the operation space, dividing the operation space into force control subspace and position control subspace, and establishing a modular dual-arm robot control framework; Step S2 involves designing a dual-arm force tracking strategy in the force control subspace and employing adaptive admittance control to achieve high-precision tracking of the desired force. Step S3 is based on closed-chain constraints and Jacobian mapping to design a dual-arm synchronization strategy to achieve coordinated movement of the dual-arm robot; Step S4: Design a dual-arm coordinated trajectory tracking strategy in the position control subspace. The left arm uses sliding mode control to robustly track the reference trajectory, while the right arm uses model predictive control to minimize the trajectory tracking error and maintain internal force balance. Step S5 designs a dynamic weight adjustment strategy for force control and position control, while simultaneously tracking the desired force and trajectory during the handling process of the dual-arm robot; step S5 includes: Step S5.1 Adaptively allocates the weights between force control and position control based on the real-time status during the handling process, including: Step S5.1.1 To allocate the priorities of force control and position control in real time, a global cost function is defined. Taking into account both position and force errors, the specific global cost function is as follows: in, Indicates the trajectory tracking error. Indicates the internal force tracking error. , Indicates the dynamic weighting coefficient; Step S5.1.2 Set an adaptive weight adjustment function to adjust the weight coefficients in real time according to the changes in position error and force error. The weight adjustment function is defined as follows: in, Indicates the trajectory tracking error. Indicates the internal force tracking error. ,when When, position control takes priority, when Prioritize time and energy control.

2. The method according to claim 1, characterized in that, Step S1 includes: Step S1.1 Define the position and orientation of the end effector of the modular dual-arm robot: in, , These represent the pose vectors of the end effectors of the left and right arms, respectively. , These represent the position vectors of the end effectors of the left and right arms, respectively. , This represents the attitude vector of the left arm end effector; Step S1.2 Analyze the constraints that the dual-arm robot must follow during the handling process and establish the constraint equations: (1) During the handling process, there is a certain constraint relationship between the arms, among which Dynamic constraints: ; End effector constraints: ; Closed-loop kinematic constraints: Position constraints: Attitude constraints: ; in: Indicates the first The state of the system at any given moment. Indicates the first The control torque constantly applied to the right arm; , These represent the joint torques of the system. The lower and upper limits; , These represent the rotation matrices of the end effectors of the left and right arms, respectively. It is a fixed relative position vector, determined by the geometric properties of the object being transported; (2) End effector torque constraint; To ensure that the right arm does not exceed the predetermined speed and force limits during operation, the following control constraints are defined: in, The function means: Used to limit control input. This indicates the torque output of the position-controlled subspace. This represents the torque output of the force-controlled subspace. This indicates the torque input of the right arm; Step S1.3 divides the control space of the dual-arm robot into force control space and position control space. Specifically, the projection matrix of the subspace is obtained by calculating the Jacobian matrix of the constraint equation and performing singular value decomposition. Step S1.3.1 Calculate the Jacobian matrix of the constraint equations: in, The constraint equations represent the process of a dual-arm robot in handling materials. The Jacobian matrix represents the constraint equations, which describes the relationship between the constraints and the pose of the robot's end effector. Step S1.3.2 Perform singular value decomposition on the Jacobian matrix: in, Describes a left singular matrix. Represents a singular value matrix. Represents a right singular matrix; Step S1.3.3 yields the force-controlled subspace projection matrix. and position control subspace projection matrix : 、 in, This represents the projection matrix that maps the control space to the force control subspace. This indicates that the control space is mapped to the projection of the bit subspace.

3. The method according to claim 1, characterized in that, Step S2 includes: Step S2.1 Design adaptive admittance control in the force control subspace, including: Step S2.1.1 Set up the admittance control model. Based on the control accuracy requirements, design the feedforward gain to ensure that the robot can accurately track the desired force and position. ； in, Represents the virtual inertia matrix. Represents the virtual damping matrix. Represents the virtual stiffness matrix. Indicates feedforward gain. , , Indicates the position, velocity, and acceleration of the end effector. Indicates the desired location. Indicates feedforward control force; Step S2.1.2 Design the adaptive law in the adaptive admittance controller as follows: ， ， in, , , Let represent the positive definite adaptive gain matrices, Represented as tracking force error, an adaptive mechanism is introduced to dynamically adjust the admittance parameter and generate a corrected position trajectory command, thereby enabling the dual-arm end effector to accurately track the internal force.

4. The method according to claim 1, characterized in that, Step S3 includes: Step S3.1 When both arms grasp the same object, establish synchronization error information: in, It is the speed error gain coefficient; Step S3.2 Design a dual-arm synchronization strategy, including: Step S3.2.1 During the synchronized movement of both arms, the actual motion state of the left arm is mapped to the joint space of the right arm: in, It is the pseudo-inverse of the right-hand Jacobian matrix. It is proportional gain. It is the damping gain; when the synchronization error When the set threshold is exceeded, the proportional gain is increased. To enhance the system's ability to correct errors and increase the damping gain To enhance system stability; Step S3.2.2 Based on the contact force feedback of the right arm, calculate the compensating torque of the left arm; During synchronous control, in order to synchronize the movement of the right arm with that of the left arm, it is necessary to calculate the compensating torque of the right arm: in, This indicates the actual contact force of the right arm. Indicates the expected force of the right arm. This indicates the compensating torque of the left arm; Step S3.2.3 Dynamically adjust the admittance parameters of the right arm; During actual contact, changes in environmental stiffness will cause changes in the contact force of the right arm, leading to an imbalance of internal forces. Therefore, the admittance parameters of the left arm are dynamically adjusted to adapt to environmental changes. in, This is represented by the updated admittance parameters for the left arm. It is the change in the contact force of the right arm. It is the adjustment coefficient. It is the initial admittance matrix of the left arm; Step S3.3 Updates the Jacobian matrix in real time, including: Step S3.3.1 Adaptive update of the Jacobian matrix; To address the failure of traditional Jacobian matrices in singular configurations, an online Jacobian estimation based on sensor data is employed. An adaptive update formula is used to adjust the value of the Jacobian matrix in real time. in, Indicates the learning rate. Represents the regularization coefficient, indicating Indicates the robot at a certain moment The joint angle vector; Step S3.3.2 Design a singular value damping strategy When the system approaches a singular configuration, the determinant of the Jacobian matrix may approach zero, thus affecting the control effect. To avoid this problem, a singularity avoidance strategy is designed: When detected At that time, by adjusting the end speed To reduce speed and thus avoid singularities; if the joint angle Its joint limit angle The difference is less than the preset threshold ,Right now ,in Adjustments are made within the null space to prevent the system from entering a singular region.

5. The method according to claim 1, characterized in that, Step S4 includes: Step S4.1 During the handling process, the left arm needs to track the reference trajectory with high precision and resist external disturbances during handling. Sliding mode control is used to achieve trajectory tracking, including: Step S4.1.1 Define the sliding surface for sliding mode control: in, This represents the error between the actual trajectory and the reference trajectory. The convergence rate parameter is adjusted by... It can control the system's response speed and ensure the stability of trajectory tracking; Step S4.1.2 uses the approach rate to suppress the influence of the sign function. The driving state drives the vibration generated by the sliding surface, and dynamic compensation is introduced to counteract the nonlinear dynamic characteristics of the system: in, Represents the Cartesian mass matrix. Denotes the velocity vector in the virtual Cartesian space. Denotes the vector of the gravity term of Descartes. This indicates the switching gain of the sliding mode control. Indicates the boundary layer thickness, taking =0.05; Step S4.2 In the dual-arm coordinated operation, the right arm needs to track the reference trajectory with high precision while achieving coordinated control of both arms, including: Step S4.2.1 Define the end-effector pose of the right arm and its derivative: in, , Let represent the end-effector pose of the right arm and its derivative, and ; Step S4.2.2 employs the Euclidean optimization method to control the right arm. During the optimization process, the control input is adjusted in real time, and the control update formula is as follows: in, This is represented as a trajectory control function. Represented as a disturbance term; Step S4.2.3: The dual-arm handling process forms a closed chain; the state of the left arm needs to be adjusted. As the external input for the predictive control of the right arm model, in order to improve the coordination accuracy between the two arms, the formula for the external input of the model is as follows: in, Represents the trajectory tracking weight matrix. This represents the internal force tracking weight matrix. This represents the control input penalty matrix. Indicates the prediction time domain, Represents the control time domain and the optimization objective function. ; Step S4.2.4 Construct the right arm control input torque Minimize function: in, Denotes the Hessian matrix, which is composed of , , constitute, This indicates that the gradient vector contains the deviation between the reference trajectory and the current state.

Images