A trajectory tracking control method and device for a 6-3-PUS parallel mechanism

Abstract

The present invention discloses a trajectory tracking control method and device for a 6-3-PUS parallel mechanism. The method comprises: establishing a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism; selecting state variables, transforming the dynamic model into a linear state space, and designing an LQR controller, wherein the input of the controller is a motion coordinate error and an error of a time differential of the motion coordinate, and the output is a force in the motion space; using the LQR controller for trajectory tracking control of the 6-3-PUS parallel mechanism, wherein the input of the tracking control is a desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the actual output joint coordinates q and joint velocities of the 6‑3‑PUS parallel mechanism #imgabs0# and obtains the actual posture T, the actual motion coordinates ρ and the actual motion coordinate time differential #imgabs1# through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint, and the 6‑3‑PUS parallel mechanism is controlled to perform motion, where ρ and #imgabs2# are used as feedback of the LQR controller.

Description

Technical Field

[0001] The present application relates to the technical fields of motion control and robotics, and in particular to a trajectory tracking control method and device for a 6-3-PUS parallel mechanism. Background Art

[0002] The 6-3-PUS parallel mechanism is a six-degree-of-freedom parallel robot, which consists of a static platform, a dynamic platform and six branches. Different from the common six-degree-of-freedom parallel mechanism, such as the Stewart platform, the branch drive of the 6-3-PUS parallel mechanism is installed on the static platform, which reduces the restriction on the movement of the branch chain, has a larger workspace and stronger dynamic performance, and has broad application prospects in production, processing and motion testing. The goal of trajectory tracking control is to change the driving torque so that the parallel mechanism moves according to the desired trajectory. The commonly used feedback control algorithm based on the kinematic model is simple in calculation, but its iterative error correction strategy leads to control lag. In addition, the actual position and posture of the parallel mechanism dynamic platform required for control feedback often requires a precise measurement system to obtain, which undoubtedly adds limitations to feedback motion control. Summary of the invention

[0003] The purpose of the embodiments of the present application is to provide a trajectory tracking control method and device for a 6-3-PUS parallel mechanism, so as to solve the technical problems existing in the related art that the control accuracy of the trajectory tracking of the 6-3-PUS parallel mechanism is insufficient and the control process is not limited by the posture measurement system.

[0004] According to a first aspect of an embodiment of the present application, a trajectory tracking control method of a 6-3-PUS parallel mechanism is provided, comprising:

[0005] Establish the kinematic model and dynamic model of 6-3-PUS parallel mechanism;

[0006] Selecting state variables, transforming the dynamic model into a linear state space, and designing an LQR controller, wherein the input of the controller is a motion coordinate error and an error of a time differential of the motion coordinate, and the output of the controller is a force in the motion space;

[0007] The LQR controller is used for trajectory tracking control of the 6-3-PUS parallel mechanism. The input of the tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint to control the 6-3-PUS parallel mechanism to perform motion, where ρ and Used as feedback for the LQR controller.

[0008] Furthermore, a kinematic model of the 6-3-PUS parallel mechanism is established, including:

[0009] Establish the inverse kinematics model of 6-3-PUS parallel mechanism;

[0010] According to the inverse kinematics model, the Newton iteration method is used to calculate the position T of the moving platform according to the input joint coordinates q;

[0011] Design an analytical method for the forward kinematics of the 6-3-PUS parallel mechanism and establish the time differential of the joint coordinates and the time differential of the motion coordinates analytic relationship.

[0012] Furthermore, a dynamic model of the 6-3-PUS parallel mechanism is established, including:

[0013] In the motion space of the 6-3-PUS parallel mechanism, an equivalent relationship between the motion coordinate ρ and its differential and the force F in the motion space is established, that is, the dynamic model of the 6-3-PUS parallel mechanism.

[0014] Furthermore, state variables are selected, the dynamic equation is transformed into a linear state space, and an LQR controller is designed, wherein the input of the controller is the motion coordinate error and the error of the time differential of the motion coordinate, and the output is the force in the motion space, including:

[0015] Take the motion coordinate ρ as the control variable and select the state variable Observe variable y=x, the state space equation of the LQR controller is:

[0016]

