High-precision double-closed-loop robust control method for rope-driven super-redundancy mechanical arm

Abstract

The invention relates to the technical field of robot control, in particular to a high-precision double-closed-loop robust control method for a rope-driven super-redundancy mechanical arm, which is characterized in that dynamics modeling is performed on the rope-driven super-redundancy mechanical arm based on an equivalent moment method, control is divided into a joint space and a driving space, and a double-closed-loop controller is designed for the two spaces. In a joint space, a kinetic parameter adaptive estimation algorithm is designed, so that kinetic parameters converge to correct values within finite time, the accuracy of a dynamic feed-forward quantity is improved, and in a driving space, feed-forward compensation is performed on nonlinear dynamics of a motor, a sliding block, a guide rail and a rope in a driving module based on time delay estimation, so that the dynamic performance of the motor is improved. And sliding mode control is used as a feedback control law, the robustness of nonlinear friction and external disturbance of the rope is improved, and accurate and stable control over the rope-driven super-redundancy mechanical arm can be achieved.

Description

Technical Field

[0001] The invention relates to the field of robot control technology, and specifically relates to a high-precision dual-closed-loop robust control method for a rope-driven super-redundant robot arm. Background Art

[0002] Redundant robots refer to robots whose degrees of freedom far exceed the minimum degrees of freedom required for the robot to complete the task. Usually, the degrees of freedom (DOF) of such robots are at least 6, while the degrees of freedom of super-redundant robots can reach 10 or more. For a target end posture, its joint configuration can have infinite sets of inverse solutions. Therefore, super-redundant robots have environmental adaptability and the ability to pass through narrow spaces and avoid obstacles that traditional robots do not have, and can perform tasks flexibly in complex environments. The rope-driven super-redundant manipulator can place the drive motor at the rear, so that the manipulator has a high degree of freedom while achieving lightweight arm.

[0003] The driving ropes of the rope-driven super-redundant manipulator are distributed in parallel. The rope length is affected by the motion of all joints, and its motion mapping is a complex nonlinear function. In addition, the rope can only bear tension but not pressure, and requires ropes greater than the number of degrees of freedom of the mechanism to drive it, that is, a two-degree-of-freedom joint requires three ropes to drive it. Therefore, the rope-driven super-redundant manipulator has redundant degrees of freedom and rope redundant drive characteristics. The rope is driven by a motor in conjunction with a lead screw, a slider and a guide rail, and nonlinear factors such as the slider inertia force and the guide rail friction force must also be considered in the control system. In summary, the overall dynamics model of the rope-driven super-redundant manipulator is complex, the dynamic parameters are difficult to accurately identify, and the motion control is highly coupled. The equivalent torque method based on the principle of virtual work can equate the rope tension to the torque of the universal joint shaft. As a passive joint, the universal joint shaft actually has no active torque, so the torque is the virtual torque of the equivalent rope tension. In this way, the entire dynamic model is divided into two spaces, one is the joint equivalent torque space (hereinafter referred to as the joint space), and the other is the drive space (i.e., the motor, slider, guide rail, and rope part). Different controllers are designed according to the motion characteristics of the two spaces, which can realize dual closed-loop control, reduce the coupling of control, and improve control accuracy and stability.

[0004] The rope-driven super-redundant manipulator can provide more flexibility and operation options due to its high degree of freedom, but the motion coupling and drive redundancy caused by the rope, as well as the complex driving method of the rope make the overall dynamic model of the manipulator very complicated.

[0005] The currently commonly used control method based on dynamic feedforward and PD feedback requires accurate modeling of the robot dynamics. The rope-driven super-redundant robot has drive redundancy characteristics, and the number of ropes is greater than the degree of freedom of the mechanism, which makes the robot's inverse dynamics have multiple solutions. In addition, the rope has elastic deformation, and there is nonlinear friction between the rope and the rope hole. The dynamic parameter identification of the rope-driven super-redundant robot is much more difficult than that of the traditional robot. It is often difficult to obtain accurate dynamic parameters, which makes the dynamic feedforward in the control algorithm become a disturbance, reducing the control accuracy and stability.

[0006] In addition, in the rope-driven super-redundant manipulator, the rope is the medium for transmitting force, and the driving source is still the motor. During the movement process, the dynamics of the motor, slider, guide rod, and rope are nonlinear, and in the current conventional control method, the force transmission relationship of the above-mentioned drive module is often simplified to a linear relationship, which reduces the control performance.

