A spatial closed-loop dual-arm robot motion control method based on self-correcting control

Abstract

The present invention discloses a space closed-loop dual-arm robot motion control method based on self-correcting control, belonging to the field of space robot control technology. For non-cooperative targets with unknown inertial parameters in space, both arms are used as task arms to capture the target to form a closed-loop system. Based on the establishment of the kinematic and dynamic models of the closed-loop robot system, the closed-loop dynamic model is converted into an identification model of the unknown inertial parameters of the target. The method first calculates the expected trajectory of the robot joints based on the expected trajectory of the target in Cartesian space, then measures the actual trajectory of the robot joints, and calculates the control torque using the expected trajectory and the actual trajectory as input. With the help of finite time estimation, expected velocity feedback, and expected position feedback, the closed-loop dynamic model of the dual-arm robot is updated in real time through the unknown parameter identification model. The control algorithm proposed in the present invention realizes the coordinated control of the space dual-arm robot to capture non-cooperative targets with unknown inertial parameters. It can be applied to the derotation of tumbling targets after capture. In addition, the method still exhibits high control accuracy and robustness under the condition of external interference.

Description

Technical Field

[0001] The invention discloses a space closed-loop dual-arm robot motion control method based on self-correction control, belonging to the technical field of space robot control. Background Art

[0002] With the advancement of space science and technology, an increasing number of faulty satellites and space debris are floating in orbit. Space robots have become a key technology for achieving on-orbit servicing. However, in a microgravity environment, the dynamic coupling between the space robot base and manipulator, as well as the various interfering forces and torques in space, complicate the control of space robot systems.

[0003] Currently, most space robot control research focuses on single-arm space robots, with relatively little research on dual-arm space robots. Research on dual-arm space robots also primarily focuses on open-loop motion planning. However, closed-loop dual-arm space robots still face several research challenges: 1) Closed-loop grasping systems introduce closed-loop constraints; 2) Closed-loop systems have controller redundancy, necessitating a rational distribution of control torques. Furthermore, current control methods for space robots are often based on accurate dynamic models of the space robot, but the dynamic parameters of space robots are difficult to accurately determine.

[0004] Commonly used methods to address these issues include sliding mode control, reinforcement learning, real-time parameter identification, and self-correcting control. This paper addresses this issue by developing a self-correcting control strategy that uses joint state feedback and a parameter identification model to update the closed-loop robot's dynamic model in real time and calculate joint input torques based on the desired trajectory. This method can be applied to derotation of tumbling targets after capture, and exhibits high control accuracy and robustness even in the presence of external interference. Summary of the Invention

[0005] To address the control problem of a closed-loop system assembly formed by a dual-arm spatial robot capturing a non-cooperative target with unknown parameters, this paper provides a motion control method for a dual-arm spatial robot based on a self-correcting control strategy. This method updates the system's dynamics model in real time based on a parameter identification model and velocity and acceleration feedback, ultimately achieving stable control of the assembly. This method is applicable to derotating tumbling targets after capture, and exhibits high control accuracy and robustness even in the presence of external interference.

[0006] A motion control method for a spatial dual-arm robot based on a self-correcting control strategy comprises the following steps:

[0007] Step 1: Establish a closed-loop kinematic model of the dual-arm space robot and use the Newton-Euler method to establish a closed-loop dynamic model of the space robot:

[0008] Step 1.1: The closed-loop motion model of the space robot established in step 1 above is:

[0009]

[0010] in represents the generalized coordinate vector, Among them, α, β, γ are the base postures, φ i (k) (i=1,2,…,n,k=1,2) are the n joint rotation angles of the two manipulators. And X t =[r tx ,r ty ,r tz ,θ tα ,θ tβ ,θ tγ ] T , r tx ,r ty ,r tz represents the target displacement, θ tα ,θ tβ ,θ tγ Indicates the target posture; represents the Jacobian matrix of the system.

[0011] Step 1.2: The system dynamics model established according to the Newton-Euler algorithm is:

[0012]

[0013] Represents the base attitude adjustment torque and joint torque. is the inertia matrix of the system, represents a nonlinear term.

[0014] Step 2: Based on the dynamic model established in step 1, the unknown inertial parameters of the capture target are used as output to establish a parameter identification model of the system, and the error function is defined by the expected joint trajectory and the actual joint trajectory;

[0015] Step 2.1: In step 2 above, the unknown inertial parameters of the captured target are used as output. The parameter identification model of the system is established as follows:

[0016] As described in step 1.2 There are some unknown inertia parameters of the target in Equation (2). The joint state is used as input and the control torque is output. In the identification model, the unknown inertia parameters are considered as input. q is a known state parameter of the system, and the output control torque is:

