A trajectory tracking control method for a cable-driven robotic arm based on a virtual reference input

Abstract

The present invention belongs to the technical field related to the motion control of robotic arms, and discloses a trajectory tracking control method for a cable-driven robotic arm based on a virtual reference input. The method includes the following steps: (1) Derive the inverse kinematic model and inverse dynamic model from the end to the motor based on the kinematic model and the mechanism dynamic model of the cable-driven robotic arm, so as to construct an end-motor command converter for converting the planned end command and the desired joint torque into the motor angle and load command; (2) Construct a T-S fuzzy model of the servo motor drive system to realize the decoupled characterization of the dynamic characteristics of the servo motor drive system; (3) Set a virtual reference input to transform the trajectory tracking problem into a system stabilization problem; (4) Use a non-parallel distributed compensation control mechanism to set a fuzzy state feedback controller and adjust the position, speed and torque of the system to track the given command. The present invention improves the response speed of the system.

Description

Technical Field

[0001] The present invention belongs to the technical field related to robot arm motion control, and more specifically, relates to a rope-driven robot arm trajectory tracking control method based on virtual reference input. Background Art

[0002] Compared with traditional industrial manipulators, rope-driven manipulators introduce a rope-driven joint movement operation mode. The joints of rope-driven manipulators are all passive universal joints, and the angle and speed of rotation are controlled by three cables. Each rope is pulled by a corresponding servo motor to adjust the length and tension. The motor-driven rope components are centrally installed at the base, which can greatly reduce the aspect ratio of the manipulator. As a result, the rope-driven manipulator is more flexible and has better end accessibility. It can perform operations in complex, narrow spaces and other extreme places, providing new implementation measures for maintenance operations in aerospace, nuclear industry and other fields.

[0003] For the trajectory tracking control of rope-driven manipulators, the commonly used measures currently include rope length control, rope tension control and rope length / tension hybrid control. Rope length control is easy to operate and has low hardware requirements. The rope length feedforward can be calculated through the kinematic model, and the feedback tracking error can be adjusted by using PID control law, sliding mode control law, neural network control law, etc., so as to ensure the terminal trajectory tracking capability. This method is difficult to control the torque of the joint and lacks the ability to deal with external uncertainty disturbances. Rope tension control derives the control law of rope force through the dynamic response of the mechanism, which can effectively reduce the error and vibration caused by structural flexibility, but has significant limitations in ensuring the terminal tracking accuracy of the system. Rope length / tension hybrid control can combine the advantages of rope length control and rope tension control, and provides a new idea for balancing tracking accuracy and motion smoothness. However, the current trajectory tracking control technology of rope-driven manipulators needs to merge the rope length command and the tension command into the position command of the motor, and then realize trajectory tracking through the cascade control of position, speed and torque. The separate control of the rope-driven joint motion process and the motor-driven rope motion process will limit the response speed of the system. The existence of internal control loops in the cascade control structure within the motor-driven rope link will limit the dynamic behavior and may lead to the deterioration of interference compensation. In addition, there is nonlinear coupling between the motor speed and current (torque), and the current control strategy is difficult to achieve linear decoupling in the entire domain, further limiting the overall performance of the system. How to achieve the linkage between the rope drive link and the motor drive link, as well as the parallel regulation of the position trajectory and torque command, still needs further research. Summary of the invention

[0004] In view of the above deficiencies or improvement requirements of the prior art, the present invention provides a trajectory tracking control method for a cable-driven robotic arm based on a virtual reference input. On the basis of the traditional motion planning based on the inverse kinematics / dynamics model, the method proposes a virtual reference input mechanism for the cable-driven robotic arm, realizing the linkage of the motor-driven cable module and the cable-driven joint module to overcome the limitations of the current trajectory tracking control strategy of the cable-driven robotic arm in terms of response speed. At the same time, a T-S fuzzy modeling method for the motor-driven cable module is proposed. On this basis, a fuzzy state feedback controller is designed to achieve parallel regulation of the position, rotational speed and torque of the system, taking into account both the trajectory tracking accuracy and the motion smoothness.

