CN114859698A - Trajectory tracking control method and device of 6-3-PUS parallel mechanism - Google Patents
Trajectory tracking control method and device of 6-3-PUS parallel mechanism Download PDFInfo
- Publication number
- CN114859698A CN114859698A CN202210362525.4A CN202210362525A CN114859698A CN 114859698 A CN114859698 A CN 114859698A CN 202210362525 A CN202210362525 A CN 202210362525A CN 114859698 A CN114859698 A CN 114859698A
- Authority
- CN
- China
- Prior art keywords
- motion
- pus
- parallel mechanism
- controller
- actual
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 230000007246 mechanism Effects 0.000 title claims abstract description 104
- 238000000034 method Methods 0.000 title claims abstract description 59
- 238000013507 mapping Methods 0.000 claims abstract description 18
- 238000004458 analytical method Methods 0.000 claims description 16
- 230000004069 differentiation Effects 0.000 claims description 11
- 238000013461 design Methods 0.000 claims description 8
- 238000004364 calculation method Methods 0.000 claims description 7
- 230000004044 response Effects 0.000 claims description 7
- 230000002441 reversible effect Effects 0.000 claims description 6
- 238000003860 storage Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims 1
- 230000003068 static effect Effects 0.000 description 12
- 230000008569 process Effects 0.000 description 11
- 230000002829 reductive effect Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 8
- 239000011159 matrix material Substances 0.000 description 8
- 238000005259 measurement Methods 0.000 description 4
- 238000006073 displacement reaction Methods 0.000 description 3
- 238000005265 energy consumption Methods 0.000 description 3
- 238000013519 translation Methods 0.000 description 3
- 230000036961 partial effect Effects 0.000 description 2
- 230000006978 adaptation Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B11/00—Automatic controllers
- G05B11/01—Automatic controllers electric
- G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
- G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P. I., P. I. D.
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Feedback Control In General (AREA)
Abstract
The invention discloses a track tracking control method and a track tracking control device for a 6-3-PUS parallel mechanism, wherein the method comprises the following steps: establishing a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism; selecting a state variable, converting 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 of a motion space; the LQR controller is used for the track tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the expected pose T d The output of the tracking control is the actual pose T, and the LQR controller is combined with the 6-3-PUS parallel mechanism to actually output the joint coordinate q and the joint speed
Obtaining the actual pose T, the actual motion coordinate rho and the actual motion coordinate time differential through a kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein rho and
used as feedback for the LQR controller.
Description
Technical Field
The application relates to the technical field of motion control and robotics, in particular to a trajectory tracking control method and device for a 6-3-PUS parallel mechanism.
Background
The 6-3-PUS parallel mechanism is a six-freedom parallel robot, which is composed of a static platform, a movable platform and six branched chains. Different from a common six-degree-of-freedom parallel mechanism, such as a Stewart platform, a branch driver of a 6-3-PUS parallel mechanism is arranged on a static platform, so that the limitation on the motion of a branched chain is reduced, the working space is larger, the dynamic performance is stronger, and the six-degree-of-freedom parallel mechanism has wide application prospects in the aspects of production, processing, motion test and the like. The aim of the track tracking control is to change the driving torque so that the parallel mechanism moves according to the expected track, and although the common feedback control algorithm based on a kinematic model is simple in calculation, the strategy of iteratively correcting errors leads to the control hysteresis. In addition, the actual pose of the parallel mechanism moving platform required for controlling feedback is often obtained by an accurate measurement system, which undoubtedly increases the limitation on feedback motion control.
Disclosure of Invention
The embodiment of the application aims to provide a method and a device for controlling the trajectory tracking of a 6-3-PUS parallel mechanism, so as to solve the technical problems 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 an attitude measurement system in the related technology.
According to a first aspect of the embodiments of the present application, there is provided a trajectory tracking control method for a 6-3-PUS parallel mechanism, including:
establishing a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism;
selecting a state variable, converting the dynamic model into a linear state space, and designing an LQR (Linear motion response) 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 of a motion space;
the LQR controller is used for the track tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the expected pose T d The output of the tracking control is the actual pose T, and the actual output of the LQR controller is combined with the actual output of the 6-3-PUS parallel mechanismJoint coordinate q and joint velocity
Obtaining an actual pose T, an actual motion coordinate rho and an actual motion coordinate time differential through the kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein rho and
used as feedback for the LQR controller.
Further, establishing a kinematic model of the 6-3-PUS parallel mechanism, comprising:
establishing a reverse kinematics model of the 6-3-PUS parallel mechanism;
calculating the pose T of the moving platform according to the input joint coordinate q by using a Newton iteration method according to the inverse kinematics model;
designing a forward kinematics analysis method of a 6-3-PUS parallel mechanism, and establishing time differentiation of joint coordinates
And time differentiation of motion coordinates
The analytical relationship of (1).
Further, establishing a kinetic model of the 6-3-PUS parallel mechanism, comprising:
in the motion space of the 6-3-PUS parallel mechanism, establishing an equivalent relation between a motion coordinate rho and a derivative thereof and a force F of the motion space, namely a dynamic model of the 6-3-PUS parallel mechanism.
Further, selecting a state variable, converting the kinetic equation 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 time differential of the motion coordinate, and the output is a force of the motion space, and the method comprises the following steps:
selecting a state variable by taking the motion coordinate rho as a control variable
And the observed variable y is x, and the state space equation of the LQR controller is as follows:
y＝x
wherein the input of the controller is e-y d -y, wherein y d To a desired value
y is an actual value; the output of the controller is the force F ═ u + G of the motion space ρ And (3) related to the control rate u of the controller, solving the problem of minimizing the linear quadratic performance index:
the control rate u is obtained.
Further, the LQR controller is used for track tracking control of the 6-3-PUS parallel mechanism, and the input of the track tracking control is the expected pose T d And the output of tracking control is an actual pose T, and the LQR controller is combined with the actual output joint coordinate q and joint speed of the 6-3-PUS parallel mechanism
Obtaining the actual pose T, the actual motion coordinate rho and the actual motion coordinate time differential through the kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein the method comprises the following steps:
acquiring a mapping relation tau of a force F in a motion space and a driving moment tau of a joint, wherein the mapping relation tau is J T F, wherein J is joint velocity
And motion coordinates
Analytic jacobian matrices between;
according to the actual coordinate q and the actual speed of the joint output by the 6-3-PUS parallel mechanism
Solving the constraint equation to obtain the actual motion coordinate rho, and solving the time differential of the actual motion coordinate by the analytic method of forward kinematics
ρ and
as control feedback, an error is calculated.
According to a second aspect of the embodiments of the present application, there is provided a trajectory tracking control device of a 6-3-PUS parallel mechanism, including:
the modeling module is used for establishing a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism;
the system comprises a design controller module, a linear state space calculation module and a linear state space calculation module, wherein the design controller module is used for selecting a state variable, converting the dynamic model into a linear state space, and designing an LQR (Linear motion response) controller, wherein the input of the controller is a motion coordinate error and an error of motion coordinate time differential, and the output is a force of a motion space;
the solving module is used for using the LQR controller for the track tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the expected pose T d And the output of tracking control is an actual pose T, and the LQR controller is combined with the actual output joint coordinate q and joint speed of the 6-3-PUS parallel mechanism
Obtaining an actual pose T, an actual motion coordinate rho and an actual motion coordinate through the kinematics modelTemporal differentiation of the motion coordinate of the world
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein rho and
used as feedback for the LQR controller.
According to a third aspect of embodiments of the present application, there is provided an electronic apparatus, including:
one or more processors;
a memory for storing one or more programs;
when executed by the one or more processors, cause the one or more processors to implement a method as described in the first aspect.
According to a fourth aspect of embodiments herein, there is provided a computer readable storage medium having stored thereon computer instructions which, when executed by a processor, implement the steps of the method according to the first aspect.
The technical scheme provided by the embodiment of the application can have the following beneficial effects:
according to the embodiment, the dynamic analysis of the 6-3-PUS parallel mechanism is performed, the dynamic moment conditions of the movable platform and the joint are considered in the control, and the control precision is improved compared with a control method based on the kinematic analysis; because the appropriate driving torque can be input in time, the energy consumption of the system is reduced; because of the adoption of the LQR controller, the dependence on control gain parameters is reduced.
It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application.
Drawings
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 application.
FIG. 1 is a block diagram illustrating a 6-3-PUS parallel mechanism according to an exemplary embodiment.
FIG. 2 is a flow diagram illustrating a trajectory tracking control method of a 6-3-PUS parallel mechanism in accordance with an exemplary embodiment.
FIG. 3 is a flow diagram illustrating a forward kinematic numerical solution for a 6-3-PUS parallel mechanism in accordance with an exemplary embodiment.
FIG. 4 is a control block diagram illustrating a 6-3-PUS parallel mechanism based on LQR control according to an exemplary embodiment.
FIG. 5 is a block diagram illustrating a trajectory tracking control device of a 6-3-PUS parallel mechanism in accordance with an exemplary embodiment.
Detailed Description
Reference will now be made in detail to the exemplary embodiments, examples of which are illustrated in the accompanying drawings. When the following description refers to the accompanying drawings, like numbers in different drawings represent the same or similar elements unless otherwise indicated. The embodiments described in the following exemplary embodiments do not represent all embodiments consistent with the present application. Rather, they are merely examples of apparatus and methods consistent with certain aspects of the present application, as detailed in the appended claims.
The terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the application. As used in this application and the appended claims, the singular forms "a", "an", and "the" are intended to include the plural forms as well, unless the context clearly indicates otherwise. It should also be understood that the term "and/or" as used herein refers to and encompasses any and all possible combinations of one or more of the associated listed items.
The 6-3-PUS parallel mechanism is a six-degree-of-freedom parallel robot, 6 and 3 in the nomenclature respectively represent the connecting point number of a branched chain on a static platform and a movable platform, and P, U, S respectively represent three kinematic pairs (elements for connecting two mechanical components and restraining the relative motion of the two mechanical components): the moving pair, the universal pair and the spherical pair are sequentially connected in sequence from bottom to top: the system comprises a static platform, a moving pair P, a universal pair U, a spherical pair S and a moving platform. Different from a common six-degree-of-freedom parallel mechanism, the PUS parallel mechanism realizes the motion of a movable platform by changing the position of a hinge point of a branched chain and a static platform, and the length of the branched chain is kept unchanged. The structure design ensures that the driver is not arranged on the branched chain any more, reduces the obstruction to the movement of the branched chain, improves the dynamic performance and is convenient for increasing the working space; and the resistance required to be overcome during the execution of the driver is reduced, the requirement on the driving torque of the motor is reduced, and the effective utilization rate of the driving torque is improved.
In order to make the description of the method more clear, the variables and symbols referred to in the description of the method are explained here as follows. FIG. 1 is a simplified schematic diagram of a 6-3-PUS parallel mechanism according to an exemplary embodiment, as shown in FIG. 1, the 6-3-PUS parallel mechanism is composed of a static platform (positive Δ A) 1 A 2 A 3 ) Moving platform (positive delta P) 1 P 2 P 3 ) And three pairs of branched chains B 2k-1 P k 、B 2k P k (k-1, 2,3) composition, Δ a 1 A 2 A 3 And Δ P 1 P 2 P 3 The side lengths of (A) are LA and LP, respectively, LP is LA/2, and the branch length is a fixed value l. { O P }、{O B And the origins of the coordinate systems are located at the central positions of the regular triangles of the movable platform and the static platform respectively. P k The upper hinge point of the branched chain and the movable platform is coincident with the upper hinge point of the two branched chains at one side of the static platform, P k Relative motion platform coordinate system { O P The position of the magnet is not changed, B 2k-1 And B 2k Is the lower hinge point of two branched chains at one side and the static platform, and the lower hinge point can be along the static platform delta A 1 A 2 A 3 Is moved. Delta A 1 A 2 A 3 The middle point of the three sides is E k (k＝1,2,3)，E k To B 2k-1 And B 2k Are each q 2k-1 And q is 2k This is the input variable controlling the motion of the parallel mechanism, also the joint space coordinates, q ═ q 1 ,q 2 ,q 3 ,q 4 ,q 5 ,q 6 ] T 。φ i Represents an isosceles Delta B 2k-1 P k B 2k (k is 1,2,3) andthe included angle of the plane where the static platform is located is phi ═ phi 1 ,φ 2 ,φ 3 ] T Phi is related to q. Defining motion space coordinates
In practical control, q also needs to be converted into a control variable of the driver, namely the input torque τ of the driving motor.
The objective of trajectory tracking control is to input the most appropriate drive variable so that the parallel mechanism executes a motion according to a predetermined trajectory. The common control method is based on a kinematic model and inputs target displacement or speed for feedback control, but because the method lacks observation of dynamic driving torque and only enables actual output to approach a target value through feedback adjustment, obvious control delay exists, and the control performance depends on gain parameters, which causes inconvenience to system design. For a nonlinear system such as a parallel mechanism, a dynamic model is required to be established for more accurate and rapid control, control input can be adjusted in time according to the state requirement of the system, control response time is shortened, and control precision is improved.
Fig. 2 is a flow chart illustrating a trajectory tracking control method of a 6-3-PUS parallel mechanism according to an exemplary embodiment, where the method is applied to the 6-3-PUS parallel mechanism, as shown in fig. 2, and may include the following steps:
step S11: establishing a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism;
step S12: selecting a state variable, converting the dynamic model into a linear state space, and designing an LQR (Linear motion response) 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 of a motion space;
step S13: the LQR controller is used for the track tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the expected pose T d And the output of tracking control is an actual pose T, and the LQR controller is combined with the actual output joint coordinate q and joint speed of the 6-3-PUS parallel mechanism
Obtaining the actual pose T, the actual motion coordinate rho and the actual motion coordinate time differential through the kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein rho and
used as feedback for the LQR controller.
According to the embodiment, the dynamic analysis of the 6-3-PUS parallel mechanism is performed, the dynamic moment conditions of the movable platform and the joint are considered in the control, and the control precision is improved compared with a control method based on the kinematic analysis; because the appropriate driving torque can be input in time, the energy consumption of the system is reduced; because of the adoption of the LQR controller, the dependence on control gain parameters is reduced.
In the specific implementation of step S11, establishing a kinematic model and a kinetic model of the 6-3-PUS parallel mechanism;
specifically, by applying a positive kinematics solving method (numerical method and analytic method), the joint coordinates and speed which are convenient to observe are converted into the pose and speed of the movable platform, an accurate pose measurement system is not needed, the actual pose and speed of the movable platform can be obtained, the limit of pose measurement is overcome, and the process of establishing the kinematics model of the 6-3-PUS parallel mechanism can comprise the following steps:
step S21: establishing a reverse kinematics model of the 6-3-PUS parallel mechanism;
specifically, for a certain known moving platform pose, also called a motion space coordinate, used for describing the position and the posture of the moving platform, T ═ T is defined x ,t y ,t z ,α,β,γ] T . T in the stationary platform coordinate system { O } B The motion vector h can be represented by a translation vector h and a rotation matrix R, wherein the translation vector h is represented by a translation amount t of three axes of x, y and z x ,t y ,t z Represents: h ═ t x ,t y ,t z ] T The rotation matrix R is expressed by euler angles α, β, γ around the three axes x, y, z:
as shown in fig. 1, in the stationary platform coordinate system O B The following space vector relationships exist in the structure:
Rp i +h-b i ＝l i (i＝1,...,6) (1)
p i is an upper hinge point
In the coordinate system { O P Position vector in (b) } b i Is a lower hinge point B i In the coordinate system { O B Position vector in }, l i Is a branched chain in the coordinate system { O B Position vector in. Due to B i Along Δ A 1 A 2 A 3 So that the vector b moves i With a linear constraint on the x and y coordinates, vector b i Can be expressed as:
b i ＝[x i μ i x i +v i 0] T (2)
wherein x i Is a lower hinge point B i X coordinate of (d), mu i And v i Is a lower hinge point B i A linear constraint coefficient between the x-coordinate and the y-coordinate,
in the PUS parallel mechanism, the branch chain l i The length remains constant, l, so it is possible to substitute equation (2) into equation (1) and simultaneously square both sides of the equal sign of equation (1) to obtain information about x i And under the condition that the equation has real roots, two real roots of each equation are effective and respectively represent the x coordinates of two lower hinge points at one side of the static platform. From x i Further obtaining a joint coordinate variable q according to the geometric relation of the regular triangle i The value of (a) is,thereby calculating joint coordinates q. Further, from the geometrical relationship existing in the parallel mechanism, the geometrical constraint equation r (q, phi) of the joint coordinate input q and the angle phi can be obtained to be 0, wherein three equations are included, for the known q, phi is solved, and according to the actual state of the parallel mechanism, a consistent unique solution is obtained, so that the motion coordinate rho is obtained, and further differential is carried out
And completing the construction of the reverse kinematics model. And establishing a basic reverse kinematics model, determining a calculation process from the platform pose to the motion coordinate, and laying a theoretical foundation for subsequent forward kinematics analysis, dynamics analysis and trajectory tracking control.
Step S22: designing a numerical method of forward kinematics of the 6-3-PUS parallel mechanism, and calculating the pose T of the moving platform according to the input joint coordinate q by using a Newton iteration method according to the reverse kinematics model;
specifically, the inverse kinematics process described at step S21 is defined as q i ＝g i (t x ,t y ,t z α, β, γ), known joint coordinate variables
The constraint equation between the pose parameter of the moving platform and the joint coordinate variable is
Recording the error f ═ f 1 ,f 2 ,f 3 ,f 4 ,f 5 ,f 6 ] T The goal of forward kinematics is to follow a given joint coordinate variable q 0 Calculating moving platform pose T ═ T x ,t y ,t z ,α,β,γ] T So that f is equal to 0, the method adopts a Newton iteration method to solve, and the solving process is shown in FIG. 3 and comprises the following steps:
s31: selecting an initial pose
k represents the iteration cycle, setting a very small positive value epsilon 1 And ε 2 For judging whether the iteration is terminated;
s32: substituting the pose constraint equation (3) to calculate the error f (T) (k) )；
S33: if | < f (T) (k) )‖＜ε 1 End of iteration, T (k) The pose of the moving platform is obtained, otherwise, the next step is carried out;
s34: calculating f with respect to pose T (k) Partial derivatives of
S35: is composed of J (T) (k) )ΔT＝-f(T (k) ) Calculating a pose correction value delta T;
s36: if | < ε | T | < |) 2 End of iteration, T (k) The pose is the position pose of the moving platform, otherwise, the pose T is updated (k+1) ＝T (k) + Δ T, the update iteration count k ═ k +1, and the process returns to step S32.
By the process, a proper initial value is selected, when a precise pose measuring instrument is lacked, an actual joint coordinate variable can be obtained according to an encoder feedback value of a driving motor, and the actual pose of the movable platform is obtained by a numerical method of forward kinematics. The forward kinematics is solved through a Newton iteration method, the actual pose can be simply and quickly obtained, and the motion state of the moving platform can be conveniently observed.
Step S23: designing a forward kinematics analysis method of a 6-3-PUS parallel mechanism, and establishing time differentiation of joint coordinates
And time differentiation of motion coordinates
The analytical relationship of (1).
Specifically, from the geometrical relationship existing in the parallel mechanism, a geometrical constraint equation r (q, phi) of the joint coordinate input q and the angle phi is 0, the geometrical constraint equation comprises three equations, phi is solved for the known q, and a consistent unique solution is obtained according to the actual state of the parallel mechanism. The motion constraint r (q, Φ) is 0, and time differentiation is performed to obtain:
can obtain the product
Comprises the following steps:
substituting equation (5) into
Can obtain the product
I.e. time differentiation of joint coordinates
And time differentiation of motion coordinates
The following analytical relationship exists:
wherein J is the analytic Jacobian matrix:
thus completingThe time differentiation of joint coordinates is described by the analytic method of forward kinematics
The time differential of the motion coordinate can be determined by equation (7)
By the above forward kinematics analysis method, the conversion relationship between the joint coordinates and the motion coordinates on the time differential can be geometrically determined, which facilitates the feedback calculation in the motion control.
Specifically, the process of establishing a kinetic model of the 6-3-PUS parallel mechanism may include:
in the motion space of the 6-3-PUS parallel mechanism, establishing an equivalent relation between a motion coordinate rho and a derivative thereof and a force F of the motion space, namely a dynamic model of the 6-3-PUS parallel mechanism.
Specifically, the dynamic model of the 6-3-PUS parallel mechanism mainly comprises a dynamic model of a kinematic joint and a dynamic model of a moving platform, and can be deduced through a Lagrange equation. In the motion space, the overall kinetic equation of the 6-3-PUS parallel mechanism with respect to the motion coordinate p is as follows:
wherein M is ρ An inertia matrix, C, integral with the parallel mechanism ρ Is a matrix of Coriolis forces and centrifugal forces, G ρ Is the gravity vector and F is the force of the motion space. The driving force required by the parallel mechanism in the motion process can be obtained through the dynamic model, so that more appropriate torque is input in the motion process, the control power consumption is reduced, and the damage to mechanical equipment is reduced.
In the specific implementation of step S12, selecting a state variable, converting the dynamic model into a linear state space, and designing an LQR controller, where the input of the controller is a motion coordinate error and an error of a motion coordinate time differential, and the output is a force of a motion space;
specifically, in order to apply an LQR (linear quadratic regulator) control method using the motion coordinate ρ as a control variable, it is necessary to convert the kinetic equation of the 6-3-PUS parallel mechanism into a linear state space equation. Consider the following equation of state space for a linear system:
selecting state variables
The observed variable y ═ x, combined with equation (10), the state space equation translates to:
as can be seen by comparison of equations (10) and (11):
in the track following task, the control target 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 As a desired variable
Including the desired pose and desired velocity of the mobile platform. Therefore, the control objective is to find a control rate u such that the linear quadratic performance index J is minimized, which is calculated by the following formula:
wherein Q is a state constraint matrix, R is a control constraint matrix, and the Q and R are diagonal matrices, and the values are selected by an optimization idea so as to achieve the balance between the response speed and the system energy consumption.
Control rate u ═ K c x+K g y d In, K c ＝-R -1 B T P, wherein P is an algebraic Riccati equation PA + A T P-PBR -1 B T P+C T Solution of QC ═ 0, K g ＝R -1 B T (PBR -1 B T -A T )C T Q，
The output that translates into the controller, i.e. the force of the motion space, is:
F＝u+G ρ ＝K c x+K g y d +G ρ (14)
the controller is established by utilizing the LQR principle and the dynamic model, so that the advantages of dynamic analysis are effectively utilized, and the constraint influence of control gain parameters is reduced, thereby improving the motion control precision of the 6-3-PUS parallel mechanism.
In the specific implementation of the step S13, the LQR controller is used for the track tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the expected pose T d And the output of tracking control is an actual pose T, and the LQR controller is combined with the actual output joint coordinate q and joint speed of the 6-3-PUS parallel mechanism
Obtaining the actual pose T, the actual motion coordinate rho and the actual motion coordinate time differential through the kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein rho and
as feedback to the LQR controller;
in particular, this step may comprise the following sub-steps:
step S41: obtaining a mapping of forces F and drive moments τ of a joint in a motion spaceThe relation τ is J T F, wherein J is joint velocity
And motion coordinates
Analytic jacobian matrices between;
specifically, in the actual control, the control torque of the 6-3-PUS parallel mechanism is not the force F of the motion space, but the control torque τ of the joint space, so that the force distribution needs to be realized, and the torque F of the motion space is mapped to each joint.
From the theorem of virtual work, it can be known that the virtual work δ W performed by all drivers and external forces is zero, that is:
δW＝τ T δq-F T δρ＝0 (15)
the imaginary displacement δ q may be equivalent to the differential of q with respect to time
δ ρ may be equivalent to the differential of ρ with respect to time
The imaginary displacements δ q and δ ρ can therefore be related by an analytical jacobian matrix defined by equation (7): δ ρ is J δ q, and (τ) is obtained by substituting the formula (15) with δ ρ T J T -F T ) δ ρ is 0, which holds true for an arbitrary virtual displacement δ ρ, so τ T J T -F T When 0, we get:
τ＝J T F (16)
this step translates the moment information from the motion space to the joint space, thereby controlling the parallel mechanism to perform an effective motion.
Step S42: according to the actual coordinate q and the actual speed of the joint output by the 6-3-PUS parallel mechanism
Solving through a constraint equation to obtain an actual motion coordinate rho, and solving through an analytic method of forward kinematics to obtain an actual motion coordinate rhoTime differentiation of the actual motion coordinate
ρ and
as control feedback, an error is calculated.
Specifically, the force F of the motion space can be converted into the driving moment τ of the joint by the mapping relationship between the force F of the motion space and the driving moment τ of the joint. FIG. 4 is a block diagram illustrating a trajectory tracking control of a 6-3-PUS parallel mechanism for a desired pose T of a moving platform according to an exemplary embodiment d And carrying out inverse kinematics solution to obtain an expected motion coordinate rho d And time differential thereof
Designing the LQR controller in the step S13, wherein the controller inputs the motion coordinate error and the motion coordinate time differential error, and outputs the force F of the motion space, namely the force applied to the movable platform; mapping the force F of the motion space to the moment tau of the joint space through the mapping relation (15) in the step S41, and driving the 6-3-PUS parallel mechanism to execute motion; the output variables of the parallel mechanism are the coordinate q and the speed of the joint space
Respectively obtaining the actual coordinate p of the motion space and the time differential thereof by the constraint equation solving method and the forward kinematics analytic relationship in the step S11
Both are used as control feedback to calculate errors. And (4) solving the actual pose T of the moving platform by the forward kinematics numerical solution method in the step S11. The designed LQR controller is used for the track tracking control of the 6-3-PUS parallel mechanism, the kinematic model and the dynamic model are effectively utilized in the process, so that the feedback information of motion and the actual pose information are obtained, and the 6-3-P is verified through the control effect of track trackingCorrectness of kinematic and kinetic models of the US parallel mechanism, and validity of the LQR controller.
Corresponding to the embodiment of the trajectory tracking control method of the 6-3-PUS parallel mechanism, the application also provides an embodiment of a trajectory tracking control device of the 6-3-PUS parallel mechanism.
FIG. 5 is a block diagram illustrating a trajectory tracking control device of a 6-3-PUS parallel mechanism in accordance with an exemplary embodiment. Referring to fig. 5, the apparatus is applied to a 6-3-PUS parallel mechanism, and may include:
the modeling module 21 is used for establishing a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism;
a design controller module 22, configured to select a state variable, convert the dynamic model into a linear state space, and design an LQR controller, where the input of the controller is a motion coordinate error and an error of motion coordinate time differential, and the output is a force of a motion space;
a solving module 23, configured to use the LQR controller for trajectory tracking control of the 6-3-PUS parallel mechanism, where an input of the tracking control is an expected pose T d And the output of tracking control is an actual pose T, and the LQR controller is combined with the actual output joint coordinate q and joint speed of the 6-3-PUS parallel mechanism
Obtaining the actual pose T, the actual motion coordinate rho and the actual motion coordinate time differential through the kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein rho and
as feedback to the LQR controller.
With regard to the apparatus in the above-described embodiment, the specific manner in which each module performs the operation has been described in detail in the embodiment related to the method, and will not be elaborated here.
For the device embodiments, since they substantially correspond to the method embodiments, reference may be made to the partial description of the method embodiments for relevant points. The above-described embodiments of the apparatus are merely illustrative, and the units described as separate parts may or may not be physically separate, and parts displayed as units may or may not be physical units, may be located in one position, or may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of the application. One of ordinary skill in the art can understand and implement it without inventive effort.
Correspondingly, the present application also provides an electronic device, comprising: one or more processors; a memory for storing one or more programs; when executed by the one or more processors, cause the one or more processors to implement a trajectory tracking control method of a 6-3-PUS parallel mechanism as described above.
Accordingly, the present application also provides a computer readable storage medium having stored thereon computer instructions, wherein the instructions, when executed by a processor, implement the trajectory tracking control method of the 6-3-PUS parallel mechanism as described above.
Other embodiments of the present application will be apparent to those skilled in the art from consideration of the specification and practice of the disclosure disclosed herein. This application is intended to cover any variations, uses, or adaptations of the invention following, in general, the principles of the application and including such departures from the present disclosure as come within known or customary practice within the art to which the invention pertains. It is intended that the specification and examples be considered as exemplary only, with a true scope and spirit of the application being indicated by the following claims.
It will be understood that the present application is not limited to the precise arrangements 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 application is limited only by the appended claims.
Claims (8)
1. A trajectory tracking control method of a 6-3-PUS parallel mechanism is characterized by comprising the following steps:
establishing a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism;
selecting a state variable, converting the dynamic model into a linear state space, and designing an LQR (Linear motion response) 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 of a motion space;
the LQR controller is used for the track tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the expected pose T d And the output of tracking control is an actual pose T, and the LQR controller is combined with the actual output joint coordinate q and joint speed of the 6-3-PUS parallel mechanism
Obtaining the actual pose T, the actual motion coordinate rho and the actual motion coordinate time differential through the kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein rho and
used as feedback for the LQR controller.
2. The method of claim 1, wherein establishing a kinematic model of the 6-3-PUS parallel mechanism comprises:
establishing a reverse kinematics model of the 6-3-PUS parallel mechanism;
calculating the pose T of the moving platform according to the input joint coordinate q by using a Newton iteration method according to the inverse kinematics model;
designing the Forward direction of the 6-3-PUS parallel mechanismKinematic analysis method for establishing time differential of joint coordinates
And time differentiation of motion coordinates
The analytical relationship of (1).
3. The method of claim 1, wherein establishing a kinetic model of the 6-3-PUS parallel mechanism comprises:
in the motion space of the 6-3-PUS parallel mechanism, establishing an equivalent relation between a motion coordinate rho and a derivative thereof and a force F of the motion space, namely a dynamic model of the 6-3-PUS parallel mechanism.
4. The method of claim 1, wherein selecting state variables, transforming the kinetic equations into a linear state space, designing an LQR controller, wherein the controller has inputs of kinematic coordinate errors and kinematic coordinate time differential errors, and outputs of kinematic space forces, comprises:
selecting a state variable by taking the motion coordinate rho as a control variable
And the observed variable y is x, and the state space equation of the LQR controller is as follows:
y＝x
wherein the input of the controller is e-y d -y, wherein y d To a desired value
y is an actual value; the output of the controller is the force F ═ u + G of the motion space ρ And (3) related to the control rate u of the controller, solving the problem of minimizing the linear quadratic performance index:
the control rate u is obtained.
5. The method according to claim 1, wherein the LQR controller is used for trajectory tracking control of a 6-3-PUS parallel mechanism, and the input of the tracking control is a desired pose T d And the output of tracking control is an actual pose T, and the LQR controller is combined with the actual output joint coordinate q and joint speed of the 6-3-PUS parallel mechanism
Obtaining the actual pose T, the actual motion coordinate rho and the actual motion coordinate time differential through the kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein the method comprises the following steps:
acquiring a mapping relation tau of a force F in a motion space and a driving moment tau of a joint, wherein the mapping relation tau is J T F, wherein J is joint velocity
And motion coordinates
Analytic jacobian matrices between;
according to the actual coordinate q and the actual speed of the joint output by the 6-3-PUS parallel mechanism
Solving the constraint equation to obtain the actual motion coordinate rho, and solving the time differential of the actual motion coordinate by the analytic method of forward kinematics
ρ and
as control feedback, an error is calculated.
6. A trajectory tracking control device of a 6-3-PUS parallel mechanism is characterized by comprising:
the modeling module is used for establishing a kinematic model and a dynamic model of the 6-3-PUS parallel mechanism;
the system comprises a design controller module, a linear state space calculation module and a linear state space calculation module, wherein the design controller module is used for selecting a state variable, converting the dynamic model into a linear state space, and designing an LQR (Linear motion response) controller, wherein the input of the controller is a motion coordinate error and an error of motion coordinate time differential, and the output is a force of a motion space;
the solving module is used for using the LQR controller for the track tracking control of the 6-3-PUS parallel mechanism, and the input of the tracking control is the expected pose T d And the output of tracking control is an actual pose T, and the LQR controller is combined with the actual output joint coordinate q and joint speed of the 6-3-PUS parallel mechanism
Obtaining the actual pose T, the actual motion coordinate rho and the actual motion coordinate time differential through the kinematic model
Mapping the force F of the motion space output by the LQR controller into the driving moment tau of the joint, and controlling the 6-3-PUS parallel mechanism to execute motion, wherein rho and
used as feedback for the LQR controller.
7. An electronic device, comprising:
one or more processors;
a memory for storing one or more programs;
when executed by the one or more processors, cause the one or more processors to implement the method of any one of claims 1-5.
8. A computer-readable storage medium having stored thereon computer instructions, which when executed by a processor, perform the steps of the method according to any one of claims 1-5.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210362525.4A CN114859698A (en) | 2022-04-07 | 2022-04-07 | Trajectory tracking control method and device of 6-3-PUS parallel mechanism |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210362525.4A CN114859698A (en) | 2022-04-07 | 2022-04-07 | Trajectory tracking control method and device of 6-3-PUS parallel mechanism |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114859698A true CN114859698A (en) | 2022-08-05 |
Family
ID=82629267
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210362525.4A Pending CN114859698A (en) | 2022-04-07 | 2022-04-07 | Trajectory tracking control method and device of 6-3-PUS parallel mechanism |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114859698A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117077498A (en) * | 2023-10-16 | 2023-11-17 | 季华实验室 | Modeling method and device for compliant parallel mechanism, electronic equipment and storage medium |
-
2022
- 2022-04-07 CN CN202210362525.4A patent/CN114859698A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117077498A (en) * | 2023-10-16 | 2023-11-17 | 季华实验室 | Modeling method and device for compliant parallel mechanism, electronic equipment and storage medium |
| CN117077498B (en) * | 2023-10-16 | 2024-02-09 | 季华实验室 | Modeling method and device for compliant parallel mechanism, electronic equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| WO2022252863A1 (en) | Control method and apparatus for wheel-legged robot, and wheel-legged robot and device | |
| US9031699B2 (en) | Kinematic predictor for articulated mechanisms | |
| CN110253574B (en) | Multi-task mechanical arm pose detection and error compensation method | |
| CN103558079B (en) | Based on the multiple degrees of freedom loading method of parallel institution driving force closed loop | |
| Liang et al. | Adaptive task-space tracking control of robots without task-space-and joint-space-velocity measurements | |
| CN107414827B (en) | Six-degree-of-freedom mechanical arm self-adaptive detection method based on linear feedback controller | |
| CN112417755A (en) | Master-slave mode surgical robot track prediction control method | |
| CN110053044B (en) | Model-free self-adaptive smooth sliding mode impedance control method for clamping serial fruits by parallel robot | |
| Shan et al. | Structural error and friction compensation control of a 2 (3PUS+ S) parallel manipulator | |
| CN107457783A (en) | Sixdegree-of-freedom simulation self-adapting intelligent detection method based on PD control device | |
| JP2018030210A (en) | Simulation device, control system, robot system, simulation method, program and recording medium | |
| CN112767493A (en) | Machine vision calibration method for kinematic parameters of Stewart platform | |
| CN114859698A (en) | Trajectory tracking control method and device of 6-3-PUS parallel mechanism | |
| CN109648564A (en) | A kind of control method of the multiple degrees of freedom flexible articulated mechanical arm system based on hierarchical structure MPC | |
| CN115946131A (en) | Flexible joint mechanical arm motion control simulation calculation method and device | |
| CN114310914A (en) | Fuzzy self-adaptive iterative trajectory tracking control method and system for multi-degree-of-freedom mechanical arm | |
| CN110900608B (en) | Robot kinematics calibration method based on optimal measurement configuration selection | |
| CN111546344A (en) | Mechanical arm control method for alignment | |
| CN108247636B (en) | Parallel robot closed-loop feedback control method, system and storage medium | |
| CN113650014B (en) | Redundant mechanical arm tracking control method based on echo state network | |
| CN114840947A (en) | Three-degree-of-freedom mechanical arm dynamic model with constraint | |
| CN115091455A (en) | Industrial robot positioning error compensation method | |
| Jiang | Vision-based Cartesian space motion control for flexible robotic manipulators | |
| CN114833828B (en) | Inertial parameter identification method, equipment and medium for two-degree-of-freedom system | |
| CN108466267A (en) | A kind of control method of 2R drive lackings planar manipulator |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |