A control method for suppressing sudden force changes in reconfigurable manipulator operation

Abstract

The present invention relates to a method for suppressing sudden changes in the operating force of a reconfigurable manipulator. Based on the matrix perturbation theory, a space manipulator joint torque compensation is proposed to achieve the compensation torque equation and the dynamic equation containing the Jacobian matrix, thereby realizing the redistribution of the joint torque of a generalized redundant space manipulator fault. At the same time, the method of the present invention combines the advantages of the minimum norm method and the minimum torque method, and proposes a joint acceleration inverse solution method obtained by minimizing the joint torque as the optimization goal, so as to solve the instability of the joint values ​​and the joint singularity problems caused by the calculation of the minimum joint torque method.

Description

Technical Field

[0001] The present invention relates to a control method for suppressing sudden change in operating force of a reconfigurable mechanical arm, which belongs to the technical field of robots and is used for redistributing joint torques when the mechanical arm is reconfigured online. Background Art

[0002] During the on-orbit operation of the space manipulator, the joints of the manipulator need to be autonomously reconstructed due to operational mission requirements or failures. However, after the reconstruction of the space manipulator, parameters such as the terminal velocity, joint velocity, and joint impact torque will change suddenly. Among them, the sudden changes in the terminal and joint impact torque have a huge impact on the motion stability of the space manipulator. How to effectively suppress the sudden force changes during the movement is of great significance to the stable operation of the space manipulator.

[0003] After searching the relevant literature, it is found that in general, when one or more joints fail, the movement of the robot arm is always stopped first, and then one joint is locked, and the motion path is replanned according to the new configuration after the degraded degree of freedom. However, this method is not suitable for robot arm systems that require dynamic continuous and uninterrupted completion of tasks. In order to expand the continuous execution motion capability of the robot arm after online reconstruction, a force mutation suppression and torque redistribution algorithm is required, which is crucial for the reliable application of the robot arm on orbit. Summary of the invention

[0004] The technical problem solved by the present invention is: to overcome the deficiencies of the prior art, to provide a control method for suppressing sudden changes in the operating force of a reconfigurable robotic arm, and to solve the optimization problem of continuous dynamic path planning after the robotic arm is reconfigured.

[0005] The technical solution of the present invention is:

[0006] A reconfigurable manipulator operating force mutation suppression control method, comprising:

[0007] According to the dynamic equation of the robot arm and the matrix perturbation theory, the sudden change of the end operation force is suppressed;

[0008] According to the fault-tolerant path planning strategy with optimal joint torque, joint torque optimization is performed to achieve adjustment of model reconstruction and sudden change of operation parameters of the spatial robot arm and suppress sudden change.

[0009] Furthermore, the method of suppressing the sudden change of the end operation force according to the mechanical arm dynamics equation and matrix perturbation theory specifically includes:

[0010] Establish the dynamic equations of redundant space manipulator;

[0011] Based on the redundant space manipulator dynamics equation, the end force of the manipulator is considered, and a manipulator dynamics equation considering the end force is established;

[0012] Determine the planning Jacobian matrix of the robot arm when the kth joint is locked at a certain time t of the movement k J;

[0013] Based on the idea of ​​perturbation model, the relationship between joint velocity and terminal velocity after failure is determined;

[0014] Determine the end force variation relationship of the degraded manipulator after the failure;

[0015] The joint torque compensation amount is determined to minimize the force jump at the end of the spatial manipulator after a fault, thereby achieving the suppression of sudden changes in the end operating force.

[0016] Furthermore, the dynamic equation of the redundant space manipulator is specifically:

[0017] The dynamic equation of the redundant space manipulator is established by the Lagrangian method:

[0018]

[0019] Where M(q) is the mass matrix of the system; are the centrifugal and Coriolis terms; G(q) is the gravity term; τ d is the joint torque required to provide joint movement, where q, They are the joint angular position, angular velocity and angular acceleration vectors respectively.