[0007] In practical applications, rope-driven super-redundant robotic arms are mainly used in confined and narrow spaces. They may be subject to external disturbances from the environment during their working process. Therefore, they also have high requirements for the anti-interference ability of the entire control system, which is difficult to meet with current conventional control algorithms. Summary of the invention

[0008] In view of the deficiencies in the prior art, an object of the present invention is to provide a high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator.

[0009] To achieve the above object, the present invention provides the following technical solution: a high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator, the steps of which are:

[0010] (1) Based on the kinematic model of the rope-driven super-redundant manipulator, the mapping between joint angle and rope length and between joint velocity and rope velocity is established;

[0011] (2) Dynamic modeling of the rope-driven super-redundant manipulator is carried out through the equivalent moment method, and the redundant driving problem of the rope is solved based on quadratic programming;

[0012] (3) The dynamic parameters are linearized in the joint space, and the dynamic parameter adaptive law is designed based on the linearized regression matrix. The closed-loop control of the joint equivalent torque space is realized by cooperating with the PD feedback control law based on the joint motion error.

[0013] (4) In the drive space, a time delay estimation algorithm is used to perform feed-forward compensation on the dynamics of the motor, slider, and rope, and a sliding mode feedback control law based on the rope motion error is used to achieve closed-loop control of the drive space;

[0014] (5) The control methods designed in step (3) and step (4) are combined to form a double closed-loop controller.

[0015] 2. A high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 1, characterized in that: according to step (1), there is a mapping relationship between the rope length and the joint angle: the rope length is obtained by summing the rope length at each joint and the rope length at each sleeve, and the rope length formula at each joint is as follows:

[0016] l i，j =|| b p u，i，j - b p d，i，j || (1)

[0017] Among them l i，j represents the length of the j-th rope at the i-th joint, b p u，i，j represents the coordinates of the intersection of the jth rope and the distal rope guide disc at the ith joint, b p d，i，j represents the coordinates of the intersection of the jth rope and the proximal rope guide disc at the ith joint. The superscript b indicates that the coordinates are expressed in the base coordinate system. The rope length formula is as follows:

[0018]

[0019] Among them l j represents the length of the jth rope, l j (0) is the initial length of the jth rope, Δl j is the change in rope length, n j represents the number of joints through which the jth rope passes, l tube Indicates the length of the rope at each sleeve;

[0020] There is a mapping relationship between the rope speed and the joint speed, and the formula is as follows:

[0021]

[0022] in is the rope speed, is the joint angular velocity, J l is the velocity Jacobian matrix of the rope velocity and the joint velocity.

[0023] 3. The high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 2 is characterized in that: according to step (2), the equivalent torque method is used to perform dynamic modeling on the manipulator: the rope tension is equivalent to the torque acting on the universal joint shaft, and the universal joint shaft, as a passive joint, actually has no active torque, so the torque is a virtual torque equivalent to the rope tension, and the principle of virtual work can be obtained:

[0024] δW=τ T δq=f T δl (4)

[0025] Where δW is the virtual work after the mechanism undergoes virtual displacement, δq is the angular virtual displacement, δl is the rope length virtual displacement, τ is the equivalent joint torque, and f is the rope tension;

[0026] Since the rope length change rate and joint angular velocity exist When the rope hole constraint is an ideal constraint, the angular velocity virtual displacement δq and the rope length virtual displacement δ satisfy the relationship:

[0027] δl=J l δq (5)

[0028] Substituting formula (5) into formula (4), we can obtain:

[0029]

[0030] On this basis, the robot arm is set as a traditional motor-driven robot arm, that is, there is an equivalent driving torque at the universal joint, and the dynamic equation can be calculated using the Newton-Euler recursion formula as follows:

[0031]

[0032] where q=(q1,q2,q3,...,q 24 ) T is the joint position vector, is the joint velocity vector, is the joint acceleration vector, M(q) is the inertia matrix of the manipulator in the joint space, is the Coriolis force and centrifugal force matrix of the robot in the joint space, G(q) is the gravity vector of the robot in the joint space;

[0033] According to the dynamic equation of the manipulator, the equivalent torque value of the joint of the manipulator can be obtained, and the rope tension can be obtained through the mapping relationship of formula (6);

[0034] By using quadratic programming, taking the minimum rope tension as the optimization target and the rope tension limit value and the mapping relationship obtained by formula (6) as constraints, the optimal rope tension solution with the minimum power consumption can be obtained. The quadratic programming problem is expressed as follows:

[0035]

[0036] 4. The high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 3 is characterized in that: according to step (3), the dynamic model of the manipulator is parameterized linearly, as shown in the following formula:

[0037]

[0038] in is the dynamic parameter regression matrix related to joint parameters, φ r is an identifiable kinetic parameter;

[0039] The dynamic parameters will be corrected in real time according to the control error, introducing the following error signals:

[0040]

[0041] Where q d is the desired joint position, is the expected joint velocity, The definition is as follows:

[0042]

[0043] Where λ is a given positive constant. Substituting formula (11) into formula (10), we can obtain:

[0044]

[0045] Substituting formula (12) into formula (9) yields:

[0046]

[0047] The controller can be designed as:

[0048]

[0049] where τ r is the robust term:

[0050]

[0051] Where K1, K2 are given control gains, Y r is the kinetic parameter regression matrix, φ r is the estimated value of the kinetic parameter, K1e s is the error feedback term. Substituting equation (14) into equation (13), the closed-loop dynamics of the joint equivalent moment space can be obtained as follows:

[0052]

[0053] in is the error of the kinetic parameter, the kinetic parameter φ r The adaptive law is designed as follows:

[0054]

[0055] The output of the joint space controller is the equivalent torque. According to the above mapping method, the rope force f required to drive the manipulator is calculated based on the equivalent torque.

[0056] 5. A high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 4, characterized in that: according to step (4), the rope length and the motor rotation angle have the following relationship:

[0057] l=l0+Nθ (18)

[0058] Where l0 is the initial length of the rope, θ is the angle vector driven by the motor, and N is the mapping relationship between the motor angle and the displacement of the slider;

[0059] By taking the first and second derivatives of equation (18), we can obtain the rope speed and acceleration and the motor speed and

[0060] The mapping relationship of acceleration is:

[0061]

[0062] The rope force is transmitted by the motor torque through the slider and the guide rail. The dynamic equation considering the inertia force and friction force of the motor and slider is as follows:

[0063]

[0064] where τ m is the motor output torque, F f is the friction force of the guide rail, J mt is the inertia of the motor and slider, B mt are the damping coefficients of the motor and slider, D(t) is the disturbance vector of the system;

[0065] In the controller of the drive space, the constant diagonal positive definite matrix is ​​introduced As the feedback control gain:

[0066]

[0067] Where H(t) is a nonlinear term, which is used to compensate for nonlinear terms such as inertia, damping, friction, etc. in addition to the rope force in equation (21). H(t) can be expressed as:

[0068]

[0069] Since H(t) is too complex and difficult to obtain, the time delay estimation technology is used for estimation.

[0070]

[0071] Where L is the delay time, is the estimated value of H(t);

[0072] The delay estimation control law can be designed as follows:

[0073]

[0074] Where v is the feedback control law to be designed;

[0075] The tracking error of the motor is defined as:

[0076] e θ (t) = θ d (t)-θ(t) (26)

[0077] The sliding mode function is selected as:

[0078]

[0079] Where Λ is a positive diagonal matrix, is the first-order derivative of the motor tracking error.

[0080] By taking the derivative of formula (26), we can get:

[0081]

[0082] According to the above formula, the synovial feedback control law is designed as follows:

[0083]

[0084] where sgn(s) is a symbolic vector function with respect to s, D is a positive diagonal matrix, and η is a positive real parameter.

[0085] Substituting equation (29) into equation (25), the controller of the driving space can be obtained as:

[0086]

[0087] Where f is the desired tension of the rope, and the equivalent torque τ output by the joint space controller is calculated according to quadratic programming.

[0088] Compared with the prior art, the present invention has the following beneficial effects:

[0089] This application adopts the equivalent torque method to model the dynamics of the manipulator, divides the dynamic model into joint space and drive space, proposes the idea of ​​designing a double closed-loop controller for the two spaces, decomposes the rope force with transient coupling into equivalent joint torque, reduces the coupling of the control system, and improves stability;