[0017] y=x

[0018] The input of the controller is e=y d -y, where y d Expected value y is the actual value; the output of the controller is the force F=u+G in the motion space ρ , which is related to the control rate u of the controller, by solving the problem of minimizing the linear quadratic performance index: Get the control rate u.

[0019] Furthermore, the LQR controller is used for trajectory tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint, and the 6-3-PUS parallel mechanism is controlled to perform motion, including:

[0020] Get the mapping relationship between the force F in the motion space and the driving torque τ of the joint τ = J T F, where J is the joint velocity and motion coordinates The analytic Jacobian matrix between ;

[0021] According to the actual coordinates q and actual speed of the joint output by the 6-3-PUS parallel mechanism By solving the constraint equations, the actual motion coordinates ρ are obtained, and by the analytical method of forward kinematics, the time differential of the actual motion coordinates is obtained. ρ and As control feedback, the error is calculated.

[0022] According to a second aspect of an embodiment of the present application, a trajectory tracking control device for a 6-3-PUS parallel mechanism is provided, comprising:

[0023] Modeling module, used to establish the kinematic model and dynamic model of the 6-3-PUS parallel mechanism;

[0024] Design a controller module for selecting state variables, transforming the dynamic model into a linear state space, and designing an LQR controller, wherein the input of the controller is a motion coordinate error and a motion coordinate time differential error, and the output is a force in the motion space;

[0025] Solving module, the LQR controller is used for trajectory tracking control of 6-3-PUS parallel mechanism, and the input of tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint to control the 6-3-PUS parallel mechanism to perform motion, where ρ and Used as feedback for the LQR controller.

[0026] According to a third aspect of an embodiment of the present application, there is provided an electronic device, including:

[0027] one or more processors;

[0028] A memory for storing one or more programs;

[0029] When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in the first aspect.

[0030] According to a fourth aspect of an embodiment of the present application, a computer-readable storage medium is provided, on which computer instructions are stored, and when the instructions are executed by a processor, the steps of the method described in the first aspect are implemented.

[0031] The technical solution provided by the embodiments of the present application may have the following beneficial effects:

[0032] It can be seen from the above embodiments that, since the present application has carried out a dynamic analysis of the 6-3-PUS parallel mechanism, the control has taken into account the dynamic torque conditions of the moving platform and the joints, thereby improving the control accuracy compared to the control method based on kinematic analysis; since the appropriate driving torque can be input in a timely manner, the energy consumption of the system is reduced; and since the LQR controller is used, the dependence on the control gain parameters is reduced.

[0033] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0035] Figure 1 is a simplified structural diagram of a 6-3-PUS parallel mechanism according to an exemplary embodiment.

[0036] Figure 2 It is a flow chart of a trajectory tracking control method of a 6-3-PUS parallel mechanism according to an exemplary embodiment.

[0037] Figure 3 It is a flow chart of a forward kinematics numerical solution of a 6-3-PUS parallel mechanism according to an exemplary embodiment.

[0038] Figure 4 is a control structure diagram of a 6-3-PUS parallel mechanism based on LQR control according to an exemplary embodiment.

[0039] Figure 5 It is a block diagram of a trajectory tracking control device of a 6-3-PUS parallel mechanism according to an exemplary embodiment. DETAILED DESCRIPTION

[0040] Exemplary embodiments will be described in detail herein, examples of which are shown in the accompanying drawings. When the following description refers to the drawings, the same numbers in different drawings represent the same or similar elements unless otherwise indicated. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application. Instead, they are merely examples of devices and methods consistent with some aspects of the present application as detailed in the appended claims.

[0041] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms of "a", "said" and "the" used in this application and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0042] The 6-3-PUS parallel mechanism is a six-degree-of-freedom parallel robot. The 6 and 3 in its name represent the number of connection points of the branch chain on the static platform and the dynamic platform, respectively. P, U, and S represent three types of kinematic pairs (elements that connect two mechanical components and constrain their relative motion): mobile pair, universal pair, and spherical pair. The connection order of the overall structure from bottom to top is: static platform-mobile pair P-universal pair U-spherical pair S-dynamic platform. Different from the common six-degree-of-freedom parallel mechanism, the PUS parallel mechanism realizes the movement of the dynamic platform by changing the position of the hinge between the branch chain and the static platform, while the length of the branch chain remains unchanged. This structural design makes it possible for the drive to no longer be installed on the branch chain, reducing the obstacles to the movement of the branch chain, improving the dynamic performance, and facilitating the increase of the working space; and the resistance that the drive needs to overcome is reduced, and the driving torque requirement of the motor is reduced, which is conducive to improving the effective utilization rate of the driving torque.

