Force-position hybrid control method for modularized double-arm robot carrying

Abstract

The invention provides a modular double-arm robot carrying-oriented force and position hybrid control method, which comprises the following steps of: dividing an operation space into a force control subspace and a position control subspace on the basis of an operation space orthogonal decomposition principle, and establishing a modular double-arm robot control framework; a double-arm force tracking strategy is designed in the force control subspace, and high-precision tracking of expected force is achieved through self-adaptive admittance control; a two-arm synchronization strategy is designed based on closed chain constraint and Jacobian mapping, and coordinated movement of the two-arm robot is achieved; a double-arm coordination trajectory tracking strategy is designed in the position control subspace, a left arm adopts sliding mode control robust tracking reference trajectory, and a right arm controls the minimum trajectory tracking error through model prediction and keeps internal force balance; and a force control and position control dynamic weight adjustment strategy is designed, and meanwhile the expected force and track of the double-arm robot in the carrying process are tracked. According to the technical scheme provided by the embodiment of the invention, the modular double-arm robot can realize stable and accurate carrying in the carrying process.

Description

[Technical field]

[0001] The invention belongs to the field of robot control and relates to a force-position hybrid control method for modular dual-arm robot handling. [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, and are highly reconfigurable and adaptable. In industrial production, handling tasks are a common and critical link. With their humanoid operating characteristics and collaborative working capabilities, dual-arm robots can efficiently complete various complex handling tasks.

[0003] At present, dual-arm robot handling mainly adopts the following methods: collaborative control method based on master-slave architecture, by setting a master arm to guide the movement, the slave arm follows the master arm to complete the handling; synchronous control method based on symmetric architecture, treating the two arms equivalently and completing the handling task together; and compliant operation method based on impedance control, by adjusting the mechanical characteristics of the robot to achieve compliant interaction. However, these methods often have problems such as insufficient control accuracy and poor robustness in practical applications.

[0004] The collaborative control method based on the master-slave architecture has a simple control structure, but the following accuracy of the slave arm is difficult to guarantee, and stable handling cannot be achieved; the synchronous control method based on the symmetric architecture avoids the inherent defects of the master-slave architecture, but due to the lack of an active guidance mechanism, the handling stability is poor in complex environments, especially under external force interference; the compliant operation method based on impedance control can achieve compliant interaction between the robot and the environment, but there is a contradiction between high-precision position tracking and force tracking, and it is difficult to ensure the control accuracy of position and force at the same time. These methods are often difficult to meet the requirements of industrial production for stability and accuracy in practical applications. Therefore, a force-position hybrid control method for dual-arm stable handling is proposed based on the synchronous control method of the symmetric architecture to ensure that the dual-arm robot can accurately and stably complete the handling task in a dynamic environment. [Summary of the invention]

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

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

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

[0008] Step S2 designs a dual-arm force tracking strategy in the force control subspace and uses adaptive admittance control to achieve high-precision tracking of the desired force;

[0009] Step S3 designs a dual-arm synchronization strategy based on closed-chain constraints and Jacobi mapping to achieve coordinated motion of the dual-arm robot;

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

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

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

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

[0014]

[0015] Among them, x L 、x R They represent the pose vectors of the left and right arm end effectors, respectively, and p L 、p R Respectively represent the position vectors of the left and right arm end effectors, θ L ,θ R represents the posture vector of the left arm end effector;

[0016] Step S1.2 analyzes the constraints that the dual-arm robot must comply with during the handling process and establishes the constraint equation:

[0017] (1) During the handling process, the two arms have certain constraints, including

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

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

[0020] Closed-chain kinematic constraints:

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

[0022] Posture constraint: R L =R R ;

[0023] Where: z i represents the state of the system at the i-th moment, τ R,K+i represents the control torque applied to the right arm at the K+ith moment; τ min , τ max Respectively represent the lower and upper limits of the system joint torque τ; R L , R R Represents the rotation matrix of the left and right arm end effectors respectively; d const is a fixed relative position vector, determined by the geometric characteristics of the transported object;

[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 meaning of the set() function is: set(v,a,b)=max(a,min(v,b)), which is used to limit the control input, τ position represents the torque output of the position control subspace, τ force represents the torque output of the force-controlled subspace, τ R represents the torque input of the right arm;

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

[0029] Step S1.3.1 calculates the Jacobian matrix of the constraint equation:

[0030]

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

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

[0033]

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

[0035] Step S1.3.3 Obtain the force control subspace projection matrix P f and the position control subspace projection matrix P m :

[0036]

[0037] Among them, P f represents the projection matrix that maps the control space to the force control subspace, P m represents the projection of the control space onto the position control subspace.

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

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

[0040] Step S2.1.1 sets up the admittance control model and designs the feedforward gain according to the control accuracy requirements to ensure that the robot can accurately track the desired force and position:

[0041]

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

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

[0044]

[0045] Among them, Γ M , Γ D , Γ K They represent the positive definite adaptive gain matrix, e=F e -F d It is expressed as tracking force error. An adaptive mechanism is introduced to dynamically adjust the admittance parameters and generate the corrected position trajectory command to achieve accurate tracking of the internal force by the dual-arm end-effector.

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

[0047] Step S3.1: When the two arms grasp the same object, synchronous error information is established:

[0048]

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

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

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

[0052]

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

[0054] Step S3.2.2: calculating the compensation torque of the left arm based on the contact force feedback of the right arm;

[0055] During synchronous control, in order to synchronize the movement of the right arm with the left arm, the compensation torque of the right arm needs to be calculated:

[0056]

[0057] in, represents the actual contact force of the right arm, represents the desired force in the right arm, represents the compensation torque of the left arm;

[0058] Step S3.2.3 dynamically adjusts the admittance parameters of the right arm;

[0059] In the actual contact process, changes in environmental stiffness will cause changes in the contact force of the right arm, resulting in an imbalance in the internal force. Therefore, the admittance parameters of the left arm are dynamically adjusted to adapt to changes in the environment:

[0060]

[0061] in, It is represented by the updated admittance parameter of the left arm, ΔF Ris the change in the right arm contact force, α is the adjustment coefficient, 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 Jacobian matrix;

[0064] In order to solve the problem that the traditional Jacobian matrix fails under singular configurations, an online Jacobian estimation based on sensor data is used to adjust the value of the Jacobian matrix in real time through an adaptive update formula:

[0065]

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

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

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

[0069] When det(JJ T )<ε, by adjusting the terminal speed To reduce the speed and avoid singularity; 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 |<δ, where δ=5°), adjustments are made in the null space to prevent the system from entering the singular region.

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

[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 the handling process. Sliding mode control is used to achieve trajectory tracking, including:

[0072] Step S4.1.1 defines the sliding surface of 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 uses the approach rate to suppress the jitter caused by the sliding surface driven by the sign function sgn(s) driving state, and introduces dynamic compensation to offset the nonlinear dynamic characteristics in the system:

[0076]

[0077] Where M(q) represents the Cartesian mass matrix, represents the velocity vector in virtual Cartesian space, G(q) represents the Cartesian gravity term vector, K represents the switching gain of sliding mode control, φ represents the boundary layer thickness, and φ=0.05;

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

[0079] Step S4.2.1 defines the end pose of the right arm and its derivatives:

[0080]

[0081] Among them, x R , represents the end position of the right arm and its derivative, and

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

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

[0084] Among them, f(z k ,τ R,k ) is expressed as the trajectory control function, Δ dist is represented as a disturbance term;

[0085] Step S4.2.3 The double-arm handling process forms a closed chain, and the left arm state x L As the external input of the right arm model predictive control to improve the coordination accuracy between the two arms, the external input formula of the model is as follows:

[0086]

[0087] in, represents the trajectory tracking weight matrix, represents the internal force tracking weight matrix, represents the control input penalty matrix, N represents the prediction time domain, M represents the control time domain, and the optimization objective function J;

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

[0089]

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

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

[0092] Step S5.1 adaptively allocates the weight between force control and position control according to the real-time status during the handling process, including:

[0093] Step S5.1.1 In order to allocate the priority of force control and position control in real time, define the global cost function J total To comprehensively consider the position error and force error, the specific global cost function is:

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

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

[0096] Step S5.1.2 sets an adaptive weight adjustment function, and adjusts the weight coefficient 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 represents the trajectory tracking error, e F =F int -F d Represents the internal force tracking error, ε>0, when γ(t)≥0.5, position control takes priority, and when β(t)≥0.5, force control takes priority.

Brief Description of the Drawings

[0099] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments are briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creativity and labor.

[0100] Figure 1 It is a flow chart of a force-position hybrid control method for modular dual-arm robot handling provided by an embodiment of the present invention;

[0101] Figure 2 Schematic diagram of a six-degree-of-freedom modular dual-arm robot model used in a simulation experiment in an embodiment of the present invention;

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

[0103] Figure 4 Schematic diagram of the actual output force result of the end of the dual-arm robot without interference force in an embodiment of the present invention;

[0104] Figure 5 Schematic diagram of the actual output force result of the end of the dual-arm robot under the interference force in the embodiment of the present invention;

[0105] Figure 6 Schematic diagram of the end trajectory of the dual-arm robot in an embodiment of the present invention. [Specific embodiment]

[0106] In order to better understand the technical solution of the present invention, the embodiments of the present invention are described in detail below with reference to the accompanying drawings.

[0107] It should be clear that the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0108] The embodiment of the present invention provides a force-position hybrid control method for modular dual-arm robot handling, please refer to Figure 1 , which is a flow chart of a new force-position hybrid control method for dual-arm stable transport provided by an example of the present invention, such as Figure 1 As shown, the method comprises the following steps:

[0109] Step 101, based on the principle of orthogonal decomposition of the operation space, the operation space is divided into a force control subspace and a position control subspace, and a modular dual-arm robot control framework is established;

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

[0111]

[0112] Among them, x L 、x R They represent the pose vectors of the left and right arm end effectors, respectively, and p L 、p R Respectively represent the position vectors of the left and right arm end effectors, θ L ,θ R represents the posture vector of the left arm end effector;

[0113] Secondly, analyze the constraints that the modular dual-arm robot must comply with during the handling process:

[0114] (1) During the handling process, the two arms have certain constraints, including

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

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

[0117] Closed-chain kinematic constraints:

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

[0119] Posture constraint: R L =R R ;

[0120] Where: z i represents the state of the system at the i-th moment, τ R,K+i represents the control torque applied to the right arm at the K+ith moment; τ min , τ max Respectively represent the lower and upper limits of the system joint torque τ; R L , R R Represents the rotation matrix of the left and right arm end effectors respectively; d const is a fixed relative position vector, determined by the geometric characteristics of the transported object;

[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 meaning of the set() function is: set(v,a,b)=max(a,min(v,b)), which is used to limit the control input, τ position represents the torque output of the position control subspace, τ force represents the torque output of the force-controlled subspace, τ R represents 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 Jacobian matrix of the constraint equation is calculated and singular value decomposition is performed to obtain the projection matrix of the subspace:

[0126] Step 1, calculate the Jacobian matrix of the constraint equation:

[0127]

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

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

[0130]

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

[0132] Step 3, get the force control and position space projection matrix:

[0133]

[0134] Among them, P f Expressed as the force control subspace projection matrix, P f represents the projection matrix that maps the control space to the force control subspace, P m Expressed as the position control subspace projection matrix, P m represents the projection of the control space onto the position control subspace.

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

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

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

[0138]

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

[0140] Step 2: Use an adaptive mechanism to dynamically adjust the gain matrix to ensure the adaptability and robustness of the control system to environmental changes.

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

[0142]

[0143] Among them, Γ M , Γ D , Γ K represents the positive definite adaptive gain matrix, e = F e -F d It is expressed as tracking force error. An adaptive mechanism is introduced to dynamically adjust the admittance parameters and generate the corrected position trajectory command to achieve accurate tracking of the internal force by the dual-arm end-effector.

[0144] Step 103, designing a dual-arm synchronization strategy based on closed-chain constraints and Jacobi mapping to achieve coordinated motion of the dual-arm robot;

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

[0146]

[0147] Among them, K vis the speed error gain coefficient, which adds the speed error term while processing the position synchronization error, so that the system can adjust the speed synchronization more accurately, thereby improving the dynamic response performance;

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

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

[0150]

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

[0152] Step 2, based on the contact force feedback of the right arm, calculate the compensation torque of the left arm;

[0153] During synchronous control, there may be a gap between the actual contact force of the right arm and the expected contact force. In order to synchronize the movement of the right arm with the left arm, the compensation torque of the right arm needs to be calculated to adjust the force of the right arm to keep it consistent with the left arm as much as possible:

[0154]

[0155] in, represents the actual contact force of the right arm, represents the desired force in the right arm, It represents the compensation torque of the left arm, which is designed to adjust the gap between the actual force and the expected force of the right arm to ensure the mechanical consistency of the two arms;

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

[0157]

[0158] in, Expressed as the admittance parameter of the left arm, ΔF R is the change in the right arm contact force, α is the adjustment coefficient, is the default mass matrix for the left arm;

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

[0160] In the first step, in order to solve the problem that the traditional Jacobian matrix fails under singular configurations, an online Jacobian estimation based on sensor data is used to adjust the value of the Jacobian matrix in real time through an adaptive update formula:

[0161]

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

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

[0164] When det(JJ T )<ε, by adjusting the terminal speed To reduce the speed and avoid singularity; 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 |<δ, where δ=5°), adjustments are made in the null space to prevent the system from entering the singular region.

[0165] Step 104, designing a dual-arm coordinated trajectory tracking strategy in the position control subspace, wherein the left arm uses sliding mode control to robustly track the reference trajectory, and 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 the handling process. Sliding mode control is used to achieve trajectory tracking, including:

[0167] Step 1: Define the sliding surface of 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] In the second step, the approach rate is used to suppress the jitter caused by the sliding surface driven by the sign function sgn(s) driving state, and dynamic compensation is introduced to offset the nonlinear dynamic characteristics in the system:

[0171]

[0172] Where M(q) represents the Cartesian mass matrix, represents the velocity vector in virtual Cartesian space, G(q) represents the Cartesian gravity term vector, K represents the switching gain of sliding mode control, φ represents the boundary layer thickness, and φ=0.05;

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

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

[0175]

[0176] Among them, x R , represents the end position of the right arm and its derivative, and

[0177] In the second step, the Euclidean optimization method is used to control the right arm. During the optimization process, the control input is adjusted in real time. The control update formula is:

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

[0179] Among them, f(z k ,τ R,k ) is expressed as the trajectory control function, Δ dist is represented as a disturbance term;

[0180] Step 3: The double-arm handling process forms a closed chain. The left arm state x L As the external input of the right arm model predictive control to improve the coordination accuracy between the two arms, the external input formula of the model is as follows:

[0181]

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

[0183] represents the control input penalty matrix, N represents the prediction time domain, and M represents 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 the function:

[0185]

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

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

[0188] Specifically, according to the real-time status during the handling process, the weight between force control and position control is adaptively allocated, including:

[0189] Step 1: In order to allocate the priority of force control and position control in real time, we define the global cost function J total To comprehensively consider the position error and force error, the specific global cost function is:

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

[0191] Among them, J position =||xx d || 2 represents the trajectory tracking error, J force =‖F int -F d ‖ 2 represents the internal force tracking error, γ(t)∈[0,1], β(t)∈[0,1] represent the dynamic weight coefficient;

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