[0005] To achieve the above object, according to one aspect of the present invention, there is provided a trajectory tracking control method for a cable-driven robotic arm based on a virtual reference input, the method comprising the following steps:

[0006] (1) Derive the inverse kinematics model and inverse dynamics model from the end to the motor based on the kinematics model of the end-joint, joint-cable, cable-motor mapping layer and the mechanism dynamics model of the cable-driven robotic arm, so as to construct an end-motor command converter for converting the planned end command and the desired joint torque into the angle and load command of the motor;

[0007] (2) Construct a T-S fuzzy model of the servo motor drive system to achieve decoupled characterization of the dynamic characteristics of the servo motor drive system; wherein, the expression of the T-S fuzzy model is:

[0008]

[0009] In the formula, x z (t) = [i q (t) i d (t) ω(t) θ(t)] T represents the system state variable matrix, ν(t) = [V q V d T represents the system control variable matrix, ε z (t) = T l represents the system disturbance variable, A zi , B z and D z represent coefficient matrices with appropriate dimensions. At the same time, i d and i q are the d-axis and q-axis currents respectively, ω is the electrical angular velocity of the motor, θ is the electrical angle of the motor, V d and V q are the d-axis and q-axis voltages respectively, and T l is the load torque;​ denotes the weight of the i-th fuzzy rule; V d and V q are the d-q axis voltages; ξ, R s , J m , B f and λ m are the number of pole pairs of the motor, stator resistance, moment of inertia, viscous friction coefficient, and magnetic flux respectively; L is the stator inductance;

[0010] (3) Set the virtual reference input to transform the trajectory tracking problem into a system stabilization problem;

[0011] (4) Use the non-parallel distributed compensation control mechanism to set the fuzzy state feedback controller and adjust the position, speed, and torque of the system to track the given command.

[0012] Furthermore, set the virtual reference input vector x r (t) = [i qr (t) i dr (t) ω r (t) θ r (t)] T , θ r (t) is the position virtual reference input, ω r (t) is the speed virtual reference input, i dr (t) and i qr (t) are the d-axis and q-axis current virtual reference inputs respectively.

[0013] Furthermore, set the discrete-time optimal differential tracker, and use the fastest time synthesis function f han to plan the acceleration virtual reference input Θ and generate the position virtual reference input θ r (t), speed virtual reference input ω r (t) and acceleration virtual reference input Θ(t).

[0014] Furthermore, first, combine the motor angle setting value θ d given by the rope-motor conversion module to construct the fastest time synthesis function, and combine the fastest time synthesis function f han to derive the acceleration virtual reference input Θ(t); then, set the system sampling step T s , and obtain the virtual reference inputs of position and speed. The q-axis current virtual reference input i qr (t) is derived from the speed equation of the motor, and then the q-axis current virtual reference command is derived.

[0015] Furthermore, the q-axis current virtual reference command is:

[0016] i qr = (Jm Θ + B f ω r + ξT l ) / (1.5λ m ξ 2 )。

[0017] Furthermore, the expression of the fuzzy state feedback controller is:

[0018]

[0019] In the formula, K j and P j are control gain matrices with set dimensions; x(t) is the tracking error state.

[0020] Furthermore, the d-q axis voltage commands of the servo motor drive system are:

[0021]

[0022] In the formula, u d and u q are the d-q axis control signals generated by the fuzzy state feedback controller, that is, u(t) = [u q (t) u d (t)] T ; i qr = (J m Θ + B f ω r + ξT l ) / (1.5λ m ξ 2 )

[0023] In the formula, J m represents the moment of inertia of the motor, B f represents the viscous friction coefficient, ξ represents the number of pole pairs of the motor, and λ m represents the magnetic flux.

[0024] Furthermore, the constraint condition of the virtual control signal u(t) is:

[0025]

[0026] Furthermore, the virtual control signal u(t) is converted into the actual voltage command ν(t) of the servo motor drive system.

[0027] Generally speaking, compared with the prior art through the above technical solutions conceived by the present invention, the trajectory tracking control method for a cable-driven robotic arm based on a virtual reference input provided by the present invention mainly has the following beneficial effects:

[0028] 1. The present invention realizes the integrated control of the motor-driven rope movement process and the rope-driven joint movement process through the designed virtual reference input mechanism, improving the response speed of the system.

[0029] 2. This method proposes a fuzzy state feedback controller for the cable-driven manipulator, realizing the parallel regulation of position, rotational speed, and torque. Compared with the traditional cascade control strategy of position, rotational speed, and torque based on a PI controller, this method can coordinately regulate the pose and joint torque of the manipulator, with better tracking accuracy and disturbance rejection ability.

[0030] 3. This method proposes a method for characterizing the system dynamic characteristics of the electrode-driven rope module based on T-S fuzzy modeling, realizing the decoupling of motor speed and current, and smoothly approximating the nonlinear system through a series of linear subsystems, reducing the difficulty of controller design and performance analysis for the nonlinear coupling system. BRIEF DESCRIPTION OF THE DRAWINGS

[0031] Figure 1 is a schematic structural diagram of the cable-driven manipulator provided by an embodiment of the present invention;

[0032] Figure 2 is Figure 1 a schematic diagram of the joint structure of the cable-driven manipulator in

[0033] Figure 3 is a schematic diagram of the trajectory tracking control of the cable-driven manipulator based on virtual reference input provided by the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0034] In order to make the objectives, technical solutions, and advantages of the present invention clearer and more understandable, the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below can be combined with each other as long as they do not conflict with each other.