[0017] τ=Yp+Y B (3)

[0018] in is the linear regression matrix of the kinetic parameter identification model, is the nonlinear term of the identification model. is the unknown parameter of the non-cooperative target, and generally p=[b xt (1) ,b yt (1) ,b zt (1) ,I tx ,I ty ,I tz ] T .

[0019] Step 2.2: The error function defined in step 2 above is:

[0020]

[0021] Where Λ=Λ T >0.

[0022] Step 3: Based on the parameter identification model established in step 2, design the parameter adaptation law and closed-loop system control law to achieve stable control of the combination:

[0023] Substituting equation (3) in step 2.1 and equation (4) in step 2.2 into equation (2) in step 1.2, we can obtain:

[0024]

[0025] Step 3.1: The closed-loop system control law designed according to equation (5) is:

[0026]

[0027] τ r =-K2e s / (||e s ||+ξ) (7)

[0028] where τ r is a robust term, K1 and K2 are the selected control gain matrices, and K1, K2> 0. Substituting Equation (6) into Equation (5), the system error dynamics expression can be obtained:

[0029]

[0030] in Adaptive variables is the estimated value of the unknown inertial parameter p of the target.

[0031] Step 3.2: According to the designed system control law, adjust the adaptive variables Design a real-time estimation law for the parameters:

[0032]

[0033] Where π is a positive definite matrix, is a positive gain constant, Q and B c is the auxiliary matrix and satisfies:

[0034]

[0035] Where δ and μ are positive gain constants, Q(0), U(0) and B c (0) Q, U and B respectively c The initial value of .

[0036] According to formula (10), it can be deduced that:

[0037]

[0038] Then use the singular value decomposition method to solve U, and we get U = νAλ T , where ν is an orthogonal matrix whose column vector is UU T The eigenvector of λ is an orthogonal matrix, and its column vector is U T The eigenvectors of U, A is a diagonal matrix and A=diag(a1,…,a n ). From this we can deduce:

[0039]

[0040] Define F(t) as an auxiliary term and but

[0041] Q(t)B c (t)=pF(t)p (13)

[0042] Where F(t) satisfies

[0043] Step 4: Use the Lyapunov method to prove the stability of the system for the designed controller:

[0044] Define the Lyapunov function as a quadratic function:

[0045]

[0046] Therefore V≥0 holds.

[0047] Taking the derivative of V and combining it with formula (13) we can get:

[0048]

[0049] Because F is bounded and satisfies Therefore, there exists a compact set near 0, and by selecting appropriate Π, δ, K2 and μ, we can make This shows that the system is stable.

[0050] Advantages of the invention:

[0051] The technical effect of this invention lies in proposing a spatial closed-loop dual-arm robot motion control method based on self-correcting control. This method addresses the problem of unknown inertial parameters of non-cooperative targets in a dual-arm closed-loop assembly system by combining the theory of finite-time estimation with real-time updates of the system dynamics model using robot joint velocity and acceleration feedback, thereby achieving derotational stabilization of the target. Furthermore, the method demonstrates high control accuracy and robustness even in the presence of external interference. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 is a control flow chart of the present invention;

[0053] Figure 2 The three-dimensional model and coordinate system definition of the space robot in the embodiment;

[0054] Figure 3 This is a diagram showing the effect of stable control during racemization in the embodiment;

[0055] Figure 4 Graph showing target displacement and velocity error during de-rotation in the embodiment;

[0056] Figure 5 This is a schematic diagram of the racemization simulation results of the embodiment;

[0057] Figure 6 : is a comparison diagram of the tracking errors of the non-cooperative target under various interference torques in the embodiment. DETAILED DESCRIPTION

[0058] In order to make the objectives, technical solutions and advantages of the present invention more clear, the present invention is further described in detail below with reference to the accompanying drawings and embodiments.

[0059] See also Figures 1-6 The technical solutions adopted by the present invention include the following: Figure 1 The control flow shown is as follows Figure 2 Taking the dual-arm seven-degree-of-freedom robot system shown in the figure as an example, in order to verify the algorithm, the simplified model is a planar dual-arm robot. The spatial closed-loop dual-arm robot motion control method based on the self-correcting control strategy described in this example includes the following steps:

[0060] Step 1: Establish a closed-loop kinematic model of the dual-arm space robot and use the Newton-Euler method to establish a closed-loop dynamic model of the space robot:

[0061] Step 1.1: The closed-loop motion model of the space robot established in step 1 above is:

[0062]