[0193]

[0194] Among them, e x =xx d represents the trajectory tracking error, e F =F int -F d 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] According to the above method provided by the embodiment of the present invention, a simulation experiment is conducted on a six-degree-of-freedom modular dual-arm robot. In order to accurately describe the kinematic characteristics of the robot, it is first necessary to establish the DH parameter model of the robot. i , connecting rod torsion angle α i , joint offset d i , joint angle θ i To describe the spatial relationship between adjacent joints. i represents the offset of joint i, that is, the displacement along the rotation axis of joint i; θ i represents the rotation angle of joint i, that is, the rotation angle around the axis of joint i; a i represents the length of connecting rod i, that is, the distance along the common vertical line; α i It represents the torsion angle of connecting rod i, that is, the angle between two adjacent rotating axes.

[0196] Table 1 Left arm DH parameters

[0197]

[0198] Table 2 DH parameters of right arm

[0199]

[0200] Matlab software was used to build a simulation environment for experimental verification. The experimental operating environment is 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-position hybrid control method. Please refer to Figure 4 , Figure 5 , Figure 6 . Figure 4 In order to conduct a modular dual-arm robot coordinated force-position hybrid control experiment without external disturbance, the control results show that the end contact force in three directions of the dual arms tends to be stable over time. Figure 5 In order to conduct a modular dual-arm robot coordinated force-position hybrid control experiment in the presence of external disturbances, the actual contact force at the end of the dual-arm robot oscillates from the control results, but the actual contact forces in the three directions tend to be stable over time. Figure 6 It can be shown that the actual trajectory of the dual-arm robot end basically coincides with the expected handling trajectory.

