A high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator

Abstract

The present invention relates to the field of robotic control technology, and in particular to a high-precision dual-closed-loop robust control method for a rope-driven hyper-redundant manipulator. The method dynamically models the rope-driven hyper-redundant manipulator based on the equivalent moment method, divides the control into a joint space and a drive space, and designs dual-closed-loop controllers for both spaces. In the joint space, an adaptive dynamic parameter estimation algorithm is designed to ensure that the dynamic parameters converge to their correct values ​​within a finite time, improving the accuracy of the dynamic feedforward. In the drive space, feedforward compensation is performed on the nonlinear dynamics of the motor, slider, guide rail, and rope in the drive module based on time delay estimation. Sliding film control is used as the feedback control law to improve robustness to nonlinear rope friction and external disturbances. The present invention enables precise and stable control of the rope-driven hyper-redundant manipulator.

Description

Technical Field

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

[0002] A redundant robot is one with degrees of freedom far exceeding the minimum required to complete its task. Typically, such robots have at least six degrees of freedom (DOF), while hyper-redundant robots can have 10 or more. For a given target end-point pose, their joint configurations can have an infinite number of inverse solutions. Therefore, hyper-redundant robots possess environmental adaptability, the ability to navigate confined spaces, and avoid obstacles, unlike traditional robots, enabling them to flexibly perform tasks in complex environments. Tether-driven hyper-redundant manipulators can place the drive motors at the rear, achieving a lightweight arm while maintaining a high degree of freedom.

[0003] The drive cables of a rope-driven hyper-redundant manipulator are distributed in parallel. The cable length is affected by the motion of all joints, and the motion mapping is a complex nonlinear function. Furthermore, the cables can only bear tension, not compression. Therefore, a cable drive is required that exceeds the number of degrees of freedom of the mechanism. In other words, a joint with two degrees of freedom requires three cables. Therefore, a rope-driven hyper-redundant manipulator exhibits redundant degrees of freedom and redundant cable drive. The cables are driven by a motor in conjunction with a lead screw, slider, and guide rail. Nonlinear factors such as slider inertia and guide rail friction must also be factored into the control system. In summary, the overall dynamic model of a rope-driven hyper-redundant manipulator is complex, dynamic parameters are difficult to accurately identify, and 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. Since the universal joint shaft, as a passive joint, does not actually have active torque, this torque is a virtual torque equivalent to the rope tension. This method divides the entire dynamic model into two spaces: the joint equivalent torque space (hereafter referred to as the joint space) and the drive space (i.e., the motor, slider, guide rail, and rope). Designing different controllers based on the kinematic characteristics of these two spaces allows for dual closed-loop control, reducing control coupling and improving 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 complex.

[0005] The currently commonly used control method based on dynamic feedforward and PD feedback requires precise modeling of the manipulator dynamics. However, the rope-driven super-redundant manipulator has drive redundancy characteristics, and the number of ropes is greater than the degree of freedom of the mechanism, which makes the manipulator'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. This makes the dynamic parameter identification of the rope-driven super-redundant manipulator much more difficult than that of the traditional manipulator. 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] Furthermore, in a rope-driven hyper-redundant manipulator, the rope is the force transmission medium, while the drive source is still the motor. During motion, the dynamics of the motor, slider, guide rod, and rope are nonlinear. Current conventional control methods often simplify the force transmission relationships of these drive modules into linear ones, resulting in reduced 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 operation. 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 shortcomings of the prior art, the 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, comprising the following steps:

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

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

[0012] (3) Linearize the dynamic parameters in the joint space, design the dynamic parameter adaptive law based on the linearized regression matrix, and cooperate with the PD feedback control law based on the joint motion error to achieve closed-loop control of the joint equivalent torque space;

[0013] (4) In the driving space, a time delay estimation algorithm is used to perform feedforward 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 driving space.

[0014] (5) Based on the control methods designed in steps (3) and (4), a dual closed-loop controller is formed.

[0015] 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:

[0016] (1)

[0017] in Indicates the The rope in the The length of the joints, Indicates the Rope and The intersection coordinates of the distal guide rope discs at the joints, Indicates the Rope and The intersection coordinates of the proximal guide rope disc at the joint, superscript If the coordinates are expressed in the base coordinate system, the rope length formula is as follows:

[0018] (2)

[0019] in Indicates the The length of the rope, For the The initial length of the rope, is the change in rope length, Indicates the The number of joints through which the rope passes, Indicates the length of the rope at each sleeve;

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