[0063] in represents the generalized coordinate vector, Among them, α, β, γ are the base postures, φ i (k) (i=1,2,…,n,k=1,2) are the n joint rotation angles of the two manipulators. And X t =[r tx ,r ty ,r tz ,θ tα ,θ tβ ,θ tγ ] T , r tx ,r ty ,r tz represents the target displacement, θ tα ,θ tβ ,θ tγ Indicates the target posture; represents the Jacobian matrix of the system.

[0064] Step 1.2: The system dynamics model established according to the Newton-Euler algorithm is:

[0065]

[0066] Represents the base attitude adjustment torque and joint torque. is the inertia matrix of the system, represents a nonlinear term.

[0067] Step 2: Based on the dynamic model established in step 1, the unknown inertial parameters of the capture target are used as output to establish a parameter identification model of the system, and the error function is defined by the expected joint trajectory and the actual joint trajectory;

[0068] Step 2.1: In step 2 above, the unknown inertial parameters of the captured target are used as output. The parameter identification model of the system is established as follows:

[0069] As described in step 1.2 There are some unknown inertia parameters of the target in Equation (2). The joint state is used as input and the control torque is output. In the identification model, the unknown inertia parameters are considered as input. q is a known state parameter of the system, and the output control torque is:

[0070] τ=Yp+Y B (3)

[0071] in is the linear regression matrix of the kinetic parameter identification model, is the nonlinear term of the identification model. is the unknown parameter of the non-cooperative target, and generally p=[b xt (1) ,b yt (1) ,b zt (1) ,I tx ,I ty ,I tz ] T .

[0072] Step 2.2: The error function defined in step 2 above is:

[0073]

[0074] Where Λ=Λ T >0.

[0075] Step 3: Based on the parameter identification model established in step 2, design the parameter adaptation law and closed-loop system control law to achieve stable control of the combination:

[0076] Substituting equation (3) in step 2.1 and equation (4) in step 2.2 into equation (2) in step 1.2, we can obtain:

[0077]

[0078] Step 3.1: The closed-loop system control law designed according to equation (5) is:

[0079]

[0080] τ r =-K2e s / (||e s ||+ξ) (7)

[0081] where τ r is a robust term, K1 and K2 are the selected control gain matrices, and K1, K2> 0. Substituting Equation (6) into Equation (5), the system error dynamics expression can be obtained:

[0082]

[0083] in Adaptive variables is the estimated value of the unknown inertial parameter p of the target.

[0084] Step 3.2: According to the designed system control law, adjust the adaptive variables Design a real-time estimation law for the parameters:

[0085]

[0086] Where π is a positive definite matrix, is a positive gain constant, Q and B c is the auxiliary matrix and satisfies:

[0087]

[0088] Where δ and μ are positive gain constants, Q(0), U(0) and B c (0) Q, U and B respectively c The initial value of .

[0089] According to formula (10), it can be deduced that:

[0090]

[0091] Then use the singular value decomposition method to solve U, and we get U = νAλ T , where ν is an orthogonal matrix whose column vector is UU T The eigenvector of λ is an orthogonal matrix, and its column vector is U T The eigenvectors of U, A is a diagonal matrix and A=diag(a1,…,a n ). From this we can deduce:

[0092]

[0093] Define F(t) as an auxiliary term and but

[0094] Q(t)B c (t)=pF(t)p (13)

[0095] Where F(t) satisfies

[0096] Step 4: Use the Lyapunov method to prove the stability of the system for the designed controller:

[0097] Define the Lyapunov function as a quadratic function:

[0098]

[0099] Therefore V≥0 holds.

[0100] Taking the derivative of V and combining it with formula (13) we can get:

[0101]

[0102] Because F is bounded and satisfies Therefore, there exists a compact set near 0, and by selecting appropriate Π, δ, K2 and μ, we can make This shows that the system is stable.

[0103] In order to verify the effectiveness of the proposed algorithm, the present invention Figure 2 The space robot shown in the figure was simulated and its parameters are shown in Table 1 and Table 2. represents the link i in the operating arm k, Indicates from the joint arrive The length of the centroid and the Center of mass to joint The length of The initial posture and joint angles of the space robot are q(0) = [0, 45°, 90°, 45°, -45°, -90°, -45°] T .

[0104] Table 1 Physical parameters of space robot

[0105]

[0106] Table 2 Geometric parameters of the space robot

[0107]

[0108]