[0201] The technical solution of the embodiment of the present invention has the following beneficial effects:

[0202] This invention can dynamically adjust the weights of force control and position control according to the real-time trajectory error and force error, ensuring that the modular dual-arm robot can accurately track the desired trajectory when performing tasks while maintaining internal force balance, thereby enhancing the robustness and stability of the system; through the combination of sliding mode control and model predictive control, the left arm and the right arm can coordinate and cooperate to adapt to environmental changes, thereby 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 in the scope of protection of the present invention.

[0204] The contents not described in detail in the specification of the present invention belong to the common knowledge of those skilled in the art.

Claims

1. A force-position hybrid control method for modular dual-arm robot handling, characterized in that: The method comprises: Step S1 divides the operation space into a force control subspace and a position control subspace based on the principle of orthogonal decomposition of the operation space, and establishes a modular dual-arm robot control framework; Step S2 designs a dual-arm force tracking strategy in the force control subspace and uses adaptive admittance control to achieve high-precision tracking of the desired force; Step S3 designs a dual-arm synchronization strategy based on closed-chain constraints and Jacobi mapping to achieve coordinated motion of the dual-arm robot; Step S4 designs a dual-arm coordinated trajectory tracking strategy in the position control subspace, wherein the left arm uses sliding mode control to robustly track the reference trajectory, and 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 tracking the expected force and trajectory during the handling process of the dual-arm robot.