[0090] The rope-driven super-redundant manipulator has a driving redundancy characteristic, which makes the inverse dynamics of the manipulator have multiple solutions. In addition, the rope has elastic deformation, and there is nonlinear friction between the rope and the rope hole, which makes it difficult to obtain accurate parameters for the dynamic parameter identification of the rope-driven super-redundant manipulator, resulting in the dynamic feedforward in the control algorithm becoming a disturbance, which reduces the control accuracy and stability. The present application designs a dynamic parameter adaptive algorithm in the joint space controller to improve the above problems and improve the control accuracy.

[0091] The rope force of the rope-driven super-redundant manipulator is transmitted through the motor, slider, guide rod, and rope module. During the movement, the dynamics of the drive module is nonlinear. In conventional control methods, the above-mentioned drive module dynamics model is often simplified to a linear relationship, which reduces the control performance. This application uses time delay estimation as feedforward in the drive space controller to compensate for nonlinear terms that are difficult to model, such as motor inertia, damping, and friction, other than the rope force, to improve the control accuracy and response speed of the system. Since the time delay estimation also has bounded errors, and the working environment of the rope-driven super-redundant manipulator is small and limited, and is subject to many external interferences, this application uses the sliding mode control algorithm as the feedback control law to improve the system's robustness to external interference and internal nonlinear errors.

[0092] Details of one or more embodiments of the present application are presented in the following drawings and descriptions to make other features, purposes and advantages of the present application more concise and easy to understand. The present application is fully described and understood through the embodiments of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0093] Figure 1 The basic structure of the redundant robot of the embodiment of the present invention;

[0094] Figure 2 Modeling the arm structure coordinate system of the redundant robot mechanical arm of the present invention;

[0095] Figure 3 It is the overall structure flow chart of the rope-driven spatial cooperative coupling super-redundant robot stiffness optimization sliding mode control system of the present invention;

[0096] Figure 4 It is the equivalent moment diagram of the redundant robot mechanical arm joints of the present invention;

[0097] Figure 5 It is the control framework of the present invention. DETAILED DESCRIPTION

[0098] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, 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.

[0099] In order to solve the problem of difficulty in dynamic modeling and poor control performance of rope-driven super-redundant manipulator, the application adopts the equivalent torque method to model the dynamics of the manipulator, and the rope tension is equivalent to the torque at the universal joint shaft. However, since the number of ropes is greater than the degree of freedom of the mechanism, there are multiple solutions for the inverse dynamics of the manipulator, that is, there are multiple solutions when the universal joint torque is mapped to the rope tension. The quadratic programming method is adopted to solve the problem with the minimum sum of rope tension as the optimization goal, so that the dynamic model is divided into two spaces, namely the joint space and the drive space, to achieve double closed-loop control, to reduce the coupling of motion control, and to improve the control accuracy and stability. In the joint space, the dynamic parameter adaptive algorithm is adopted to correct the dynamic parameters according to the control error, so that the parameters converge within a limited time, compensate for the error of the offline dynamic model parameters, and improve the control accuracy. In the drive space, the delay estimation control algorithm is adopted to feedforward compensation for this part in a model-free manner, and the sliding mode control algorithm is used as the feedback control law, the synovial variable is introduced, and the control parameters are dynamically adjusted to improve the robustness of the rope-driven super-redundant manipulator to external disturbances and nonlinear friction of the rope. Based on the above reasons, the present application provides a high-precision dual closed-loop control system for a rope-driven super-redundant robotic arm.

[0100] See also Figure 1-5 The present invention provides a technical solution: a high-precision double closed-loop robust control method for a rope-driven super-redundant manipulator. For the convenience of demonstration, a redundant robot formed by a combination of four universal joints is used as a case demonstration. The steps are as follows:

[0101] (1) Based on the kinematic model of the rope-driven super-redundant manipulator, the mapping between joint angle and rope length and between joint velocity and rope velocity is established;

[0102] (2) Dynamic modeling of the rope-driven super-redundant manipulator is carried out through the equivalent moment method, and the redundant driving problem of the rope is solved based on quadratic programming;

[0103] (3) The dynamic parameters are linearized in the joint space, and the dynamic parameter adaptive law is designed based on the linearized regression matrix. The closed-loop control of the joint equivalent torque space is realized by cooperating with the PD feedback control law based on the joint motion error.

[0104] (4) In the drive space, a time delay estimation algorithm is used to perform feed-forward compensation on the dynamics of the motor, slider, and rope, and the closed-loop control of the drive space is achieved in conjunction with the sliding film feedback control law based on the rope motion error;

[0105] (5) The control methods designed in step (3) and step (4) are combined to form a double closed-loop controller.

[0106] In the above scheme, the structure of the controlled rope-driven redundant robot includes a base rope driving mechanism, a driving rope and a multi-section serial universal joint kinematic chain U pair controlled by the driving rope, such as Figure 2 As shown: the first section of the base, which is also fixed to the base, is connected to the arm sleeve of the next section of the robot arm through a U pair, and then the sleeve is connected to the U pair of the next section, and the two are nested in series to the end. Each U pair consists of a universal joint. Due to the structure of the multi-section universal joint, it is impossible to directly control the joint transmission. Therefore, it is driven and controlled by a guide rope disc. The guide rope disc is divided into a proximal guide rope disc and a distal guide rope disc. The proximal guide rope disc is fixed to the upper arm sleeve of the joint, and the distal guide rope disc is fixed to the next arm sleeve. Each joint is redundantly controlled by three ropes that are 120° apart.

[0107] like Figure 3 As shown, there is a mapping relationship between the rope length and the joint angle. The rope length is obtained by summing the rope length at each joint and the rope length at each sleeve. The rope length formula at each joint is as follows:

[0108] l i，j =|| b p u，i，j - b p d，i，j || (1)

[0109] Among them l i，j represents the length of the j-th rope at the i-th joint, b p u，i，j represents the coordinates of the intersection of the jth rope and the distal rope guide disc at the ith joint, b p d,i，j represents the coordinates of the intersection of the jth rope and the proximal guide drum at the ith joint, and the superscript b indicates that the coordinates are expressed in the base coordinate system. The rope length formula is as follows:

[0110]

[0111] Among them l j represents the length of the jth rope, l j (0) is the initial length of the jth rope, Δl j is the change in rope length, n j represents the number of joints through which the jth rope passes, l tubeIndicates the length of the rope at each sleeve.

[0112] There is a mapping relationship between the rope speed and the joint speed, and the formula is as follows:

[0113]

[0114] in is the rope speed, is the joint angular velocity, J l is the velocity Jacobian matrix of the rope velocity and the joint velocity.

[0115] In order to simplify the dynamic modeling and reduce the control coupling, the equivalent torque method is used to model the dynamics of the manipulator. The rope tension is equivalent to the torque acting on the universal joint shaft. The universal joint shaft, as a passive joint, actually has no active torque. Therefore, this torque is a virtual torque equivalent to the rope tension, such as Figure 4 As shown. According to the principle of virtual work:

[0116] δW=τ T δq=f T δl (4)

[0117] Where δW is the virtual work after the mechanism undergoes virtual displacement, δq is the angular virtual displacement, δl is the rope length virtual displacement, τ is the equivalent joint torque, and f is the rope tension.

[0118] Since the rope length change rate and joint angular velocity exist When the rope hole constraint is an ideal constraint, the angular velocity virtual displacement δq and the rope length virtual displacement δl satisfy the relationship:

[0119] δl=J l δq (5)

[0120] Substituting formula (5) into formula (4), we can obtain:

[0121]

[0122] On this basis, the robot arm can be imagined as a traditional motor-driven robot arm, that is, there is an equivalent driving torque at the universal joint. The dynamic equation can be calculated using the Newton-Euler recursion formula as follows:

[0123]

[0124] where q=(q1,q2,q3,...,q 24 ) T is the joint position vector, is the joint velocity vector, is the joint acceleration vector, M(q) is the inertia matrix of the manipulator in the joint space, is the Coriolis force and centrifugal force matrix of the robot in the joint space, and G(q) is the gravity vector of the robot in the joint space.

[0125] According to the dynamic equation of the robot arm, the joint equivalent torque value of the robot arm can be obtained, and the rope tension can be obtained through the mapping relationship of formula (6).

[0126] Each joint of the robot arm has two rotational degrees of freedom, but because the rope can only bear tension but not pressure, three ropes are needed to drive it, which is a driving redundancy. That is, in the mapping relationship of formula (6), there are multiple solutions for the rope tension after the equivalent torque is determined. For this constrained optimization problem, quadratic programming is used, with the rope tension and minimum as the optimization target, and the rope tension limit value and the mapping relationship obtained by formula (6) as the constraint condition, to obtain the optimal rope tension solution with the minimum power consumption. The quadratic programming problem is expressed as follows:

[0127]

[0128] Based on the above dynamic modeling, the dynamics of the manipulator can be divided into two spaces, namely the joint space and the drive space. Since the ropes in the rope-driven super-redundant manipulator are highly coupled with each joint, the movement of each rope will be affected by multiple joints, and the driving force of each rope will also affect multiple joints. Therefore, when the rope tension is directly controlled, there is transient coupling in the entire system. When the controllers are designed for the two split spaces separately, in the joint space, the coupled rope force is decomposed into the equivalent torque of each joint, which makes the transient coupling of the system smaller and the overall control performance more stable.

[0129] In the controller of the joint space, in order to improve the dynamic performance of the system control, a control method of dynamic feedforward combined with PD feedback is adopted. However, due to the complex dynamic model of the rope-driven super-redundant manipulator and the existence of drive redundancy characteristics, it is difficult to identify the dynamic parameters of the manipulator. Conventional dynamic feedforward may have large errors and disturbances. Therefore, this application linearizes the parameters of the dynamic model of the manipulator, as shown in the following formula:

[0130]

[0131] in is the dynamic parameter regression matrix related to joint parameters, φ r is an identifiable kinetic parameter;

[0132] The dynamic parameters will be corrected in real time according to the control error, introducing the following error signals:

[0133]

[0134] Where q dis the desired joint position, is the expected joint velocity, The definition is as follows:

[0135]

[0136] Where λ is a given positive constant. Substituting formula (11) into formula (10), we can obtain:

[0137]

[0138] Substituting formula (12) into formula (9) yields:

[0139]

[0140] The controller can be designed as:

[0141]

[0142] where τ r is the robust term:

[0143]

[0144] Where K1, K2 are given control gains, Y r is the kinetic parameter regression matrix, φ r is the estimated value of the kinetic parameter, K1e s is the error feedback term. Substituting equation (14) into equation (13), the closed-loop dynamics of the joint equivalent moment space can be obtained as follows:

[0145]

[0146] in is the error of the kinetic parameter, the kinetic parameter φ r The adaptive law is designed as follows:

[0147]

[0148] The output of the joint space controller is the equivalent torque. According to the above mapping method, the rope force f required to drive the manipulator is calculated based on the equivalent torque.

[0149] The rope force is provided by the motor, the lead screw, the slider and the guide rail, and the driving source is the motor torque. Therefore, when designing the drive space controller, nonlinear factors such as the slider inertia force and the guide rail friction force need to be taken into account in the control system. The rope length and the motor angle have the following relationship:

[0150] l=l0+Nθ (18)

[0151] Where l0 is the initial length of the rope, θ is the angle vector driven by the motor, and N is the mapping relationship between the motor angle and the slider displacement.

[0152] By taking the first-order and second-order derivatives of equation (19), we can obtain the mapping relationship between rope speed and acceleration and motor speed and acceleration:

[0153]

[0154] The rope force is transmitted by the motor torque through the slider and the guide rail. The dynamic equation considering the inertia force and friction force of the motor and slider is as follows:

[0155]

[0156] where τ m is the motor output torque, F f is the friction force of the guide rail, J mt is the inertia of the motor and slider, B mt are the damping coefficients of the motor and slider, and D(t) is the disturbance vector of the system.

[0157] In the controller of the drive space, the constant diagonal positive definite matrix is ​​introduced As the feedback control gain:

[0158]

[0159] Where H(t) is a nonlinear term, which is used to compensate for nonlinear terms such as inertia, damping, friction, etc. in addition to the rope force in equation (21). H(t) can be expressed as:

[0160]

[0161] Since H(t) is too complex and difficult to obtain, the time delay estimation technology is used for estimation.

[0162]

[0163] Where L is the delay time, is the estimated value of H(t).

[0164] The delay estimation control law can be designed as follows:

[0165]

[0166] Where v is the feedback control law to be designed.

[0167] Since the time delay estimation also has bounded errors, and the rope-driven super-redundant manipulator has a narrow and limited working environment and is subject to many external disturbances, the sliding mode control method is introduced as the feedback control law of the drive space controller. Sliding mode control is a classic nonlinear control system control strategy, which is essentially a variable structure control. This control method has the characteristics of excellent robustness and fast response, and can effectively combat model uncertainty and external disturbances.

[0168] The tracking error of the motor is defined as:

[0169] e θ (t) = θ d (t)-θ(t) (26)

[0170] The sliding mode function is selected as:

[0171]

[0172] Where Λ is a positive diagonal matrix, is the first-order derivative of the motor tracking error.

[0173] By taking the derivative of formula (26), we can get:

[0174]

[0175] According to the above formula, the synovial feedback control law is designed as follows:

[0176]

[0177] where sgn(s) is a symbolic vector function with respect to s, D is a positive diagonal matrix, and η is a positive real parameter.

[0178] Substituting equation (29) into equation (25), the controller of the driving space can be obtained as:

[0179]

[0180] Where f is the desired tension of the rope, and the equivalent torque τ output by the joint space controller is calculated according to quadratic programming.

[0181] The overall control process of the rope-driven super-redundant manipulator is as follows: Figure 5 shown.

[0182] Through this technical solution, the equivalent torque method is used to carry out dynamic modeling of the robotic arm, and the dynamic model is divided into joint space and drive space. The idea of ​​designing a double closed-loop controller for the two spaces is proposed, and the rope force with transient coupling is decomposed into equivalent joint torque, which reduces the coupling of the control system and improves stability.

[0183] The above embodiments only express several implementation methods of the present application, and the descriptions thereof are relatively specific and detailed, but they cannot be understood as limiting the scope of the invention patent. It should be pointed out that, for a person of ordinary skill in the art, several variations and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application shall be subject to the attached claims.

[0184] Although embodiments of the present invention have been shown and described, it will be appreciated by those skilled in the art that various changes, modifications, substitutions and variations may be made to the embodiments without departing from the principles and spirit of the present invention, and that the scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator, characterized in that: The steps are: (1) Based on the kinematic model of the rope-driven super-redundant manipulator, the mapping between joint angle and rope length and between joint velocity and rope velocity is established; (2) Dynamic modeling of the rope-driven super-redundant manipulator is carried out through the equivalent moment method, and the redundant driving problem of the rope is solved based on quadratic programming; (3) The dynamic parameters are linearized in the joint space, and the dynamic parameter adaptive law is designed based on the linearized regression matrix. The closed-loop control of the joint equivalent torque space is realized by cooperating with the PD feedback control law based on the joint motion error. (4) In the drive space, a time delay estimation algorithm is used to perform feed-forward compensation on the dynamics of the motor, slider, and rope, and a sliding mode feedback control law based on the rope motion error is used to achieve closed-loop control of the drive space; (5) The control methods designed in step (3) and step (4) are combined to form a double closed-loop controller.

2. The high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 1 is characterized in that: According to step (1), there is a mapping relationship between the rope length and the joint angle: the rope length is obtained by summing the rope length at each joint and the rope length at each sleeve. The rope length formula at each joint is as follows: l i,j =|| b p u,i,j - b p d,i,j || (1) Among them l i,j represents the length of the j-th rope at the i-th joint, b p u,i,j represents the coordinates of the intersection of the jth rope and the distal rope guide disc at the ith joint, b p d,i,j represents the coordinates of the intersection of the jth rope and the proximal rope guide disc at the ith joint. The superscript b indicates that the coordinates are expressed in the base coordinate system. The rope length formula is as follows: Among them l j represents the length of the jth rope, l j (0) is the initial length of the jth rope, Δl j is the change in rope length, n j represents the number of joints through which the jth rope passes, l tube Indicates the length of the rope at each sleeve; There is a mapping relationship between the rope speed and the joint speed, and the formula is as follows: in is the rope speed, is the joint angular velocity, J l is the velocity Jacobian matrix of the rope velocity and the joint velocity.