[0109] This experiment takes into account that non-cooperative targets in space are often in a tumbling state. For such situations, the target is set to have an initial angular velocity of about 30° / s, and the target expected displacement is specified as X td =[r tx ,r ty ,θ t ] T =[0.04mm,0.06mm,0.08rad] T , and plans the target trajectory using a cubic polynomial. Figure 3 Demonstrated the angle and angular velocity planning and tracking of a tumbling target to achieve derotation; Figure 4The displacement, angle, velocity, and angular velocity tracking error effects during the target derotation control process are shown. It can be seen that the algorithm can still achieve stable tracking of the target displacement tracking error and velocity tracking error when the target has an initial angular velocity of 30° / s. Figure 5 This is a schematic diagram of the racemization simulation results for this example. Figure 3-Figure 5 It can be seen that the algorithm can achieve stable derotation control of the tumbling target and achieve high control accuracy. Figure 6 In order to test the robustness of the algorithm, the test did not introduce the initial angular velocity of the target due to the single variable of interference. By designing a displacement trajectory and adding an external torque to the base with a gradually increasing Gaussian interference mean, the trend of target tracking error change with the increase of interference torque was observed. It can be seen from the figure that with the increase of external interference torque, the algorithm still maintains a high accuracy, which proves the robustness of the algorithm.

Claims

1. A spatial closed-loop dual-arm robot motion control method based on self-correcting control, characterized by: The specific steps include: Step 1: Establish a closed-loop kinematic model of the dual-arm space robot and use the Newton-Euler method to establish a closed-loop dynamic model of the space robot: Step 1.1: The closed-loop motion model of the space robot established in step 1 above is: in represents the generalized coordinate vector, Among them, α, β, γ are the base postures, φ i (k) (i=1,2,…,n,k=1,2) are the n joint rotation angles of the two manipulators, And X t =[r tx ,r ty ,r tz ,θ tα ,θ tβ ,θ tγ ] T , r tx ,r ty ,r tz represents the target displacement, θ tα ,θ tβ ,θ tγ Indicates the target posture; represents the Jacobian matrix of the system; Step 1.2: The system dynamics model established according to the Newton-Euler algorithm is: Represents the base posture adjustment torque and joint torque, is the inertia matrix of the system, represents a nonlinear term; Step 2: Based on the dynamic model established in step 1, the unknown inertial parameters of the capture target are used as output to establish a parameter identification model of the system, and the error function is defined by the expected joint trajectory and the actual joint trajectory; Step 2.1: In step 2 above, the unknown inertial parameters of the captured target are used as output. The parameter identification model of the system is established as follows: As described in step 1.2 There are some unknown inertia parameters of the target. In formula (2), the joint state is used as input and the control torque is output; In the identification model, considering unknown inertia parameters as input, q is a known state parameter of the system, and the output control torque is: τ=Yp+Y B (3) in is the linear regression matrix of the kinetic parameter identification model, is the nonlinear term of the identification model, is the unknown parameter of the non-cooperative target, and p=[b xt (1) ,b yt (1) ,b zt (1) ,I tx ,I ty ,I tz ] T ; Step 2.2: The error function defined in step 2 above is: Where L=L T >0; Step 3: Based on the parameter identification model established in step 2, design the parameter adaptation law and closed-loop system control law to achieve stable control of the combination: Substituting equation (3) in step 2.1 and equation (4) in step 2.2 into equation (2) in step 1.2, we can obtain: Step 3.1: The closed-loop system control law designed according to equation (5) is: t r =-K2e s / (||e s ||+ξ) (7) where τ r is a robust term, K1 and K2 are the selected control gain matrices, and K1, K2>0; Substituting Equation (6) into Equation (5), the system error dynamics expression can be obtained: in Adaptive variables is the estimated value of the unknown inertial parameter p of the target; Step 3.2: According to the designed system control law, adjust the adaptive variables Design a real-time estimation law for the parameters: Where π is a positive definite matrix, is a positive gain constant, Q and B c is the auxiliary matrix and satisfies: Where δ and μ are positive gain constants, Q(0), U(0) and B c (0) Q, U and B respectively c The initial value of According to formula (10), it can be deduced that: Then use the singular value decomposition method to solve U, and we get U = νAλ T , where ν is an orthogonal matrix whose column vector is UU T The eigenvector of λ is an orthogonal matrix, and its column vector is U T The eigenvectors of U, A is a diagonal matrix and A=diag(a1,…,a n ), from which we can deduce: Define F(t) as an auxiliary term and but Q(t)B c (t)=p-F(t)p (13) Where F(t) satisfies Step 4: Use the Lyapunov method to prove the stability of the system for the designed controller: Define the Lyapunov function as a quadratic function: Therefore, V ≥ 0 holds; Taking the derivative of V and combining it with formula (13) we can get: Because F is bounded and satisfies Therefore, there exists a compact set near 0, and by selecting appropriate Π, δ, K2 and μ, we can make This shows that the system is stable.

Images