[0035] As a new type of special robot, the cable-driven manipulator has far more degrees of freedom than traditional industrial manipulators and has a higher length-to-diameter ratio. Therefore, it has stronger flexibility and flexibility, is suitable for complex, narrow spaces, and other extreme places, and has broad application prospects in important fields such as aerospace manufacturing, nuclear equipment maintenance, and medical intervention surgery. Such as Figure 1 and Figure 2As shown in the figure, the joints of the cable-driven robotic arm adopt passive universal joints, and the rotation angle and speed are controlled by three cables. The cables pass through the cable holes of each joint and are connected to the servo motor drive components inside the base. Therefore, the trajectory tracking process of the cable-driven robotic arm can be divided into two links: 1) The process of the motor driving the cable to move, where the length and tension of the cable are adjusted by the servo motor; 2) The process of the cable driving the key movement, where the joint is pulled by the cable to adjust the pose of the robotic arm to track the end trajectory. In order to ensure the response speed, accuracy, and stability of trajectory tracking, it is necessary to explore a suitable linkage mechanism between the motor driving link and the cable driving link; at the same time, it is necessary to design a suitable position and torque control law.

[0036] Please refer to Figure 3 , a trajectory tracking control method for a cable-driven robotic arm based on a virtual reference input provided by the present invention mainly includes the following steps:

[0037] Step 1, construct the kinematic models of the end-joint, joint-cable, and cable-motor mapping layers, and determine the mechanism dynamics model of the cable-driven robotic arm through the Lagrange equation.

[0038] The cable-driven robotic arm has a large number of moving parts and a complex configuration description. According to the kinematic differences between the components, the entire cable-driven robotic arm transmission system is divided into three levels: the task space, the joint space, and the drive space, and the motion interaction links are divided into three parts: end-joint, joint-cable, and cable-motor. In this embodiment, the D-H coordinate system is used to describe the kinematic mapping of each link one by one. At the same time, the dynamic equation of the cable-driven snake-like robotic arm is constructed with the help of the Lagrange equation, specifically:

[0039]

[0040] In the formula, H is the mechanism inertia matrix, C is the mechanism Coriolis matrix, G is the equivalent gravity, q is the robotic arm joint angle vector, f is the cable tension vector, and J t is the cable tension Jacobian matrix.

[0041] Step 2, based on the obtained kinematic model and dynamic model, deduce the inverse kinematic model and inverse dynamic model from the end to the motor to construct an end-motor command converter for converting the planned end command and the desired joint torque into the motor angle and load command.