3. The high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 2, characterized in that: According to step (2), the equivalent torque method is used to model the dynamics of the robot arm: the rope tension is equivalent to the torque acting on the universal joint shaft. The universal joint shaft, as a passive joint, actually has no active torque. Therefore, this torque is a virtual torque equivalent to the rope tension. According to the principle of virtual work, it can be obtained: δW=τ T δq=f T δl (4) Where δW is the virtual work after the mechanism undergoes virtual displacement, δq is the angular virtual displacement, δl is the rope length virtual displacement, τ is the equivalent joint torque, and f is the rope tension; Since the rope length change rate and joint angular velocity exist When the rope hole constraint is an ideal constraint, the angular velocity virtual displacement δq and the rope length virtual displacement δl satisfy the relationship: δl=J l δq (5) Substituting formula (5) into formula (4), we can obtain: τ=J l T f (6) On this basis, the robot arm is set as a traditional motor-driven robot arm, that is, there is an equivalent driving torque at the universal joint, and the dynamic equation can be calculated using the Newton-Euler recursion formula as follows: where q=(q1,q2,q3,...,q 24 ) T is the joint position vector, is the joint velocity vector, is the joint acceleration vector, M(q) is the inertia matrix of the manipulator in the joint space, is the Coriolis force and centrifugal force matrix of the robot in the joint space, G(q) is the gravity vector of the robot in the joint space; According to the dynamic equation of the manipulator, the equivalent torque value of the joint of the manipulator can be obtained, and the rope tension can be obtained through the mapping relationship of formula (6); By using quadratic programming, taking the minimum rope tension as the optimization target and the rope tension limit value and the mapping relationship obtained by formula (6) as constraints, the optimal rope tension solution with the minimum power consumption can be obtained. The quadratic programming problem is expressed as follows:

4. The high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 3 is characterized in that: According to step (3), the dynamic model of the robot arm is linearized as shown in the following formula: in is the dynamic parameter regression matrix related to joint parameters, φ r is an identifiable kinetic parameter; The dynamic parameters will be corrected in real time according to the control error, introducing the following error signals: Where q d is the desired joint position, is the expected joint velocity, The definition is as follows: Where λ is a given positive constant. Substituting formula (11) into formula (10), we can obtain: Substituting formula (12) into formula (9) yields: The controller can be designed as: where τ r is the robust term: Where K1, K2 are given control gains, Y r is the kinetic parameter regression matrix, φ r is the estimated value of the kinetic parameter, K1e s is the error feedback term. Substituting equation (14) into equation (13), the closed-loop dynamics of the joint equivalent moment space can be obtained as follows: in is the error of the kinetic parameter, the kinetic parameter φ r The adaptive law is designed as follows: The output of the joint space controller is the equivalent torque. According to the above mapping method, the rope force f required to drive the manipulator is calculated based on the equivalent torque.

5. The high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 4, characterized in that: According to step (4), the rope length and the motor angle have the following relationship: l=l0+Nθ (18) Where l0 is the initial length of the rope, θ is the angle vector driven by the motor, and N is the mapping relationship between the motor angle and the displacement of the slider; By taking the first-order and second-order derivatives of equation (18), we can obtain the mapping relationship between rope speed and acceleration and motor speed and acceleration: The rope force is transmitted by the motor torque through the slider and the guide rail. The dynamic equation considering the inertia force and friction force of the motor and slider is as follows: where τ m is the motor output torque, F f is the friction force of the guide rail, J mt is the inertia of the motor and slider, B mt are the damping coefficients of the motor and slider, D(t) is the disturbance vector of the system; In the controller of the drive space, the constant diagonal positive definite matrix is ​​introduced As feedback control gain: Where H(t) is a nonlinear term, which is used to compensate for nonlinear terms such as inertia, damping, friction, etc. in addition to the rope force in equation (21). H(t) can be expressed as: Since H(t) is too complex and difficult to obtain, the time delay estimation technology is used for estimation. Where L is the delay time, is the estimated value of H(t); The delay estimation control law can be designed as follows: Where v is the feedback control law to be designed; The tracking error of the motor is defined as: e θ (t)=θ d (t)-θ(t) (26) The sliding mode function is selected as: Where Λ is a positive diagonal matrix, is the first-order derivative of the motor tracking error. By taking the derivative of formula (26), we can get: According to the above formula, the synovial feedback control law is designed as follows: where sgn(s) is a symbolic vector function with respect to s, D is a positive diagonal matrix, and η is a positive real parameter. Substituting equation (29) into equation (25), the controller of the driving space can be obtained as: Where f is the desired tension of the rope, and the equivalent torque τ output by the joint space controller is calculated according to quadratic programming.

Images