[0043] In order to make the subsequent method description clearer, the variables and symbols involved in the method description are explained as follows. Figure 1 is a simplified structural diagram of a 6-3-PUS parallel mechanism according to an exemplary embodiment. Figure 1 As shown in the figure, the 6-3-PUS parallel mechanism consists of a static platform (positive ΔA1A2A3), a dynamic platform (positive ΔP1P2P3) and three pairs of branch chains B 2k-1 P k , B 2k P k(k=1,2,3), the side lengths of ΔA1A2A3 and ΔP1P2P3 are LA and LP respectively, LP=LA / 2, and the length of the branch chain is a fixed value l. P}、{O B} are the coordinate systems on the moving platform and the static platform respectively, and the origin of the coordinate system is located at the center of their respective equilateral triangles. k is the upper hinge point between the branch chain and the moving platform. The upper hinge points of the two branch chains on one side of the static platform coincide with each other. k Relative moving platform coordinate system {O P} remains unchanged, B 2k-1 and B 2k It is the lower hinge point between the two branches on one side and the static platform. The lower hinge point can move along the edge of the static platform ΔA1A2A3. The midpoint of the three sides of ΔA1A2A3 is E k (k=1,2,3),E k To B 2k-1 and B 2k The distances are q 2k-1 and q 2k , which is the input variable for controlling the motion of the parallel mechanism and is also the joint space coordinate, q = [q1, q2, q3, q4, q5, q6] T φ i Indicates isosceles ΔB 2k-1 P k B 2k The angle between the plane where (k=1,2,3) is located and the plane where the static platform is located, φ=[φ1,φ2,φ3] T , φ is related to q. Define the coordinates of the motion space In actual control, q also needs to be converted into the control variable of the driver, that is, the input torque τ of the drive motor.

[0044] The goal of trajectory tracking control is to input the most appropriate driving variable so that the parallel mechanism can execute motion according to the predetermined trajectory. The common control method is based on the kinematic model, inputting the target displacement or speed for feedback control. However, due to the lack of observation of the dynamic driving torque, this method can only make the actual output approach the target value through feedback adjustment, so there is a significant control delay, and the control performance is very dependent on the gain parameter, which causes inconvenience to the system design. For nonlinear systems such as parallel mechanisms, more accurate and faster control also requires the establishment of a dynamic model, which can adjust the control input in time according to the system state, reduce the control response time, and improve the control accuracy.

[0045] Figure 2 is a flow chart of a trajectory tracking control method of a 6-3-PUS parallel mechanism according to an exemplary embodiment. Figure 2As shown, the method is applied to a 6-3-PUS parallel mechanism and may include the following steps:

[0046] Step S11: establishing a kinematic model and a dynamic model of a 6-3-PUS parallel mechanism;

[0047] Step S12: Select state variables, transform the dynamic model into a linear state space, and design an LQR controller, wherein the input of the controller is the motion coordinate error and the error of the time differential of the motion coordinate, and the output is the force in the motion space;

[0048] Step S13: Use the LQR controller for trajectory tracking control of the 6-3-PUS parallel mechanism. The input of the tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint to control the 6-3-PUS parallel mechanism to perform motion, where ρ and Used as feedback for the LQR controller.

[0049] It can be seen from the above embodiments that, since the present application has carried out a dynamic analysis of the 6-3-PUS parallel mechanism, the control has taken into account the dynamic torque conditions of the moving platform and the joints, thereby improving the control accuracy compared to the control method based on kinematic analysis; since the appropriate driving torque can be input in a timely manner, the energy consumption of the system is reduced; and since the LQR controller is used, the dependence on the control gain parameters is reduced.

[0050] In the specific implementation of step S11, a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism are established;