[0042] Step 3, construct a T-S fuzzy model of the servo motor drive system to realize the decoupled characterization of the dynamic characteristics of the servo motor drive system.

[0043] In this embodiment, a surface-mounted permanent magnet synchronous motor is used as the direct drive source of the cable, and its nonlinear dynamics are expressed as:

[0044]

[0045] wherein, i d and i q are the d-q axis currents, V d and V q are the d-q axis voltages, ω is the electrical angular velocity of the motor, θ is the motor rotation angle, T l is the load torque, L is the stator inductance, ξ, R s , J m , B f and λ m are the number of pole pairs of the motor, stator resistance, moment of inertia, viscous friction coefficient, and magnetic flux, respectively.

[0046] In the dynamic characteristic model of the permanent magnet synchronous motor, there are state variable cross-coupling terms and nonlinear terms ωi d and ωi q . Therefore, in this embodiment, the T-S fuzzy modeling technology is introduced, with the speed ω as the antecedent variable of the fuzzy rule, to decouple and model the servo system. Through a series of equilibrium points (F i ) of the antecedent variable, the execution unit servo system is decoupled into a group of linear subsystems organized by fuzzy rules.

[0047] Model rule i: If ω is F i , then

[0048]

[0049] On this basis, a T-S fuzzy model of the servo motor drive system is constructed by integrating the mechanism model of the linear subsystem and the center defuzzification mechanism. The mathematical expression of the T-S fuzzy model is:

[0050]