[0020] Furthermore, the dynamic equation of the manipulator considering the end force is specifically:

[0021]

[0022] Among them, J T is the transpose of the Jacobian matrix, and F is the end force.

[0023] Furthermore, the planning Jacobian matrix of the robot arm is k J, specifically:

[0024] k J=[j1,...,j k-1 ,j k+1 ,...,j n ]

[0025] in, k J is the Jacobian matrix after k-joint failure.

[0026] Furthermore, the relationship between the joint speed and the terminal speed after the fault is determined is specifically:

[0027]

[0028] In the formula, is the change of the spatial manipulator joint velocity after the fault, ΔJ is the perturbation Jacobian matrix of the manipulator joint after the fault; is the terminal velocity vector, is the velocity jump of the manipulator end in the degenerate space before velocity compensation; J is the Jacobian matrix before the fault.

[0029] Furthermore, the relationship of the end force change of the degraded robot arm after the fault is determined is specifically:

[0030] F+ΔF=((J T ) + +Δ(J T ) + )(τ+Δτ)

[0031] Where τ is the joint output torque, Δτ is the change in the torque of the spatial manipulator after the joint failure occurs, and J T is the transposed Jacobian matrix, Δ(J T ) + is the perturbation Jacobian matrix, and ΔF is the sudden change of the end force of the spatial manipulator without joint torque compensation.

[0032] Furthermore, the determination of the joint torque compensation amount is specifically as follows:

[0033] Using ΔU as the compensation of joint torque, we have:

[0034]

[0035] In the formula, It is the force jump of the end of the manipulator after the torque compensation is introduced; when the kth joint of the space manipulator has a joint fault and is locked:

[0036]

[0037] in, k (J T ) + is the pseudo-inverse of the Jacobian matrix after the k-th joint fails.

[0038] Simplifying the above formula, we can get the torque compensation with the smallest force jump at the end of the redundant space manipulator as follows:

[0039] ΔU={Δ(J T ) + τ}{ k (J T ) +} +

[0040] Furthermore, the joint torque optimization is performed based on the fault-tolerant path planning strategy with the best joint torque, including:

[0041] (1) Considering the weight matrix W, Z = τ T Minimizing Wτ is the optimization objective, and the inverse solution of joint acceleration is determined;

[0042] (2) Take the weight matrix W = D -1 , and ignore the gravity term G, and redetermine the inverse solution of the joint acceleration;

[0043] (3) Control during the calculation process The value of is used to determine the improved inverse solution of joint acceleration.

[0044] Furthermore,

[0045] Considering the weight matrix W, Z = τ T The inverse solution of the joint acceleration obtained by minimizing Wτ as the optimization objective is:

[0046]

[0047] In the formula, A=DWD∈R n×n , I∈R n×n is the identity matrix, is the end acceleration, H is the mass matrix of the robot arm, G is the gravity vector, is the first-order derivative of the Jacobian matrix;

[0048] Take the weight matrix W = D -1 , and ignoring the gravity term G, the inverse solution of the joint acceleration is:

[0049]

[0050] In the formula,

[0051] Control during calculation The improved calculation method of the inverse solution of joint acceleration is as follows:

[0052]

[0053] In the formula, β is the pre-set Upper limit, when When the minimum joint moment method is used, When , the minimum norm method is used to calculate, J + is the pseudoinverse of the Jacobian matrix.

[0054] The beneficial effects of the present invention compared with the prior art are:

[0055] (1) The present invention realizes the reconstruction of the robot control model, the suppression of sudden disturbances of the end and joint forces, and the fault-tolerant path planning design with optimal torque, which is of great significance for the rapid model reconstruction and reconfigurable path planning optimization algorithm within the limited robot motion space.

[0056] (2) The model reconstruction of the robot arm of the present invention mainly includes the reconstruction of the mathematical model, including the kinematic model and the dynamic model; the online adjustment includes the adjustment of the control model and the adjustment of the operating parameters. The adjustment of the motion parameters mainly includes the adjustment of the sudden change of the operating parameters caused by the model reconstruction. By introducing the compensation term, the sudden change is suppressed, so that the robot arm has a smooth characteristic during the model reconstruction and adjustment process.

[0057] (3) Through the joint fault-tolerant path planning design, the motion model can be reconstructed online and the real-time path planning can be realized in the case of joint failure of the robot arm, and the trajectory of the terminal task can be guaranteed not to mutate and continue to complete the current motion task, thereby improving the working reliability and safety of the robot arm and its fault response capability, and reducing the impact of the robot arm failure on the system task. BRIEF DESCRIPTION OF THE DRAWINGS

[0058] Figure 1 Schematic diagram of the end operating force of the robot arm under normal working conditions;

[0059] Figure 2 Schematic diagram of the end operating force when the robot arm joint is locked;

[0060] Figure 3 Schematic diagram of the end-operation force when the robot arm joints are locked and controlled. DETAILED DESCRIPTION

[0061] The specific embodiments of the present invention are further described in detail below with reference to the accompanying drawings.

[0062] The main idea of ​​the present invention is to reconstruct the kinematic model algorithm during the operation of the manipulator in the case of joint failure, and use the joint angle sequence and joint angular velocity sequence at this time as the basis for new planning to achieve real-time online adjustment of the control model. The present invention realizes the adjustment of model reconstruction and operation parameter mutations of the spatial manipulator, and suppresses mutations, so that the manipulator has smooth characteristics during model reconstruction and adjustment.

[0063] Specifically, the present invention proposes a method for suppressing sudden change in operating force of a reconfigurable manipulator, comprising the following steps:

[0064] Step 1: According to the dynamic equation of the robot arm and the matrix perturbation theory, the end operation force mutation is suppressed;

[0065] (1.1) According to the robot arm dynamics equation and matrix perturbation theory, the end operation force mutation suppression is performed, specifically including:

[0066] (1.2) Establish the dynamic equations of redundant space manipulator;

[0067] (1.3) Based on the redundant space manipulator dynamics equation, the end force of the manipulator is considered, and the manipulator dynamics equation considering the end force is established;

[0068] (1.4) Determine the planning Jacobian matrix of the robot arm when the kth joint is locked at a certain time t of the motion k J;

[0069] (1.5) Based on the idea of ​​perturbation model, determine the relationship between joint velocity and terminal velocity after failure;

[0070] (1.6) Determine the end force variation relationship of the degraded manipulator after the failure;

[0071] (1.7) Determine the joint torque compensation amount so that the end force jump of the spatial robot arm after the failure is minimized, thereby achieving the suppression of the sudden change of the end operation force.

[0072] Step 2: Optimize the joint torque based on the fault-tolerant path planning strategy with the best joint torque, so as to achieve the adjustment of the model reconstruction and operation parameter mutation of the spatial robot arm and suppress the mutation.

[0073] (2.1) Considering the weight matrix W, Z = τ T Minimizing Wτ is the optimization objective, and the inverse solution of joint acceleration is determined;

[0074] (2.2) Take the weight matrix W = D -1 , and ignore the gravity term G, and redetermine the inverse solution of the joint acceleration;

[0075] (2.3) Control during the calculation process The value of is used to determine the improved inverse solution of joint acceleration;

[0076] By combining the respective advantages of the minimum norm method and the minimum torque method, the inverse solution calculation of joint acceleration considering the optimization of joint torque is improved.

[0077] Example:

[0078] (1) Terminal force mutation suppression algorithm

[0079] For the reconfigurable space manipulator system, the nonlinear equation of the inverse dynamics of the space manipulator is established to analyze the suppression and redistribution of the sudden change of joint torque. The main methods and derivation steps are as follows:

[0080] The dynamic equation of the redundant space manipulator established by the Lagrangian method is:

[0081]

[0082] Where M(q) is the mass matrix of the system; are the centrifugal and Coriolis terms; G(q) is the gravity term; τ d is the joint torque required to provide joint movement, q, They are the joint angular position, angular velocity and angular acceleration vectors respectively.

[0083] Considering the force at the end of the robot arm, the dynamic equation of the system becomes:

[0084]

[0085] When the kth joint of the redundant space manipulator is reconstructed and locked, the original Jacobian matrix degenerates into k J. Delete the columns corresponding to the faulty joints in the Jacobian matrix J before the fault and introduce the degenerate Jacobian matrix k J is used to complete the inverse kinematic solution after the robot arm fails. Taking the single joint failure of the robot arm as an example, the kth joint is locked at a certain time t of the movement, then the planning Jacobian matrix of the robot arm is k J, as shown in formula (3):

[0086] k J=[j1,...,j k-1 ,j k+1 ,...,j n ] (3)

[0087] Based on the idea of ​​perturbation model, the changes of joint velocity and terminal velocity after failure are expressed as:

[0088]

[0089] In the formula, is the change of the spatial robot arm joint velocity after the fault, and ΔJ is the perturbation Jacobian matrix of the robot arm joint failure. Assume It is the speed jump of the end of the manipulator in the degenerate space before speed compensation is performed.

[0090] The change in the end force of the degraded robot arm after the fault is expressed as:

[0091] F+ΔF=((J T ) + +Δ(J T ) + )(τ+Δτ) (5)

[0092] Where Δτ is the change in torque of the spatial manipulator after the joint failure occurs, Δ(J T ) + is the perturbation Jacobian matrix after the fault, and ΔF is the sudden change of the end force of the spatial manipulator without joint torque compensation.

[0093] In order to minimize the force jump at the end of the space manipulator after a fault, the joint torque compensation is introduced, and ΔU is used as the compensation of the joint torque:

[0094]

[0095] In the formula, It is the force jump at the end of the manipulator after the torque compensation is introduced. When the kth joint of the space manipulator is locked due to joint failure, there are:

[0096]

[0097] Simplifying the above formula, we can get the torque compensation with the smallest force jump at the end of the redundant space manipulator as follows:

[0098] ΔU={Δ(J T ) + τ}{ k (J T ) +} + (8)

[0099] In particular, the principle followed by the joint torque redistribution of the redundant space manipulator is: under the condition of maintaining the generalized operating force of the space manipulator's task space unchanged, the torques in the joint space are redistributed, and the actual output torque of each joint of the manipulator is within its driving capacity, so as to minimize the torque jump before the reconstruction of the space manipulator model.

[0100] (2) Joint torque optimal fault-tolerant planning algorithm

[0101] Planning based on the minimum joint norm can ensure the stability of joint motion, but the joint torque is relatively large, and even the joint fails due to excessive torque. Therefore, an optimization strategy to minimize the joint torque after the robot arm is reconstructed is proposed. Considering the weight matrix W, Z = τ T The inverse solution of the joint acceleration obtained by minimizing Wτ as the optimization objective is:

[0102]

[0103] In the formula, A=DWD∈R n×n , I∈R n×n is the identity matrix.

[0104] Considering the special case, take the weight matrix W = D -1 , and ignoring the influence of the gravity term G, we can get from formula (9)

[0105]

[0106] In the formula,

[0107] Comparing the two planning strategies of optimizing joint torque, the minimum joint norm method and the minimum joint torque method, the fourth-order Runge-Kutta method can also be used to solve equation (10). When solving, the function definition is changed to And the initial joint acceleration is given by the minimum norm method Then perform numerical integration.

[0108] According to the simulation results: the results calculated by the minimum joint torque method will cause instability in joint values, and even joint singularity. The reasons for this mainly include two aspects:

[0109] 1) Calculated by numerical integration There are errors, which cause error accumulation.