[0021] (3)

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

[0023] According to step (2), 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. Since the universal joint shaft is a passive joint and does not actually have an active torque, the torque is a virtual torque equivalent to the rope tension. According to the principle of virtual work, it can be obtained:

[0024] (4)

[0025] In the formula is the virtual work after the mechanism undergoes virtual displacement, is the angular virtual displacement, is the virtual displacement of the rope length, is the equivalent joint torque, 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 and rope length virtual displacement Satisfaction relationship:

[0027] (5)

[0028] Substituting formula (5) into formula (4) yields:

[0029] (6)

[0030] On this basis, the robotic arm is set as a traditional motor-driven robotic 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:

[0031] (7)

[0032] in is the joint position vector, is the joint velocity vector, is the joint acceleration vector, is the inertia matrix of the manipulator in the joint space, is the Coriolis force and centrifugal force matrix of the manipulator in the joint space, is the gravity vector of the manipulator in the joint space;

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

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

[0035] (8)

[0036] According to step (3), the dynamic model of the robot arm is linearized as shown below:

[0037] (9)

[0038] in is the dynamic parameter regression matrix related to joint parameters, 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] (10)

[0041] In the formula is the desired joint position, is the desired joint velocity, The definition is as follows:

[0042] (11)

[0043] in is a given positive constant. Substituting formula (11) into formula (10) yields:

[0044] (12)

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

[0046] (13)

[0047] The controller can be designed as:

[0048] (14)

[0049] in is the robust term:

[0050] (15)

[0051] in is a given control gain, is the kinetic parameter regression matrix, are the estimated kinetic parameters, 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] (16)

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

[0054] (17)

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

[0056] According to step (4), the rope length and the motor angle have the following relationship:

[0057] (18)

[0058] in is the initial length of the rope, is the angle vector of the motor drive, is the mapping relationship between the motor angle and the slider displacement;

[0059] 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:

[0060] (19)

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

[0062] (20)

[0063] in is the motor output torque, is the guide rail friction, is the inertia of the motor and slider, are the damping coefficients of the motor and slider, is the disturbance vector of the system;

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

[0065] (twenty one)

[0066] in is a nonlinear term used to compensate for the nonlinear terms such as inertia, damping, friction, etc. in addition to the rope force in Eq. (21). It can be expressed as:

[0067] (twenty two)

[0068] because Too complex and difficult to obtain, the time delay estimation technology is used for estimation.

[0069] (twenty three)

[0070] (twenty four)

[0071] in is the delay time, for estimated value of;

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

[0073] (25)

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

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

[0076] (26)

[0077] The sliding mode function is selected as:

[0078] (27)

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

[0080] Taking the derivative of formula (27) we can get:

[0081] (28)

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

[0083] (29)

[0084] in For about The symbolic vector function of is a positive diagonal matrix, is a positive real parameter.

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

[0086] (30)

[0087] in 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 uses the equivalent torque method to model the dynamics of the robotic arm. The dynamic model is divided into the joint space and the drive space. The idea of ​​designing a dual closed-loop controller for the two spaces is proposed. The rope force with transient coupling is decomposed into equivalent joint torques, which reduces the coupling of the control system and improves the stability.

[0090] The rope-driven super-redundant manipulator has a driving redundancy characteristic, which leads to a multi-solution problem in the inverse dynamics of the manipulator. In addition, the rope has elastic deformation and nonlinear friction between the rope and the rope hole, making it difficult to obtain accurate parameters for the dynamic parameter identification of the rope-driven super-redundant manipulator. As a result, the dynamic feedforward in the control algorithm becomes a disturbance, reducing the control accuracy and stability. This 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 a rope-driven hyper-redundant manipulator is transmitted through a motor, slider, guide rod, and rope module. During motion, the dynamics of the drive module are nonlinear. Conventional control methods often simplify the dynamic model of this drive module into a linear relationship, which degrades control performance. This application uses time delay estimation as feedforward in the drive space controller to compensate for nonlinear terms other than the rope force, such as motor inertia, damping, and friction, which are difficult to model. This improves the system's control accuracy and response speed. Because time delay estimation also has bounded errors, and the rope-driven hyper-redundant manipulator operates in a confined and restricted environment, subject to significant external interference, this application uses a 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, and 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 This is a flow chart of the overall structure of the cable-driven spatially coordinated coupled super-redundant robot stiffness optimization sliding mode control system of the present invention;

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

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

[0098] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. 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 making creative efforts are within the scope of protection of the present invention.

[0099] This application addresses the difficulties in dynamic modeling and poor control performance of a rope-driven super-redundant manipulator. The equivalent torque method is used to model the manipulator's dynamics, equating the rope tension to the torque at the universal joint axis. However, because the number of ropes is greater than the degree of freedom of the mechanism, the inverse dynamics of the manipulator have multiple solutions—that is, multiple solutions exist when mapping the universal joint torque to the rope tension. A quadratic programming method is used to solve the problem, with the optimization objective of minimizing the sum of the rope tensions. This method divides the dynamic model into two spaces: the joint space and the drive space, achieving dual closed-loop control to reduce the coupling of motion control and improve control accuracy and stability. In the joint space, a dynamic parameter adaptive algorithm is used to modify the dynamic parameters based on the control error, ensuring that the parameters converge within a finite time, compensating for the errors in the offline dynamic model parameters and improving control accuracy. In the drive space, a time-delay estimation control algorithm is used to perform feedforward compensation in a model-free manner. A sliding mode control algorithm is used as the feedback control law, introducing a sliding film variable to dynamically adjust the control parameters, improving 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 dual-closed-loop robust control method for a rope-driven super-redundant manipulator. For ease of demonstration, a redundant robot formed by combining four universal joints is used as an example. The steps are as follows:

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

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

[0103] (3) Linearize the dynamic parameters in the joint space, design the dynamic parameter adaptive law based on the linearized regression matrix, and cooperate with the PD feedback control law based on the joint motion error to achieve closed-loop control of the joint equivalent torque space;

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

[0105] (5) Based on the control methods designed in steps (3) and (4), a dual closed-loop controller is formed.

[0106] In the above scheme, the structure of the controlled rope-driven redundant robot includes a base rope drive 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 base section, which is also fixed to the base, is connected to the next arm sleeve via a U-joint, which in turn connects to the next U-joint, and so on, nested in series to the end. Each U-joint consists of a universal joint. Due to the structure of multi-section universal joints, direct joint transmission control is impossible. Instead, it is driven and controlled through 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 arm sleeve above the joint, and the distal guide rope disc is fixed to the next arm sleeve. Each joint is controlled by three ropes with 120° redundant control.

[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 formula for the rope length at each joint is as follows:

[0108] (1)

[0109] in Indicates the The rope in the The length of the joints, Indicates the Rope and The intersection coordinates of the distal guide rope discs at the joints, Indicates the Rope and The intersection coordinates of the proximal guide rope disc at the joint, superscript The coordinates are expressed in the base coordinate system. The rope length formula is as follows:

[0110] (2)

[0111] in Indicates the The length of the rope, For the The initial length of the rope, is the change in rope length, Indicates the The number of joints through which the rope passes, Indicates the length of the rope at each sleeve.

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

[0113] (3)

[0114] in is the rope speed, is the joint angular velocity, 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. As the universal joint shaft is a passive joint, it actually has no active torque. Therefore, this torque is a virtual torque equivalent to the rope tension, as shown in the following example: Figure 4 As shown. According to the principle of virtual work, we can get:

[0116] (4)

[0117] In the formula is the virtual work after the mechanism undergoes virtual displacement, is the angular virtual displacement, is the virtual displacement of the rope length, is the equivalent joint torque, 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 and rope length virtual displacement Satisfaction relationship:

[0119] (5)

[0120] Substituting formula (5) into formula (4) yields:

[0121] (6)

[0122] On this basis, the robotic arm can be imagined as a traditional motor-driven robotic 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] (7)

[0124] in is the joint position vector, is the joint velocity vector, is the joint acceleration vector, is the inertia matrix of the manipulator in the joint space, is the Coriolis force and centrifugal force matrix of the manipulator in the joint space, is the gravity vector of the manipulator in joint space.

[0125] 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).

[0126] Each joint of the manipulator has two rotational degrees of freedom. However, since the rope can only bear tension but not pressure, three ropes are required to drive it, which is redundant. That is, in the mapping relationship of Equation (6), after determining the equivalent torque, there are multiple solutions for the rope tension. For this constrained optimization problem, quadratic programming is used. With the minimum rope tension as the optimization goal and the rope tension limit value and the mapping relationship obtained by Equation (6) as the constraint conditions, the optimal rope tension solution with the minimum power consumption can be obtained. The quadratic programming problem is expressed as follows:

[0127] (8)

[0128] Based on the above dynamic modeling, the dynamics of the manipulator can be divided into two spaces: the joint space and the drive space. Because the ropes in a rope-driven hyper-redundant manipulator are highly coupled to each joint, the motion of each rope is affected by multiple joints, and the driving force of each rope also affects multiple joints. Therefore, when directly controlling the rope tension, the entire system is subject to transient coupling. When controllers are designed for each of the two separate spaces, the coupled rope forces are decomposed into equivalent torques at each joint in the joint space, reducing the system's transient coupling and improving overall control performance.

[0129] In the joint space controller, in order to improve the dynamic performance of the system control, a control method combining dynamic feedforward and PD feedback is adopted. However, due to the complex dynamic model of the rope-driven super-redundant manipulator and the existence of drive redundancy, the dynamic parameters of the manipulator are difficult to identify. 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] (9)

[0131] in is the dynamic parameter regression matrix related to joint parameters, 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] (10)

[0134] In the formula is the desired joint position, is the desired joint velocity, The definition is as follows:

[0135] (11)

[0136] in is a given positive constant. Substituting formula (11) into formula (10) yields:

[0137] (12)

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

[0139] (13)

[0140] The controller can be designed as:

[0141] (14)

[0142] in is the robust term:

[0143] (15)

[0144] in is a given control gain, is the kinetic parameter regression matrix, are the estimated kinetic parameters, 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] (16)

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

[0147] (17)

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

[0149] The rope force is provided by the motor in conjunction with the lead screw, slider, and guide rail. 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 must also be taken into account in the control system. The relationship between the rope length and the motor angle is as follows:

[0150] (18)

[0151] in is the initial length of the rope, is the angle vector of the motor drive, 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] (19)

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

[0155] (20)

[0156] in is the motor output torque, is the guide rail friction, is the inertia of the motor and slider, are the damping coefficients of the motor and slider, is the disturbance vector of the system.

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

[0158] (twenty one)

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

[0160] (twenty two)

[0161] because Too complex and difficult to obtain, the time delay estimation technology is used for estimation.

[0162] (twenty three)

[0163] (twenty four)

[0164] in is the delay time, for estimated value.

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

[0166] (25)

[0167] in is the feedback control law to be designed.

[0168] Because time delay estimation also has bounded errors and the rope-driven super-redundant manipulator operates in a confined and restricted environment, subject to numerous external disturbances, a sliding mode control method is introduced as the feedback control law for the drive space controller. Sliding mode control is a classic control strategy for nonlinear control systems, essentially a form of variable structure control. This control approach offers excellent robustness and fast response, effectively combating model uncertainty and external disturbances.

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

[0170] (26)

[0171] The sliding mode function is selected as:

[0172] (27)

[0173] in is a positive diagonal matrix, is the first-order derivative of the motor tracking error.

[0174] Taking the derivative of formula (27) we can get:

[0175] (28)

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

[0177] (29)

[0178] in For about The symbolic vector function of is a positive diagonal matrix, is a positive real parameter.

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

[0180] (30)

[0181] in is the desired tension of the rope, and the equivalent torque output by the joint space controller is Calculated according to quadratic programming.

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

[0183] Through this technical solution, the equivalent torque method is used to dynamically model the robotic arm, and the dynamic model is divided into joint space and drive space. The idea of ​​designing a dual 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.

[0184] The above embodiments merely represent several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present invention. It should be noted that a person skilled in the art could make various modifications and improvements without departing from the spirit of the present application, all of which fall within the scope of protection of the present application. Therefore, the scope of protection of the present patent application shall be determined by the appended claims.

[0185] While 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 these embodiments without departing from the principles and spirit of the invention, and that the scope of the 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 by: The steps are: (1) Based on the kinematic model of the rope-driven hyper-redundant manipulator, the mapping between joint angle and rope length, as well as between joint velocity and rope velocity, is established; (2) Dynamic modeling of a rope-driven super-redundant manipulator is performed using the equivalent moment method, and the redundant driving problem of the rope is solved based on quadratic programming; (3) Linearize the dynamic parameters in the joint space, design the dynamic parameter adaptive law based on the linearized regression matrix, and cooperate with the PD feedback control law based on the joint motion error to achieve closed-loop control of the joint equivalent torque space; (4) In the driving space, a time delay estimation algorithm is used to perform feedforward 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 driving space. (5) Based on the control methods designed in steps (3) and (4), a dual closed-loop controller is formed.