[0051] wherein, x z (t) = [i q (t) i d (t) ω(t) θ(t)] T represents the system state variable matrix, ν(t) = [V q V d T represents the system control variable matrix, ε z (t) = T l represents the system disturbance variable, A zi , B z and D z represent coefficient matrices with appropriate dimensions. Meanwhile, i d and i q ​They are the d-axis and q-axis currents respectively, ω is the electrical angular velocity of the motor, θ is the electrical angle of the motor, and V d and V q are the d-axis and q-axis voltages respectively, and T l is the load torque.

[0052] In addition, h i (ω) represents the weight of the i-th fuzzy rule and has the following form:

[0053]

[0054] where m i (ν) is the membership degree of the antecedent variable for the i-th fuzzy rule, and m n (ω) is the membership degree of the antecedent variable for the n-th fuzzy rule.

[0055] Step 4: Set the virtual reference input to transform the trajectory tracking problem into a system stabilization problem.

[0056] Set the virtual reference input vector x r (t) = [i qr (t) i dr (t) ω r (t) θ r (t)] T , θ r (t) is the position virtual reference input, ω r (t) is the speed virtual reference input, and i dr (t) and i qr (t) are the d-axis and q-axis current virtual reference inputs respectively. In this embodiment, the specific settings of each virtual reference input are as follows:

[0057] Design a discrete-time optimal differential tracker, and use the fastest time synthesis function f han to plan the acceleration virtual reference input Θ and generate the position virtual reference input θ r (t), speed virtual reference input ω r (t) and acceleration virtual reference input Θ(t). The specific implementation steps are as follows:

[0058] First, combine the motor angle setting value θ d given by the rope-motor conversion module to construct the fastest time synthesis function as:

[0059] f han = fhan(θ r (k) - θ d , ω r (k), α, β)

[0060] = -r(a / d)fsg(a,d) - r sign(a)(1 - fsg(a,d))

[0061] where θ d represents the motor angle setting value generated by the "end - motor" command converter, α and β are the convergence speed coefficient and the filtering factor respectively, and the variables a and d are given by the following formula

[0062]

[0063] Meanwhile, fsg(a,d) and fsg(y,d) are expressed as

[0064]

[0065] In the formula, sign(·) represents the sign function.

[0066] Furthermore, combining with the fastest - time synthesis function f han the acceleration virtual reference input Θ(t) is derived as follows:

[0067]

[0068] Finally, setting the system sampling step T s , from the above, the virtual reference inputs of position and speed are:

[0069]

[0070] The virtual reference input of q - axis current i qr (t) is derived from the speed equation of the motor.

[0071] The new motor speed equation is:

[0072]

[0073] From the above simple derivation, the virtual reference command of q - axis current is:

[0074] i qr = (J m Θ + B f ω r + ξT l ) / (1.5λ m ξ 2 ).

[0075] Derive the speed equation of the permanent - magnet synchronous motor drive system, and combine the acceleration virtual reference input Θ and the torque command T generated by the end - motor command converter l , and set the virtual reference input of q - axis current i qr , the specific expression is:

[0076] iqr =(J m Θ + B f ω r + ξT l ) / (1.5λ m ξ 2 )

[0077] Wherein, J m represents the moment of inertia of the motor, B f represents the viscous friction coefficient, ξ represents the number of pole pairs of the motor, and λ m represents the magnetic flux.

[0078] Set the d-axis current reference input i dr (t) to 0 to reduce the reluctance effect and torque ripple.

[0079] Step 5: Use the non-parallel distributed compensation control mechanism to set the fuzzy state feedback controller and adjust the position, speed, and torque of the system to quickly track the given command.

[0080] The setting of the fuzzy state feedback controller includes the following steps:

[0081] First, combine the virtual reference command x r (t) and the state feedback x z (t) of the system to construct a permanent magnet synchronous motor error regulation system, and the following tracking error is given:

[0082] x e (t) = x z (t) - x r (t)

[0083] Thus, the following permanent magnet synchronous motor error regulation system is constructed:

[0084]

[0085] Then, design the virtual control variable u(t), which satisfies the following conditions:

[0086]

[0087] In this way, the servo motor error regulation system is transformed into:

[0088]

[0089] It can be seen that under the adjustment of the fuzzy state feedback controller, once the state feedback x z (t) of the system converges to the virtual reference input x r (t), the system can quickly track the given position command while meeting the input torque requirement.

[0090] Using a non-parallel distributed compensation control mechanism, a fuzzy state feedback controller is designed to generate the above control signal u(t) and adjust the tracking error state x(t) to converge to 0. Then, the servo motor system can quickly track the given position command while meeting the input torque requirement. The specific expression of the fuzzy state feedback controller designed in the present invention is as follows:

[0091] In the formula, K j and P j are control gain matrices with appropriate dimensions.

[0092] Finally, the virtual control signal u(t) is converted into the actual voltage command ν(t) of the servo motor drive system and applied to the actual system. The specific expression is:

[0093]

[0094] For this purpose, first, the first derivative of the virtual reference input is solved

[0095] In this embodiment, since i dr = 0, so and are both generated by the tracking differentiator.

[0096] For i qr = (J m Θ + B f ω r + ξT l ) / (1.5λ m ξ 2 ), ignoring the first derivatives of the acceleration input and the load input, the first derivative of the virtual reference input of the q-axis current is:

[0097]

[0098] In summary,

[0099] Then, the constraint condition of the virtual control signal u(t) is clarified as:

[0100]

[0101] Combined with the first derivative of the virtual reference input, the above formula can be rewritten as:

[0102]

[0103] From the above formula, the d-q axis voltage commands of the servo motor drive system are:

[0104]

[0105] In the formula, u d and u q is the dq axis control signal generated by the fuzzy state feedback controller, that is, u(t) = [u q (t)u d (t)] T Through the above steps, we finally get the trajectory tracking control scheme for the rope-driven manipulator, realize the linkage between the rope-driven motion link and the motor drive link, and improve the response speed of the system; at the same time, the constructed position-speed-current single-loop control strategy can achieve the purpose of taking into account both the end trajectory tracking accuracy and motion smoothness.

[0106] It will be easily understood by those skilled in the art that the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions and improvements made within the spirit and principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A trajectory tracking control method for a cable-driven robotic arm based on a virtual reference input, characterized in that, The method includes the following steps: (1) Based on the kinematic models of the end-effector-joint, joint-cable, and cable-motor mapping layers and the mechanism dynamics model of the cable-driven robotic arm, derive the inverse kinematic model and inverse dynamics model from the end-effector to the motor to construct an end-effector-motor command converter for converting the planned end-effector commands and the desired joint torques into the motor angles and load commands; (2) Construct a T-S fuzzy model of the servo motor drive system to achieve the decoupled characterization of the dynamic characteristics of the servo motor drive system; where the expression of the T-S fuzzy model is: wherein x z (t) = [i q (t) i d (t) ω(t) θ(t)] T represents the system state variable matrix, ν(t) = [V q V d T represents the system control variable matrix, ε z (t) = T l represents the system disturbance variable, A zi , B z and D z represent coefficient matrices with appropriate dimensions. Meanwhile, i d and i q are the d-axis and q-axis currents respectively, ω is the electrical angular velocity of the motor, θ is the electrical angle of the motor, V d and V q are the d-axis and q-axis voltages respectively, T l is the load torque; h i (ω) represents the weight of the i-th fuzzy rule; V d and V q are the d-q axis voltages; ξ, R s , J m , B f and λ m are the number of pole pairs of the motor, stator resistance, moment of inertia, viscous friction coefficient and magnetic flux respectively; L is the stator inductance;​ (3) Set a virtual reference input to transform the trajectory tracking problem into a system stabilization problem; (4) Use a non-parallel distributed compensation control mechanism to set a fuzzy state feedback controller and adjust the position, speed, and torque of the system to track the given commands.

2. The trajectory tracking control method of the cable-driven robotic arm based on virtual reference input according to claim 1, characterized in that: Set the virtual reference input vector x r (t) = [i qr (t) i dr (t) ω r (t) θ r (t)] T , θ r (t) is the position virtual reference input, ω r (t) is the speed virtual reference input, i dr (t) and i qr (t) are the d-axis and q-axis current virtual reference inputs respectively.

3. The trajectory tracking control method of the cable-driven robotic arm based on virtual reference input according to claim 2, characterized in that: Set a discrete-time optimal differential tracker and utilize the fastest-time synthesis function f han Plan the acceleration virtual reference input Θ and generate the position virtual reference input θ r (t), the velocity virtual reference input ω r (t) and the acceleration virtual reference input Θ(t).

4. The trajectory tracking control method of the cable-driven robotic arm based on virtual reference input according to claim 3, wherein: First, combine the motor angle setting value θ given by the rope-motor conversion module d , construct the fastest time synthesis function, and combine the fastest time synthesis function f han to derive the acceleration virtual reference input Θ(t); then, set the system sampling step T s , obtain the virtual reference inputs of position and velocity, and the virtual reference input i qr (t) is derived from the speed equation of the motor, and then the virtual reference command of the q-axis current is derived.

5. The trajectory tracking control method of the cable-driven robotic arm based on virtual reference input according to claim 4, characterized in that: The virtual reference command of the q-axis current is: i qr = (J m Θ + B f ω r + ξT l ) / (1.5λ m ξ 2 )。 6. The trajectory tracking control method of the cable-driven robotic arm based on virtual reference input according to claim 1, characterized in that: The expression of the fuzzy state feedback controller is: where K j and P j are control gain matrices with set dimensions; x(t) is the tracking error state.

7. The trajectory tracking control method of the cable-driven robotic arm based on virtual reference input according to claim 4, characterized in that: The d-q axis voltage commands of the servo motor drive system are: where, u d and u q are the d-q axis control signals generated by the fuzzy state feedback controller, i.e., u(t) = [u q (t) u d (t)] T ; i qr = (J m Θ + B f ω r + ξT l ) / (1.5λ m ξ 2 ) In the formula, J m represents the moment of inertia of the motor, B f represents the viscous friction coefficient, ξ represents the number of pole pairs of the motor, and λ m represents the magnetic flux.

8. The trajectory tracking control method of the cable-driven robotic arm based on virtual reference input according to claim 7, characterized in that: The constraint condition of the virtual control signal u(t) is:

9. The trajectory tracking control method of the cable-driven robotic arm based on virtual reference input according to claim 7, characterized in that: Convert the virtual control signal u(t) into the actual voltage command ν(t) of the servo motor drive system,

Images