2. The method according to claim 1, characterized in that The step S1 comprises: Step S1.1 defines the end effector position and posture of the modular dual-arm robot: Among them, x L 、x R They represent the pose vectors of the left and right arm end effectors, respectively, and p L 、p R Respectively represent the position vectors of the left and right arm end effectors, θ L ,θ R represents the posture vector of the left arm end effector; Step S1.2 analyzes the constraints that the dual-arm robot must comply with during the handling process and establishes the constraint equation: (1) During the handling process, the two arms have certain constraints, including Dynamic Constraint: z k+i+1 =f(z k+i ,τ R,K+i )(i=0,1,...,N-1); End effector constraint: τ min ≤τ≤τ max ; Closed-chain kinematic constraints: Position constraint: p R =p L -d const Posture constraint: R L =R R ; Where: z i represents the state of the system at the i-th moment, τ R,K+i represents the control torque applied to the right arm at the K+ith moment; τ min , τ max Respectively represent the lower and upper limits of the system joint torque τ; R L , R R Represents the rotation matrix of the left and right arm end effectors respectively; d const is a fixed relative position vector, determined by the geometric characteristics of the transported object; (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: t R =set(τ position +t force ,t min ,t max ) The meaning of the set() function is: set(v,a,b)=max(a,min(v,b)), which is used to limit the control input, τ position represents the torque output of the position control subspace, τ force represents the torque output of the force-controlled subspace, τ R represents the torque input of the right arm; Step S1.3 divides the control space of the dual-arm robot into a force control space and a position control space, specifically by calculating the Jacobian matrix of the constraint equation and performing singular value decomposition to obtain the projection matrix of the subspace; Step S1.3.1 calculates the Jacobian matrix of the constraint equation: Among them, Φ(x) represents the constraint equation of the dual-arm robot during the handling process, J c The Jacobian matrix representing the constraint equations, which describes the relationship between the constraints and the robot end-effector pose; Step S1.3.2 performs singular value decomposition on the Jacobian matrix: Among them, U represents the left singular matrix, Σ represents the singular value matrix, and V represents the right singular matrix; Step S1.3.3 Obtain the force control subspace projection matrix P f and the position control subspace projection matrix P m : P f =V1V1 T 、P m =V2V2 T =1-P f Among them, P f represents the projection matrix that maps the control space to the force control subspace, P m represents the projection of the control space onto the position control subspace.

3. The method according to claim 1, characterized in that The step S2 comprises: Step S2.1 designs an adaptive admittance control in the force control subspace, including: Step S2.1.1 sets up the admittance control model and designs the feedforward gain according to the control accuracy requirements to ensure that the robot can accurately track the desired force and position: Among them, M d represents the virtual inertia matrix, D d represents the virtual damping matrix, K d represents the virtual stiffness matrix, K e represents the feedforward gain, x, represents the position, velocity, and acceleration of the end effector, x d represents the desired position, F ff represents the feedforward control force; Step S2.1.2 designs the adaptive law in the adaptive admittance controller as: Among them, Γ M , Γ D , Γ K They represent the positive definite adaptive gain matrix, e=F e -F d It is expressed as tracking force error. An adaptive mechanism is introduced to dynamically adjust the admittance parameters and generate the corrected position trajectory command to achieve accurate tracking of the internal force by the dual-arm end-effector.

4. The method according to claim 1, characterized in that: The step S3 comprises: Step S3.1: When the two arms grasp the same object, synchronous error information is established: Among them, K v is the velocity error gain coefficient; Step S3.2 designs a dual-arm synchronization strategy, including: Step S3.2.1 During the synchronization of both arms, the actual motion state of the left arm is mapped to the joint space of the right arm: in, is the pseudo-inverse of the right-arm Jacobian matrix, K p is the proportional gain, D p is the damping gain; when the synchronization error e sync When the set threshold is exceeded, the proportional gain K is increased. p To enhance the system's ability to correct errors and increase the damping gain D p To enhance the stability of the system; Step S3.2.2: calculating the compensation torque of the left arm based on the contact force feedback of the right arm; During synchronous control, in order to synchronize the movement of the right arm with the left arm, the compensation torque of the right arm needs to be calculated: in, represents the actual contact force of the right arm, represents the desired force in the right arm, represents the compensation torque of the left arm; Step S3.2.3 dynamically adjusts the admittance parameters of the right arm; In the actual contact process, changes in environmental stiffness will cause changes in the contact force of the right arm, resulting in an imbalance in the internal force. Therefore, the admittance parameters of the left arm are dynamically adjusted to adapt to changes in the environment: in, It is represented by the updated admittance parameter of the left arm, ΔF R is the change in the right arm contact force, α is the adjustment coefficient, 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 Jacobian matrix; In order to solve the problem that the traditional Jacobian matrix fails under singular configurations, an online Jacobian estimation based on sensor data is used to adjust the value of the Jacobian matrix in real time through an adaptive update formula: Among them, γ = 0.2 represents the learning rate, λ = 0.00001 represents the regularization coefficient, and q i represents the joint angle vector of the robot at time i; Step S3.3.2 Design singular value damping strategy When the system is close to a singular configuration, the determinant of the Jacobian matrix may approach zero, thus affecting the control effect. To avoid this problem, a singular avoidance strategy is designed: When det(JJ T )<ε, by adjusting the terminal speed To reduce the speed and avoid singularity; 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 |<δ, where δ=5°), adjustments are made in the null space to prevent the system from entering the singular region.

5. The method according to claim 1, characterized in that The step S4 comprises: Step S4.1 During the handling process, the left arm needs to track the reference trajectory with high precision and resist external disturbances during the handling process. Sliding mode control is used to achieve trajectory tracking, including: Step S4.1.1 defines the sliding surface of sliding mode control: 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. Step S4.1.2 uses the approach rate to suppress the jitter caused by the sliding surface driven by the sign function sgn(s) driving state, and introduces dynamic compensation to offset the nonlinear dynamic characteristics in the system: Where M(q) represents the Cartesian mass matrix, represents the velocity vector in virtual Cartesian space, G(q) represents the Cartesian gravity term vector, K represents the switching gain of sliding mode control, φ represents the boundary layer thickness, and φ=0.05; Step S4.2: In the dual-arm collaborative operation, the right arm needs to track the reference trajectory with high precision while achieving dual-arm coordinated control, including: Step S4.2.1 defines the end pose of the right arm and its derivatives: Among them, x R , represents the end position of the right arm and its derivative, and Step S4.2.2 uses the Euclidean optimization method to control the right arm. During the optimization process, the control input is adjusted in real time. The control update formula is: z k+1 =z k +T s ·f(z k ,τ R,k )+Δ dist Among them, f(z k ,τ R,k ) is expressed as the trajectory control function, Δ dist is represented as a disturbance term; Step S4.2.3 The double-arm handling process forms a closed chain, and the left arm state x L As the external input of the right arm model predictive control to improve the coordination accuracy between the two arms, the external input formula of the model is as follows: in, represents the trajectory tracking weight matrix, represents the internal force tracking weight matrix, represents the control input penalty matrix, N represents the prediction time domain, M represents the control time domain, and the optimization objective function J; Step S4.2.4 Construct the right arm control input torque τ R Minimize the function: Where H represents the Hessian matrix, which is composed of Q x , Q f , R, g represents the gradient vector containing the deviation between the reference trajectory and the current state.

6. The method according to claim 1, characterized in that The step S5 comprises: Step S5.1 adaptively allocates the weight between force control and position control according to the real-time status during the handling process, including: Step S5.1.1 In order to allocate the priority of force control and position control in real time, define the global cost function J total To comprehensively consider the position error and force error, the specific global cost function is: J total =γ(t)·J position +β(t)·J force ,γ(t)+β(t)=1 Among them, J position =||xx d || 2 represents the trajectory tracking error, J force =||F int -F d || 2 represents the internal force tracking error, γ(t)∈[0,1], β(t)∈[0,1] represent the dynamic weight coefficient; Step S5.1.2 sets an adaptive weight adjustment function, and adjusts the weight coefficient in real time according to the changes in position error and force error. The weight adjustment function is defined as follows: β(t)=1-γ(t) Among them, e x =xx d represents the trajectory tracking error, e F =F int -F d Represents the internal force tracking error, ε>0, when γ(t)≥0.5, position control takes priority, and when β(t)≥0.5, force control takes priority.

Images