[0051] Specifically, by using the forward kinematics solution method (numerical method and analytical method), the joint coordinates and velocities that are easy to observe are converted into the posture and velocity of the moving platform. The actual posture and velocity of the moving platform can be obtained without the need for an accurate posture measurement system, overcoming the limitations of posture measurement. The process of establishing the kinematic model of the 6-3-PUS parallel mechanism can include:

[0052] Step S21: establishing an inverse kinematics model of the 6-3-PUS parallel mechanism;

[0053] Specifically, for a known moving platform posture, also known as motion space coordinates, used to describe the position and posture of the moving platform, it is defined as T = [t x ,t y ,tz ,α,β,γ] T . T in the static platform coordinate system {O B} can be represented by the translation vector h and the rotation matrix R, where the translation vector h is the translation amount t of the three axes x, y, and z. x ,t y ,t z It means: h = [t x ,t y ,t z ] T , the rotation matrix R is represented by the Euler angles α, β, γ around the three axes x, y, and z:

[0054]

[0055] like Figure 1 As shown, in the static platform coordinate system {O B}There is the following space vector relationship:

[0056] R i +hb i = l i (i=1,...,6) (1)

[0057] p i Upper hinge point In the coordinate system {O P}, b i The lower hinge point B i In the coordinate system {O B}, l i For the branch chain in the coordinate system {O B}. Since B i Move along the edge of ΔA1A2A3, so vector b i There are linear constraints on the x- and y-coordinates of the vector b. i It can be expressed as:

[0058] b i =[x i μ i x i +v i 0] T (2)

[0059] where x i The lower hinge point B i The x-coordinate of i and v i The lower hinge point B i The linear constraint coefficient between the x-coordinate and the y-coordinate, In the PUS parallel mechanism, the branch chain l iThe length remains unchanged at l, so we can substitute formula (2) into formula (1) and square both sides of the equal sign of formula (1) to obtain i A quadratic equation of one variable is obtained, and three equations are obtained. When the equation has real roots, the two real roots of each equation are valid, representing the x coordinates of the two lower hinge points on one side of the static platform. i Further, the joint coordinate variable q is obtained according to the geometric relationship of the equilateral triangle i The value of , from which the joint coordinate q is calculated. Furthermore, from the geometric relationship of the parallel mechanism, the geometric constraint equation r(q,φ)=0 of the joint coordinate input q and the angle φ can be obtained, which contains three equations. For a known q, solve φ, and obtain a unique solution that meets the actual state of the parallel mechanism, thereby obtaining the motion coordinate ρ, and further differentiate The inverse kinematics model is completed. A basic inverse kinematics model is established to clarify the calculation process from platform posture to motion coordinates, laying a theoretical foundation for subsequent forward kinematics analysis, dynamics analysis and trajectory tracking control.

[0060] Step S22: designing a numerical method for the forward kinematics of the 6-3-PUS parallel mechanism, and using the Newton iteration method according to the inverse kinematics model to calculate the position T of the moving platform according to the input joint coordinates q;

[0061] Specifically, the inverse kinematics process described in step S21 is defined as q i =g i (t x ,t y ,t z ,α,β,γ), known joint coordinate variables The constraint equation between the dynamic platform posture parameters and joint coordinate variables is:

[0062]

[0063] Let error f = [f1,f2,f3,f4,f5,f6] T , the goal of forward kinematics is to obtain the given joint coordinate variables q 0 , calculate the dynamic platform posture T = [t x ,t y ,t z ,α,β,γ] T , so that f = 0, the present invention adopts Newton iteration method to solve, the solution process is as follows Figure 3 As shown, it consists of the following steps:

[0064] S31: Select initial pose k represents the iteration cycle, and sets the minimum positive values ​​ε1 and ε2 to determine whether the iteration is terminated;

[0065] S32: Substitute into the pose constraint equation (3) and calculate the error f(T (k) );

[0066] S33: If ‖f(T (k) )‖<ε1, the iteration ends, T (k) This is the desired position of the moving platform. Otherwise, proceed to the next step.

[0067] S34: Calculate f with respect to posture T (k) The partial derivative of

[0068] S35: By J(T (k) )ΔT=-f(T (k) ) Calculate the posture correction value ΔT;

[0069] S36: If ‖ΔT‖<ε2, the iteration ends, T (k) That is the desired posture of the moving platform, otherwise, update the posture T (k+1) =T (k) +ΔT, update the number of iterations k=k+1, and return to step S32.

[0070] Through the above process, a suitable initial value is selected. In the absence of precise posture measurement instruments, the actual joint coordinate variables can be obtained according to the encoder feedback value of the drive motor, and then the actual posture of the moving platform can be obtained through the numerical method of forward kinematics. Solving forward kinematics through the Newton iteration method can easily and quickly obtain the actual posture, which is convenient for observing the motion state of the moving platform.

[0071] Step S23: Design an analytical method for the forward kinematics of the 6-3-PUS parallel mechanism and establish the time differential of the joint coordinates and the time differential of the motion coordinates analytic relationship.

[0072] Specifically, from the geometric relationship of the parallel mechanism, the geometric constraint equation r(q,φ)=0 of the joint coordinate input q and the angle φ can be obtained, which contains three equations. For a known q, solve φ, and obtain the only solution that meets the requirements according to the actual state of the parallel mechanism. For the motion constraint r(q,φ)=0, time differentiation is performed to obtain:

[0073]

[0074] Available for:

[0075]

[0076] Substituting formula (5) into Available

[0077]

[0078] That is, the time differential of the joint coordinates and the time differential of the motion coordinates The following analytical relationship exists:

[0079]

[0080] Where J is the analytic Jacobian matrix:

[0081]

[0082] This completes the description of the analytical method of forward kinematics. For the time differential of the joint coordinates The time differential of the motion coordinates can be obtained by formula (7): Through the above analytical method of forward kinematics, the transformation relationship between joint coordinates and motion coordinates in time differential can be geometrically determined, which is convenient for feedback calculation in motion control.

[0083] Specifically, the process of establishing the dynamic model of the 6-3-PUS parallel mechanism may include:

[0084] In the motion space of the 6-3-PUS parallel mechanism, an equivalent relationship between the motion coordinate ρ and its differential and the force F in the motion space is established, that is, the dynamic model of the 6-3-PUS parallel mechanism.

[0085] Specifically, the dynamic model of the 6-3-PUS parallel mechanism is mainly composed of the dynamic model of the moving joint and the dynamic model of the moving platform, which can be derived by the Lagrange equation. In the motion space, the overall dynamic equation of the 6-3-PUS parallel mechanism about the motion coordinate ρ is as follows:

[0086]

[0087] Among them, M ρ is the inertia matrix of the parallel mechanism as a whole, C ρ is the Coriolis force and centrifugal force matrix, G ρ is the gravity vector, and F is the force in the motion space. The dynamic model can be used to obtain the driving force required by the parallel mechanism during the motion process, so that a more appropriate torque can be input during the motion process, reducing control power consumption and reducing mechanical equipment damage.

[0088] In the specific implementation of step S12, state variables are selected, the dynamic model is transformed into a linear state space, and an LQR controller is designed, wherein the input of the controller is the motion coordinate error and the error of the time differential of the motion coordinate, and the output is the force in the motion space;

[0089] Specifically, taking the motion coordinate ρ as the control variable, in order to apply the LQR (linear quadratic regulator) control method, the dynamic equation of the 6-3-PUS parallel mechanism needs to be converted into a linear state space equation. Consider the following state space equation of the linear system:

[0090]

[0091] Select state variables The observed variable y=x, combined with formula (10), the state space equation is transformed into:

[0092]

[0093] By comparing formula (10) and (11), we can see that:

[0094]

[0095] In the trajectory tracking task, the control goal of the LQR controller is not to make the state variable x tend to zero, but to make the observation error e = y d -y tends to zero, where y d The expected variable Including the desired position and velocity of the moving platform. Therefore, the control goal is to find a control rate u that minimizes the linear quadratic performance index J. The calculation formula of J is:

[0096]

[0097] Among them, Q is the state constraint matrix, R is the control constraint matrix, both are diagonal matrices, and the values ​​of Q and R are selected with the optimization idea to achieve a balance between response speed and system energy consumption.

[0098] Control rate u=K c x+K g y d In, K c =-R -1 B T P, where P is the algebraic Riccati equation PA+A T P-PBR -1 B T P+C T The solution of QC=0, K g =R-1 B T (PBR -1 B T -A T )C T Q,

[0099] Transformed into the output of the controller, that is, the force in the motion space is:

[0100] F=u+G ρ =K c x+K g y d +G ρ (14)

[0101] The controller is established using the LQR principle and dynamic model, which not only effectively utilizes the advantages of dynamic analysis, but also reduces the constraint influence of control gain parameters, thereby improving the motion control accuracy of the 6-3-PUS parallel mechanism.

[0102] In the specific implementation of step S13, the LQR controller is used for trajectory tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint to control the 6-3-PUS parallel mechanism to perform motion, where ρ and Used as feedback for LQR controller;

[0103] Specifically, this step may include the following sub-steps:

[0104] Step S41: Obtain the mapping relationship between the force F in the motion space and the driving torque τ of the joint τ=J T F, where J is the joint velocity and motion coordinates The analytic Jacobian matrix between ;

[0105] Specifically, in actual control, the control torque of the 6-3-PUS parallel mechanism is not the force F in the motion space, but the control torque τ in the joint space. Therefore, it is necessary to realize force distribution and map the torque F in the motion space to each joint.

[0106] From the virtual work theorem, we can know that the virtual work δW done by all drivers and external forces is zero, that is:

[0107] δW=τ T δq-FT δρ=0 (15)

[0108] The virtual displacement δq can be equivalent to the differential of q with respect to time δρ can be equivalent to the differential of ρ with respect to time Therefore, the virtual displacement δq and δρ can be related by the analytical Jacobian matrix defined in formula (7): δρ = Jδq, which is substituted into formula (15) to obtain (τ T J T -F T )δρ=0, which is true for any virtual displacement δρ, so τ T J T -F T =0, we can get:

[0109] τ=J T F (16)

[0110] This step transforms the torque information from the motion space to the joint space, thereby controlling the parallel mechanism to perform effective motion.

[0111] Step S42: According to the actual coordinates q and actual speed of the joint output by the 6-3-PUS parallel mechanism By solving the constraint equations, the actual motion coordinates ρ are obtained, and by the analytical method of forward kinematics, the time differential of the actual motion coordinates is obtained. ρ and As control feedback, the error is calculated.

[0112] Specifically, the force F in the motion space can be converted into the driving torque τ of the joint according to the mapping relationship between the force F in the motion space and the driving torque τ of the joint. Figure 4 is a trajectory tracking control structure diagram of a 6-3-PUS parallel mechanism according to an exemplary embodiment, for the desired position T of the moving platform d , perform inverse kinematics solution to obtain the desired motion coordinates ρ d and its time derivative The LQR controller described in step S13 is designed. The controller input is the motion coordinate error and the error of the time differential of the motion coordinate. The controller output is the force F in the motion space, that is, the force on the moving platform. Through the mapping relationship (15) described in step S41, the force F in the motion space is mapped to the torque τ in the joint space, driving the 6-3-PUS parallel mechanism to perform the movement. The output variables of the parallel mechanism are the coordinate q and velocity in the joint space. Through the constraint equation solving method and forward kinematics analytical relationship described in step S11, the actual coordinate ρ of the motion space and its time differential are obtained respectively. Both are used as control feedback to calculate the error. The actual position and posture T of the moving platform is obtained by the forward kinematics numerical solution method described in step S11. The designed LQR controller is used for trajectory tracking control of the 6-3-PUS parallel mechanism. The aforementioned kinematic model and dynamic model are effectively used in the process to obtain motion feedback information and actual position and posture information. The correctness of the kinematic model and dynamic model of the 6-3-PUS parallel mechanism and the effectiveness of the LQR controller are verified by the control effect of trajectory tracking.

[0113] Corresponding to the aforementioned embodiment of the trajectory tracking control method of the 6-3-PUS parallel mechanism, the present application also provides an embodiment of the trajectory tracking control device of the 6-3-PUS parallel mechanism.