2. The 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. The rope length formula at each joint is as follows: (1) in Indicates the The rope in the The length of the joints, Indicates the Rope and The intersection coordinates of the distal guide rope discs at the joints, Indicates the Rope and The intersection coordinates of the proximal guide rope disc at the joint, superscript If the coordinates are expressed in the base coordinate system, the rope length formula is as follows: (2) in Indicates the The length of the rope, For the The initial length of the rope, is the change in rope length, Indicates the The number of joints through which the rope passes, Indicates the length of the rope at each sleeve; There is a mapping relationship between rope speed and joint speed, and the formula is as follows: (3) in is the rope speed, is the joint angular velocity, 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 manipulator: the rope tension is equivalent to the torque acting on the universal joint shaft. Since the universal joint shaft is a passive joint and does not actually have an active torque, the torque is a virtual torque equivalent to the rope tension. According to the principle of virtual work, it can be obtained: (4) In the formula is the virtual work after the mechanism undergoes virtual displacement, is the angular virtual displacement, is the virtual displacement of the rope length, is the equivalent joint torque, 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 and rope length virtual displacement Satisfaction relationship: (5) Substituting formula (5) into formula (4) yields: (6) On this basis, the robotic arm is set as a traditional motor-driven robotic 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: (7) in is the joint position vector, is the joint velocity vector, is the joint acceleration vector, is the inertia matrix of the manipulator in the joint space, is the Coriolis force and centrifugal force matrix of the manipulator in the joint space, is the gravity vector of the manipulator in the joint space; According to the dynamic equation of the manipulator, the equivalent torque value of the manipulator's joint can be obtained, and the rope tension can be obtained through the mapping relationship of formula (6); By using quadratic programming, with the minimum rope tension as the optimization goal and the rope tension limit value and the mapping relationship obtained by formula (6) as constraints, the optimal rope tension solution with minimum power consumption can be obtained. The quadratic programming problem is expressed as follows: (8)。 4. The high-precision dual closed-loop robust control method for a rope-driven super-redundant manipulator according to claim 3, characterized in that: According to step (3), the dynamic model of the robot arm is linearized as shown below: (9) in is the dynamic parameter regression matrix related to joint parameters, is an identifiable kinetic parameter; The dynamic parameters will be corrected in real time according to the control error, introducing the following error signals: (10) In the formula is the desired joint position, is the desired joint velocity, The definition is as follows: (11) in is a given positive constant. Substituting formula (11) into formula (10) yields: (12) Substituting formula (12) into formula (9) yields: (13) The controller can be designed as: (14) in is the robust term: (15) in is a given control gain, is the kinetic parameter regression matrix, are the estimated kinetic parameters, 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: (16) in is the error of the kinetic parameters, The adaptive law is designed as follows: (17) The output of the joint space controller is the equivalent torque. According to the above mapping method, the rope force 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: (18) in is the initial length of the rope, is the angle vector of the motor drive, is the mapping relationship between the motor angle and the slider displacement; 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: (19) The rope force is transmitted by the motor torque through the slider and guide rail. The dynamic equation considering the inertia force and friction force of the motor and slider is as follows: (20) in is the motor output torque, is the guide rail friction, is the inertia of the motor and slider, are the damping coefficients of the motor and slider, is the disturbance vector of the system; In the controller of the driving space, the constant diagonal positive definite matrix is ​​introduced As the feedback control gain: (21) in is a nonlinear term used to compensate for the nonlinear terms of inertia, damping, and friction in addition to the rope force in Eq. (21). It can be expressed as: (22) because Too complex and difficult to obtain, the time delay estimation technology is used for estimation. (23) (24) in is the delay time, for estimated value of; The delay estimation control law can be designed as follows: (25) in is the feedback control law to be designed; The tracking error of the motor is defined as: (26) The sliding mode function is selected as: (27) in is a positive diagonal matrix, is the first-order derivative of the motor tracking error, Taking the derivative of formula (27) we can get: (28) According to the above formula, the synovial feedback control law is designed as follows: (29) in For about The symbolic vector function of is a positive diagonal matrix, is a positive real parameter, Substituting Equation (29) into Equation (25), the controller of the driving space can be obtained as: (30) in is the desired tension of the rope, and the equivalent torque output by the joint space controller is Calculated according to quadratic programming.

Images