[0110] 2) Matrix J H The pseudo-inverse singular value decomposition is malformed, resulting in errors, Calculation instability.

[0111] Therefore, corresponding correction measures are proposed, and an improved method for optimizing torque planning is obtained. The value is relatively large, and the minimum norm solution The most stable, therefore, in the calculation process control The numerical value of , combining the respective advantages of the minimum norm method and the minimum moment method, the improved calculation method is as follows:

[0112]

[0113] In the formula, β is the pre-set Upper limit, when When the minimum joint moment method is used, When , the minimum norm method is used for calculation. By using this switching technology, the stability of numerical calculation is guaranteed while the joint torque is minimized to a certain extent at the expense of some minimum joint torque indicators.

[0114] The method for suppressing sudden change of end force of redundant degree of freedom mechanical arm joint failure of the present invention proposes space mechanical arm joint torque compensation based on matrix perturbation theory, realizes satisfying compensation torque equation and dynamic equation containing Jacobian matrix, thereby realizing redistribution of joint torque of generalized redundant space mechanical arm failure. At the same time, the method of the present invention combines the advantages of minimum norm method and minimum torque method, proposes joint acceleration inverse solution method with joint torque minimization as optimization goal, and solves the instability of joint values ​​and joint singularity problems caused by minimum joint torque method calculation.

[0115] Conduct simulation to suppress sudden changes in end-operation force

[0116] The initial DH parameters of the robot are as follows:

[0117] Table 1 DH parameters of the robot arm

[0118]

[0119] The initial joint angle sequence is [-20° 0° -10° -140° 110° 155° 70°], and the target joint angle sequence is [-10° 10° -30° -120° 125° 135° 80°]. The mass of the base and joints of the robot arm are shown in Table 2. The running time is 40 seconds, and the action is divided into 400 steps, each step is 0.1 seconds. The fault situation is set as the third joint fault lock, and the fault occurs at the 20th second. According to the planned joint angle, the Newton-Euler recursion method is used to finally obtain the torque of each joint and the force on the base, and then the end operation force is obtained. There are three cases for research: when the robot arm is operating normally, when the robot arm is operating with a fault, and when the robot arm fails and suppression control is applied. It is intended to evaluate the proposed suppression algorithm by comparing the operating results obtained in the three cases. The mass of the base and joints of the robot arm (in kilograms) is as follows:

[0120] Table 2 Mass of the robotic arm base and joints (in kilograms)

[0121] link Base 1 2 3 4 5 6 7 quality 100000 42.5 42.5 70 70 42.5 42.5 42.5

[0122] Figure 1 This is the end operation force curve obtained when the robot arm works normally. It can be observed that both sets of curves are relatively smooth. Figure 2 The curve of the end operating force when the third joint fails and locks at 20 seconds. It can be observed that the end operating force undergoes a great mutation at the moment of failure, which can easily have a destructive effect on the robot arm, and the magnitude of the mutation is much greater than the dimension of the end operating force when the robot arm is working normally. Figure 3This is the end operating force curve obtained when the third joint fails and locks at 20 seconds and inhibitory control is applied. It is close to the end operating force when the robot arm works normally. The zero jump of the end operating force is achieved by redistributing the joint torque at the moment of failure.

[0123] Parts of the present invention that are not described in detail belong to common knowledge among those skilled in the art.

Claims

1. A control method for suppressing sudden changes in the operating force of a reconfigurable manipulator, characterized in that include: According to the dynamic equation of the robot arm and the matrix perturbation theory, the sudden change of the end operation force is suppressed; According to the fault-tolerant path planning strategy with the best joint torque, the joint torque is optimized, so as to realize the adjustment of the model reconstruction and operation parameter mutation of the space manipulator and suppress the mutation; The method of suppressing the sudden change of the end operation force according to the mechanical arm dynamics equation and matrix perturbation theory specifically includes: Establish the dynamic equations of redundant space manipulator; Based on the redundant space manipulator dynamics equation, the end force of the manipulator is considered, and a manipulator dynamics equation considering the end force is established; Determine the planning Jacobian matrix of the robot arm when the kth joint is locked at a certain time t of the movement k J; Based on the idea of ​​perturbation model, the relationship between joint velocity and terminal velocity after failure is determined; Determine the end force variation relationship of the degraded manipulator after the failure; Determine the joint torque compensation amount to minimize the force jump at the end of the space manipulator after the fault, thereby achieving the suppression of the sudden change of the end operation force; The dynamic equation of the redundant space manipulator is specifically: The dynamic equation of the redundant space manipulator is established by the Lagrangian method: Where M(q) is the mass matrix of the system; are the centrifugal and Coriolis terms; G(q) is the gravity term; τ d is the joint torque required to provide joint movement, where q, They are the joint angular position, angular velocity and angular acceleration vectors respectively; The dynamic equation of the manipulator considering the end force is specifically: Among them, J T is the transpose of the Jacobian matrix, F is the end force; Planning Jacobian matrix of the robot arm k J, specifically: k J=[j1,...,j k-1 ,j k+1 ,...,j n ] in, k J is the Jacobian matrix after k-joint failure; The relationship between the joint speed and the terminal speed after the fault is determined is specifically: In the formula, is the change of the spatial manipulator joint velocity after the fault, ΔJ is the perturbation Jacobian matrix of the manipulator joint after the fault; is the terminal velocity vector, is the velocity jump of the manipulator end in the degenerate space before velocity compensation; J is the Jacobian matrix before the fault; The relationship between the end force changes of the degraded robot arm after the fault is determined is specifically: F+ΔF=((J T ) + +Δ(J T ) + (τ+Δτ) Where τ is the joint output torque, Δτ is the change in the torque of the spatial manipulator after the joint failure occurs, and J T is the transposed Jacobian matrix, Δ(J T ) + is the perturbation Jacobian matrix, and ΔF is the sudden change of the end force of the spatial manipulator without joint torque compensation; The determination of the joint torque compensation amount is specifically as follows: Using ΔU as the compensation of joint torque, we have: In the formula, It is the force jump of the end of the manipulator after the torque compensation is introduced; when the kth joint of the space manipulator has a joint fault and is locked: in, k (J T ) + is the pseudo-inverse of the Jacobian matrix after the kth joint fails; Simplifying the above formula, we can get the torque compensation with the smallest force jump at the end of the redundant space manipulator as follows: ΔU={Δ(J T ) + t}{ k (J T ) + } + 。 2. The method for suppressing sudden change of operating force of a reconfigurable manipulator according to claim 1, characterized in that: The joint torque optimization is performed based on the fault-tolerant path planning strategy with the best joint torque, including: (1) Considering the weight matrix W, Z = τ T Minimizing Wτ is the optimization objective, and the inverse solution of joint acceleration is determined; (2) Take the weight matrix W = D -1 , and ignore the gravity term G, and redetermine the inverse solution of the joint acceleration; (3) Control during the calculation process The value of is used to determine the improved inverse solution of joint acceleration.

3. The method for suppressing sudden change of operating force of a reconfigurable manipulator according to claim 2, characterized in that: Considering the weight matrix W, Z = τ T The inverse solution of the joint acceleration obtained by minimizing Wτ as the optimization objective is: In the formula, A=DWD∈R n×n , I∈R n×n is the identity matrix, is the end acceleration, H is the mass matrix of the robot arm, G is the gravity vector, is the first-order derivative of the Jacobian matrix; Take the weight matrix W = D -1 , and ignoring the gravity term G, the inverse solution of the joint acceleration is: In the formula, Control during calculation The improved calculation method of the inverse solution of joint acceleration is as follows: In the formula, β is the pre-set Upper limit, when When the minimum joint moment method is used, When , the minimum norm method is used to calculate, J + is the pseudoinverse of the Jacobian matrix.

Images