[0114] Figure 5 FIG. 1 is a block diagram of a trajectory tracking control device for a 6-3-PUS parallel mechanism according to an exemplary embodiment. Figure 5 , the device is applied to the 6-3-PUS parallel mechanism and may include:

[0115] Modeling module 21, used to establish kinematic model and dynamic model of 6-3-PUS parallel mechanism;

[0116] A controller design module 22 is used to select state variables, transform the dynamic model into a linear state space, and design an LQR controller, wherein the input of the controller is the motion coordinate error and the error of the motion coordinate time differential, and the output is the force in the motion space;

[0117] The solution module 23 is used to use the LQR controller for trajectory tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint to control the 6-3-PUS parallel mechanism to perform motion, where ρ and Used as feedback for the LQR controller.

[0118] Regarding the device in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.

[0119] For the device embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer to the partial description of the method embodiment. The device embodiment described above is only schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present application scheme. A person of ordinary skill in the art can understand and implement it without paying any creative work.

[0120] Correspondingly, the present application also provides an electronic device, comprising: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the trajectory tracking control method of the 6-3-PUS parallel mechanism as described above.

[0121] Correspondingly, the present application also provides a computer-readable storage medium on which computer instructions are stored, characterized in that when the instructions are executed by a processor, the trajectory tracking control method of the 6-3-PUS parallel mechanism as described above is implemented.

[0122] Those skilled in the art will readily appreciate other embodiments of the present application after considering the description and practicing the contents disclosed herein. The present application is intended to cover any variations, uses or adaptations of the present application, which follow the general principles of the present application and include common knowledge or customary techniques in the art that are not disclosed in the present application. The description and examples are intended to be exemplary only, and the true scope and spirit of the present application are indicated by the following claims.

[0123] It should be understood that the present application is not limited to the precise structures that have been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present application is limited only by the appended claims.

Claims

1. A trajectory tracking control method for a 6-3-PUS parallel mechanism, characterized in that: The 6-3-PUS parallel mechanism is a parallel robot with six degrees of freedom. The 6 and 3 in the name represent the number of connection points of the branch chain on the static platform and the dynamic platform, respectively. P, U, and S represent three types of kinematic pairs: mobile pair, universal pair, and spherical pair. The connection order of the overall structure from bottom to top is: static platform-mobile pair P-universal pair U-spherical pair S-dynamic platform. The method includes: Establish the kinematic model and dynamic model of 6-3-PUS parallel mechanism; Selecting state variables, transforming the dynamic model into a linear state space, and designing an LQR controller, wherein the input of the controller is a motion coordinate error and an error of a time differential of the motion coordinate, and the output of the controller is a force in the motion space; The LQR controller is used for trajectory tracking control of the 6-3-PUS parallel mechanism. The input of the tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint to control the 6-3-PUS parallel mechanism to perform motion, where ρ and Used as feedback for LQR controller; Among them, the kinematic model of the 6-3-PUS parallel mechanism is established, including: Establish the inverse kinematics model of 6-3-PUS parallel mechanism; According to the inverse kinematics model, the Newton iteration method is used to calculate the position T of the moving platform according to the input joint coordinates q; Design an analytical method for the forward kinematics of the 6-3-PUS parallel mechanism and establish the time differential of the joint coordinates and the time differential of the motion coordinates The analytical relationship of Among them, the dynamic model of the 6-3-PUS parallel mechanism is established, including: In the motion space of the 6-3-PUS parallel mechanism, an equivalent relationship between the motion coordinate ρ and its differential and the force F in the motion space is established, namely, the dynamic model of the 6-3-PUS parallel mechanism; Among them, the state variables are selected, the dynamic model is transformed into a linear state space, and an LQR controller is designed, wherein the input of the controller is the motion coordinate error and the error of the time differential of the motion coordinate, and the output is the force in the motion space, including: Take the motion coordinate ρ as the control variable and select the state variable Observe variable y=x, the state space equation of the LQR controller is: y=x The input of the controller is e=y d -y, where y d Expected value y is the actual value; the output of the controller is the force F=u+G in the motion space ρ , which is related to the control rate u of the controller, by solving the problem of minimizing the linear quadratic performance index: Get the control rate u, M ρ is the inertia matrix of the parallel mechanism as a whole, C ρ is the Coriolis force and centrifugal force matrix, G ρ is the gravity vector, Q is the state constraint matrix, R is the control constraint matrix, ρ d is the desired motion coordinate, is d The time differential of .

2. The method according to claim 1, characterized in that The LQR controller is used for trajectory tracking control of the 6-3-PUS parallel mechanism. The input of the tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint, and the 6-3-PUS parallel mechanism is controlled to perform motion, including: Get the mapping relationship between the force F in the motion space and the driving torque τ of the joint τ = J T F, where J is the joint velocity and motion coordinates The analytic Jacobian matrix between ; According to the actual coordinates q and actual speed of the joint output by the 6-3-PUS parallel mechanism By solving the constraint equations, the actual motion coordinates ρ are obtained, and by the analytical method of forward kinematics, the time differential of the actual motion coordinates is obtained. ρ and As control feedback, the error is calculated.

3. A trajectory tracking control device for a 6-3-PUS parallel mechanism, characterized in that: The 6-3-PUS parallel mechanism is a parallel robot with six degrees of freedom. The 6 and 3 in its name represent the number of connection points of the branch chain on the static platform and the dynamic platform respectively. P, U, and S represent three types of kinematic pairs: mobile pair, universal pair, and spherical pair. The connection order of the overall structure from bottom to top is: static platform-mobile pair P-universal pair U-spherical pair S-dynamic platform. The device includes: Modeling module, used to establish the kinematic model and dynamic model of the 6-3-PUS parallel mechanism; Design a controller module for selecting state variables, transforming the dynamic model into a linear state space, and designing an LQR controller, wherein the input of the controller is a motion coordinate error and an error of a time differential of the motion coordinate, and the output is a force in the motion space; Solving module, the LQR controller is used for trajectory tracking control of 6-3-PUS parallel mechanism, and the input of tracking control is the desired posture T d The output of the tracking control is the actual posture T. The LQR controller combines the 6-3-PUS parallel mechanism to actually output the joint coordinates q and joint velocities The actual position T, the actual motion coordinate ρ and the actual motion coordinate time differential are obtained through the kinematic model. The force F in the motion space output by the LQR controller is mapped to the driving torque τ of the joint to control the 6-3-PUS parallel mechanism to perform motion, where ρ and Used as feedback for LQR controller; Among them, the kinematic model of the 6-3-PUS parallel mechanism is established, including: Establish the inverse kinematics model of 6-3-PUS parallel mechanism; According to the inverse kinematics model, the Newton iteration method is used to calculate the position T of the moving platform according to the input joint coordinates q; Design an analytical method for the forward kinematics of the 6-3-PUS parallel mechanism and establish the time differential of the joint coordinates and the time differential of the motion coordinates The analytical relationship of Among them, the dynamic model of the 6-3-PUS parallel mechanism is established, including: In the motion space of the 6-3-PUS parallel mechanism, an equivalent relationship between the motion coordinate ρ and its differential and the force F in the motion space is established, namely, the dynamic model of the 6-3-PUS parallel mechanism; Among them, the state variables are selected, the dynamic model is transformed into a linear state space, and an LQR controller is designed, wherein the input of the controller is the motion coordinate error and the error of the time differential of the motion coordinate, and the output is the force in the motion space, including: Take the motion coordinate ρ as the control variable and select the state variable Observe variable y=x, the state space equation of the LQR controller is: y=x The input of the controller is e=y d -y, where y d Expected value y is the actual value; the output of the controller is the force F=u+G in the motion space ρ , which is related to the control rate u of the controller, by solving the problem of minimizing the linear quadratic performance index: Get the control rate u, M ρ is the inertia matrix of the parallel mechanism as a whole, C ρ is the Coriolis force and centrifugal force matrix, G ρ is the gravity vector, Q is the state constraint matrix, R is the control constraint matrix, ρ d is the desired motion coordinate, is d The time differential of .

4. An electronic device, characterized in that: include: one or more processors; A memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 2.

5. A computer-readable storage medium having computer instructions stored thereon, characterized in that: When the instruction is executed by a processor, the steps of the method according to any one of claims 1 to 2 are implemented.

Images